From 1e067622d295692e247b3d141a43efdad3bedb6b Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Thu, 30 Jan 2025 12:01:21 +1100 Subject: [PATCH] AP_Networking: make can multicast an endpoint by default also add option to enable multicast with bridging to CAN bus in application and disabled in bootloader --- libraries/AP_Networking/AP_Networking.cpp | 10 ++--- libraries/AP_Networking/AP_Networking.h | 15 ++++++- libraries/AP_Networking/AP_Networking_CAN.cpp | 43 ++++++++++++++++--- libraries/AP_Networking/AP_Networking_CAN.h | 2 +- .../AP_Networking/AP_Networking_Config.h | 4 ++ 5 files changed, 60 insertions(+), 14 deletions(-) diff --git a/libraries/AP_Networking/AP_Networking.cpp b/libraries/AP_Networking/AP_Networking.cpp index 12b0fb1ae3..c5a8cb974f 100644 --- a/libraries/AP_Networking/AP_Networking.cpp +++ b/libraries/AP_Networking/AP_Networking.cpp @@ -90,7 +90,7 @@ const AP_Param::GroupInfo AP_Networking::var_info[] = { // @Param: OPTIONS // @DisplayName: Networking options // @Description: Networking options - // @Bitmask: 0:EnablePPP Ethernet gateway, 1:Enable CAN1 multicast gateway, 2:Enable CAN2 multicast gateway + // @Bitmask: 0:EnablePPP Ethernet gateway, 1:Enable CAN1 multicast endpoint, 2:Enable CAN2 multicast endpoint, 3:Enable CAN1 multicast bridged, 4:Enable CAN2 multicast bridged // @RebootRequired: True // @User: Advanced AP_GROUPINFO("OPTIONS", 9, AP_Networking, param.options, 0), @@ -199,14 +199,14 @@ 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)) { +#if AP_NETWORKING_CAN_MCAST_BRIDGING_ENABLED + if (option_is_set(OPTION::CAN1_MCAST_ENDPOINT) || option_is_set(OPTION::CAN1_MCAST_ENDPOINT)) { // get mask of enabled buses uint8_t bus_mask = 0; - if (option_is_set(OPTION::CAN1_MCAST_GATEWAY)) { + if (option_is_set(OPTION::CAN1_MCAST_ENDPOINT)) { bus_mask |= (1U<<0); } - if (option_is_set(OPTION::CAN2_MCAST_GATEWAY)) { + if (option_is_set(OPTION::CAN1_MCAST_ENDPOINT)) { bus_mask |= (1U<<1); } mcast_server.start(bus_mask); diff --git a/libraries/AP_Networking/AP_Networking.h b/libraries/AP_Networking/AP_Networking.h index 5c7e8ebca1..e9f4c78136 100644 --- a/libraries/AP_Networking/AP_Networking.h +++ b/libraries/AP_Networking/AP_Networking.h @@ -157,14 +157,25 @@ 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), + CAN1_MCAST_ENDPOINT=(1U<<1), + CAN2_MCAST_ENDPOINT=(1U<<2), +#if AP_NETWORKING_CAN_MCAST_BRIDGING_ENABLED + CAN1_MCAST_BRIDGED=(1U<<3), + CAN2_MCAST_BRIDGED=(1U<<4), +#endif #endif }; bool option_is_set(OPTION option) const { return (param.options.get() & int32_t(option)) != 0; } +#if AP_NETWORKING_CAN_MCAST_BRIDGING_ENABLED + bool is_can_mcast_ep_bridged(uint8_t bus) const { + return (option_is_set(OPTION::CAN1_MCAST_BRIDGED) && bus == 0) || + (option_is_set(OPTION::CAN2_MCAST_BRIDGED) && bus == 1); + } +#endif + private: static AP_Networking *singleton; diff --git a/libraries/AP_Networking/AP_Networking_CAN.cpp b/libraries/AP_Networking/AP_Networking_CAN.cpp index 875fece574..07e4d77f55 100644 --- a/libraries/AP_Networking/AP_Networking_CAN.cpp +++ b/libraries/AP_Networking/AP_Networking_CAN.cpp @@ -117,7 +117,7 @@ void AP_Networking_CAN::mcast_server(void) } if (!cbus->register_frame_callback( - FUNCTOR_BIND_MEMBER(&AP_Networking_CAN::can_frame_callback, void, uint8_t, const AP_HAL::CANFrame &), + FUNCTOR_BIND_MEMBER(&AP_Networking_CAN::can_frame_callback, void, uint8_t, const AP_HAL::CANFrame &, AP_HAL::CANIface::CanIOFlags), callback_id)) { GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "CAN_MCAST[%u]: failed to register", unsigned(bus)); goto de_allocate; @@ -180,14 +180,30 @@ void AP_Networking_CAN::mcast_server(void) */ AP_HAL::CANFrame frame; const uint16_t timeout_us = 2000; +#if AP_NETWORKING_CAN_MCAST_BRIDGING_ENABLED + const bool bridged = AP::network().is_can_mcast_ep_bridged(bus); +#endif 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) { + auto *cbus = get_caniface(bus); + if (cbus == nullptr) { break; } +#if AP_NETWORKING_CAN_MCAST_BRIDGING_ENABLED + if (bridged) { + auto retcode = cbus->send(frame, AP_HAL::micros64() + timeout_us, + AP_HAL::CANIface::IsForwardedFrame); + if (retcode == 0) { + break; + } + } else +#endif + { + // only call the AP_HAL::CANIface send if we are not in bridged mode + cbus->AP_HAL::CANIface::send(frame, AP_HAL::micros64() + timeout_us, + AP_HAL::CANIface::IsForwardedFrame); + } + // we either sent it or there was an error, either way we discard the frame frame_buffers[bus]->pop(); } @@ -199,12 +215,27 @@ void AP_Networking_CAN::mcast_server(void) 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) +void AP_Networking_CAN::can_frame_callback(uint8_t bus, const AP_HAL::CANFrame &frame, AP_HAL::CANIface::CanIOFlags flags) { if (bus >= HAL_NUM_CAN_IFACES || mcast_sockets[bus] == nullptr) { return; } +#if AP_NETWORKING_CAN_MCAST_BRIDGING_ENABLED + // check if bridged mode is enabled + const bool bridged = AP::network().is_can_mcast_ep_bridged(bus); +#else + // never bridge in bootloader, as we can cause loops if multiple + // bootloaders with mcast are running on the same network and CAN Bus + const bool bridged = false; +#endif + + if ((flags & AP_HAL::CANIface::IsForwardedFrame) && !bridged) { + // we don't forward frames that we received from the multicast + // server if not in bridged mode + return; + } + struct mcast_pkt pkt {}; pkt.magic = MCAST_MAGIC; pkt.flags = 0; diff --git a/libraries/AP_Networking/AP_Networking_CAN.h b/libraries/AP_Networking/AP_Networking_CAN.h index 3e968e4fdb..65f4ad393e 100644 --- a/libraries/AP_Networking/AP_Networking_CAN.h +++ b/libraries/AP_Networking/AP_Networking_CAN.h @@ -9,7 +9,7 @@ public: private: void mcast_server(void); - void can_frame_callback(uint8_t bus, const AP_HAL::CANFrame &frame); + void can_frame_callback(uint8_t bus, const AP_HAL::CANFrame &frame, AP_HAL::CANIface::CanIOFlags flags); SocketAPM *mcast_sockets[HAL_NUM_CAN_IFACES]; uint8_t bus_mask; diff --git a/libraries/AP_Networking/AP_Networking_Config.h b/libraries/AP_Networking/AP_Networking_Config.h index 342257972f..74e5f98ece 100644 --- a/libraries/AP_Networking/AP_Networking_Config.h +++ b/libraries/AP_Networking/AP_Networking_Config.h @@ -107,6 +107,10 @@ #define AP_NETWORKING_CAN_MCAST_ENABLED 0 #endif +#ifndef AP_NETWORKING_CAN_MCAST_BRIDGING_ENABLED +#define AP_NETWORKING_CAN_MCAST_BRIDGING_ENABLED AP_NETWORKING_CAN_MCAST_ENABLED +#endif + #if AP_NETWORKING_TESTS_ENABLED #ifndef AP_NETWORKING_TEST_IP #define AP_NETWORKING_TEST_IP "192.168.144.2"