Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 :
16 : /*
17 : * Many thanks to members of the UAVCAN project:
18 : * Pavel Kirienko <[email protected]>
19 : * Ilia Sheremet <[email protected]>
20 : *
21 : * license info can be found in the uavcan submodule located:
22 : * modules/uavcan/libuavcan_drivers/linux/include/uavcan_linux/socketcan.hpp
23 : */
24 :
25 : #include <AP_HAL/AP_HAL.h>
26 : #include <AP_HAL/system.h>
27 :
28 : #if HAL_NUM_CAN_IFACES
29 :
30 : #include "CANSocketIface.h"
31 :
32 : #include <unistd.h>
33 : #include <fcntl.h>
34 :
35 : #include <sys/socket.h>
36 : #include <sys/ioctl.h>
37 : #include <sys/time.h>
38 : #include <net/if.h>
39 : #include <linux/can/raw.h>
40 : #include <cstring>
41 : #include "Scheduler.h"
42 : #include <AP_CANManager/AP_CANManager.h>
43 : #include <AP_Common/ExpandingString.h>
44 :
45 : extern const AP_HAL::HAL& hal;
46 :
47 : using namespace Linux;
48 :
49 0 : static can_frame makeSocketCanFrame(const AP_HAL::CANFrame& uavcan_frame)
50 : {
51 0 : can_frame sockcan_frame { uavcan_frame.id& AP_HAL::CANFrame::MaskExtID, uavcan_frame.dlc, { } };
52 0 : std::copy(uavcan_frame.data, uavcan_frame.data + uavcan_frame.dlc, sockcan_frame.data);
53 0 : if (uavcan_frame.isExtended()) {
54 0 : sockcan_frame.can_id |= CAN_EFF_FLAG;
55 : }
56 0 : if (uavcan_frame.isErrorFrame()) {
57 0 : sockcan_frame.can_id |= CAN_ERR_FLAG;
58 : }
59 0 : if (uavcan_frame.isRemoteTransmissionRequest()) {
60 0 : sockcan_frame.can_id |= CAN_RTR_FLAG;
61 : }
62 0 : return sockcan_frame;
63 : }
64 :
65 0 : static AP_HAL::CANFrame makeUavcanFrame(const can_frame& sockcan_frame)
66 : {
67 0 : AP_HAL::CANFrame uavcan_frame(sockcan_frame.can_id & CAN_EFF_MASK, sockcan_frame.data, sockcan_frame.can_dlc);
68 0 : if (sockcan_frame.can_id & CAN_EFF_FLAG) {
69 0 : uavcan_frame.id |= AP_HAL::CANFrame::FlagEFF;
70 : }
71 0 : if (sockcan_frame.can_id & CAN_ERR_FLAG) {
72 0 : uavcan_frame.id |= AP_HAL::CANFrame::FlagERR;
73 : }
74 0 : if (sockcan_frame.can_id & CAN_RTR_FLAG) {
75 0 : uavcan_frame.id |= AP_HAL::CANFrame::FlagRTR;
76 : }
77 0 : return uavcan_frame;
78 : }
79 :
80 0 : bool CANIface::is_initialized() const
81 : {
82 0 : return _initialized;
83 : }
84 :
85 0 : int CANIface::_openSocket(const std::string& iface_name)
86 : {
87 0 : errno = 0;
88 :
89 0 : int s = socket(PF_CAN, SOCK_RAW, CAN_RAW);
90 0 : if (s < 0) {
91 0 : return s;
92 : }
93 :
94 0 : std::shared_ptr<void> defer(&s, [](int* fd) { if (*fd >= 0) close(*fd); });
95 0 : const int ret = s;
96 :
97 : // Detect the iface index
98 0 : auto ifr = ifreq();
99 0 : if (iface_name.length() >= IFNAMSIZ) {
100 0 : errno = ENAMETOOLONG;
101 0 : return -1;
102 : }
103 0 : std::strncpy(ifr.ifr_name, iface_name.c_str(), iface_name.length());
104 0 : if (ioctl(s, SIOCGIFINDEX, &ifr) < 0 || ifr.ifr_ifindex < 0) {
105 0 : return -1;
106 : }
107 :
108 : // Bind to the specified CAN iface
109 : {
110 0 : auto addr = sockaddr_can();
111 0 : addr.can_family = AF_CAN;
112 0 : addr.can_ifindex = ifr.ifr_ifindex;
113 0 : if (bind(s, reinterpret_cast<sockaddr*>(&addr), sizeof(addr)) < 0) {
114 0 : return -1;
115 : }
116 : }
117 :
118 : // Configure
119 : {
120 0 : const int on = 1;
121 : // Timestamping
122 0 : if (setsockopt(s, SOL_SOCKET, SO_TIMESTAMP, &on, sizeof(on)) < 0) {
123 0 : return -1;
124 : }
125 : // Socket loopback
126 0 : if (setsockopt(s, SOL_CAN_RAW, CAN_RAW_RECV_OWN_MSGS, &on, sizeof(on)) < 0) {
127 0 : return -1;
128 : }
129 : // Non-blocking
130 0 : if (fcntl(s, F_SETFL, O_NONBLOCK) < 0) {
131 0 : return -1;
132 : }
133 : }
134 :
135 : // Validate the resulting socket
136 : {
137 0 : int socket_error = 0;
138 0 : socklen_t errlen = sizeof(socket_error);
139 0 : getsockopt(s, SOL_SOCKET, SO_ERROR, reinterpret_cast<void*>(&socket_error), &errlen);
140 0 : if (socket_error != 0) {
141 0 : errno = socket_error;
142 0 : return -1;
143 : }
144 : }
145 0 : s = -1;
146 0 : return ret;
147 0 : }
148 :
149 0 : int16_t CANIface::send(const AP_HAL::CANFrame& frame, const uint64_t tx_deadline,
150 : const CANIface::CanIOFlags flags)
151 : {
152 0 : CanTxItem tx_item {};
153 0 : tx_item.frame = frame;
154 0 : if (flags & Loopback) {
155 0 : tx_item.loopback = true;
156 : }
157 0 : if (flags & AbortOnError) {
158 0 : tx_item.abort_on_error = true;
159 : }
160 0 : tx_item.setup = true;
161 0 : tx_item.index = _tx_frame_counter;
162 0 : tx_item.deadline = tx_deadline;
163 0 : WITH_SEMAPHORE(sem);
164 0 : _tx_queue.emplace(tx_item);
165 0 : _tx_frame_counter++;
166 0 : stats.tx_requests++;
167 0 : _pollRead(); // Read poll is necessary because it can release the pending TX flag
168 0 : _pollWrite();
169 0 : return AP_HAL::CANIface::send(frame, tx_deadline, flags);
170 0 : }
171 :
172 0 : int16_t CANIface::receive(AP_HAL::CANFrame& out_frame, uint64_t& out_timestamp_us,
173 : CANIface::CanIOFlags& out_flags)
174 : {
175 0 : WITH_SEMAPHORE(sem);
176 0 : if (_rx_queue.empty()) {
177 0 : _pollRead(); // This allows to use the socket not calling poll() explicitly.
178 0 : if (_rx_queue.empty()) {
179 0 : return 0;
180 : }
181 : }
182 : {
183 0 : const CanRxItem& rx = _rx_queue.front();
184 0 : out_frame = rx.frame;
185 0 : out_timestamp_us = rx.timestamp_us;
186 0 : out_flags = rx.flags;
187 : }
188 0 : (void)_rx_queue.pop();
189 0 : if (sem_handle != nullptr) {
190 0 : sem_handle->signal();
191 : }
192 0 : return AP_HAL::CANIface::receive(out_frame, out_timestamp_us, out_flags);
193 0 : }
194 :
195 0 : bool CANIface::_hasReadyTx()
196 : {
197 0 : WITH_SEMAPHORE(sem);
198 0 : return !_tx_queue.empty() && (_frames_in_socket_tx_queue < _max_frames_in_socket_tx_queue);
199 0 : }
200 :
201 0 : bool CANIface::_hasReadyRx()
202 : {
203 0 : WITH_SEMAPHORE(sem);
204 0 : return !_rx_queue.empty();
205 0 : }
206 :
207 0 : void CANIface::_poll(bool read, bool write)
208 : {
209 0 : if (read) {
210 0 : stats.num_poll_rx_events++;
211 0 : _pollRead(); // Read poll must be executed first because it may decrement _frames_in_socket_tx_queue
212 : }
213 0 : if (write) {
214 0 : stats.num_poll_tx_events++;
215 0 : _pollWrite();
216 : }
217 0 : }
218 :
219 0 : uint32_t CANIface::getErrorCount() const
220 : {
221 0 : uint32_t ec = 0;
222 0 : for (auto& kv : _errors) { ec += kv.second; }
223 0 : return ec;
224 : }
225 :
226 0 : void CANIface::_pollWrite()
227 : {
228 0 : while (_hasReadyTx()) {
229 0 : WITH_SEMAPHORE(sem);
230 0 : if (!_hasReadyTx()) {
231 0 : break;
232 : }
233 0 : const CanTxItem tx = _tx_queue.top();
234 0 : uint64_t curr_time = AP_HAL::micros64();
235 0 : if (tx.deadline >= curr_time) {
236 : // hal.console->printf("%x TDEAD: %lu CURRT: %lu DEL: %lu\n",tx.frame.id, tx.deadline, curr_time, tx.deadline-curr_time);
237 0 : const int res = _write(tx.frame);
238 0 : if (res == 1) { // Transmitted successfully
239 0 : _incrementNumFramesInSocketTxQueue();
240 0 : if (tx.loopback) {
241 0 : _pending_loopback_ids.insert(tx.frame.id);
242 : }
243 0 : stats.tx_success++;
244 0 : stats.last_transmit_us = curr_time;
245 0 : } else if (res == 0) { // Not transmitted, nor is it an error
246 0 : stats.tx_overflow++;
247 0 : break; // Leaving the loop, the frame remains enqueued for the next retry
248 : } else { // Transmission error
249 0 : stats.tx_rejected++;
250 : }
251 : } else {
252 : // hal.console->printf("TDEAD: %lu CURRT: %lu DEL: %lu\n", tx.deadline, curr_time, curr_time-tx.deadline);
253 0 : stats.tx_timedout++;
254 : }
255 :
256 : // Removing the frame from the queue even if transmission failed
257 0 : (void)_tx_queue.pop();
258 0 : }
259 0 : }
260 :
261 0 : bool CANIface::_pollRead()
262 : {
263 0 : uint8_t iterations_count = 0;
264 0 : while (iterations_count < CAN_MAX_POLL_ITERATIONS_COUNT)
265 : {
266 0 : iterations_count++;
267 0 : CanRxItem rx;
268 0 : rx.timestamp_us = AP_HAL::micros64(); // Monotonic timestamp is not required to be precise (unlike UTC)
269 0 : bool loopback = false;
270 0 : const int res = _read(rx.frame, rx.timestamp_us, loopback);
271 0 : if (res == 1) {
272 0 : bool accept = true;
273 0 : if (loopback) { // We receive loopback for all CAN frames
274 0 : _confirmSentFrame();
275 0 : rx.flags |= Loopback;
276 0 : accept = _wasInPendingLoopbackSet(rx.frame);
277 0 : stats.tx_confirmed++;
278 : }
279 0 : if (accept) {
280 0 : WITH_SEMAPHORE(sem);
281 0 : _rx_queue.push(rx);
282 0 : stats.rx_received++;
283 0 : return true;
284 0 : }
285 0 : } else if (res == 0) {
286 0 : break;
287 : } else {
288 0 : stats.rx_errors++;
289 0 : break;
290 : }
291 : }
292 0 : return false;
293 : }
294 :
295 0 : int CANIface::_write(const AP_HAL::CANFrame& frame) const
296 : {
297 0 : if (_fd < 0) {
298 0 : return -1;
299 : }
300 0 : errno = 0;
301 :
302 0 : const can_frame sockcan_frame = makeSocketCanFrame(frame);
303 :
304 0 : const int res = write(_fd, &sockcan_frame, sizeof(sockcan_frame));
305 0 : if (res <= 0) {
306 0 : if (errno == ENOBUFS || errno == EAGAIN) { // Writing is not possible atm, not an error
307 0 : return 0;
308 : }
309 0 : return res;
310 : }
311 0 : if (res != sizeof(sockcan_frame)) {
312 0 : return -1;
313 : }
314 0 : return 1;
315 : }
316 :
317 :
318 0 : int CANIface::_read(AP_HAL::CANFrame& frame, uint64_t& timestamp_us, bool& loopback) const
319 : {
320 0 : if (_fd < 0) {
321 0 : return -1;
322 : }
323 0 : auto iov = iovec();
324 0 : auto sockcan_frame = can_frame();
325 0 : iov.iov_base = &sockcan_frame;
326 0 : iov.iov_len = sizeof(sockcan_frame);
327 : union {
328 : uint8_t data[CMSG_SPACE(sizeof(::timeval))];
329 : struct cmsghdr align;
330 : } control;
331 :
332 0 : auto msg = msghdr();
333 0 : msg.msg_iov = &iov;
334 0 : msg.msg_iovlen = 1;
335 0 : msg.msg_control = control.data;
336 0 : msg.msg_controllen = sizeof(control.data);
337 :
338 0 : const int res = recvmsg(_fd, &msg, MSG_DONTWAIT);
339 0 : if (res <= 0) {
340 0 : return (res < 0 && errno == EWOULDBLOCK) ? 0 : res;
341 : }
342 : /*
343 : * Flags
344 : */
345 0 : loopback = (msg.msg_flags & static_cast<int>(MSG_CONFIRM)) != 0;
346 :
347 0 : frame = makeUavcanFrame(sockcan_frame);
348 : /*
349 : * Timestamp
350 : */
351 0 : timestamp_us = AP_HAL::micros64();
352 0 : return 1;
353 : }
354 :
355 : // Might block forever, only to be used for testing
356 0 : void CANIface::flush_tx()
357 : {
358 0 : WITH_SEMAPHORE(sem);
359 : do {
360 0 : _updateDownStatusFromPollResult(_pollfd);
361 0 : _poll(true, true);
362 0 : } while(!_tx_queue.empty() && !_down);
363 0 : }
364 :
365 0 : void CANIface::clear_rx()
366 : {
367 0 : WITH_SEMAPHORE(sem);
368 : // Clean Rx Queue
369 0 : std::queue<CanRxItem> empty;
370 0 : std::swap( _rx_queue, empty );
371 0 : }
372 :
373 0 : void CANIface::_incrementNumFramesInSocketTxQueue()
374 : {
375 0 : _frames_in_socket_tx_queue++;
376 0 : }
377 :
378 0 : void CANIface::_confirmSentFrame()
379 : {
380 0 : if (_frames_in_socket_tx_queue > 0) {
381 0 : _frames_in_socket_tx_queue--;
382 : }
383 0 : }
384 :
385 0 : bool CANIface::_wasInPendingLoopbackSet(const AP_HAL::CANFrame& frame)
386 : {
387 0 : if (_pending_loopback_ids.count(frame.id) > 0) {
388 0 : _pending_loopback_ids.erase(frame.id);
389 0 : return true;
390 : }
391 0 : return false;
392 : }
393 :
394 0 : void CANIface::_updateDownStatusFromPollResult(const pollfd& pfd)
395 : {
396 0 : if (!_down && (pfd.revents & POLLERR)) {
397 0 : int error = 0;
398 0 : socklen_t errlen = sizeof(error);
399 0 : getsockopt(pfd.fd, SOL_SOCKET, SO_ERROR, reinterpret_cast<void*>(&error), &errlen);
400 :
401 0 : _down= error == ENETDOWN || error == ENODEV;
402 0 : stats.num_downs++;
403 : }
404 0 : }
405 :
406 0 : bool CANIface::init(const uint32_t bitrate)
407 : {
408 : char iface_name[16];
409 : #if HAL_LINUX_USE_VIRTUAL_CAN
410 0 : sprintf(iface_name, "vcan%u", _self_index);
411 : #else
412 : sprintf(iface_name, "can%u", _self_index);
413 : #endif
414 0 : if (_initialized) {
415 0 : return _initialized;
416 : }
417 0 : bitrate_ = bitrate;
418 : // TODO: Add possibility change bitrate
419 0 : _fd = _openSocket(iface_name);
420 0 : if (_fd > 0) {
421 0 : _bitrate = bitrate;
422 0 : _initialized = true;
423 : } else {
424 0 : _initialized = false;
425 : }
426 0 : return _initialized;
427 : }
428 :
429 0 : bool CANIface::select(bool &read_select, bool &write_select,
430 : const AP_HAL::CANFrame* const pending_tx, uint64_t blocking_deadline)
431 : {
432 : // Detecting whether we need to block at all
433 0 : bool need_block = !write_select; // Write queue is infinite
434 :
435 0 : if (read_select && _hasReadyRx()) {
436 0 : need_block = false;
437 : }
438 :
439 0 : if (need_block) {
440 0 : if (_down) {
441 0 : return false;
442 : } else {
443 0 : _pollfd.fd = _fd;
444 0 : _pollfd.events |= POLLIN;
445 0 : stats.num_rx_poll_req++;
446 0 : if (_hasReadyTx() && write_select) {
447 0 : _pollfd.events |= POLLOUT;
448 0 : stats.num_tx_poll_req++;
449 : }
450 : }
451 0 : const uint64_t now_us = AP_HAL::micros64();
452 0 : if (sem_handle != nullptr && blocking_deadline > now_us) {
453 0 : IGNORE_RETURN(sem_handle->wait(blocking_deadline - now_us));
454 : }
455 : }
456 :
457 : // Writing the output masks
458 0 : if (!_down) {
459 0 : write_select = true; // Always ready to write if not down
460 : } else {
461 0 : write_select = false;
462 : }
463 0 : if (_hasReadyRx()) {
464 0 : read_select = true; // Readability depends only on RX buf, even if down
465 : } else {
466 0 : read_select = false;
467 : }
468 :
469 : // Return value is irrelevant as long as it's non-negative
470 0 : return true;
471 : }
472 :
473 0 : bool CANIface::set_event_handle(AP_HAL::BinarySemaphore *handle)
474 : {
475 0 : sem_handle = handle;
476 0 : return true;
477 : }
478 :
479 :
480 0 : void CANIface::get_stats(ExpandingString &str)
481 : {
482 0 : str.printf("tx_requests: %u\n"
483 : "tx_rejected: %u\n"
484 : "tx_overflow: %u\n"
485 : "tx_confirmed: %u\n"
486 : "tx_success: %u\n"
487 : "tx_timedout: %u\n"
488 : "rx_received: %u\n"
489 : "rx_errors: %u\n"
490 : "num_downs: %u\n"
491 : "num_rx_poll_req: %u\n"
492 : "num_tx_poll_req: %u\n"
493 : "num_poll_waits: %u\n"
494 : "num_poll_tx_events: %u\n"
495 : "num_poll_rx_events: %u\n",
496 : stats.tx_requests,
497 : stats.tx_rejected,
498 : stats.tx_overflow,
499 : stats.tx_confirmed,
500 : stats.tx_success,
501 : stats.tx_timedout,
502 : stats.rx_received,
503 : stats.rx_errors,
504 : stats.num_downs,
505 : stats.num_rx_poll_req,
506 : stats.num_tx_poll_req,
507 : stats.num_poll_waits,
508 : stats.num_poll_tx_events,
509 : stats.num_poll_rx_events);
510 0 : }
511 :
512 : #endif
|
