OpenCPN Partial API docs
Loading...
Searching...
No Matches
comm_drv_n2k_socketcan.cpp
Go to the documentation of this file.
1/***************************************************************************
2 * Copyright (C) 2022 by David Register *
3 * Copyright (C) 2022 Alec Leamas *
4 * *
5 * This program is free software; you can redistribute it and/or modify *
6 * it under the terms of the GNU General Public License as published by *
7 * the Free Software Foundation; either version 2 of the License, or *
8 * (at your option) any later version. *
9 * *
10 * This program is distributed in the hope that it will be useful, *
11 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
12 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
13 * GNU General Public License for more details. *
14 * *
15 * You should have received a copy of the GNU General Public License *
16 * along with this program; if not, see <https://www.gnu.org/licenses/>. *
17 **************************************************************************/
18
25#if !defined(__linux__) || defined(__ANDROID__)
26#error "This file can only be compiled on Linux"
27#endif
28
29#include "config.h"
30
31#include <algorithm>
32#include <atomic>
33#include <chrono>
34#include <mutex>
35#include <thread>
36#include <vector>
37#include <future>
38
39#include <net/if.h>
40#include <serial/serial.h>
41#include <sys/ioctl.h>
42#include <sys/socket.h>
43#include <sys/time.h>
44
45#include <wx/log.h>
46#include <wx/string.h>
47#include <wx/utils.h>
48#include <wx/thread.h>
49
50#include "model/comm_can_util.h"
54#include "model/config_vars.h"
55
56#define DEFAULT_N2K_SOURCE_ADDRESS 72
57
58wxDEFINE_EVENT(EVT_N2K_59904, ObservedEvt);
59
60static const int kNotFound = -1;
61
63static const int kSocketTimeoutSeconds = 2;
64
65typedef struct can_frame CanFrame;
66
68using namespace std::literals::chrono_literals;
69
94class Worker {
95public:
96 Worker(CommDriverN2KSocketCAN* parent, const wxString& PortName);
97
98 bool StartThread();
99 void StopThread();
100 int GetSocket() { return m_socket; }
101
102private:
103 void Entry();
104
105 void ThreadMessage(const std::string& msg, wxLogLevel l = wxLOG_Message);
106
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);
111
112 std::vector<unsigned char> PushCompleteMsg(const CanHeader header,
113 int position,
114 const CanFrame frame);
115 std::vector<unsigned char> PushFastMsgFragment(const CanHeader& header,
116 int position);
117
118 CommDriverN2KSocketCanImpl* const m_parent_driver;
119 const wxString m_port_name;
120 std::atomic<int> m_run_flag;
121 FastMessageMap fast_messages;
122 int m_socket;
123};
124
127 friend class Worker;
128
129public:
132 m_worker(this, p->socketCAN_port),
133 m_source_address(-1),
134 m_last_TX_sequence(0) {
135 SetN2K_Name();
136 Open();
137 }
138
139 ~CommDriverN2KSocketCanImpl() { Close(); }
140
141 bool Open();
142 void Close();
143 void SetN2K_Name();
144
145 bool SendMessage(std::shared_ptr<const NavMsg> msg,
146 std::shared_ptr<const NavAddr> addr);
147
148 int DoAddressClaim();
149 bool SendAddressClaim(int proposed_source_address);
150 bool SendProductInfo();
151
152 Worker& GetWorker() { return m_worker; }
153 void UpdateAttrCanAddress();
154
155private:
156 N2kName node_name;
157 Worker m_worker;
158 int m_source_address;
159 int m_last_TX_sequence;
160 std::future<int> m_AddressClaimFuture;
161 wxMutex m_TX_mutex;
162 int m_unique_number;
163
164 ObservableListener listener_N2K_59904;
165 bool HandleN2K_59904(std::shared_ptr<const Nmea2000Msg> n2k_msg);
166};
167
168// Static CommDriverN2KSocketCAN factory implementation.
169
170std::unique_ptr<CommDriverN2KSocketCAN> CommDriverN2KSocketCAN::Create(
171 const ConnectionParams* params, DriverListener& listener) {
172 return std::unique_ptr<CommDriverN2KSocketCAN>(
173 new CommDriverN2KSocketCanImpl(params, listener));
174}
175
176// CommDriverN2KSocketCanImpl implementation
177
178void CommDriverN2KSocketCanImpl::SetN2K_Name() {
179 // We choose some "benign" values for OCPN socketCan interface
180 node_name.value.Name = 0;
181
182 m_unique_number = 1;
183 // Build a simple 16 bit hash of g_hostname, to use as unique "serial number"
184 int hash = 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;
191
192 node_name.SetManufacturerCode(2046);
193 node_name.SetUniqueNumber(m_unique_number);
194 node_name.SetDeviceFunction(130); // Display
195 node_name.SetDeviceClass(120); // Display
196 node_name.SetIndustryGroup(4); // Marine
197 node_name.SetSystemInstance(0);
198}
199
200void CommDriverN2KSocketCanImpl::UpdateAttrCanAddress() {
201 this->attributes["canAddress"] = std::to_string(m_source_address);
202}
203
204bool CommDriverN2KSocketCanImpl::Open() {
205 // Start the RX worker thread
206 bool bws = m_worker.StartThread();
207 return bws;
208}
209
210void CommDriverN2KSocketCanImpl::Close() {
211 wxLogMessage("Closing N2K socketCAN: %s", m_params.socketCAN_port.c_str());
212 m_stats_timer.Stop();
213 m_worker.StopThread();
214}
215
216bool CommDriverN2KSocketCanImpl::SendAddressClaim(int proposed_source_address) {
217 wxMutexLocker lock(m_TX_mutex);
218
219 int socket = GetWorker().GetSocket();
220
221 if (socket < 0) return false;
222
223 CanFrame frame;
224 memset(&frame, 0, sizeof(frame));
225
226 uint64_t _pgn = 60928;
227 unsigned long canId = BuildCanID(6, proposed_source_address, 255, _pgn);
228 frame.can_id = canId | CAN_EFF_FLAG;
229
230 // Load the data
231 uint32_t b32_0 = node_name.value.UnicNumberAndManCode;
232 memcpy(&frame.data, &b32_0, 4);
233
234 unsigned char b81 = node_name.value.DeviceInstance;
235 memcpy(&frame.data[4], &b81, 1);
236
237 b81 = node_name.value.DeviceFunction;
238 memcpy(&frame.data[5], &b81, 1);
239
240 b81 = (node_name.value.DeviceClass);
241 memcpy(&frame.data[6], &b81, 1);
242
243 b81 = node_name.value.IndustryGroupAndSystemInstance;
244 memcpy(&frame.data[7], &b81, 1);
245
246 frame.can_dlc = 8; // data length
247
248 int sentbytes = write(socket, &frame, sizeof(frame));
249
250 return (sentbytes == 16);
251}
252
253void AddStr(std::vector<uint8_t>& vec, std::string str, size_t max_len) {
254 size_t i;
255 for (i = 0; i < str.size(); i++) {
256 vec.push_back(str[i]);
257 ;
258 }
259 for (; i < max_len; i++) {
260 vec.push_back(0);
261 }
262}
263
264bool CommDriverN2KSocketCanImpl::SendProductInfo() {
265 // Create the payload
266 std::vector<uint8_t> payload;
267
268 payload.push_back(2100 & 0xFF); // N2KVersion
269 payload.push_back(2100 >> 8);
270 payload.push_back(0xEC); // Product Code, 1772
271 payload.push_back(0x06);
272
273 std::string ModelID("OpenCPN"); // Model ID
274 AddStr(payload, ModelID, 32);
275
276 std::string ModelSWCode(PACKAGE_VERSION); // SwCode
277 AddStr(payload, ModelSWCode, 32);
278
279 std::string ModelVersion(PACKAGE_VERSION); // Model Version
280 AddStr(payload, ModelVersion, 32);
281
282 std::string ModelSerialCode(
283 std::to_string(m_unique_number)); // Model Serial Code
284 AddStr(payload, ModelSerialCode, 32);
285
286 payload.push_back(0); // CertificationLevel
287 payload.push_back(0); // LoadEquivalency
288
289 auto dest_addr = std::make_shared<const NavAddr2000>(iface, 255);
290 uint64_t _PGN;
291 _PGN = 126996;
292
293 auto msg = std::make_shared<const Nmea2000Msg>(_PGN, payload, dest_addr);
294 SendMessage(msg, dest_addr);
295
296 return true;
297}
298
299bool CommDriverN2KSocketCanImpl::SendMessage(
300 std::shared_ptr<const NavMsg> msg, std::shared_ptr<const NavAddr> addr) {
301 wxMutexLocker lock(m_TX_mutex);
302
303 // Verify claimed address is useable
304 if (m_source_address < 0) return false;
305
306 if (m_source_address > 253) // Could not claim...
307 return false;
308
309 int socket = GetWorker().GetSocket();
310
311 if (socket < 0) return false;
312
313 CanFrame frame;
314 memset(&frame, 0, sizeof(frame));
315
316 auto msg_n2k = std::dynamic_pointer_cast<const Nmea2000Msg>(msg);
317 std::vector<uint8_t> load = msg_n2k->payload;
318
319 uint64_t _pgn = msg_n2k->PGN.pgn;
320 auto destination_address = std::static_pointer_cast<const NavAddr2000>(addr);
321
322 unsigned long canId = BuildCanID(msg_n2k->priority, m_source_address,
323 destination_address->address, _pgn);
324
325 frame.can_id = canId | CAN_EFF_FLAG;
326
327 int sentbytes = 0;
328
329 if (load.size() <= 8) {
330 frame.can_dlc = load.size();
331 if (load.size() > 0) memcpy(&frame.data, load.data(), load.size());
332
333 sentbytes += write(socket, &frame, sizeof(frame));
334 } else { // Fast Packet
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();
339
340 // First packet
341 frame.can_dlc = 8;
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);
346
347 sentbytes += write(socket, &frame, sizeof(frame));
348
349 data_ptr += data_len_0;
350 n_remaining -= data_len_0;
351 sequence++;
352
353 // The rest of the bytes
354 while (n_remaining > 0) {
355 wxMilliSleep(10);
356 frame.data[0] = sequence;
357 int data_len_n = wxMin(n_remaining, 7);
358 memcpy(&frame.data[1], data_ptr, data_len_n);
359
360 sentbytes += write(socket, &frame, sizeof(frame));
361
362 data_ptr += data_len_n;
363 n_remaining -= data_len_n;
364 sequence++;
365 }
366 }
367
368 DriverStats stats = GetDriverStats();
369 stats.tx_count += sentbytes;
370 SetDriverStats(stats);
371
372 return true;
373}
374
375// CommDriverN2KSocketCAN implementation
376
377CommDriverN2KSocketCAN::CommDriverN2KSocketCAN(const ConnectionParams* params,
378 DriverListener& listener)
379 : CommDriverN2K(params->GetStrippedDSPort()),
380 m_params(*params),
381 m_listener(listener),
382 m_stats_timer(*this, 2s),
383 m_ok(false),
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");
390
391 m_driver_stats.driver_bus = NavAddr::Bus::N2000;
392 m_driver_stats.driver_iface = params->GetStrippedDSPort();
393}
394
395CommDriverN2KSocketCAN::~CommDriverN2KSocketCAN() {}
396
397// Worker implementation
398
399Worker::Worker(CommDriverN2KSocketCAN* parent, const wxString& port_name)
400 : m_parent_driver(dynamic_cast<CommDriverN2KSocketCanImpl*>(parent)),
401 m_port_name(port_name.Clone()),
402 m_run_flag(-1),
403 m_socket(-1) {
404 assert(m_parent_driver != 0);
405}
406
407std::vector<unsigned char> Worker::PushCompleteMsg(const CanHeader header,
408 int position,
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); // FIXME (dave) generate the time fields
420 data.push_back(0xFF);
421 data.push_back(0xFF);
422 data.push_back(0xFF);
423 data.push_back(CAN_MAX_DLEN); // nominally 8
424 for (size_t n = 0; n < CAN_MAX_DLEN; n++) data.push_back(frame.data[n]);
425 data.push_back(0x55); // CRC dummy, not checked
426 return data;
427}
428
429std::vector<unsigned char> Worker::PushFastMsgFragment(const CanHeader& header,
430 int position) {
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); // FIXME (dave) Could generate the time fields
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); // CRC dummy
448 fast_messages.Remove(position);
449 return data;
450}
451
452void Worker::ThreadMessage(const std::string& msg, wxLogLevel level) {
453 wxLogGeneric(level, wxString(msg.c_str()));
454 auto s = std::string("CommDriverN2KSocketCAN: ") + msg;
455 CommDriverRegistry::GetInstance().evt_driver_msg.Notify(level, s);
456}
457
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());
462}
463
470int Worker::InitSocket(const std::string port_name) {
471 int sock = socket(PF_CAN, SOCK_RAW, CAN_RAW);
472 if (sock < 0) {
473 SocketMessage("SocketCAN socket create failed: ", port_name);
474 return -1;
475 }
476
477 // Get the interface index
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);
482 return -1;
483 }
484
485 // Check if interface is UP
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);
491 return -1;
492 }
493 if (if_request.ifr_flags & IFF_UP) {
494 ThreadMessage("socketCan interface is UP");
495 } else {
496 ThreadMessage("socketCan interface is NOT UP");
497 return -1;
498 }
499
500 // Set timeout and bind
501 struct timeval tv;
502 tv.tv_sec = kSocketTimeoutSeconds;
503 tv.tv_usec = 0;
504 int r =
505 setsockopt(sock, SOL_SOCKET, SO_RCVTIMEO, (const char*)&tv, sizeof tv);
506 if (r < 0) {
507 SocketMessage("SocketCAN setsockopt SO_RCVTIMEO failed on device: ",
508 port_name);
509 return -1;
510 }
511 r = bind(sock, (struct sockaddr*)&can_address, sizeof(can_address));
512 if (r < 0) {
513 SocketMessage("SocketCAN socket bind() failed: ", port_name);
514 return -1;
515 }
516 DriverStats stats = m_parent_driver->GetDriverStats();
517 stats.available = true;
518 m_parent_driver->SetDriverStats(stats);
519
520 return sock;
521}
522
529void Worker::HandleInput(CanFrame frame) {
530 int position = -1;
531 bool ready = true;
532
533 CanHeader header(frame);
534 if (header.IsFastMessage()) {
535 position = fast_messages.FindMatchingEntry(header, frame.data[0]);
536 if (position == kNotFound) {
537 // Not an existing fast message: create new entry and insert first frame
538 position = fast_messages.AddNewEntry();
539 ready = fast_messages.InsertEntry(header, frame.data, position);
540 } else {
541 // An existing fast message entry is present, append the frame
542 ready = fast_messages.AppendEntry(header, frame.data, position);
543 }
544 }
545 if (ready) {
546 std::vector<unsigned char> vec;
547 if (position >= 0) {
548 // Re-assembled fast message
549 vec = PushFastMsgFragment(header, position);
550 } else {
551 // Single frame message
552 vec = PushCompleteMsg(header, position, frame);
553 }
554 // auto name = N2kName(static_cast<uint64_t>(header.pgn));
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);
557
558 ProcessRxMessages(msg);
559 m_parent_driver->m_listener.Notify(std::move(msg));
560
561 DriverStats stats = m_parent_driver->GetDriverStats();
562 stats.rx_count += vec.size();
563 m_parent_driver->SetDriverStats(stats);
564 }
565}
566
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);
574
575 switch (RequestedPGN) {
576 case 60928:
577 m_parent_driver->SendAddressClaim(m_parent_driver->m_source_address);
578 break;
579 case 126996:
580 m_parent_driver->SendProductInfo();
581 break;
582 default:
583 break;
584 }
585 }
586
587 else if (n2k_msg->PGN.pgn == 60928) {
588 // Watch for conflicting source address
589 if (n2k_msg->payload.at(7) == m_parent_driver->m_source_address) {
590 // My name
591 uint64_t my_name = m_parent_driver->node_name.GetName();
592
593 // His name
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);
597
598 // Compare literally the NAME values
599 if (his_name < my_name) {
600 // I lose, so select a new address
601 m_parent_driver->m_source_address++;
602 if (m_parent_driver->m_source_address > 253)
603 // Could not claim an address
604 m_parent_driver->m_source_address = 254;
605 m_parent_driver->UpdateAttrCanAddress();
606 }
607
608 // Claim the existing or modified address
609 m_parent_driver->SendAddressClaim(m_parent_driver->m_source_address);
610 }
611 }
612}
613
615void Worker::Entry() {
616 int recvbytes;
617 int socket;
618 CanFrame frame;
619
620 socket = InitSocket(m_port_name.ToStdString());
621 if (socket < 0) {
622 std::string msg("SocketCAN socket create failed: ");
623 ThreadMessage(msg + m_port_name.ToStdString());
624 m_run_flag = -1;
625 return;
626 }
627 m_socket = socket;
628
629 // Claim our default address
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();
633 }
634
635 // The main loop
636 while (m_run_flag > 0) {
637 recvbytes = read(socket, &frame, sizeof(frame));
638 if (recvbytes == -1) {
639 if (errno == EAGAIN || errno == EWOULDBLOCK) continue; // timeout
640
641 wxLogWarning("can socket %s: fatal error %s", m_port_name.c_str(),
642 strerror(errno));
643 break;
644 }
645 if (recvbytes != 16) {
646 wxLogWarning("can socket %s: bad frame size: %d (ignored)",
647 m_port_name.c_str(), recvbytes);
648 sleep(1);
649 continue;
650 }
651 HandleInput(frame);
652 }
653 m_run_flag = -1;
654 return;
655}
656
657bool Worker::StartThread() {
658 m_run_flag = 1;
659 std::thread t(&Worker::Entry, this);
660 t.detach();
661 return true;
662}
663
664void Worker::StopThread() {
665 if (m_run_flag < 0) {
666 wxLogMessage("Attempt to stop already dead thread (ignored).");
667 return;
668 }
669 wxLogMessage("Stopping Worker Thread");
670
671 m_run_flag = 0;
672 int tsec = 10;
673 while ((m_run_flag >= 0) && (tsec--)) wxSleep(1);
674
675 if (m_run_flag < 0)
676 wxLogMessage("StopThread: Stopped in %d sec.", 10 - tsec);
677 else
678 wxLogWarning("StopThread: Not Stopped after 10 sec.");
679}
const std::string iface
Physical device for 0183, else a unique string.
Definition comm_driver.h:95
CAN v2.0 29 bit header as used by NMEA 2000.
bool IsFastMessage() const
Return true if header reflects a multipart fast message.
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.
Definition comm_driver.h:50
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.
Definition observable.h:155
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.
Definition comm_navmsg.h:70