HAL_SITL: support multicast UDP for CAN in SITL

this will work on windows and in WSL

libraries/AP_HAL_SITL/CANSocketIface.cpp

@@ -32,21 +32,21 @@
 #include <unistd.h>
 #include <fcntl.h>
 
-#include <sys/socket.h>
 #include <sys/ioctl.h>
 #include <net/if.h>
-#include <linux/can/raw.h>
 #include <cstring>
 #include "Scheduler.h"
 #include <AP_CANManager/AP_CANManager.h>
 #include <AP_Common/ExpandingString.h>
+#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 +55,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;
+    return transport != nullptr;
-}
-
-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<void> 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<sockaddr*>(&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<void*>(&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,
@@ -251,180 +119,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;
+    return 0;
-    for (auto& kv : _errors) { ec += kv.second; }
-    return ec;
 }
 
 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);
+            bool ok = transport->send(tx.frame);
-            if (res == 1) {                   // Transmitted successfully
+            if (ok) {
-                _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
+            } else {
-                stats.tx_full++;
+                break;
-                break;                        // Leaving the loop, the frame remains enqueued for the next retry
-            } else {                          // Transmission error
-                stats.tx_rejected++;
             }
         } 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;
+    if (transport == nullptr) {
-    while (iterations_count < CAN_MAX_POLL_ITERATIONS_COUNT)
+        return false;
-    {
-        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;
+    CanRxItem rx {};
-}
+    bool ok = transport->receive(rx.frame);
-
+    if (!ok) {
-int CANIface::_write(const AP_HAL::CANFrame& frame) const
+        return false;
-{
-    if (_fd < 0) {
-        return -1;
     }
-    errno = 0;
+    rx.timestamp_us = AP_HAL::micros64();
-    int res = 0;
+    WITH_SEMAPHORE(sem);
-
+    add_to_rx_queue(rx);
-    if (frame.isCanFDFrame()) {
+    stats.rx_received++;
-        const canfd_frame sockcan_frame = makeSocketCanFDFrame(frame);
+    return true;
-        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 (res == sizeof(can_frame)) {
-        frame = makeCanFrame(frames.frame);
-    } else {
-        frame = makeCanFDFrame(frames.frame_fd);
-    }
-    /*
-     * Timestamp
-     */
-    timestamp_us = AP_HAL::micros64();
-    return 1;
 }
 
 // Might block forever, only to be used for testing
@@ -432,9 +179,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 +191,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 +198,51 @@ void CANIface::_confirmSentFrame()
     }
 }
 
-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;
-    }
-}
-
-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;
-            }
-        }
-        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<void*>(&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 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];
+    const auto *_sitl = AP::sitl();
-    sprintf(iface_name, "vcan%u", _self_index);
+    if (_sitl == nullptr) {
-    bitrate_ = bitrate;
+        return false;
-    mode_ = mode;
-    if (_initialized) {
-        return _initialized;
     }
-
+    if (_self_index >= HAL_NUM_CAN_IFACES) {
-    // TODO: Add possibility change bitrate
+        return false;
-    _fd = _openSocket(iface_name);
-    if (_fd > 0) {
-        _bitrate = bitrate;
-        _initialized = true;
-    } else {
-        _initialized = false;
     }
-    return _initialized;
+    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;
+    }
+    if (transport == nullptr) {
+        return false;
+    }
+    if (!transport->init(_self_index)) {
+        delete transport;
+        transport = nullptr;
+        return false;
+    }
+    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 +251,22 @@ bool CANIface::select(bool &read_select, bool &write_select,
     }
 
     if (need_block) {
-        if (_down) {
+        _pollfd.fd = transport->get_read_fd();
-            return false;
+        _pollfd.events |= POLLIN;
-        } else {
+    }
-            _pollfd.fd = _fd;
+    if (_evt_handle != nullptr && blocking_deadline > AP_HAL::micros64()) {
-            _pollfd.events |= POLLIN;
+        _evt_handle->wait(blocking_deadline - AP_HAL::micros64());
-            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());
-        }
     }
 
     // Writing the output masks
