From cd14b2e1c5a9ad90bca835065704fa512036994a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 8 Sep 2024 12:38:47 +1000 Subject: [PATCH] AP_Networking: added multicast UDP CAN server this allows for operation of CAN interfaces over UDP multicast networking --- libraries/AP_Networking/AP_Networking.cpp | 17 +- libraries/AP_Networking/AP_Networking.h | 9 + libraries/AP_Networking/AP_Networking_CAN.cpp | 227 ++++++++++++++++++ libraries/AP_Networking/AP_Networking_CAN.h | 23 ++ .../AP_Networking/AP_Networking_Config.h | 4 + 5 files changed, 279 insertions(+), 1 deletion(-) create mode 100644 libraries/AP_Networking/AP_Networking_CAN.cpp create mode 100644 libraries/AP_Networking/AP_Networking_CAN.h diff --git a/libraries/AP_Networking/AP_Networking.cpp b/libraries/AP_Networking/AP_Networking.cpp index c00ef4a713..12b0fb1ae3 100644 --- a/libraries/AP_Networking/AP_Networking.cpp +++ b/libraries/AP_Networking/AP_Networking.cpp @@ -9,6 +9,7 @@ #include #include #include +#include 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(); diff --git a/libraries/AP_Networking/AP_Networking.h b/libraries/AP_Networking/AP_Networking.h index ea94afe8e6..71d323dc8b 100644 --- a/libraries/AP_Networking/AP_Networking.h +++ b/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 #include @@ -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; diff --git a/libraries/AP_Networking/AP_Networking_CAN.cpp b/libraries/AP_Networking/AP_Networking_CAN.cpp new file mode 100644 index 0000000000..875fece574 --- /dev/null +++ b/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 +#include +#include +#include + +#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS +#include "hal.h" +#include +#include +#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 *frame_buffers[HAL_NUM_CAN_IFACES] {}; + + for (uint8_t bus=0; busconnect(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(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; busrecv((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 diff --git a/libraries/AP_Networking/AP_Networking_CAN.h b/libraries/AP_Networking/AP_Networking_CAN.h new file mode 100644 index 0000000000..3e968e4fdb --- /dev/null +++ b/libraries/AP_Networking/AP_Networking_CAN.h @@ -0,0 +1,23 @@ +#pragma once + +#include "AP_Networking.h" +#include + +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 +}; + diff --git a/libraries/AP_Networking/AP_Networking_Config.h b/libraries/AP_Networking/AP_Networking_Config.h index 7a5dbad3cf..a29aed082c 100644 --- a/libraries/AP_Networking/AP_Networking_Config.h +++ b/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"