25#if !defined(__linux__) || defined(__ANDROID__)
26#error "This file can only be compiled on Linux"
42#include <sys/socket.h>
56#define DEFAULT_N2K_SOURCE_ADDRESS 72
60static const int kNotFound = -1;
63static const int kSocketTimeoutSeconds = 2;
65typedef struct can_frame CanFrame;
68using namespace std::literals::chrono_literals;
100 int GetSocket() {
return m_socket; }
105 void ThreadMessage(
const std::string& msg, wxLogLevel l = wxLOG_Message);
107 int InitSocket(
const std::string port_name);
108 void SocketMessage(
const std::string& msg,
const std::string& device);
109 void HandleInput(CanFrame frame);
110 void ProcessRxMessages(std::shared_ptr<const Nmea2000Msg> n2k_msg);
112 std::vector<unsigned char> PushCompleteMsg(
const CanHeader header,
114 const CanFrame frame);
115 std::vector<unsigned char> PushFastMsgFragment(
const CanHeader& header,
119 const wxString m_port_name;
120 std::atomic<int> m_run_flag;
132 m_worker(
this, p->socketCAN_port),
133 m_source_address(-1),
134 m_last_TX_sequence(0) {
145 bool SendMessage(std::shared_ptr<const NavMsg> msg,
146 std::shared_ptr<const NavAddr> addr);
148 int DoAddressClaim();
149 bool SendAddressClaim(
int proposed_source_address);
150 bool SendProductInfo();
152 Worker& GetWorker() {
return m_worker; }
153 void UpdateAttrCanAddress();
158 int m_source_address;
159 int m_last_TX_sequence;
160 std::future<int> m_AddressClaimFuture;
165 bool HandleN2K_59904(std::shared_ptr<const Nmea2000Msg> n2k_msg);
170std::unique_ptr<CommDriverN2KSocketCAN> CommDriverN2KSocketCAN::Create(
172 return std::unique_ptr<CommDriverN2KSocketCAN>(
178void CommDriverN2KSocketCanImpl::SetN2K_Name() {
180 node_name.value.Name = 0;
185 std::string str(g_hostname.mb_str());
186 int len = str.size();
187 const char* ch = str.data();
188 for (
int i = 0; i < len; i++)
189 hash = hash + ((hash) << 5) + *(ch + i) + ((*(ch + i)) << 7);
190 m_unique_number = ((hash) ^ (hash >> 16)) & 0xffff;
192 node_name.SetManufacturerCode(2046);
193 node_name.SetUniqueNumber(m_unique_number);
194 node_name.SetDeviceFunction(130);
195 node_name.SetDeviceClass(120);
196 node_name.SetIndustryGroup(4);
197 node_name.SetSystemInstance(0);
200void CommDriverN2KSocketCanImpl::UpdateAttrCanAddress() {
201 this->attributes[
"canAddress"] = std::to_string(m_source_address);
204bool CommDriverN2KSocketCanImpl::Open() {
206 bool bws = m_worker.StartThread();
210void CommDriverN2KSocketCanImpl::Close() {
211 wxLogMessage(
"Closing N2K socketCAN: %s", m_params.socketCAN_port.c_str());
212 m_stats_timer.Stop();
213 m_worker.StopThread();
216bool CommDriverN2KSocketCanImpl::SendAddressClaim(
int proposed_source_address) {
217 wxMutexLocker lock(m_TX_mutex);
219 int socket = GetWorker().GetSocket();
221 if (socket < 0)
return false;
224 memset(&frame, 0,
sizeof(frame));
226 uint64_t _pgn = 60928;
227 unsigned long canId = BuildCanID(6, proposed_source_address, 255, _pgn);
228 frame.can_id = canId | CAN_EFF_FLAG;
231 uint32_t b32_0 = node_name.value.UnicNumberAndManCode;
232 memcpy(&frame.data, &b32_0, 4);
234 unsigned char b81 = node_name.value.DeviceInstance;
235 memcpy(&frame.data[4], &b81, 1);
237 b81 = node_name.value.DeviceFunction;
238 memcpy(&frame.data[5], &b81, 1);
240 b81 = (node_name.value.DeviceClass);
241 memcpy(&frame.data[6], &b81, 1);
243 b81 = node_name.value.IndustryGroupAndSystemInstance;
244 memcpy(&frame.data[7], &b81, 1);
248 int sentbytes = write(socket, &frame,
sizeof(frame));
250 return (sentbytes == 16);
253void AddStr(std::vector<uint8_t>& vec, std::string str,
size_t max_len) {
255 for (i = 0; i < str.size(); i++) {
256 vec.push_back(str[i]);
259 for (; i < max_len; i++) {
264bool CommDriverN2KSocketCanImpl::SendProductInfo() {
266 std::vector<uint8_t> payload;
268 payload.push_back(2100 & 0xFF);
269 payload.push_back(2100 >> 8);
270 payload.push_back(0xEC);
271 payload.push_back(0x06);
273 std::string ModelID(
"OpenCPN");
274 AddStr(payload, ModelID, 32);
276 std::string ModelSWCode(PACKAGE_VERSION);
277 AddStr(payload, ModelSWCode, 32);
279 std::string ModelVersion(PACKAGE_VERSION);
280 AddStr(payload, ModelVersion, 32);
282 std::string ModelSerialCode(
283 std::to_string(m_unique_number));
284 AddStr(payload, ModelSerialCode, 32);
286 payload.push_back(0);
287 payload.push_back(0);
289 auto dest_addr = std::make_shared<const NavAddr2000>(
iface, 255);
293 auto msg = std::make_shared<const Nmea2000Msg>(_PGN, payload, dest_addr);
294 SendMessage(msg, dest_addr);
299bool CommDriverN2KSocketCanImpl::SendMessage(
300 std::shared_ptr<const NavMsg> msg, std::shared_ptr<const NavAddr> addr) {
301 wxMutexLocker lock(m_TX_mutex);
304 if (m_source_address < 0)
return false;
306 if (m_source_address > 253)
309 int socket = GetWorker().GetSocket();
311 if (socket < 0)
return false;
314 memset(&frame, 0,
sizeof(frame));
316 auto msg_n2k = std::dynamic_pointer_cast<const Nmea2000Msg>(msg);
317 std::vector<uint8_t> load = msg_n2k->payload;
319 uint64_t _pgn = msg_n2k->PGN.pgn;
320 auto destination_address = std::static_pointer_cast<const NavAddr2000>(addr);
322 unsigned long canId = BuildCanID(msg_n2k->priority, m_source_address,
323 destination_address->address, _pgn);
325 frame.can_id = canId | CAN_EFF_FLAG;
329 if (load.size() <= 8) {
330 frame.can_dlc = load.size();
331 if (load.size() > 0) memcpy(&frame.data, load.data(), load.size());
333 sentbytes += write(socket, &frame,
sizeof(frame));
335 int sequence = (m_last_TX_sequence + 0x20) & 0xE0;
336 m_last_TX_sequence = sequence;
337 unsigned char* data_ptr = load.data();
338 int n_remaining = load.size();
342 frame.data[0] = sequence;
343 frame.data[1] = load.size();
344 int data_len_0 = wxMin(load.size(), 6);
345 memcpy(&frame.data[2], load.data(), data_len_0);
347 sentbytes += write(socket, &frame,
sizeof(frame));
349 data_ptr += data_len_0;
350 n_remaining -= data_len_0;
354 while (n_remaining > 0) {
356 frame.data[0] = sequence;
357 int data_len_n = wxMin(n_remaining, 7);
358 memcpy(&frame.data[1], data_ptr, data_len_n);
360 sentbytes += write(socket, &frame,
sizeof(frame));
362 data_ptr += data_len_n;
363 n_remaining -= data_len_n;
370 SetDriverStats(stats);
377CommDriverN2KSocketCAN::CommDriverN2KSocketCAN(
const ConnectionParams* params,
381 m_listener(listener),
382 m_stats_timer(*this, 2s),
384 m_portstring(params->GetDSPort()),
385 m_baudrate(wxString::Format(
"%i", params->Baudrate)) {
386 this->attributes[
"canPort"] = params->socketCAN_port.ToStdString();
387 this->attributes[
"canAddress"] = std::to_string(DEFAULT_N2K_SOURCE_ADDRESS);
388 this->attributes[
"userComment"] = params->UserComment.ToStdString();
389 this->attributes[
"ioDirection"] = std::string(
"IN/OUT");
391 m_driver_stats.driver_bus = NavAddr::Bus::N2000;
392 m_driver_stats.driver_iface = params->GetStrippedDSPort();
395CommDriverN2KSocketCAN::~CommDriverN2KSocketCAN() {}
401 m_port_name(port_name.Clone()),
404 assert(m_parent_driver != 0);
407std::vector<unsigned char> Worker::PushCompleteMsg(
const CanHeader header,
409 const CanFrame frame) {
410 std::vector<unsigned char> data;
411 data.push_back(0x93);
412 data.push_back(0x13);
413 data.push_back(header.priority);
414 data.push_back(header.pgn & 0xFF);
415 data.push_back((header.pgn >> 8) & 0xFF);
416 data.push_back((header.pgn >> 16) & 0xFF);
417 data.push_back(header.destination);
418 data.push_back(header.source);
419 data.push_back(0xFF);
420 data.push_back(0xFF);
421 data.push_back(0xFF);
422 data.push_back(0xFF);
423 data.push_back(CAN_MAX_DLEN);
424 for (
size_t n = 0; n < CAN_MAX_DLEN; n++) data.push_back(frame.data[n]);
425 data.push_back(0x55);
429std::vector<unsigned char> Worker::PushFastMsgFragment(
const CanHeader& header,
431 std::vector<unsigned char> data;
432 data.push_back(0x93);
433 data.push_back(fast_messages[position].expected_length + 11);
434 data.push_back(header.priority);
435 data.push_back(header.pgn & 0xFF);
436 data.push_back((header.pgn >> 8) & 0xFF);
437 data.push_back((header.pgn >> 16) & 0xFF);
438 data.push_back(header.destination);
439 data.push_back(header.source);
440 data.push_back(0xFF);
441 data.push_back(0xFF);
442 data.push_back(0xFF);
443 data.push_back(0xFF);
444 data.push_back(fast_messages[position].expected_length);
445 for (
size_t n = 0; n < fast_messages[position].expected_length; n++)
446 data.push_back(fast_messages[position].data[n]);
447 data.push_back(0x55);
448 fast_messages.
Remove(position);
452void Worker::ThreadMessage(
const std::string& msg, wxLogLevel level) {
453 wxLogGeneric(level, wxString(msg.c_str()));
454 auto s = std::string(
"CommDriverN2KSocketCAN: ") + msg;
458void Worker::SocketMessage(
const std::string& msg,
const std::string& device) {
459 std::stringstream ss;
460 ss << msg << device <<
": " << strerror(errno);
461 ThreadMessage(ss.str());
470int Worker::InitSocket(
const std::string port_name) {
471 int sock = socket(PF_CAN, SOCK_RAW, CAN_RAW);
473 SocketMessage(
"SocketCAN socket create failed: ", port_name);
478 struct ifreq if_request;
479 strcpy(if_request.ifr_name, port_name.c_str());
480 if (ioctl(sock, SIOCGIFINDEX, &if_request) < 0) {
481 SocketMessage(
"SocketCAN ioctl (SIOCGIFINDEX) failed: ", port_name);
486 struct sockaddr_can can_address;
487 can_address.can_family = AF_CAN;
488 can_address.can_ifindex = if_request.ifr_ifindex;
489 if (ioctl(sock, SIOCGIFFLAGS, &if_request) < 0) {
490 SocketMessage(
"SocketCAN socket IOCTL (SIOCGIFFLAGS) failed: ", port_name);
493 if (if_request.ifr_flags & IFF_UP) {
494 ThreadMessage(
"socketCan interface is UP");
496 ThreadMessage(
"socketCan interface is NOT UP");
502 tv.tv_sec = kSocketTimeoutSeconds;
505 setsockopt(sock, SOL_SOCKET, SO_RCVTIMEO, (
const char*)&tv,
sizeof tv);
507 SocketMessage(
"SocketCAN setsockopt SO_RCVTIMEO failed on device: ",
511 r = bind(sock, (
struct sockaddr*)&can_address,
sizeof(can_address));
513 SocketMessage(
"SocketCAN socket bind() failed: ", port_name);
517 stats.available =
true;
518 m_parent_driver->SetDriverStats(stats);
529void Worker::HandleInput(CanFrame frame) {
536 if (position == kNotFound) {
539 ready = fast_messages.
InsertEntry(header, frame.data, position);
542 ready = fast_messages.
AppendEntry(header, frame.data, position);
546 std::vector<unsigned char> vec;
549 vec = PushFastMsgFragment(header, position);
552 vec = PushCompleteMsg(header, position, frame);
555 auto src_addr = m_parent_driver->GetAddress(m_parent_driver->node_name);
556 auto msg = std::make_shared<const Nmea2000Msg>(header.pgn, vec, src_addr);
558 ProcessRxMessages(msg);
559 m_parent_driver->m_listener.
Notify(std::move(msg));
563 m_parent_driver->SetDriverStats(stats);
568void Worker::ProcessRxMessages(std::shared_ptr<const Nmea2000Msg> n2k_msg) {
569 if (n2k_msg->PGN.pgn == 59904) {
570 unsigned long RequestedPGN = 0;
571 RequestedPGN = n2k_msg->payload.at(15) << 16;
572 RequestedPGN += n2k_msg->payload.at(14) << 8;
573 RequestedPGN += n2k_msg->payload.at(13);
575 switch (RequestedPGN) {
577 m_parent_driver->SendAddressClaim(m_parent_driver->m_source_address);
580 m_parent_driver->SendProductInfo();
587 else if (n2k_msg->PGN.pgn == 60928) {
589 if (n2k_msg->payload.at(7) == m_parent_driver->m_source_address) {
591 uint64_t my_name = m_parent_driver->node_name.GetName();
594 uint64_t his_name = 0;
595 unsigned char* p = (
unsigned char*)&his_name;
596 for (
unsigned int i = 0; i < 8; i++) *p++ = n2k_msg->payload.at(13 + i);
599 if (his_name < my_name) {
601 m_parent_driver->m_source_address++;
602 if (m_parent_driver->m_source_address > 253)
604 m_parent_driver->m_source_address = 254;
605 m_parent_driver->UpdateAttrCanAddress();
609 m_parent_driver->SendAddressClaim(m_parent_driver->m_source_address);
615void Worker::Entry() {
620 socket = InitSocket(m_port_name.ToStdString());
622 std::string msg(
"SocketCAN socket create failed: ");
623 ThreadMessage(msg + m_port_name.ToStdString());
630 if (m_parent_driver->SendAddressClaim(DEFAULT_N2K_SOURCE_ADDRESS)) {
631 m_parent_driver->m_source_address = DEFAULT_N2K_SOURCE_ADDRESS;
632 m_parent_driver->UpdateAttrCanAddress();
636 while (m_run_flag > 0) {
637 recvbytes = read(socket, &frame,
sizeof(frame));
638 if (recvbytes == -1) {
639 if (errno == EAGAIN || errno == EWOULDBLOCK)
continue;
641 wxLogWarning(
"can socket %s: fatal error %s", m_port_name.c_str(),
645 if (recvbytes != 16) {
646 wxLogWarning(
"can socket %s: bad frame size: %d (ignored)",
647 m_port_name.c_str(), recvbytes);
657bool Worker::StartThread() {
659 std::thread t(&Worker::Entry,
this);
664void Worker::StopThread() {
665 if (m_run_flag < 0) {
666 wxLogMessage(
"Attempt to stop already dead thread (ignored).");
669 wxLogMessage(
"Stopping Worker Thread");
673 while ((m_run_flag >= 0) && (tsec--)) wxSleep(1);
676 wxLogMessage(
"StopThread: Stopped in %d sec.", 10 - tsec);
678 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.
void Notify() override
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 its 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.
Low-level socketcan utility functions.
Low-level driver for socketcan devices (linux only).
Driver registration container, a singleton.
Raw messages layer, supports sending and recieving navmsg messages.
Global variables stored in configuration file.
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.