-    if (!_down) {
+    write_select = true;
-        write_select = true;     // Always ready to write if not down
+    read_select = _hasReadyRx();
-    } 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) {
+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 +288,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) {
@@ -617,13 +300,9 @@ bool CANIface::CANSocketEventSource::wait(uint16_t duration_us, AP_HAL::EventHan
     const uint32_t start_us = AP_HAL::micros();
     do {
         uint16_t wait_us = MIN(100, duration_us);
-        // Timeout conversion
-        auto ts = timespec();
-        ts.tv_sec = 0;
-        ts.tv_nsec = wait_us * 1000UL;
 
         // check FD for input
-        const int res = ppoll(pollfds, num_pollfds, &ts, nullptr);
+        const int res = poll(pollfds, num_pollfds, wait_us/1000U);
 
         if (res < 0) {
             return false;
@@ -642,7 +321,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 +333,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"
+               "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_rejected,
-               stats.tx_full,
-               stats.tx_confirmed,
                stats.tx_success,
                stats.tx_timedout,
                stats.rx_received,
-               stats.rx_errors,
+               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);
 }
 
 #endif

libraries/AP_HAL_SITL/CANSocketIface.h

@@ -13,17 +13,6 @@
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */
 
-/*
- * Many thanks to members of the UAVCAN project:
- *  Pavel Kirienko <pavel.kirienko@gmail.com>
- *  Ilia Sheremet <illia.sheremet@gmail.com>
- *
- *  license info can be found in the uavcan submodule located:
- *  modules/uavcan/LICENSE
- *  modules/uavcan/libuavcan_drivers/linux/include/uavcan_linux/socketcan.hpp
- */
-
-
 #pragma once
 
 #include "AP_HAL_SITL.h"
@@ -32,28 +21,16 @@
 
 #include <AP_HAL/CANIface.h>
 
-#include <linux/can.h>
-
 #include <string>
 #include <queue>
 #include <memory>
 #include <map>
 #include <unordered_set>
 #include <poll.h>
+#include "CAN_Transport.h"
 
 namespace HALSITL {
 
-enum class SocketCanError
-{
-    SocketReadFailure,
-    SocketWriteFailure,
-    TxTimeout
-};
-
-#define CAN_MAX_POLL_ITERATIONS_COUNT 100
-#define CAN_MAX_INIT_TRIES_COUNT 100
-#define CAN_FILTER_NUMBER 8
-
 class CANIface: public AP_HAL::CANIface {
 public:
     CANIface(int index)
@@ -86,10 +63,6 @@ public:
     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 +73,6 @@ public:
 
     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 +116,8 @@ private:
 
     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 +128,7 @@ private:
 
     void _updateDownStatusFromPollResult(const pollfd& pfd);
 
-    uint32_t _bitrate;
+    CAN_Transport *transport;
-
-    bool _down;
-    bool _initialized;
-
-    int _fd;
 
     const uint8_t _self_index;
 
@@ -184,29 +138,16 @@ private:
     static CANSocketEventSource evt_can_socket[HAL_NUM_CAN_IFACES];
 
     pollfd _pollfd;
-    std::map<SocketCanError, uint64_t> _errors;
     std::priority_queue<CanTxItem> _tx_queue;
     std::queue<CanRxItem> _rx_queue;
-    std::unordered_multiset<uint32_t> _pending_loopback_ids;
-    std::vector<can_filter> _hw_filters_container;
 
     /*
-      additional statistics
+      bus statistics
      */
-    struct bus_stats : public AP_HAL::CANIface::bus_stats_t {
+    AP_HAL::CANIface::bus_stats_t stats;
-        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;
 
     HAL_Semaphore sem;
 
-protected:
     bool add_to_rx_queue(const CanRxItem &rx_item) override {
         _rx_queue.push(rx_item);
         return true;

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 <net/if.h>
+#include <unistd.h>
+#include <sys/ioctl.h>
+#include <errno.h>
+#include <stdlib.h>
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <netinet/udp.h>
+#include <arpa/inet.h>
+#include <fcntl.h>
+#include <AP_Math/crc.h>
+
+#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

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

libraries/AP_HAL_SITL/CAN_SocketCAN.cpp

@@ -0,0 +1,91 @@
+/*
+  socketcan transport for SITL CAN
+ */
+#include "CAN_SocketCAN.h"
+
+#if HAL_NUM_CAN_IFACES && HAL_CAN_WITH_SOCKETCAN
+
+#include <net/if.h>
+#include <poll.h>
+#include <string.h>
+#include <unistd.h>
+#include <sys/ioctl.h>
+#include <linux/can.h>
+#include <errno.h>
+#include <stdlib.h>
+#include "CAN_SocketCAN.h"
+
+/*
+  initialise socketcan transport
+ */
+bool CAN_SocketCAN::init(uint8_t instance)
+{
+    struct sockaddr_can addr {};
+    struct ifreq ifr {};
+    int ret;
+
+    fd = socket(PF_CAN, SOCK_RAW | SOCK_NONBLOCK, CAN_RAW);
+    if (fd < 0) {
+        goto fail;
+    }
+
+    snprintf(ifr.ifr_name, sizeof(ifr.ifr_name), "vcan%u", instance);
+    ret = ioctl(fd, SIOCGIFINDEX, &ifr);
+    if (ret == -1) {
+        goto fail;
+    }
+
+    addr.can_family = AF_CAN;
+    addr.can_ifindex = ifr.ifr_ifindex;
+
+    ret = bind(fd, (struct sockaddr*)&addr, sizeof(addr));
+    if (ret == -1) {
+        goto fail;
+    }
+
+    return true;
+
+fail:
+    if (fd != -1) {
+        close(fd);
+        fd = -1;
+    }
+    return false;
+}
+
+/*
+  send a CAN frame
+ */
+bool CAN_SocketCAN::send(const AP_HAL::CANFrame &frame)
+{
+    if (frame.canfd) {
+        // not supported on socketcan
+        return false;
+    }
+    struct can_frame transmit_frame {};
+    transmit_frame.can_id = frame.id;
+    transmit_frame.can_dlc = frame.dlc;
+
+    const uint8_t data_length = AP_HAL::CANFrame::dlcToDataLength(frame.dlc);
+    memcpy(transmit_frame.data, frame.data, data_length);
+
+    return ::write(fd, &transmit_frame, sizeof(transmit_frame)) == sizeof(transmit_frame);
+}
+
+/*
+  receive a CAN frame
+ */
+bool CAN_SocketCAN::receive(AP_HAL::CANFrame &frame)
+{
+    struct can_frame receive_frame;
+    const ssize_t ret = ::read(fd, &receive_frame, sizeof(receive_frame));
+    if (ret != sizeof(receive_frame)) {
+        return false;
+    }
+
+    // run constructor to initialise
+    new(&frame) AP_HAL::CANFrame(receive_frame.can_id, receive_frame.data, receive_frame.can_dlc, false);
+    return true;
+}
+
+#endif // HAL_NUM_CAN_IFACES

libraries/AP_HAL_SITL/CAN_SocketCAN.h

@@ -0,0 +1,23 @@
+/*
+  socketcan transport for SITL CAN
+ */
+#pragma once
+
+#include "CAN_Transport.h"
+
+#if HAL_NUM_CAN_IFACES && HAL_CAN_WITH_SOCKETCAN
+
+class CAN_SocketCAN : 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;
+    }
+
+private:
+    int fd = -1;
+};
+
+#endif // HAL_NUM_CAN_IFACES

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 <AP_HAL/CANIface.h>
+
+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