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 if (!msg)
return false;
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);
559 ProcessRxMessages(msg);
560 m_parent_driver->m_listener.
Notify(std::move(msg));
564 m_parent_driver->SetDriverStats(stats);
569void Worker::ProcessRxMessages(std::shared_ptr<const Nmea2000Msg> n2k_msg) {
570 if (n2k_msg->PGN.pgn == 59904) {
571 unsigned long RequestedPGN = 0;
572 RequestedPGN = n2k_msg->payload.at(15) << 16;
573 RequestedPGN += n2k_msg->payload.at(14) << 8;
574 RequestedPGN += n2k_msg->payload.at(13);
576 switch (RequestedPGN) {
578 m_parent_driver->SendAddressClaim(m_parent_driver->m_source_address);
581 m_parent_driver->SendProductInfo();
588 else if (n2k_msg->PGN.pgn == 60928) {
590 if (n2k_msg->payload.at(7) == m_parent_driver->m_source_address) {
592 uint64_t my_name = m_parent_driver->node_name.GetName();
595 uint64_t his_name = 0;
596 unsigned char* p = (
unsigned char*)&his_name;
597 for (
unsigned int i = 0; i < 8; i++) *p++ = n2k_msg->payload.at(13 + i);
600 if (his_name < my_name) {
602 m_parent_driver->m_source_address++;
603 if (m_parent_driver->m_source_address > 253)
605 m_parent_driver->m_source_address = 254;
606 m_parent_driver->UpdateAttrCanAddress();
610 m_parent_driver->SendAddressClaim(m_parent_driver->m_source_address);
616void Worker::Entry() {
621 socket = InitSocket(m_port_name.ToStdString());
623 std::string msg(
"SocketCAN socket create failed: ");
624 ThreadMessage(msg + m_port_name.ToStdString());
631 if (m_parent_driver->SendAddressClaim(DEFAULT_N2K_SOURCE_ADDRESS)) {
632 m_parent_driver->m_source_address = DEFAULT_N2K_SOURCE_ADDRESS;
633 m_parent_driver->UpdateAttrCanAddress();
637 while (m_run_flag > 0) {
638 recvbytes = read(socket, &frame,
sizeof(frame));
639 if (recvbytes == -1) {
640 if (errno == EAGAIN || errno == EWOULDBLOCK)
continue;
642 wxLogWarning(
"can socket %s: fatal error %s", m_port_name.c_str(),
646 if (recvbytes != 16) {
647 wxLogWarning(
"can socket %s: bad frame size: %d (ignored)",
648 m_port_name.c_str(), recvbytes);
658bool Worker::StartThread() {
660 std::thread t(&Worker::Entry,
this);
665void Worker::StopThread() {
666 if (m_run_flag < 0) {
667 wxLogMessage(
"Attempt to stop already dead thread (ignored).");
670 wxLogMessage(
"Stopping Worker Thread");
674 while ((m_run_flag >= 0) && (tsec--)) wxSleep(1);
677 wxLogMessage(
"StopThread: Stopped in %d sec.", 10 - tsec);
679 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.