/* This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with this program. If not, see . */ /* * Many thanks to members of the UAVCAN project: * Pavel Kirienko * Ilia Sheremet * * license info can be found in the uavcan submodule located: * modules/uavcan/libuavcan_drivers/linux/include/uavcan_linux/socketcan.hpp */ #include #include #if HAL_NUM_CAN_IFACES #include "CANSocketIface.h" #include #include #include #include #include #include #include #include "Scheduler.h" #include extern const AP_HAL::HAL& hal; using namespace Linux; #define Debug(fmt, args...) do { AP::can().log_text(AP_CANManager::LOG_DEBUG, "CANLinuxIface", fmt, ##args); } while (0) CANIface::CANSocketEventSource CANIface::evt_can_socket[HAL_NUM_CAN_IFACES]; static can_frame makeSocketCanFrame(const AP_HAL::CANFrame& uavcan_frame) { can_frame sockcan_frame { uavcan_frame.id& AP_HAL::CANFrame::MaskExtID, uavcan_frame.dlc, { } }; std::copy(uavcan_frame.data, uavcan_frame.data + uavcan_frame.dlc, sockcan_frame.data); if (uavcan_frame.isExtended()) { sockcan_frame.can_id |= CAN_EFF_FLAG; } if (uavcan_frame.isErrorFrame()) { sockcan_frame.can_id |= CAN_ERR_FLAG; } if (uavcan_frame.isRemoteTransmissionRequest()) { sockcan_frame.can_id |= CAN_RTR_FLAG; } return sockcan_frame; } static AP_HAL::CANFrame makeUavcanFrame(const can_frame& sockcan_frame) { AP_HAL::CANFrame uavcan_frame(sockcan_frame.can_id & CAN_EFF_MASK, sockcan_frame.data, sockcan_frame.can_dlc); if (sockcan_frame.can_id & CAN_EFF_FLAG) { uavcan_frame.id |= AP_HAL::CANFrame::FlagEFF; } if (sockcan_frame.can_id & CAN_ERR_FLAG) { uavcan_frame.id |= AP_HAL::CANFrame::FlagERR; } if (sockcan_frame.can_id & CAN_RTR_FLAG) { uavcan_frame.id |= AP_HAL::CANFrame::FlagRTR; } return uavcan_frame; } bool CANIface::is_initialized() const { return _initialized; } int CANIface::_openSocket(const std::string& iface_name) { errno = 0; int s = socket(PF_CAN, SOCK_RAW, CAN_RAW); if (s < 0) { return s; } std::shared_ptr defer(&s, [](int* fd) { if (*fd >= 0) close(*fd); }); const int ret = s; // Detect the iface index auto ifr = ifreq(); if (iface_name.length() >= IFNAMSIZ) { errno = ENAMETOOLONG; return -1; } std::strncpy(ifr.ifr_name, iface_name.c_str(), iface_name.length()); if (ioctl(s, SIOCGIFINDEX, &ifr) < 0 || ifr.ifr_ifindex < 0) { return -1; } // Bind to the specified CAN iface { auto addr = sockaddr_can(); addr.can_family = AF_CAN; addr.can_ifindex = ifr.ifr_ifindex; if (bind(s, reinterpret_cast(&addr), sizeof(addr)) < 0) { return -1; } } // Configure { const int on = 1; // Timestamping if (setsockopt(s, SOL_SOCKET, SO_TIMESTAMP, &on, sizeof(on)) < 0) { return -1; } // Socket loopback if (setsockopt(s, SOL_CAN_RAW, CAN_RAW_RECV_OWN_MSGS, &on, sizeof(on)) < 0) { return -1; } // Non-blocking if (fcntl(s, F_SETFL, O_NONBLOCK) < 0) { return -1; } } // Validate the resulting socket { int socket_error = 0; socklen_t errlen = sizeof(socket_error); getsockopt(s, SOL_SOCKET, SO_ERROR, reinterpret_cast(&socket_error), &errlen); if (socket_error != 0) { errno = socket_error; return -1; } } s = -1; return ret; } int16_t CANIface::send(const AP_HAL::CANFrame& frame, const uint64_t tx_deadline, const CANIface::CanIOFlags flags) { CanTxItem tx_item {}; tx_item.frame = frame; if (flags & Loopback) { tx_item.loopback = true; } if (flags & AbortOnError) { tx_item.abort_on_error = true; } tx_item.setup = true; tx_item.index = _tx_frame_counter; tx_item.deadline = tx_deadline; _tx_queue.emplace(tx_item); _tx_frame_counter++; stats.tx_requests++; _pollRead(); // Read poll is necessary because it can release the pending TX flag _pollWrite(); return 1; } int16_t CANIface::receive(AP_HAL::CANFrame& out_frame, uint64_t& out_timestamp_us, CANIface::CanIOFlags& out_flags) { if (_rx_queue.empty()) { _pollRead(); // This allows to use the socket not calling poll() explicitly. if (_rx_queue.empty()) { return 0; } } { const CanRxItem& rx = _rx_queue.front(); out_frame = rx.frame; out_timestamp_us = rx.timestamp_us; out_flags = rx.flags; } (void)_rx_queue.pop(); return 1; } bool CANIface::_hasReadyTx() const { return !_tx_queue.empty() && (_frames_in_socket_tx_queue < _max_frames_in_socket_tx_queue); } bool CANIface::_hasReadyRx() const { return !_rx_queue.empty(); } void CANIface::_poll(bool read, bool write) { if (read) { stats.num_poll_rx_events++; _pollRead(); // Read poll must be executed first because it may decrement _frames_in_socket_tx_queue } if (write) { stats.num_poll_tx_events++; _pollWrite(); } } bool CANIface::configureFilters(const CanFilterConfig* const filter_configs, const std::uint16_t num_configs) { if (filter_configs == nullptr || mode_ != FilteredMode) { return false; } _hw_filters_container.clear(); _hw_filters_container.resize(num_configs); for (unsigned i = 0; i < num_configs; i++) { const CanFilterConfig& fc = filter_configs[i]; _hw_filters_container[i].can_id = fc.id & AP_HAL::CANFrame::MaskExtID; _hw_filters_container[i].can_mask = fc.mask & AP_HAL::CANFrame::MaskExtID; if (fc.id & AP_HAL::CANFrame::FlagEFF) { _hw_filters_container[i].can_id |= CAN_EFF_FLAG; } if (fc.id & AP_HAL::CANFrame::FlagRTR) { _hw_filters_container[i].can_id |= CAN_RTR_FLAG; } if (fc.mask & AP_HAL::CANFrame::FlagEFF) { _hw_filters_container[i].can_mask |= CAN_EFF_FLAG; } if (fc.mask & AP_HAL::CANFrame::FlagRTR) { _hw_filters_container[i].can_mask |= CAN_RTR_FLAG; } } return true; } /** * SocketCAN emulates the CAN filters in software, so the number of filters is virtually unlimited. * This method returns a constant value. */ static constexpr unsigned NumFilters = CAN_FILTER_NUMBER; uint16_t CANIface::getNumFilters() const { return NumFilters; } uint32_t CANIface::getErrorCount() const { uint32_t ec = 0; for (auto& kv : _errors) { ec += kv.second; } return ec; } void CANIface::_pollWrite() { while (_hasReadyTx()) { const CanTxItem tx = _tx_queue.top(); uint64_t curr_time = AP_HAL::native_micros64(); if (tx.deadline >= curr_time) { // hal.console->printf("%x TDEAD: %lu CURRT: %lu DEL: %lu\n",tx.frame.id, tx.deadline, curr_time, tx.deadline-curr_time); const int res = _write(tx.frame); if (res == 1) { // Transmitted successfully _incrementNumFramesInSocketTxQueue(); if (tx.loopback) { _pending_loopback_ids.insert(tx.frame.id); } stats.tx_success++; } else if (res == 0) { // Not transmitted, nor is it an error stats.tx_full++; break; // Leaving the loop, the frame remains enqueued for the next retry } else { // Transmission error stats.tx_write_fail++; } } else { // hal.console->printf("TDEAD: %lu CURRT: %lu DEL: %lu\n", tx.deadline, curr_time, curr_time-tx.deadline); stats.tx_timedout++; } // Removing the frame from the queue even if transmission failed (void)_tx_queue.pop(); } } bool CANIface::_pollRead() { uint8_t iterations_count = 0; while (iterations_count < CAN_MAX_POLL_ITERATIONS_COUNT) { iterations_count++; CanRxItem rx; rx.timestamp_us = AP_HAL::native_micros64(); // Monotonic timestamp is not required to be precise (unlike UTC) bool loopback = false; const int res = _read(rx.frame, rx.timestamp_us, loopback); if (res == 1) { bool accept = true; if (loopback) { // We receive loopback for all CAN frames _confirmSentFrame(); rx.flags |= Loopback; accept = _wasInPendingLoopbackSet(rx.frame); stats.tx_confirmed++; } if (accept) { _rx_queue.push(rx); stats.rx_received++; return true; } } else if (res == 0) { break; } else { stats.rx_errors++; break; } } return false; } int CANIface::_write(const AP_HAL::CANFrame& frame) const { errno = 0; const can_frame sockcan_frame = makeSocketCanFrame(frame); const int res = write(_fd, &sockcan_frame, sizeof(sockcan_frame)); if (res <= 0) { if (errno == ENOBUFS || errno == EAGAIN) { // Writing is not possible atm, not an error return 0; } return res; } if (res != sizeof(sockcan_frame)) { return -1; } return 1; } int CANIface::_read(AP_HAL::CANFrame& frame, uint64_t& timestamp_us, bool& loopback) const { auto iov = iovec(); auto sockcan_frame = can_frame(); iov.iov_base = &sockcan_frame; iov.iov_len = sizeof(sockcan_frame); union { uint8_t data[CMSG_SPACE(sizeof(::timeval))]; struct cmsghdr align; } control; auto msg = msghdr(); msg.msg_iov = &iov; msg.msg_iovlen = 1; msg.msg_control = control.data; msg.msg_controllen = sizeof(control.data); const int res = recvmsg(_fd, &msg, MSG_DONTWAIT); if (res <= 0) { return (res < 0 && errno == EWOULDBLOCK) ? 0 : res; } /* * Flags */ loopback = (msg.msg_flags & static_cast(MSG_CONFIRM)) != 0; if (!loopback && !_checkHWFilters(sockcan_frame)) { return 0; } frame = makeUavcanFrame(sockcan_frame); /* * Timestamp */ timestamp_us = AP_HAL::native_micros64(); return 1; } // Might block forever, only to be used for testing void CANIface::flush_tx() { do { _updateDownStatusFromPollResult(_pollfd); _poll(true, true); } while(!_tx_queue.empty() && !_down); } void CANIface::clear_rx() { // Clean Rx Queue std::queue empty; std::swap( _rx_queue, empty ); } void CANIface::_incrementNumFramesInSocketTxQueue() { _frames_in_socket_tx_queue++; } void CANIface::_confirmSentFrame() { if (_frames_in_socket_tx_queue > 0) { _frames_in_socket_tx_queue--; } } bool CANIface::_wasInPendingLoopbackSet(const AP_HAL::CANFrame& frame) { if (_pending_loopback_ids.count(frame.id) > 0) { _pending_loopback_ids.erase(frame.id); return true; } return false; } bool CANIface::_checkHWFilters(const can_frame& frame) const { if (!_hw_filters_container.empty()) { for (auto& f : _hw_filters_container) { if (((frame.can_id & f.can_mask) ^ f.can_id) == 0) { return true; } } return false; } else { return true; } } void CANIface::_updateDownStatusFromPollResult(const pollfd& pfd) { if (!_down && (pfd.revents & POLLERR)) { int error = 0; socklen_t errlen = sizeof(error); getsockopt(pfd.fd, SOL_SOCKET, SO_ERROR, reinterpret_cast(&error), &errlen); _down= error == ENETDOWN || error == ENODEV; stats.num_downs++; Debug("Iface %d is dead; error %d", _fd, error); } } bool CANIface::init(const uint32_t bitrate, const OperatingMode mode) { char iface_name[16]; sprintf(iface_name, "can%u", _self_index); if (_initialized) { return _initialized; } bitrate_ = bitrate; mode_ = mode; // TODO: Add possibility change bitrate _fd = _openSocket(iface_name); Debug("Socket opened iface_name: %s fd: %d", iface_name, _fd); if (_fd > 0) { _bitrate = bitrate; _initialized = true; } else { _initialized = false; } return _initialized; } bool CANIface::select(bool &read_select, bool &write_select, const AP_HAL::CANFrame* const pending_tx, uint64_t blocking_deadline) { // Detecting whether we need to block at all bool need_block = !write_select; // Write queue is infinite if (read_select && _hasReadyRx()) { need_block = false; } if (need_block) { if (_down) { return false; } else { _pollfd.fd = _fd; _pollfd.events |= POLLIN; stats.num_rx_poll_req++; if (_hasReadyTx() && write_select) { _pollfd.events |= POLLOUT; stats.num_tx_poll_req++; } } if (_evt_handle != nullptr && blocking_deadline > AP_HAL::native_micros64()) { _evt_handle->wait(blocking_deadline - AP_HAL::native_micros64()); } } // Writing the output masks if (!_down) { write_select = true; // Always ready to write if not down } else { write_select = false; } if (_hasReadyRx()) { read_select = true; // Readability depends only on RX buf, even if down } else { read_select = false; } // Return value is irrelevant as long as it's non-negative return true; } bool CANIface::set_event_handle(AP_HAL::EventHandle* handle) { _evt_handle = handle; evt_can_socket[_self_index]._ifaces[_self_index] = this; _evt_handle->set_source(&evt_can_socket[_self_index]); return true; } bool CANIface::CANSocketEventSource::wait(uint64_t duration, AP_HAL::EventHandle* evt_handle) { if (evt_handle == nullptr) { return false; } pollfd pollfds[HAL_NUM_CAN_IFACES] {}; uint8_t pollfd_iface_map[HAL_NUM_CAN_IFACES] {}; unsigned long int num_pollfds = 0; // Poll FD set setup for (unsigned i = 0; i < HAL_NUM_CAN_IFACES; i++) { if (_ifaces[i] == nullptr) { continue; } if (_ifaces[i]->_down) { continue; } pollfds[num_pollfds] = _ifaces[i]->_pollfd; pollfd_iface_map[num_pollfds] = i; num_pollfds++; _ifaces[i]->stats.num_poll_waits++; } if (num_pollfds == 0) { return true; } // Timeout conversion auto ts = timespec(); ts.tv_sec = duration / 1000000LL; ts.tv_nsec = (duration % 1000000LL) * 1000; // Blocking here const int res = ppoll(pollfds, num_pollfds, &ts, nullptr); if (res < 0) { return false; } // Handling poll output for (unsigned i = 0; i < num_pollfds; i++) { if (_ifaces[pollfd_iface_map[i]] == nullptr) { continue; } _ifaces[pollfd_iface_map[i]]->_updateDownStatusFromPollResult(pollfds[i]); const bool poll_read = pollfds[i].revents & POLLIN; const bool poll_write = pollfds[i].revents & POLLOUT; _ifaces[pollfd_iface_map[i]]->_poll(poll_read, poll_write); } return true; } uint32_t CANIface::get_stats(char* data, uint32_t max_size) { if (data == nullptr) { return 0; } uint32_t ret = snprintf(data, max_size, "tx_requests: %u\n" "tx_write_fail: %u\n" "tx_full: %u\n" "tx_confirmed: %u\n" "tx_success: %u\n" "tx_timedout: %u\n" "rx_received: %u\n" "rx_errors: %u\n" "num_downs: %u\n" "num_rx_poll_req: %u\n" "num_tx_poll_req: %u\n" "num_poll_waits: %u\n" "num_poll_tx_events: %u\n" "num_poll_rx_events: %u\n", stats.tx_requests, stats.tx_write_fail, stats.tx_full, stats.tx_confirmed, stats.tx_success, stats.tx_timedout, stats.rx_received, stats.rx_errors, stats.num_downs, stats.num_rx_poll_req, stats.num_tx_poll_req, stats.num_poll_waits, stats.num_poll_tx_events, stats.num_poll_rx_events); return ret; } #endif