AP_Networking: added multicast UDP CAN server

this allows for operation of CAN interfaces over UDP multicast
networking

libraries/AP_Networking/AP_Networking.cpp

@@ -9,6 +9,7 @@
 #include <AP_Math/crc.h>
 #include <AP_InternalError/AP_InternalError.h>
 #include <AP_Filesystem/AP_Filesystem.h>
+#include <AP_HAL/CANIface.h>
 
 extern const AP_HAL::HAL& hal;
 
@@ -89,7 +90,7 @@ const AP_Param::GroupInfo AP_Networking::var_info[] = {
     // @Param: OPTIONS
     // @DisplayName: Networking options
     // @Description: Networking options
-    // @Bitmask: 0:EnablePPP Ethernet gateway
+    // @Bitmask: 0:EnablePPP Ethernet gateway, 1:Enable CAN1 multicast gateway, 2:Enable CAN2 multicast gateway
     // @RebootRequired: True
     // @User: Advanced
     AP_GROUPINFO("OPTIONS", 9,  AP_Networking,    param.options, 0),
@@ -198,6 +199,20 @@ void AP_Networking::init()
     start_tests();
 #endif
 
+#if AP_NETWORKING_CAN_MCAST_ENABLED && !defined(HAL_BOOTLOADER_BUILD)
+    if (option_is_set(OPTION::CAN1_MCAST_GATEWAY) || option_is_set(OPTION::CAN2_MCAST_GATEWAY)) {
+        // get mask of enabled buses
+        uint8_t bus_mask = 0;
+        if (option_is_set(OPTION::CAN1_MCAST_GATEWAY)) {
+            bus_mask |= (1U<<0);
+        }
+        if (option_is_set(OPTION::CAN2_MCAST_GATEWAY)) {
+            bus_mask |= (1U<<1);
+        }
+        mcast_server.start(bus_mask);
+    }
+#endif
+    
 #if AP_NETWORKING_REGISTER_PORT_ENABLED
     // init network mapped serialmanager ports
     ports_init();

libraries/AP_Networking/AP_Networking.h

@@ -8,6 +8,7 @@
 
 #include "AP_Networking_address.h"
 #include "AP_Networking_Backend.h"
+#include "AP_Networking_CAN.h"
 #include <AP_SerialManager/AP_SerialManager.h>
 #include <AP_HAL/utility/RingBuffer.h>
 
@@ -155,6 +156,10 @@ public:
 
     enum class OPTION {
         PPP_ETHERNET_GATEWAY=(1U<<0),
+#if AP_NETWORKING_CAN_MCAST_ENABLED
+        CAN1_MCAST_GATEWAY=(1U<<1),
+        CAN2_MCAST_GATEWAY=(1U<<2),
+#endif
     };
     bool option_is_set(OPTION option) const {
         return (param.options.get() & int32_t(option)) != 0;
@@ -297,6 +302,10 @@ private:
     Port ports[AP_NETWORKING_NUM_PORTS];
 #endif
 
+#if AP_NETWORKING_CAN_MCAST_ENABLED
+    AP_Networking_CAN mcast_server;
+#endif
+
     // support for sendfile()
     struct SendFile {
         SocketAPM *sock;

libraries/AP_Networking/AP_Networking_CAN.cpp

@@ -0,0 +1,227 @@
+/*
+  CAN UDP multicast server
+ */
+
+#include "AP_Networking_Config.h"
+
+#if AP_NETWORKING_CAN_MCAST_ENABLED
+
+#include "AP_Networking.h"
+
+#include <AP_HAL/utility/Socket.h>
+#include <AP_HAL/CANIface.h>
+#include <GCS_MAVLink/GCS.h>
+#include <AP_Math/crc.h>
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
+#include "hal.h"
+#include <AP_HAL_ChibiOS/hwdef/common/stm32_util.h>
+#include <AP_HAL_ChibiOS/CANIface.h>
+#endif
+
+#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];
+};
+
+#define MCAST_HDR_LENGTH offsetof(mcast_pkt, data)
+
+extern const AP_HAL::HAL& hal;
+
+#ifdef HAL_BOOTLOADER_BUILD
+void AP_Networking_CAN::mcast_trampoline(void *ctx)
+{
+    auto *mcast = (AP_Networking_CAN *)ctx;
+    mcast->mcast_server();
+}
+extern ChibiOS::CANIface can_iface[HAL_NUM_CAN_IFACES];
+extern void thread_sleep_us(uint32_t us);
+#endif // HAL_BOOTLOADER_BUILD
+
+/*
+  get CAN interface for a bus
+ */
+AP_HAL::CANIface *AP_Networking_CAN::get_caniface(uint8_t bus) const
+{
+#ifdef HAL_BOOTLOADER_BUILD
+    return &can_iface[bus];
+#else
+    return hal.can[bus];
+#endif
+}
+
+/*
+  start the CAN multicast server
+ */
+void AP_Networking_CAN::start(const uint8_t _bus_mask)
+{
+    const uint32_t stack_size = 8192;
+    bus_mask = _bus_mask;
+
+#ifdef HAL_BOOTLOADER_BUILD
+    thread_create_alloc(THD_WORKING_AREA_SIZE(stack_size),
+                        "CAN_MCAST",
+                        60,
+                        mcast_trampoline,
+                        this);
+#else
+    hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_Networking_CAN::mcast_server, void),
+                                 "CAN_MCAST",
+                                 stack_size, AP_HAL::Scheduler::PRIORITY_CAN, -1);
+#endif
+}
+
+/*
+  main thread for CAN multicast server
+ */
+void AP_Networking_CAN::mcast_server(void)
+{
+#ifndef HAL_BOOTLOADER_BUILD
+    while (!hal.scheduler->is_system_initialized()) {
+        hal.scheduler->delay(100);
+    }
+#endif
+    GCS_SEND_TEXT(MAV_SEVERITY_INFO, "CAN_MCAST: starting");
+
+    ObjectBuffer<AP_HAL::CANFrame> *frame_buffers[HAL_NUM_CAN_IFACES] {};
+
+    for (uint8_t bus=0; bus<HAL_NUM_CAN_IFACES; bus++) {
+        auto *cbus = get_caniface(bus);
+        if (cbus == nullptr) {
+            continue;
+        }
+        if (bus_mask & (1U<<bus)) {
+            mcast_sockets[bus] = NEW_NOTHROW SocketAPM(true);
+            if (mcast_sockets[bus] == nullptr) {
+                GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "CAN_MCAST[%u]: failed to create socket", unsigned(bus));
+                continue;
+            }
+        }
+        char address[] = MCAST_ADDRESS_BASE;
+        const uint32_t buffer_size = 20; // good for fw upload
+        uint8_t callback_id = 0;
+
+        address[strlen(address)-1] = '0' + bus;
+        if (!mcast_sockets[bus]->connect(address, MCAST_PORT)) {
+            GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "CAN_MCAST[%u]: failed to connect", unsigned(bus));
+            goto de_allocate;
+        }
+
+        if (!cbus->register_frame_callback(
+                FUNCTOR_BIND_MEMBER(&AP_Networking_CAN::can_frame_callback, void, uint8_t, const AP_HAL::CANFrame &),
+                callback_id)) {
+            GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "CAN_MCAST[%u]: failed to register", unsigned(bus));
+            goto de_allocate;
+        }
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
+        // tell the ethernet interface that we want to receive all
+        // multicast packets
+        ETH->MACPFR |= ETH_MACPFR_PM;
+#endif
+
+        frame_buffers[bus] = NEW_NOTHROW ObjectBuffer<AP_HAL::CANFrame>(buffer_size);
+        if (frame_buffers[bus] == nullptr) {
+            GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "CAN_MCAST[%u]: failed to allocate buffers", unsigned(bus));
+            goto de_allocate;
+        }
+        continue;
+
+    de_allocate:
+        delete mcast_sockets[bus];
+        mcast_sockets[bus] = nullptr;
+    }
+
+
+    // main loop
+    while (true) {
+        const uint32_t delay_us = 100; // limit to 10k packets/s
+#ifndef HAL_BOOTLOADER_BUILD
+        hal.scheduler->delay_microseconds(delay_us);
+#else
+        thread_sleep_us(delay_us);
+#endif
+        for (uint8_t bus=0; bus<HAL_NUM_CAN_IFACES; bus++) {
+            if (mcast_sockets[bus] == nullptr) {
+                continue;
+            }
+
+            struct mcast_pkt pkt;
+            const ssize_t ret = mcast_sockets[bus]->recv((void*)&pkt, sizeof(pkt), 0);
+            if (ret > MCAST_HDR_LENGTH && ret <= sizeof(pkt)) {
+                const uint8_t data_len = ret - MCAST_HDR_LENGTH;
+                bool is_canfd = false;
+#if HAL_CANFD_SUPPORTED
+                is_canfd = (pkt.flags & MCAST_FLAG_CANFD) != 0;
+#endif
+                if (pkt.magic != MCAST_MAGIC) {
+                    continue;
+                }
+                const auto crc = crc16_ccitt((uint8_t*)&pkt.flags, ret - offsetof(mcast_pkt,flags), 0xFFFFU);
+                if (pkt.crc != crc) {
+                    continue;
+                }
+
+                // push into queue
+                frame_buffers[bus]->push(AP_HAL::CANFrame(pkt.message_id, pkt.data, data_len, is_canfd));
+            }
+
+            /*
+              send pending frames
+            */
+            AP_HAL::CANFrame frame;
+            const uint16_t timeout_us = 2000;
+
+            while (frame_buffers[bus]->peek(frame)) {
+                auto retcode = get_caniface(bus)->send(frame,
+                                                      AP_HAL::micros64() + timeout_us,
+                                                      AP_HAL::CANIface::IsMAVCAN);
+                if (retcode == 0) {
+                    break;
+                }
+                // we either sent it or there was an error, either way we discard the frame
+                frame_buffers[bus]->pop();
+            }
+        }
+    }
+}
+
+/*
+  handler for CAN frames from the registered callback, sending frames
+  out as multicast UDP
+ */
+void AP_Networking_CAN::can_frame_callback(uint8_t bus, const AP_HAL::CANFrame &frame)
+{
+    if (bus >= HAL_NUM_CAN_IFACES || mcast_sockets[bus] == nullptr) {
+        return;
+    }
+
+    struct mcast_pkt pkt {};
+    pkt.magic = MCAST_MAGIC;
+    pkt.flags = 0;
+#if HAL_CANFD_SUPPORTED
+    if (frame.isCanFDFrame()) {
+        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);
+    // 6 is the size of the flags and message_id, ie header data after crc
+    pkt.crc = crc16_ccitt((uint8_t*)&pkt.flags, data_length+6, 0xFFFFU);
+
+    mcast_sockets[bus]->send((void*)&pkt, data_length+MCAST_HDR_LENGTH);
+}
+
+#endif // AP_NETWORKING_ENABLED && AP_NETWORKING_CAN_MCAST_ENABLED

