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();
217 auto& registry = CommDriverRegistry::GetInstance();
219 registry.Deactivate(me);
222bool CommDriverN2KSocketCanImpl::SendAddressClaim(
int proposed_source_address) {
223 wxMutexLocker lock(m_TX_mutex);
225 int socket = GetWorker().GetSocket();
227 if (socket < 0)
return false;
230 memset(&frame, 0,
sizeof(frame));
232 uint64_t _pgn = 60928;
233 unsigned long canId = BuildCanID(6, proposed_source_address, 255, _pgn);
234 frame.can_id = canId | CAN_EFF_FLAG;
237 uint32_t b32_0 = node_name.value.UnicNumberAndManCode;
238 memcpy(&frame.data, &b32_0, 4);
240 unsigned char b81 = node_name.value.DeviceInstance;
241 memcpy(&frame.data[4], &b81, 1);
243 b81 = node_name.value.DeviceFunction;
244 memcpy(&frame.data[5], &b81, 1);
246 b81 = (node_name.value.DeviceClass);
247 memcpy(&frame.data[6], &b81, 1);
249 b81 = node_name.value.IndustryGroupAndSystemInstance;
250 memcpy(&frame.data[7], &b81, 1);
254 int sentbytes = write(socket, &frame,
sizeof(frame));
256 return (sentbytes == 16);
259void AddStr(std::vector<uint8_t>& vec, std::string str,
size_t max_len) {
261 for (i = 0; i < str.size(); i++) {
262 vec.push_back(str[i]);
265 for (; i < max_len; i++) {
270bool CommDriverN2KSocketCanImpl::SendProductInfo() {
272 std::vector<uint8_t> payload;
274 payload.push_back(2100 & 0xFF);
275 payload.push_back(2100 >> 8);
276 payload.push_back(0xEC);
277 payload.push_back(0x06);
279 std::string ModelID(
"OpenCPN");
280 AddStr(payload, ModelID, 32);
282 std::string ModelSWCode(PACKAGE_VERSION);
283 AddStr(payload, ModelSWCode, 32);
285 std::string ModelVersion(PACKAGE_VERSION);
286 AddStr(payload, ModelVersion, 32);
288 std::string ModelSerialCode(
289 std::to_string(m_unique_number));
290 AddStr(payload, ModelSerialCode, 32);
292 payload.push_back(0);
293 payload.push_back(0);
295 auto dest_addr = std::make_shared<const NavAddr2000>(
iface, 255);
299 auto msg = std::make_shared<const Nmea2000Msg>(_PGN, payload, dest_addr);
300 SendMessage(msg, dest_addr);
305bool CommDriverN2KSocketCanImpl::SendMessage(
306 std::shared_ptr<const NavMsg> msg, std::shared_ptr<const NavAddr> addr) {
307 wxMutexLocker lock(m_TX_mutex);
310 if (m_source_address < 0)
return false;
312 if (m_source_address > 253)
315 int socket = GetWorker().GetSocket();
317 if (socket < 0)
return false;
320 memset(&frame, 0,
sizeof(frame));
322 auto msg_n2k = std::dynamic_pointer_cast<const Nmea2000Msg>(msg);
323 std::vector<uint8_t> load = msg_n2k->payload;
325 uint64_t _pgn = msg_n2k->PGN.pgn;
326 auto destination_address = std::static_pointer_cast<const NavAddr2000>(addr);
328 unsigned long canId = BuildCanID(msg_n2k->priority, m_source_address,
329 destination_address->address, _pgn);
331 frame.can_id = canId | CAN_EFF_FLAG;
335 if (load.size() <= 8) {
336 frame.can_dlc = load.size();
337 if (load.size() > 0) memcpy(&frame.data, load.data(), load.size());
339 sentbytes += write(socket, &frame,
sizeof(frame));
341 int sequence = (m_last_TX_sequence + 0x20) & 0xE0;
342 m_last_TX_sequence = sequence;
343 unsigned char* data_ptr = load.data();
344 int n_remaining = load.size();
348 frame.data[0] = sequence;
349 frame.data[1] = load.size();
350 int data_len_0 = wxMin(load.size(), 6);
351 memcpy(&frame.data[2], load.data(), data_len_0);
353 sentbytes += write(socket, &frame,
sizeof(frame));
355 data_ptr += data_len_0;
356 n_remaining -= data_len_0;
360 while (n_remaining > 0) {
362 frame.data[0] = sequence;
363 int data_len_n = wxMin(n_remaining, 7);
364 memcpy(&frame.data[1], data_ptr, data_len_n);
366 sentbytes += write(socket, &frame,
sizeof(frame));
368 data_ptr += data_len_n;
369 n_remaining -= data_len_n;
376 SetDriverStats(stats);
383CommDriverN2KSocketCAN::CommDriverN2KSocketCAN(
const ConnectionParams* params,
387 m_listener(listener),
388 m_stats_timer(*this, 2s),
390 m_portstring(params->GetDSPort()),
391 m_baudrate(wxString::Format(
"%i", params->Baudrate)) {
392 this->attributes[
"canPort"] = params->socketCAN_port.ToStdString();
393 this->attributes[
"canAddress"] = std::to_string(DEFAULT_N2K_SOURCE_ADDRESS);
394 this->attributes[
"userComment"] = params->UserComment.ToStdString();
395 this->attributes[
"ioDirection"] = std::string(
"IN/OUT");
397 m_driver_stats.driver_bus = NavAddr::Bus::N2000;
398 m_driver_stats.driver_iface = params->GetStrippedDSPort();
401CommDriverN2KSocketCAN::~CommDriverN2KSocketCAN() {}
407 m_port_name(port_name.Clone()),
410 assert(m_parent_driver != 0);
413std::vector<unsigned char> Worker::PushCompleteMsg(
const CanHeader header,
415 const CanFrame frame) {
416 std::vector<unsigned char> data;
417 data.push_back(0x93);
418 data.push_back(0x13);
419 data.push_back(header.priority);
420 data.push_back(header.pgn & 0xFF);
421 data.push_back((header.pgn >> 8) & 0xFF);
422 data.push_back((header.pgn >> 16) & 0xFF);
423 data.push_back(header.destination);
424 data.push_back(header.source);
425 data.push_back(0xFF);
426 data.push_back(0xFF);
427 data.push_back(0xFF);
428 data.push_back(0xFF);
429 data.push_back(CAN_MAX_DLEN);
430 for (
size_t n = 0; n < CAN_MAX_DLEN; n++) data.push_back(frame.data[n]);
431 data.push_back(0x55);
435std::vector<unsigned char> Worker::PushFastMsgFragment(
const CanHeader& header,
437 std::vector<unsigned char> data;
438 data.push_back(0x93);
439 data.push_back(fast_messages[position].expected_length + 11);
440 data.push_back(header.priority);
441 data.push_back(header.pgn & 0xFF);
442 data.push_back((header.pgn >> 8) & 0xFF);
443 data.push_back((header.pgn >> 16) & 0xFF);
444 data.push_back(header.destination);
445 data.push_back(header.source);
446 data.push_back(0xFF);
447 data.push_back(0xFF);
448 data.push_back(0xFF);
449 data.push_back(0xFF);
450 data.push_back(fast_messages[position].expected_length);
451 for (
size_t n = 0; n < fast_messages[position].expected_length; n++)
452 data.push_back(fast_messages[position].data[n]);
453 data.push_back(0x55);
454 fast_messages.
Remove(position);
458void Worker::ThreadMessage(
const std::string& msg, wxLogLevel level) {
459 wxLogGeneric(level, wxString(msg.c_str()));
460 auto s = std::string(
"CommDriverN2KSocketCAN: ") + msg;
464void Worker::SocketMessage(
const std::string& msg,
const std::string& device) {
465 std::stringstream ss;
466 ss << msg << device <<
": " << strerror(errno);
467 ThreadMessage(ss.str());
476int Worker::InitSocket(
const std::string port_name) {
477 int sock = socket(PF_CAN, SOCK_RAW, CAN_RAW);
479 SocketMessage(
"SocketCAN socket create failed: ", port_name);
484 struct ifreq if_request;
485 strcpy(if_request.ifr_name, port_name.c_str());
486 if (ioctl(sock, SIOCGIFINDEX, &if_request) < 0) {
487 SocketMessage(
"SocketCAN ioctl (SIOCGIFINDEX) failed: ", port_name);
492 struct sockaddr_can can_address;
493 can_address.can_family = AF_CAN;
494 can_address.can_ifindex = if_request.ifr_ifindex;
495 if (ioctl(sock, SIOCGIFFLAGS, &if_request) < 0) {
496 SocketMessage(
"SocketCAN socket IOCTL (SIOCGIFFLAGS) failed: ", port_name);
499 if (if_request.ifr_flags & IFF_UP) {
500 ThreadMessage(
"socketCan interface is UP");
502 ThreadMessage(
"socketCan interface is NOT UP");
508 tv.tv_sec = kSocketTimeoutSeconds;
511 setsockopt(sock, SOL_SOCKET, SO_RCVTIMEO, (
const char*)&tv,
sizeof tv);
513 SocketMessage(
"SocketCAN setsockopt SO_RCVTIMEO failed on device: ",
517 r = bind(sock, (
struct sockaddr*)&can_address,
sizeof(can_address));
519 SocketMessage(
"SocketCAN socket bind() failed: ", port_name);
522 DriverStats stats = m_parent_driver->GetDriverStats();
523 stats.available =
true;
524 m_parent_driver->SetDriverStats(stats);
535void Worker::HandleInput(CanFrame frame) {
542 if (position == kNotFound) {
545 ready = fast_messages.
InsertEntry(header, frame.data, position);
548 ready = fast_messages.
AppendEntry(header, frame.data, position);
552 std::vector<unsigned char> vec;
555 vec = PushFastMsgFragment(header, position);
558 vec = PushCompleteMsg(header, position, frame);
561 auto src_addr = m_parent_driver->GetAddress(m_parent_driver->node_name);
562 auto msg = std::make_shared<const Nmea2000Msg>(header.pgn, vec, src_addr);
563 auto msg_all = std::make_shared<const Nmea2000Msg>(1, vec, src_addr);
565 ProcessRxMessages(msg);
566 m_parent_driver->m_listener.
Notify(std::move(msg));
567 m_parent_driver->m_listener.
Notify(std::move(msg_all));
569 DriverStats stats = m_parent_driver->GetDriverStats();
571 m_parent_driver->SetDriverStats(stats);
576void Worker::ProcessRxMessages(std::shared_ptr<const Nmea2000Msg> n2k_msg) {
577 if (n2k_msg->PGN.pgn == 59904) {
578 unsigned long RequestedPGN = 0;
579 RequestedPGN = n2k_msg->payload.at(15) << 16;
580 RequestedPGN += n2k_msg->payload.at(14) << 8;
581 RequestedPGN += n2k_msg->payload.at(13);
583 switch (RequestedPGN) {
585 m_parent_driver->SendAddressClaim(m_parent_driver->m_source_address);
588 m_parent_driver->SendProductInfo();
595 else if (n2k_msg->PGN.pgn == 60928) {
597 if (n2k_msg->payload.at(7) == m_parent_driver->m_source_address) {
599 uint64_t my_name = m_parent_driver->node_name.GetName();
602 uint64_t his_name = 0;
603 unsigned char* p = (
unsigned char*)&his_name;
604 for (
unsigned int i = 0; i < 8; i++) *p++ = n2k_msg->payload.at(13 + i);
607 if (his_name < my_name) {
609 m_parent_driver->m_source_address++;
610 if (m_parent_driver->m_source_address > 253)
612 m_parent_driver->m_source_address = 254;
613 m_parent_driver->UpdateAttrCanAddress();
617 m_parent_driver->SendAddressClaim(m_parent_driver->m_source_address);
623void Worker::Entry() {
628 socket = InitSocket(m_port_name.ToStdString());
630 std::string msg(
"SocketCAN socket create failed: ");
631 ThreadMessage(msg + m_port_name.ToStdString());
638 if (m_parent_driver->SendAddressClaim(DEFAULT_N2K_SOURCE_ADDRESS)) {
639 m_parent_driver->m_source_address = DEFAULT_N2K_SOURCE_ADDRESS;
640 m_parent_driver->UpdateAttrCanAddress();
644 while (m_run_flag > 0) {
645 recvbytes = read(socket, &frame,
sizeof(frame));
646 if (recvbytes == -1) {
647 if (errno == EAGAIN || errno == EWOULDBLOCK)
continue;
649 wxLogWarning(
"can socket %s: fatal error %s", m_port_name.c_str(),
653 if (recvbytes != 16) {
654 wxLogWarning(
"can socket %s: bad frame size: %d (ignored)",
655 m_port_name.c_str(), recvbytes);
665bool Worker::StartThread() {
667 std::thread t(&Worker::Entry,
this);
672void Worker::StopThread() {
673 if (m_run_flag < 0) {
674 wxLogMessage(
"Attempt to stop already dead thread (ignored).");
677 wxLogMessage(
"Stopping Worker Thread");
681 while ((m_run_flag >= 0) && (tsec--)) wxSleep(1);
684 wxLogMessage(
"StopThread: Stopped in %d sec.", 10 - tsec);
686 wxLogWarning(
"StopThread: Not Stopped after 10 sec.");
const std::string iface
Physical device for 0183, else a unique string.
Local driver implementation, not visible outside this file.
EventVar evt_driver_msg
Notified for messages from drivers.
Interface implemented by transport layer and possible other parties like test code which should handl...
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.
DriverPtr & FindDriver(const std::vector< DriverPtr > &drivers, const std::string &iface, const NavAddr::Bus _bus)
Search list of drivers for a driver with given interface string.
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.