libraries/AP_Networking/AP_Networking_CAN.cpp

   1 /*
   2   CAN UDP multicast server
   3  */
   4
   5 #include "AP_Networking_Config.h"
   6
   7 #if AP_NETWORKING_CAN_MCAST_ENABLED
   8
   9 #include "AP_Networking.h"
  10
  11 #include <AP_HAL/utility/Socket.h>
  12 #include <AP_HAL/CANIface.h>
  13 #include <GCS_MAVLink/GCS.h>
  14 #include <AP_Math/crc.h>
  15
  16 #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
  17 #include "hal.h"
  18 #include <AP_HAL_ChibiOS/hwdef/common/stm32_util.h>
  19 #include <AP_HAL_ChibiOS/CANIface.h>
  20 #endif
  21
  22 #define MCAST_ADDRESS_BASE "239.65.82.0"
  23 #define MCAST_PORT 57732U
  24 #define MCAST_MAGIC 0x2934U
  25 #define MCAST_FLAG_CANFD 0x0001
  26 #define MCAST_MAX_PKT_LEN 74 // 64 byte data + 10 byte header
  27
  28 struct PACKED mcast_pkt {
  29     uint16_t magic;
  30     uint16_t crc;
  31     uint16_t flags;
  32     uint32_t message_id;
  33     uint8_t data[MCAST_MAX_PKT_LEN-10];
  34 };
  35
  36 #define MCAST_HDR_LENGTH offsetof(mcast_pkt, data)
  37
  38 extern const AP_HAL::HAL& hal;
  39
  40 #ifdef HAL_BOOTLOADER_BUILD
  41 void AP_Networking_CAN::mcast_trampoline(void *ctx)
  42 {
  43     auto *mcast = (AP_Networking_CAN *)ctx;
  44     mcast->mcast_server();
  45 }
  46 extern ChibiOS::CANIface can_iface[HAL_NUM_CAN_IFACES];
  47 extern void thread_sleep_us(uint32_t us);
  48 #endif // HAL_BOOTLOADER_BUILD
  49
  50 /*
  51   get CAN interface for a bus
  52  */
  53 AP_HAL::CANIface *AP_Networking_CAN::get_caniface(uint8_t bus) const
  54 {
  55 #ifdef HAL_BOOTLOADER_BUILD
  56     return &can_iface[bus];
  57 #else
  58     return hal.can[bus];
  59 #endif
  60 }
  61
  62 /*
  63   start the CAN multicast server
  64  */
  65 void AP_Networking_CAN::start(const uint8_t _bus_mask)
  66 {
  67     const uint32_t stack_size = 8192;
  68     bus_mask = _bus_mask;
  69
  70 #ifdef HAL_BOOTLOADER_BUILD
  71     thread_create_alloc(THD_WORKING_AREA_SIZE(stack_size),
  72                         "CAN_MCAST",
  73                         60,
  74                         mcast_trampoline,
  75                         this);
  76 #else
  77     hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_Networking_CAN::mcast_server, void),
  78                                  "CAN_MCAST",
  79                                  stack_size, AP_HAL::Scheduler::PRIORITY_CAN, -1);
  80 #endif
  81 }
  82
  83 /*
  84   main thread for CAN multicast server
  85  */
  86 void AP_Networking_CAN::mcast_server(void)
  87 {
  88 #ifndef HAL_BOOTLOADER_BUILD
  89     while (!hal.scheduler->is_system_initialized()) {
  90         hal.scheduler->delay(100);
  91     }
  92 #endif
  93     GCS_SEND_TEXT(MAV_SEVERITY_INFO, "CAN_MCAST: starting");
  94
  95     ObjectBuffer<AP_HAL::CANFrame> *frame_buffers[HAL_NUM_CAN_IFACES] {};
  96
  97     for (uint8_t bus=0; bus<HAL_NUM_CAN_IFACES; bus++) {
  98         auto *cbus = get_caniface(bus);
  99         if (cbus == nullptr) {
 100             continue;
 101         }
 102         if (bus_mask & (1U<<bus)) {
 103             mcast_sockets[bus] = NEW_NOTHROW SocketAPM(true);
 104             if (mcast_sockets[bus] == nullptr) {
 105                 GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "CAN_MCAST[%u]: failed to create socket", unsigned(bus));
 106                 continue;
 107             }
 108         }
 109         char address[] = MCAST_ADDRESS_BASE;
 110         const uint32_t buffer_size = 20; // good for fw upload
 111         uint8_t callback_id = 0;
 112
 113         address[strlen(address)-1] = '0' + bus;
 114         if (!mcast_sockets[bus]->connect(address, MCAST_PORT)) {
 115             GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "CAN_MCAST[%u]: failed to connect", unsigned(bus));
 116             goto de_allocate;
 117         }
 118
 119         if (!cbus->register_frame_callback(
 120                 FUNCTOR_BIND_MEMBER(&AP_Networking_CAN::can_frame_callback, void, uint8_t, const AP_HAL::CANFrame &, AP_HAL::CANIface::CanIOFlags),
 121                 callback_id)) {
 122             GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "CAN_MCAST[%u]: failed to register", unsigned(bus));
 123             goto de_allocate;
 124         }
 125
 126 #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
 127         // tell the ethernet interface that we want to receive all
 128         // multicast packets
 129         ETH->MACPFR |= ETH_MACPFR_PM;
 130 #endif
 131
 132         frame_buffers[bus] = NEW_NOTHROW ObjectBuffer<AP_HAL::CANFrame>(buffer_size);
 133         if (frame_buffers[bus] == nullptr) {
 134             GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "CAN_MCAST[%u]: failed to allocate buffers", unsigned(bus));
 135             goto de_allocate;
 136         }
 137         continue;
 138
 139     de_allocate:
 140         delete mcast_sockets[bus];
 141         mcast_sockets[bus] = nullptr;
 142     }
 143
 144
 145     // main loop
 146     while (true) {
 147         const uint32_t delay_us = 100; // limit to 10k packets/s
 148 #ifndef HAL_BOOTLOADER_BUILD
 149         hal.scheduler->delay_microseconds(delay_us);
 150 #else
 151         thread_sleep_us(delay_us);
 152 #endif
 153         for (uint8_t bus=0; bus<HAL_NUM_CAN_IFACES; bus++) {
 154             if (mcast_sockets[bus] == nullptr) {
 155                 continue;
 156             }
 157
 158             struct mcast_pkt pkt;
 159             const ssize_t ret = mcast_sockets[bus]->recv((void*)&pkt, sizeof(pkt), 0);
 160             if (ret > MCAST_HDR_LENGTH && ret <= sizeof(pkt)) {
 161                 const uint8_t data_len = ret - MCAST_HDR_LENGTH;
 162                 bool is_canfd = false;
 163 #if HAL_CANFD_SUPPORTED
 164                 is_canfd = (pkt.flags & MCAST_FLAG_CANFD) != 0;
 165 #endif
 166                 if (pkt.magic != MCAST_MAGIC) {
 167                     continue;
 168                 }
 169                 const auto crc = crc16_ccitt((uint8_t*)&pkt.flags, ret - offsetof(mcast_pkt,flags), 0xFFFFU);
 170                 if (pkt.crc != crc) {
 171                     continue;
 172                 }
 173
 174                 // push into queue
 175                 frame_buffers[bus]->push(AP_HAL::CANFrame(pkt.message_id, pkt.data, data_len, is_canfd));
 176             }
 177
 178             /*
 179               send pending frames
 180             */
 181             AP_HAL::CANFrame frame;
 182             const uint16_t timeout_us = 2000;
 183 #if AP_NETWORKING_CAN_MCAST_BRIDGING_ENABLED
 184             const bool bridged = AP::network().is_can_mcast_ep_bridged(bus);
 185 #endif
 186
 187             while (frame_buffers[bus]->peek(frame)) {
 188                 auto *cbus = get_caniface(bus);
 189                 if (cbus == nullptr) {
 190                     break;
 191                 }
 192 #if AP_NETWORKING_CAN_MCAST_BRIDGING_ENABLED
 193                 if (bridged) {
 194                     auto retcode = cbus->send(frame, AP_HAL::micros64() + timeout_us,
 195                                                  AP_HAL::CANIface::IsForwardedFrame);
 196                     if (retcode == 0) {
 197                         break;
 198                     }
 199                 } else
 200 #endif
 201                 {
 202                     // only call the AP_HAL::CANIface send if we are not in bridged mode
 203                     cbus->AP_HAL::CANIface::send(frame, AP_HAL::micros64() + timeout_us,
 204                                                  AP_HAL::CANIface::IsForwardedFrame);
 205                 }
 206
 207                 // we either sent it or there was an error, either way we discard the frame
 208                 frame_buffers[bus]->pop();
 209             }
 210         }
 211     }
 212 }
 213
 214 /*
 215   handler for CAN frames from the registered callback, sending frames
 216   out as multicast UDP
 217  */
 218 void AP_Networking_CAN::can_frame_callback(uint8_t bus, const AP_HAL::CANFrame &frame, AP_HAL::CANIface::CanIOFlags flags)
 219 {
 220     if (bus >= HAL_NUM_CAN_IFACES || mcast_sockets[bus] == nullptr) {
 221         return;
 222     }
 223
 224 #if AP_NETWORKING_CAN_MCAST_BRIDGING_ENABLED
 225     // check if bridged mode is enabled
 226     const bool bridged = AP::network().is_can_mcast_ep_bridged(bus);
 227 #else
 228     // never bridge in bootloader, as we can cause loops if multiple
 229     // bootloaders with mcast are running on the same network and CAN Bus
 230     const bool bridged = false;
 231 #endif
 232
 233     if ((flags & AP_HAL::CANIface::IsForwardedFrame) && !bridged) {
 234         // we don't forward frames that we received from the multicast
 235         // server if not in bridged mode
 236         return;
 237     }
 238
 239     struct mcast_pkt pkt {};
 240     pkt.magic = MCAST_MAGIC;
 241     pkt.flags = 0;
 242 #if HAL_CANFD_SUPPORTED
 243     if (frame.isCanFDFrame()) {
 244         pkt.flags |= MCAST_FLAG_CANFD;
 245     }
 246 #endif
 247     pkt.message_id = frame.id;
 248
 249     const uint8_t data_length = AP_HAL::CANFrame::dlcToDataLength(frame.dlc);
 250
 251     memcpy(pkt.data, frame.data, data_length);
 252     // 6 is the size of the flags and message_id, ie header data after crc
 253     pkt.crc = crc16_ccitt((uint8_t*)&pkt.flags, data_length+6, 0xFFFFU);
 254
 255     mcast_sockets[bus]->send((void*)&pkt, data_length+MCAST_HDR_LENGTH);
 256 }
 257
 258 #endif // AP_NETWORKING_ENABLED