libraries/AP_Networking/AP_Networking_CAN.h

@@ -0,0 +1,23 @@
+#pragma once
+
+#include "AP_Networking.h"
+#include <AP_HAL/utility/Socket.h>
+
+class AP_Networking_CAN {
+public:
+    void start(const uint8_t bus_mask);
+
+private:
+    void mcast_server(void);
+    void can_frame_callback(uint8_t bus, const AP_HAL::CANFrame &frame);
+    SocketAPM *mcast_sockets[HAL_NUM_CAN_IFACES];
+
+    uint8_t bus_mask;
+
+    AP_HAL::CANIface *get_caniface(uint8_t) const;
+
+#ifdef HAL_BOOTLOADER_BUILD
+    static void mcast_trampoline(void *ctx);
+#endif
+};
+

libraries/AP_Networking/AP_Networking_Config.h

@@ -103,6 +103,10 @@
 #define AP_NETWORKING_TESTS_ENABLED 0
 #endif
 
+#ifndef AP_NETWORKING_CAN_MCAST_ENABLED
+#define AP_NETWORKING_CAN_MCAST_ENABLED 0
+#endif
+
 #if AP_NETWORKING_TESTS_ENABLED
 #ifndef AP_NETWORKING_TEST_IP
 #define AP_NETWORKING_TEST_IP "192.168.13.2"