26#if !defined(__linux__) || defined(__ANDROID__)
27#error "This file can only be compiled on Linux"
43#include <sys/socket.h>
51#include "model/comm_can_util.h"
52#include "model/comm_drv_n2k_socketcan.h"
55#include "model/config_vars.h"
57#define DEFAULT_N2K_SOURCE_ADDRESS 72
61static const int kNotFound = -1;
64static const int kSocketTimeoutSeconds = 2;
66typedef struct can_frame CanFrame;
69using namespace std::literals::chrono_literals;
101 int GetSocket() {
return m_socket; }
106 void ThreadMessage(
const std::string& msg, wxLogLevel l = wxLOG_Message);
108 int InitSocket(
const std::string port_name);
109 void SocketMessage(
const std::string& msg,
const std::string& device);
110 void HandleInput(CanFrame frame);
111 void ProcessRxMessages(std::shared_ptr<const Nmea2000Msg> n2k_msg);
113 std::vector<unsigned char> PushCompleteMsg(
const CanHeader header,
115 const CanFrame frame);
116 std::vector<unsigned char> PushFastMsgFragment(
const CanHeader& header,
120 const wxString m_port_name;
121 std::atomic<int> m_run_flag;
133 m_worker(
this, p->socketCAN_port),
134 m_source_address(-1),
135 m_last_TX_sequence(0) {
146 bool SendMessage(std::shared_ptr<const NavMsg> msg,
147 std::shared_ptr<const NavAddr> addr);
149 int DoAddressClaim();
150 bool SendAddressClaim(
int proposed_source_address);
151 bool SendProductInfo();
153 Worker& GetWorker() {
return m_worker; }
154 void UpdateAttrCanAddress();
159 int m_source_address;
160 int m_last_TX_sequence;
161 std::future<int> m_AddressClaimFuture;
166 bool HandleN2K_59904(std::shared_ptr<const Nmea2000Msg> n2k_msg);
171std::unique_ptr<CommDriverN2KSocketCAN> CommDriverN2KSocketCAN::Create(
173 return std::unique_ptr<CommDriverN2KSocketCAN>(
179void CommDriverN2KSocketCanImpl::SetN2K_Name() {
181 node_name.value.Name = 0;
186 std::string str(g_hostname.mb_str());
187 int len = str.size();
188 const char* ch = str.data();
189 for (
int i = 0; i < len; i++)
190 hash = hash + ((hash) << 5) + *(ch + i) + ((*(ch + i)) << 7);
191 m_unique_number = ((hash) ^ (hash >> 16)) & 0xffff;
193 node_name.SetManufacturerCode(2046);
194 node_name.SetUniqueNumber(m_unique_number);
195 node_name.SetDeviceFunction(130);
196 node_name.SetDeviceClass(120);
197 node_name.SetIndustryGroup(4);
198 node_name.SetSystemInstance(0);
201void CommDriverN2KSocketCanImpl::UpdateAttrCanAddress() {
202 this->attributes[
"canAddress"] = std::to_string(m_source_address);
205bool CommDriverN2KSocketCanImpl::Open() {
207 bool bws = m_worker.StartThread();
211void CommDriverN2KSocketCanImpl::Close() {
212 wxLogMessage(
"Closing N2K socketCAN: %s", m_params.socketCAN_port.c_str());
213 m_stats_timer.Stop();
214 m_worker.StopThread();
217bool CommDriverN2KSocketCanImpl::SendAddressClaim(
int proposed_source_address) {
218 wxMutexLocker lock(m_TX_mutex);
220 int socket = GetWorker().GetSocket();
222 if (socket < 0)
return false;
225 memset(&frame, 0,
sizeof(frame));
227 uint64_t _pgn = 60928;
228 unsigned long canId = BuildCanID(6, proposed_source_address, 255, _pgn);
229 frame.can_id = canId | CAN_EFF_FLAG;
232 uint32_t b32_0 = node_name.value.UnicNumberAndManCode;
233 memcpy(&frame.data, &b32_0, 4);
235 unsigned char b81 = node_name.value.DeviceInstance;
236 memcpy(&frame.data[4], &b81, 1);
238 b81 = node_name.value.DeviceFunction;
239 memcpy(&frame.data[5], &b81, 1);
241 b81 = (node_name.value.DeviceClass);
242 memcpy(&frame.data[6], &b81, 1);
244 b81 = node_name.value.IndustryGroupAndSystemInstance;
245 memcpy(&frame.data[7], &b81, 1);
249 int sentbytes = write(socket, &frame,
sizeof(frame));
251 return (sentbytes == 16);
254void AddStr(std::vector<uint8_t>& vec, std::string str,
size_t max_len) {
256 for (i = 0; i < str.size(); i++) {
257 vec.push_back(str[i]);
260 for (; i < max_len; i++) {
265bool CommDriverN2KSocketCanImpl::SendProductInfo() {
267 std::vector<uint8_t> payload;
269 payload.push_back(2100 & 0xFF);
270 payload.push_back(2100 >> 8);
271 payload.push_back(0xEC);
272 payload.push_back(0x06);
274 std::string ModelID(
"OpenCPN");
275 AddStr(payload, ModelID, 32);
277 std::string ModelSWCode(PACKAGE_VERSION);
278 AddStr(payload, ModelSWCode, 32);
280 std::string ModelVersion(PACKAGE_VERSION);
281 AddStr(payload, ModelVersion, 32);
283 std::string ModelSerialCode(
284 std::to_string(m_unique_number));
285 AddStr(payload, ModelSerialCode, 32);
287 payload.push_back(0);
288 payload.push_back(0);
290 auto dest_addr = std::make_shared<const NavAddr2000>(
iface, 255);
294 auto msg = std::make_shared<const Nmea2000Msg>(_PGN, payload, dest_addr);
295 SendMessage(msg, dest_addr);
300bool CommDriverN2KSocketCanImpl::SendMessage(
301 std::shared_ptr<const NavMsg> msg, std::shared_ptr<const NavAddr> addr) {
302 wxMutexLocker lock(m_TX_mutex);
305 if (m_source_address < 0)
return false;
307 if (m_source_address > 253)
310 int socket = GetWorker().GetSocket();
312 if (socket < 0)
return false;
315 memset(&frame, 0,
sizeof(frame));
317 auto msg_n2k = std::dynamic_pointer_cast<const Nmea2000Msg>(msg);
318 std::vector<uint8_t> load = msg_n2k->payload;
320 uint64_t _pgn = msg_n2k->PGN.pgn;
321 auto destination_address = std::static_pointer_cast<const NavAddr2000>(addr);
323 unsigned long canId = BuildCanID(msg_n2k->priority, m_source_address,
324 destination_address->address, _pgn);
326 frame.can_id = canId | CAN_EFF_FLAG;
330 if (load.size() <= 8) {
331 frame.can_dlc = load.size();
332 if (load.size() > 0) memcpy(&frame.data, load.data(), load.size());
334 sentbytes += write(socket, &frame,
sizeof(frame));
336 int sequence = (m_last_TX_sequence + 0x20) & 0xE0;
337 m_last_TX_sequence = sequence;
338 unsigned char* data_ptr = load.data();
339 int n_remaining = load.size();
343 frame.data[0] = sequence;
344 frame.data[1] = load.size();
345 int data_len_0 = wxMin(load.size(), 6);
346 memcpy(&frame.data[2], load.data(), data_len_0);
348 sentbytes += write(socket, &frame,
sizeof(frame));
350 data_ptr += data_len_0;
351 n_remaining -= data_len_0;
355 while (n_remaining > 0) {
357 frame.data[0] = sequence;
358 int data_len_n = wxMin(n_remaining, 7);
359 memcpy(&frame.data[1], data_ptr, data_len_n);
361 sentbytes += write(socket, &frame,
sizeof(frame));
363 data_ptr += data_len_n;
364 n_remaining -= data_len_n;
371 SetDriverStats(stats);
378CommDriverN2KSocketCAN::CommDriverN2KSocketCAN(
const ConnectionParams* params,
382 m_listener(listener),
383 m_stats_timer(*this, 2s),
385 m_portstring(params->GetDSPort()),
386 m_baudrate(wxString::Format(
"%i", params->Baudrate)) {
387 this->attributes[
"canPort"] = params->socketCAN_port.ToStdString();
388 this->attributes[
"canAddress"] = std::to_string(DEFAULT_N2K_SOURCE_ADDRESS);
389 this->attributes[
"userComment"] = params->UserComment.ToStdString();
390 this->attributes[
"ioDirection"] = std::string(
"IN/OUT");
392 m_driver_stats.driver_bus = NavAddr::Bus::N2000;
393 m_driver_stats.driver_iface = params->GetStrippedDSPort();
396CommDriverN2KSocketCAN::~CommDriverN2KSocketCAN() {}
402 m_port_name(port_name.Clone()),
405 assert(m_parent_driver != 0);
408std::vector<unsigned char> Worker::PushCompleteMsg(
const CanHeader header,
410 const CanFrame frame) {
411 std::vector<unsigned char> data;
412 data.push_back(0x93);
413 data.push_back(0x13);
414 data.push_back(header.priority);
415 data.push_back(header.pgn & 0xFF);
416 data.push_back((header.pgn >> 8) & 0xFF);
417 data.push_back((header.pgn >> 16) & 0xFF);
418 data.push_back(header.destination);
419 data.push_back(header.source);
420 data.push_back(0xFF);
421 data.push_back(0xFF);
422 data.push_back(0xFF);
423 data.push_back(0xFF);
424 data.push_back(CAN_MAX_DLEN);
425 for (
size_t n = 0; n < CAN_MAX_DLEN; n++) data.push_back(frame.data[n]);
426 data.push_back(0x55);
430std::vector<unsigned char> Worker::PushFastMsgFragment(
const CanHeader& header,
432 std::vector<unsigned char> data;
433 data.push_back(0x93);
434 data.push_back(fast_messages[position].expected_length + 11);
435 data.push_back(header.priority);
436 data.push_back(header.pgn & 0xFF);
437 data.push_back((header.pgn >> 8) & 0xFF);
438 data.push_back((header.pgn >> 16) & 0xFF);
439 data.push_back(header.destination);
440 data.push_back(header.source);
441 data.push_back(0xFF);
442 data.push_back(0xFF);
443 data.push_back(0xFF);
444 data.push_back(0xFF);
445 data.push_back(fast_messages[position].expected_length);
446 for (
size_t n = 0; n < fast_messages[position].expected_length; n++)
447 data.push_back(fast_messages[position].data[n]);
448 data.push_back(0x55);
449 fast_messages.
Remove(position);
453void Worker::ThreadMessage(
const std::string& msg, wxLogLevel level) {
454 wxLogGeneric(level, wxString(msg.c_str()));
455 auto s = std::string(
"CommDriverN2KSocketCAN: ") + msg;
459void Worker::SocketMessage(
const std::string& msg,
const std::string& device) {
460 std::stringstream ss;
461 ss << msg << device <<
": " << strerror(errno);
462 ThreadMessage(ss.str());
471int Worker::InitSocket(
const std::string port_name) {
472 int sock = socket(PF_CAN, SOCK_RAW, CAN_RAW);
474 SocketMessage(
"SocketCAN socket create failed: ", port_name);
479 struct ifreq if_request;
480 strcpy(if_request.ifr_name, port_name.c_str());
481 if (ioctl(sock, SIOCGIFINDEX, &if_request) < 0) {
482 SocketMessage(
"SocketCAN ioctl (SIOCGIFINDEX) failed: ", port_name);
487 struct sockaddr_can can_address;
488 can_address.can_family = AF_CAN;
489 can_address.can_ifindex = if_request.ifr_ifindex;
490 if (ioctl(sock, SIOCGIFFLAGS, &if_request) < 0) {
491 SocketMessage(
"SocketCAN socket IOCTL (SIOCGIFFLAGS) failed: ", port_name);
494 if (if_request.ifr_flags & IFF_UP) {
495 ThreadMessage(
"socketCan interface is UP");
497 ThreadMessage(
"socketCan interface is NOT UP");
503 tv.tv_sec = kSocketTimeoutSeconds;
506 setsockopt(sock, SOL_SOCKET, SO_RCVTIMEO, (
const char*)&tv,
sizeof tv);
508 SocketMessage(
"SocketCAN setsockopt SO_RCVTIMEO failed on device: ",
512 r = bind(sock, (
struct sockaddr*)&can_address,
sizeof(can_address));
514 SocketMessage(
"SocketCAN socket bind() failed: ", port_name);
518 stats.available =
true;
519 m_parent_driver->SetDriverStats(stats);
530void Worker::HandleInput(CanFrame frame) {
537 if (position == kNotFound) {
540 ready = fast_messages.
InsertEntry(header, frame.data, position);
543 ready = fast_messages.
AppendEntry(header, frame.data, position);
547 std::vector<unsigned char> vec;
550 vec = PushFastMsgFragment(header, position);
553 vec = PushCompleteMsg(header, position, frame);
556 auto src_addr = m_parent_driver->GetAddress(m_parent_driver->node_name);
557 auto msg = std::make_shared<const Nmea2000Msg>(header.pgn, vec, src_addr);
558 auto msg_all = std::make_shared<const Nmea2000Msg>(1, vec, src_addr);
560 ProcessRxMessages(msg);
561 m_parent_driver->m_listener.
Notify(std::move(msg));
562 m_parent_driver->m_listener.
Notify(std::move(msg_all));
566 m_parent_driver->SetDriverStats(stats);
571void Worker::ProcessRxMessages(std::shared_ptr<const Nmea2000Msg> n2k_msg) {
572 if (n2k_msg->PGN.pgn == 59904) {
573 unsigned long RequestedPGN = 0;
574 RequestedPGN = n2k_msg->payload.at(15) << 16;
575 RequestedPGN += n2k_msg->payload.at(14) << 8;
576 RequestedPGN += n2k_msg->payload.at(13);
578 switch (RequestedPGN) {
580 m_parent_driver->SendAddressClaim(m_parent_driver->m_source_address);
583 m_parent_driver->SendProductInfo();
590 else if (n2k_msg->PGN.pgn == 60928) {
592 if (n2k_msg->payload.at(7) == m_parent_driver->m_source_address) {
594 uint64_t my_name = m_parent_driver->node_name.GetName();
597 uint64_t his_name = 0;
598 unsigned char* p = (
unsigned char*)&his_name;
599 for (
unsigned int i = 0; i < 8; i++) *p++ = n2k_msg->payload.at(13 + i);
602 if (his_name < my_name) {
604 m_parent_driver->m_source_address++;
605 if (m_parent_driver->m_source_address > 253)
607 m_parent_driver->m_source_address = 254;
608 m_parent_driver->UpdateAttrCanAddress();
612 m_parent_driver->SendAddressClaim(m_parent_driver->m_source_address);
618void Worker::Entry() {
623 socket = InitSocket(m_port_name.ToStdString());
625 std::string msg(
"SocketCAN socket create failed: ");
626 ThreadMessage(msg + m_port_name.ToStdString());
633 if (m_parent_driver->SendAddressClaim(DEFAULT_N2K_SOURCE_ADDRESS)) {
634 m_parent_driver->m_source_address = DEFAULT_N2K_SOURCE_ADDRESS;
635 m_parent_driver->UpdateAttrCanAddress();
639 while (m_run_flag > 0) {
640 recvbytes = read(socket, &frame,
sizeof(frame));
641 if (recvbytes == -1) {
642 if (errno == EAGAIN || errno == EWOULDBLOCK)
continue;
644 wxLogWarning(
"can socket %s: fatal error %s", m_port_name.c_str(),
648 if (recvbytes != 16) {
649 wxLogWarning(
"can socket %s: bad frame size: %d (ignored)",
650 m_port_name.c_str(), recvbytes);
660bool Worker::StartThread() {
662 std::thread t(&Worker::Entry,
this);
667void Worker::StopThread() {
668 if (m_run_flag < 0) {
669 wxLogMessage(
"Attempt to stop already dead thread (ignored).");
672 wxLogMessage(
"Stopping Worker Thread");
676 while ((m_run_flag >= 0) && (tsec--)) wxSleep(1);
679 wxLogMessage(
"StopThread: Stopped in %d sec.", 10 - tsec);
681 wxLogWarning(
"StopThread: Not Stopped after 10 sec.");
const std::string iface
Physical device for 0183, else a unique string.
DriverStats GetDriverStats() const override
Get the Driver Statistics.
Local driver implementation, not visible outside this file.
EventVar evt_driver_msg
Notified for messages from drivers.
Interface for handling incoming messages.
virtual void Notify(std::shared_ptr< const NavMsg > message)=0
Handle a received message.
const void Notify()
Notify all listeners, no data supplied.
Track fast message fragments eventually forming complete messages.
int AddNewEntry(void)
Allocate a new, fresh entry and return index to it.
void Remove(int pos)
Remove entry at pos.
bool AppendEntry(const CanHeader hdr, const unsigned char *data, int index)
Append fragment to existing multipart message.
int FindMatchingEntry(const CanHeader header, const unsigned char sid)
Setter.
bool InsertEntry(const CanHeader header, const unsigned char *data, int index)
Insert a new entry, first part of a multipart message.
Keeps listening over it's lifespan, removes itself on destruction.
Custom event class for OpenCPN's notification system.
Manages reading the N2K data stream provided by some N2K gateways from the declared serial port.
Driver registration container, a singleton.
Raw messages layer, supports sending and recieving navmsg messages.
Driver statistics report.
unsigned tx_count
Number of bytes sent since program start.
unsigned rx_count
Number of bytes received since program start.
N2k uses CAN which defines the basic properties of messages.