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;
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_worker.StopThread();
215 auto& registry = CommDriverRegistry::GetInstance();
217 registry.Deactivate(me);
220bool CommDriverN2KSocketCanImpl::SendAddressClaim(
int proposed_source_address) {
221 wxMutexLocker lock(m_TX_mutex);
223 int socket = GetWorker().GetSocket();
225 if (socket < 0)
return false;
228 memset(&frame, 0,
sizeof(frame));
230 uint64_t _pgn = 60928;
231 unsigned long canId = BuildCanID(6, proposed_source_address, 255, _pgn);
232 frame.can_id = canId | CAN_EFF_FLAG;
235 uint32_t b32_0 = node_name.value.UnicNumberAndManCode;
236 memcpy(&frame.data, &b32_0, 4);
238 unsigned char b81 = node_name.value.DeviceInstance;
239 memcpy(&frame.data[4], &b81, 1);
241 b81 = node_name.value.DeviceFunction;
242 memcpy(&frame.data[5], &b81, 1);
244 b81 = (node_name.value.DeviceClass);
245 memcpy(&frame.data[6], &b81, 1);
247 b81 = node_name.value.IndustryGroupAndSystemInstance;
248 memcpy(&frame.data[7], &b81, 1);
252 int sentbytes = write(socket, &frame,
sizeof(frame));
254 return (sentbytes == 16);
257void AddStr(std::vector<uint8_t>& vec, std::string str,
size_t max_len) {
259 for (i = 0; i < str.size(); i++) {
260 vec.push_back(str[i]);
263 for (; i < max_len; i++) {
268bool CommDriverN2KSocketCanImpl::SendProductInfo() {
270 std::vector<uint8_t> payload;
272 payload.push_back(2100 & 0xFF);
273 payload.push_back(2100 >> 8);
274 payload.push_back(0xEC);
275 payload.push_back(0x06);
277 std::string ModelID(
"OpenCPN");
278 AddStr(payload, ModelID, 32);
280 std::string ModelSWCode(PACKAGE_VERSION);
281 AddStr(payload, ModelSWCode, 32);
283 std::string ModelVersion(PACKAGE_VERSION);
284 AddStr(payload, ModelVersion, 32);
286 std::string ModelSerialCode(
287 std::to_string(m_unique_number));
288 AddStr(payload, ModelSerialCode, 32);
290 payload.push_back(0);
291 payload.push_back(0);
293 auto dest_addr = std::make_shared<const NavAddr2000>(
iface, 255);
297 auto msg = std::make_shared<const Nmea2000Msg>(_PGN, payload, dest_addr);
298 SendMessage(msg, dest_addr);
303bool CommDriverN2KSocketCanImpl::SendMessage(
304 std::shared_ptr<const NavMsg> msg, std::shared_ptr<const NavAddr> addr) {
305 wxMutexLocker lock(m_TX_mutex);
308 if (m_source_address < 0)
return false;
310 if (m_source_address > 253)
313 int socket = GetWorker().GetSocket();
315 if (socket < 0)
return false;
318 memset(&frame, 0,
sizeof(frame));
320 auto msg_n2k = std::dynamic_pointer_cast<const Nmea2000Msg>(msg);
321 std::vector<uint8_t> load = msg_n2k->payload;
323 uint64_t _pgn = msg_n2k->PGN.pgn;
324 auto destination_address = std::static_pointer_cast<const NavAddr2000>(addr);
326 unsigned long canId = BuildCanID(msg_n2k->priority, m_source_address,
327 destination_address->address, _pgn);
329 frame.can_id = canId | CAN_EFF_FLAG;
331 if (load.size() <= 8) {
332 frame.can_dlc = load.size();
333 if (load.size() > 0) memcpy(&frame.data, load.data(), load.size());
335 int sentbytes = write(socket, &frame,
sizeof(frame));
337 int sequence = (m_last_TX_sequence + 0x20) & 0xE0;
338 m_last_TX_sequence = sequence;
339 unsigned char* data_ptr = load.data();
340 int n_remaining = load.size();
344 frame.data[0] = sequence;
345 frame.data[1] = load.size();
346 int data_len_0 = wxMin(load.size(), 6);
347 memcpy(&frame.data[2], load.data(), data_len_0);
349 int sentbytes0 = write(socket, &frame,
sizeof(frame));
351 data_ptr += data_len_0;
352 n_remaining -= data_len_0;
356 while (n_remaining > 0) {
358 frame.data[0] = sequence;
359 int data_len_n = wxMin(n_remaining, 7);
360 memcpy(&frame.data[1], data_ptr, data_len_n);
362 int sentbytesn = write(socket, &frame,
sizeof(frame));
364 data_ptr += data_len_n;
365 n_remaining -= data_len_n;
375CommDriverN2KSocketCAN::CommDriverN2KSocketCAN(
const ConnectionParams* params,
379 m_listener(listener),
381 m_portstring(params->GetDSPort()),
382 m_baudrate(wxString::Format(
"%i", params->Baudrate)) {
383 this->attributes[
"canPort"] = params->socketCAN_port.ToStdString();
384 this->attributes[
"canAddress"] = std::to_string(DEFAULT_N2K_SOURCE_ADDRESS);
385 this->attributes[
"userComment"] = params->UserComment.ToStdString();
386 this->attributes[
"ioDirection"] = std::string(
"IN/OUT");
389CommDriverN2KSocketCAN::~CommDriverN2KSocketCAN() {}
395 m_port_name(port_name.Clone()),
398 assert(m_parent_driver != 0);
401std::vector<unsigned char> Worker::PushCompleteMsg(
const CanHeader header,
403 const CanFrame frame) {
404 std::vector<unsigned char> data;
405 data.push_back(0x93);
406 data.push_back(0x13);
407 data.push_back(header.priority);
408 data.push_back(header.pgn & 0xFF);
409 data.push_back((header.pgn >> 8) & 0xFF);
410 data.push_back((header.pgn >> 16) & 0xFF);
411 data.push_back(header.destination);
412 data.push_back(header.source);
413 data.push_back(0xFF);
414 data.push_back(0xFF);
415 data.push_back(0xFF);
416 data.push_back(0xFF);
417 data.push_back(CAN_MAX_DLEN);
418 for (
size_t n = 0; n < CAN_MAX_DLEN; n++) data.push_back(frame.data[n]);
419 data.push_back(0x55);
423std::vector<unsigned char> Worker::PushFastMsgFragment(
const CanHeader& header,
425 std::vector<unsigned char> data;
426 data.push_back(0x93);
427 data.push_back(fast_messages[position].expected_length + 11);
428 data.push_back(header.priority);
429 data.push_back(header.pgn & 0xFF);
430 data.push_back((header.pgn >> 8) & 0xFF);
431 data.push_back((header.pgn >> 16) & 0xFF);
432 data.push_back(header.destination);
433 data.push_back(header.source);
434 data.push_back(0xFF);
435 data.push_back(0xFF);
436 data.push_back(0xFF);
437 data.push_back(0xFF);
438 data.push_back(fast_messages[position].expected_length);
439 for (
size_t n = 0; n < fast_messages[position].expected_length; n++)
440 data.push_back(fast_messages[position].data[n]);
441 data.push_back(0x55);
442 fast_messages.
Remove(position);
446void Worker::ThreadMessage(
const std::string& msg, wxLogLevel level) {
447 wxLogGeneric(level, wxString(msg.c_str()));
448 auto s = std::string(
"CommDriverN2KSocketCAN: ") + msg;
452void Worker::SocketMessage(
const std::string& msg,
const std::string& device) {
453 std::stringstream ss;
454 ss << msg << device <<
": " << strerror(errno);
455 ThreadMessage(ss.str());
464int Worker::InitSocket(
const std::string port_name) {
465 int sock = socket(PF_CAN, SOCK_RAW, CAN_RAW);
467 SocketMessage(
"SocketCAN socket create failed: ", port_name);
472 struct ifreq if_request;
473 strcpy(if_request.ifr_name, port_name.c_str());
474 if (ioctl(sock, SIOCGIFINDEX, &if_request) < 0) {
475 SocketMessage(
"SocketCAN ioctl (SIOCGIFINDEX) failed: ", port_name);
480 struct sockaddr_can can_address;
481 can_address.can_family = AF_CAN;
482 can_address.can_ifindex = if_request.ifr_ifindex;
483 if (ioctl(sock, SIOCGIFFLAGS, &if_request) < 0) {
484 SocketMessage(
"SocketCAN socket IOCTL (SIOCGIFFLAGS) failed: ", port_name);
487 if (if_request.ifr_flags & IFF_UP) {
488 ThreadMessage(
"socketCan interface is UP");
490 ThreadMessage(
"socketCan interface is NOT UP");
496 tv.tv_sec = kSocketTimeoutSeconds;
499 setsockopt(sock, SOL_SOCKET, SO_RCVTIMEO, (
const char*)&tv,
sizeof tv);
501 SocketMessage(
"SocketCAN setsockopt SO_RCVTIMEO failed on device: ",
505 r = bind(sock, (
struct sockaddr*)&can_address,
sizeof(can_address));
507 SocketMessage(
"SocketCAN socket bind() failed: ", port_name);
519void Worker::HandleInput(CanFrame frame) {
526 if (position == kNotFound) {
529 ready = fast_messages.
InsertEntry(header, frame.data, position);
532 ready = fast_messages.
AppendEntry(header, frame.data, position);
536 std::vector<unsigned char> vec;
539 vec = PushFastMsgFragment(header, position);
542 vec = PushCompleteMsg(header, position, frame);
545 auto src_addr = m_parent_driver->GetAddress(m_parent_driver->node_name);
546 auto msg = std::make_shared<const Nmea2000Msg>(header.pgn, vec, src_addr);
547 auto msg_all = std::make_shared<const Nmea2000Msg>(1, vec, src_addr);
549 ProcessRxMessages(msg);
550 m_parent_driver->m_listener.
Notify(std::move(msg));
551 m_parent_driver->m_listener.
Notify(std::move(msg_all));
556void Worker::ProcessRxMessages(std::shared_ptr<const Nmea2000Msg> n2k_msg) {
557 if (n2k_msg->PGN.pgn == 59904) {
558 unsigned long RequestedPGN = 0;
559 RequestedPGN = n2k_msg->payload.at(15) << 16;
560 RequestedPGN += n2k_msg->payload.at(14) << 8;
561 RequestedPGN += n2k_msg->payload.at(13);
563 switch (RequestedPGN) {
565 m_parent_driver->SendAddressClaim(m_parent_driver->m_source_address);
568 m_parent_driver->SendProductInfo();
575 else if (n2k_msg->PGN.pgn == 60928) {
577 if (n2k_msg->payload.at(7) == m_parent_driver->m_source_address) {
579 uint64_t my_name = m_parent_driver->node_name.GetName();
582 uint64_t his_name = 0;
583 unsigned char* p = (
unsigned char*)&his_name;
584 for (
unsigned int i = 0; i < 8; i++) *p++ = n2k_msg->payload.at(13 + i);
587 if (his_name < my_name) {
589 m_parent_driver->m_source_address++;
590 if (m_parent_driver->m_source_address > 253)
592 m_parent_driver->m_source_address = 254;
593 m_parent_driver->UpdateAttrCanAddress();
597 m_parent_driver->SendAddressClaim(m_parent_driver->m_source_address);
603void Worker::Entry() {
608 socket = InitSocket(m_port_name.ToStdString());
610 std::string msg(
"SocketCAN socket create failed: ");
611 ThreadMessage(msg + m_port_name.ToStdString());
618 if (m_parent_driver->SendAddressClaim(DEFAULT_N2K_SOURCE_ADDRESS)) {
619 m_parent_driver->m_source_address = DEFAULT_N2K_SOURCE_ADDRESS;
620 m_parent_driver->UpdateAttrCanAddress();
624 while (m_run_flag > 0) {
625 recvbytes = read(socket, &frame,
sizeof(frame));
626 if (recvbytes == -1) {
627 if (errno == EAGAIN || errno == EWOULDBLOCK)
continue;
629 wxLogWarning(
"can socket %s: fatal error %s", m_port_name.c_str(),
633 if (recvbytes != 16) {
634 wxLogWarning(
"can socket %s: bad frame size: %d (ignored)",
635 m_port_name.c_str(), recvbytes);
645bool Worker::StartThread() {
647 std::thread t(&Worker::Entry,
this);
652void Worker::StopThread() {
653 if (m_run_flag < 0) {
654 wxLogMessage(
"Attempt to stop already dead thread (ignored).");
657 wxLogMessage(
"Stopping Worker Thread");
661 while ((m_run_flag >= 0) && (tsec--)) wxSleep(1);
664 wxLogMessage(
"StopThread: Stopped in %d sec.", 10 - tsec);
666 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.
N2k uses CAN which defines the basic properties of messages.