diff --git a/libraries/AP_HAL_SITL/CANSocketIface.cpp b/libraries/AP_HAL_SITL/CANSocketIface.cpp index 5244830f1f2078..bab8e9e3349454 100644 --- a/libraries/AP_HAL_SITL/CANSocketIface.cpp +++ b/libraries/AP_HAL_SITL/CANSocketIface.cpp @@ -32,7 +32,6 @@ #include #include -#include #include #include #include @@ -40,13 +39,15 @@ #include "Scheduler.h" #include #include +#include "CAN_Multicast.h" +#include "CAN_SocketCAN.h" extern const AP_HAL::HAL& hal; using namespace HALSITL; #if HAL_CANMANAGER_ENABLED -#define Debug(fmt, args...) do { AP::can().log_text(AP_CANManager::LOG_DEBUG, "CANLinuxIface", fmt, ##args); } while (0) +#define Debug(fmt, args...) do { AP::can().log_text(AP_CANManager::LOG_DEBUG, "CANSITLIface", fmt, ##args); } while (0) #else #define Debug(fmt, args...) #endif @@ -55,141 +56,9 @@ CANIface::CANSocketEventSource CANIface::evt_can_socket[HAL_NUM_CAN_IFACES]; uint8_t CANIface::_num_interfaces; -static can_frame makeSocketCanFrame(const AP_HAL::CANFrame& uavcan_frame) -{ - can_frame sockcan_frame { uavcan_frame.id& AP_HAL::CANFrame::MaskExtID, uavcan_frame.dlc, { }, }; - memset(sockcan_frame.data, 0, sizeof(sockcan_frame.data)); - 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 canfd_frame makeSocketCanFDFrame(const AP_HAL::CANFrame& uavcan_frame) -{ - canfd_frame sockcan_frame { uavcan_frame.id& AP_HAL::CANFrame::MaskExtID, AP_HAL::CANFrame::dlcToDataLength(uavcan_frame.dlc), CANFD_BRS, 0, 0, { }, }; - memset(sockcan_frame.data, 0, sizeof(sockcan_frame.data)); - std::copy(uavcan_frame.data, uavcan_frame.data + AP_HAL::CANFrame::dlcToDataLength(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 makeCanFrame(const can_frame& sockcan_frame) -{ - AP_HAL::CANFrame can_frame(sockcan_frame.can_id & CAN_EFF_MASK, sockcan_frame.data, sockcan_frame.can_dlc); - if (sockcan_frame.can_id & CAN_EFF_FLAG) { - can_frame.id |= AP_HAL::CANFrame::FlagEFF; - } - if (sockcan_frame.can_id & CAN_ERR_FLAG) { - can_frame.id |= AP_HAL::CANFrame::FlagERR; - } - if (sockcan_frame.can_id & CAN_RTR_FLAG) { - can_frame.id |= AP_HAL::CANFrame::FlagRTR; - } - return can_frame; -} - -static AP_HAL::CANFrame makeCanFDFrame(const canfd_frame& sockcan_frame) -{ - AP_HAL::CANFrame can_frame(sockcan_frame.can_id & CAN_EFF_MASK, sockcan_frame.data, sockcan_frame.len, true); - if (sockcan_frame.can_id & CAN_EFF_FLAG) { - can_frame.id |= AP_HAL::CANFrame::FlagEFF; - } - if (sockcan_frame.can_id & CAN_ERR_FLAG) { - can_frame.id |= AP_HAL::CANFrame::FlagERR; - } - if (sockcan_frame.can_id & CAN_RTR_FLAG) { - can_frame.id |= AP_HAL::CANFrame::FlagRTR; - } - return can_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; - } - // Allow CANFD - if (setsockopt(s, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &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; + return transport != nullptr; } int16_t CANIface::send(const AP_HAL::CANFrame& frame, const uint64_t tx_deadline, @@ -251,180 +120,59 @@ bool CANIface::_hasReadyRx() 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 uint16_t num_configs) -{ -#if 0 - 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; - } - } -#endif - 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; + return 0; } void CANIface::_pollWrite() { + if (transport == nullptr) { + return; + } while (_hasReadyTx()) { WITH_SEMAPHORE(sem); const CanTxItem tx = _tx_queue.top(); - uint64_t curr_time = AP_HAL::micros64(); + const uint64_t curr_time = AP_HAL::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); - } + bool ok = transport->send(tx.frame); + if (ok) { 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_rejected++; + } else { + break; } } 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 + // Removing the frame from the queue (void)_tx_queue.pop(); } } bool CANIface::_pollRead() { - uint8_t iterations_count = 0; - while (iterations_count < CAN_MAX_POLL_ITERATIONS_COUNT) - { - CanRxItem rx; - rx.timestamp_us = AP_HAL::micros64(); // Monotonic timestamp is not required to be precise (unlike UTC) - bool loopback = false; - int res; - 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) { - WITH_SEMAPHORE(sem); - add_to_rx_queue(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 -{ - if (_fd < 0) { - return -1; - } - errno = 0; - int res = 0; - - if (frame.isCanFDFrame()) { - const canfd_frame sockcan_frame = makeSocketCanFDFrame(frame); - res = write(_fd, &sockcan_frame, sizeof(sockcan_frame)); - if (res > 0 && res != sizeof(sockcan_frame)) { - return -1; - } - } else { - const can_frame sockcan_frame = makeSocketCanFrame(frame); - res = write(_fd, &sockcan_frame, sizeof(sockcan_frame)); - if (res > 0 && res != sizeof(sockcan_frame)) { - return -1; - } - } - if (res <= 0) { - if (errno == ENOBUFS || errno == EAGAIN) { // Writing is not possible atm, not an error - return 0; - } - return res; - } - return 1; -} - - -int CANIface::_read(AP_HAL::CANFrame& frame, uint64_t& timestamp_us, bool& loopback) const -{ - if (_fd < 0) { - return -1; - } - union { - can_frame frame; - canfd_frame frame_fd; - } frames; - - const int res = read(_fd, &frames, sizeof(frames)); - if (res <= 0) { - return (res < 0 && errno == EWOULDBLOCK) ? 0 : res; + if (transport == nullptr) { + return false; } - if (res == sizeof(can_frame)) { - frame = makeCanFrame(frames.frame); - } else { - frame = makeCanFDFrame(frames.frame_fd); + CanRxItem rx {}; + bool ok = transport->receive(rx.frame); + if (!ok) { + return false; } - /* - * Timestamp - */ - timestamp_us = AP_HAL::micros64(); - return 1; + rx.timestamp_us = AP_HAL::micros64(); + WITH_SEMAPHORE(sem); + add_to_rx_queue(rx); + stats.rx_received++; + return true; } // Might block forever, only to be used for testing @@ -432,9 +180,8 @@ void CANIface::flush_tx() { WITH_SEMAPHORE(sem); do { - _updateDownStatusFromPollResult(_pollfd); _poll(true, true); - } while(!_tx_queue.empty() && !_down); + } while(!_tx_queue.empty()); } void CANIface::clear_rx() @@ -445,11 +192,6 @@ void CANIface::clear_rx() std::swap( _rx_queue, empty ); } -void CANIface::_incrementNumFramesInSocketTxQueue() -{ - _frames_in_socket_tx_queue++; -} - void CANIface::_confirmSentFrame() { if (_frames_in_socket_tx_queue > 0) { @@ -457,88 +199,51 @@ void CANIface::_confirmSentFrame() } } -bool CANIface::_wasInPendingLoopbackSet(const AP_HAL::CANFrame& frame) +bool CANIface::init(const uint32_t bitrate, const uint32_t fdbitrate, const OperatingMode mode) { - if (_pending_loopback_ids.count(frame.id) > 0) { - _pending_loopback_ids.erase(frame.id); - return true; - } - return false; + return init(bitrate, mode); } -bool CANIface::_checkHWFilters(const can_frame& frame) const +bool CANIface::init(const uint32_t bitrate, const OperatingMode mode) { - if (!_hw_filters_container.empty()) { - for (auto& f : _hw_filters_container) { - if (((frame.can_id & f.can_mask) ^ f.can_id) == 0) { - return true; - } - } + const auto *_sitl = AP::sitl(); + if (_sitl == nullptr) { return false; - } else { - return true; } -} - -bool CANIface::_checkHWFilters(const canfd_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; - } - } + if (_self_index >= HAL_NUM_CAN_IFACES) { 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); + const SITL::SIM::CANTransport can_type = _sitl->can_transport[_self_index]; + switch (can_type) { + case SITL::SIM::CANTransport::MulticastUDP: + transport = new CAN_Multicast(); + break; + case SITL::SIM::CANTransport::SocketCAN: +#if HAL_CAN_WITH_SOCKETCAN + transport = new CAN_SocketCAN(); +#endif + break; } -} - -bool CANIface::init(const uint32_t bitrate, const uint32_t fdbitrate, const OperatingMode mode) -{ - // we are using vcan, so bitrate is irrelevant - return init(bitrate, mode); -} - -bool CANIface::init(const uint32_t bitrate, const OperatingMode mode) -{ - char iface_name[16]; - sprintf(iface_name, "vcan%u", _self_index); - bitrate_ = bitrate; - mode_ = mode; - if (_initialized) { - return _initialized; + if (transport == nullptr) { + return false; } - - // TODO: Add possibility change bitrate - _fd = _openSocket(iface_name); - if (_fd > 0) { - _bitrate = bitrate; - _initialized = true; - } else { - _initialized = false; + if (!transport->init(_self_index)) { + delete transport; + transport = nullptr; + return false; } - return _initialized; + return true; } bool CANIface::select(bool &read_select, bool &write_select, - const AP_HAL::CANFrame* const pending_tx, uint64_t blocking_deadline) + const AP_HAL::CANFrame* const pending_tx, uint64_t blocking_deadline) { + if (transport == nullptr) { + return false; + } // Detecting whether we need to block at all bool need_block = !write_select; // Write queue is infinite + // call poll here to flush some tx _poll(true, true); @@ -547,39 +252,22 @@ bool CANIface::select(bool &read_select, bool &write_select, } 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::micros64()) { - _evt_handle->wait(blocking_deadline - AP_HAL::micros64()); - } + _pollfd.fd = transport->get_read_fd(); + _pollfd.events |= POLLIN; + } + if (_evt_handle != nullptr && blocking_deadline > AP_HAL::micros64()) { + _evt_handle->wait(blocking_deadline - AP_HAL::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; - } + write_select = true; + read_select = _hasReadyRx(); - // Return value is irrelevant as long as it's non-negative return true; } -bool CANIface::set_event_handle(AP_HAL::EventHandle* handle) { +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]); @@ -601,13 +289,9 @@ bool CANIface::CANSocketEventSource::wait(uint16_t duration_us, AP_HAL::EventHan 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) { @@ -642,7 +326,6 @@ bool CANIface::CANSocketEventSource::wait(uint16_t duration_us, AP_HAL::EventHan 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; @@ -655,32 +338,16 @@ void CANIface::get_stats(ExpandingString &str) { str.printf("tx_requests: %u\n" "tx_rejected: %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", + "rx_errors: %u\n", stats.tx_requests, stats.tx_rejected, - 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); + stats.rx_errors); } #endif diff --git a/libraries/AP_HAL_SITL/CANSocketIface.h b/libraries/AP_HAL_SITL/CANSocketIface.h index ab9e8e67ca9c48..dc1375c8e810c0 100644 --- a/libraries/AP_HAL_SITL/CANSocketIface.h +++ b/libraries/AP_HAL_SITL/CANSocketIface.h @@ -40,6 +40,7 @@ #include #include #include +#include "CAN_Transport.h" namespace HALSITL { @@ -86,10 +87,6 @@ class CANIface: public AP_HAL::CANIface { int16_t receive(AP_HAL::CANFrame& out_frame, uint64_t& out_timestamp_us, CanIOFlags& out_flags) override; - // Set Filters to ignore frames not to be handled by us - bool configureFilters(const CanFilterConfig* filter_configs, - uint16_t num_configs) override; - // Always return false, there's no busoff condition in virtual CAN bool is_busoff() const override { @@ -100,9 +97,6 @@ class CANIface: public AP_HAL::CANIface { void clear_rx() override; - // Get number of Filter configurations - uint16_t getNumFilters() const override; - // Get total number of Errors discovered uint32_t getErrorCount() const override; @@ -146,19 +140,8 @@ class CANIface: public AP_HAL::CANIface { bool _pollRead(); - int _write(const AP_HAL::CANFrame& frame) const; - - int _read(AP_HAL::CANFrame& frame, uint64_t& ts_usec, bool& loopback) const; - - void _incrementNumFramesInSocketTxQueue(); - void _confirmSentFrame(); - bool _wasInPendingLoopbackSet(const AP_HAL::CANFrame& frame); - - bool _checkHWFilters(const can_frame& frame) const; - bool _checkHWFilters(const canfd_frame& frame) const; - bool _hasReadyTx(); bool _hasReadyRx(); @@ -169,12 +152,7 @@ class CANIface: public AP_HAL::CANIface { void _updateDownStatusFromPollResult(const pollfd& pfd); - uint32_t _bitrate; - - bool _down; - bool _initialized; - - int _fd; + CAN_Transport *transport; const uint8_t _self_index; @@ -184,29 +162,16 @@ class CANIface: public AP_HAL::CANIface { static CANSocketEventSource evt_can_socket[HAL_NUM_CAN_IFACES]; pollfd _pollfd; - std::map _errors; std::priority_queue _tx_queue; std::queue _rx_queue; - std::unordered_multiset _pending_loopback_ids; - std::vector _hw_filters_container; /* - additional statistics + bus statistics */ - struct bus_stats : public AP_HAL::CANIface::bus_stats_t { - uint32_t tx_full; - uint32_t tx_confirmed; - uint32_t num_downs; - uint32_t num_rx_poll_req; - uint32_t num_tx_poll_req; - uint32_t num_poll_waits; - uint32_t num_poll_tx_events; - uint32_t num_poll_rx_events; - } stats; + AP_HAL::CANIface::bus_stats_t stats; HAL_Semaphore sem; -protected: bool add_to_rx_queue(const CanRxItem &rx_item) override { _rx_queue.push(rx_item); return true; diff --git a/libraries/AP_HAL_SITL/CAN_Multicast.cpp b/libraries/AP_HAL_SITL/CAN_Multicast.cpp new file mode 100644 index 00000000000000..b83642f7ef73c0 --- /dev/null +++ b/libraries/AP_HAL_SITL/CAN_Multicast.cpp @@ -0,0 +1,178 @@ +/* + multicast UDP transport for SITL CAN + */ +#include "CAN_Multicast.h" + +#if HAL_NUM_CAN_IFACES + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define MCAST_ADDRESS_BASE "239.65.82.0" +#define MCAST_PORT 57732U +#define MCAST_MAGIC 0x2934U +#define MCAST_FLAG_CANFD 0x0001 +#define MCAST_MAX_PKT_LEN 74 // 64 byte data + 10 byte header + +struct PACKED mcast_pkt { + uint16_t magic; + uint16_t crc; + uint16_t flags; + uint32_t message_id; + uint8_t data[MCAST_MAX_PKT_LEN-10]; +}; + +/* + initialise multicast transport + */ +bool CAN_Multicast::init(uint8_t instance) +{ + // setup incoming multicast socket + char address[] = MCAST_ADDRESS_BASE; + struct sockaddr_in sockaddr {}; + struct ip_mreq mreq {}; + int one = 1; + int ret; + +#ifdef HAVE_SOCK_SIN_LEN + sockaddr.sin_len = sizeof(sockaddr); +#endif + address[strlen(address)-1] = '0' + instance; + + sockaddr.sin_port = htons(MCAST_PORT); + sockaddr.sin_family = AF_INET; + sockaddr.sin_addr.s_addr = inet_addr(address); + + fd_in = socket(AF_INET, SOCK_DGRAM, 0); + if (fd_in == -1) { + goto fail; + } + ret = fcntl(fd_in, F_SETFD, FD_CLOEXEC); + if (ret == -1) { + goto fail; + } + if (setsockopt(fd_in, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one)) == -1) { + goto fail; + } + + // close on exec, to allow reboot + fcntl(fd_in, F_SETFD, FD_CLOEXEC); + +#if defined(__CYGWIN__) || defined(__CYGWIN64__) || defined(CYGWIN_BUILD) + /* + on cygwin you need to bind to INADDR_ANY then use the multicast + IP_ADD_MEMBERSHIP to get on the right address + */ + sockaddr.sin_addr.s_addr = htonl(INADDR_ANY); +#endif + + ret = bind(fd_in, (struct sockaddr *)&sockaddr, sizeof(sockaddr)); + if (ret == -1) { + goto fail; + } + + mreq.imr_multiaddr.s_addr = inet_addr(address); + mreq.imr_interface.s_addr = htonl(INADDR_ANY); + + ret = setsockopt(fd_in, IPPROTO_IP, IP_ADD_MEMBERSHIP, &mreq, sizeof(mreq)); + if (ret == -1) { + goto fail; + } + + // setup outgoing socket + fd_out = socket(AF_INET, SOCK_DGRAM, 0); + if (fd_out == -1) { + goto fail; + } + ret = fcntl(fd_out, F_SETFD, FD_CLOEXEC); + if (ret == -1) { + goto fail; + } + + ret = connect(fd_out, (struct sockaddr *)&sockaddr, sizeof(sockaddr)); + if (ret == -1) { + goto fail; + } + + return true; + +fail: + if (fd_in != -1) { + (void)close(fd_in); + fd_in = -1; + } + if (fd_out != -1) { + (void)close(fd_out); + fd_out = -1; + } + return false; +} + +/* + send a CAN frame + */ +bool CAN_Multicast::send(const AP_HAL::CANFrame &frame) +{ + struct mcast_pkt pkt {}; + pkt.magic = MCAST_MAGIC; + pkt.flags = 0; +#if HAL_CANFD_SUPPORTED + if (frame.canfd) { + pkt.flags |= MCAST_FLAG_CANFD; + } +#endif + pkt.message_id = frame.id; + const uint8_t data_length = AP_HAL::CANFrame::dlcToDataLength(frame.dlc); + memcpy(pkt.data, frame.data, data_length); + pkt.crc = crc16_ccitt((uint8_t*)&pkt.flags, data_length+6, 0xFFFFU); + + return ::send(fd_out, (void*)&pkt, data_length+10, 0) == data_length+10; +} + +/* + receive a CAN frame + */ +bool CAN_Multicast::receive(AP_HAL::CANFrame &frame) +{ + struct mcast_pkt pkt; + struct sockaddr_in src_addr; + socklen_t src_len = sizeof(src_addr); + ssize_t ret = ::recvfrom(fd_in, (void*)&pkt, sizeof(pkt), MSG_DONTWAIT, (struct sockaddr *)&src_addr, &src_len); + if (ret < 10) { + return false; + } + if (pkt.magic != MCAST_MAGIC) { + return false; + } + if (pkt.crc != crc16_ccitt((uint8_t*)&pkt.flags, ret-4, 0xFFFFU)) { + return false; + } + + // ensure it isn't a packet we sent + struct sockaddr_in send_addr; + socklen_t send_len = sizeof(send_addr); + if (getsockname(fd_out, (struct sockaddr *)&send_addr, &send_len) != 0) { + return false; + } + if (src_addr.sin_port == send_addr.sin_port && + src_addr.sin_family == send_addr.sin_family && + src_addr.sin_addr.s_addr == send_addr.sin_addr.s_addr) { + return false; + } + + // run constructor to initialise + new(&frame) AP_HAL::CANFrame(pkt.message_id, pkt.data, ret-10, (pkt.flags & MCAST_FLAG_CANFD) != 0); + + return true; +} + +#endif // HAL_NUM_CAN_IFACES diff --git a/libraries/AP_HAL_SITL/CAN_Multicast.h b/libraries/AP_HAL_SITL/CAN_Multicast.h new file mode 100644 index 00000000000000..be8144cb65ec9f --- /dev/null +++ b/libraries/AP_HAL_SITL/CAN_Multicast.h @@ -0,0 +1,24 @@ +/* + multicast UDP transport for SITL CAN + */ +#pragma once + +#include "CAN_Transport.h" + +#if HAL_NUM_CAN_IFACES + +class CAN_Multicast : public CAN_Transport { +public: + bool init(uint8_t instance) override; + bool send(const AP_HAL::CANFrame &frame) override; + bool receive(AP_HAL::CANFrame &frame) override; + int get_read_fd(void) const override { + return fd_in; + } + +private: + int fd_in = -1; + int fd_out = -1; +}; + +#endif // HAL_NUM_CAN_IFACES diff --git a/libraries/AP_HAL_SITL/CAN_Transport.h b/libraries/AP_HAL_SITL/CAN_Transport.h new file mode 100644 index 00000000000000..52307c4c95799f --- /dev/null +++ b/libraries/AP_HAL_SITL/CAN_Transport.h @@ -0,0 +1,21 @@ +/* + parent class for CAN transports in ArduPilot SITL + */ +#pragma once + +#include "AP_HAL_SITL.h" + +#if HAL_NUM_CAN_IFACES + +#include + +class CAN_Transport { +public: + virtual ~CAN_Transport() {} + virtual bool init(uint8_t instance) = 0; + virtual bool send(const AP_HAL::CANFrame &frame) = 0; + virtual bool receive(AP_HAL::CANFrame &frame) = 0; + virtual int get_read_fd(void) const = 0; +}; + +#endif // HAL_NUM_CAN_IFACES