diff --git a/drivers/stm32g4_fdcan/README.md b/drivers/stm32g4_fdcan/README.md new file mode 100644 index 0000000..22c6d4c --- /dev/null +++ b/drivers/stm32g4_fdcan/README.md @@ -0,0 +1,52 @@ +# STM32G4 FDCAN driver for libcanard/DroneCAN + +This is a bare-bones driver for the variety of FDCAN that's found in STM32G4 series. + +* TX FIFO is used in queue mode +* Incoming messages are prioritized based on filters (see below). +* Multiple instances are supported +* No dependencies, no interrupts (unless you want them) +* Specific for DroneCAN. + +FDCANs are slightly different between STM32G4, STM32H7 (this has a very configurable SRAM) and STM32G0. +So this is only for G4, but can probably be extended to support everything. + +## General usage + +### Initialization + +1. Store a `canard_stm32g4_fdcan_driver` somewhere and set the `fdcan` pointer +to one of `FDCAN1_ADDR`/`FDCAN2_ADDR`/`FDCAN3_ADDR`, then +call `canard_stm32g4fdcan_init`. + +2. Then configure filters by calling `canard_stm32g4fdcan_type_id_filter` as many times as needed (8 double +rules can be created all in all). + +3. Call `canard_stm32g4fdcan_start`. + +4. After a Node ID has been assigned (or if you know it beforehand), call `canard_stm32g4fdcan_enable_automatic_retransmission`. + +It's the user's responsibility to provide clock to the chosen FDCAN peripheral and configure I/O pins. + +### Reception + +The driver sets up two RX FIFOs, FIFO0 for "more important" and FIFO1 for "less important" messages. + +Three things can happen to a message on reception (see `canard_stm32g4fdcan_type_id_filter`): +1. The DroneCAN data type ID is in the "accept" filter (`accept_not_reject=1`) -> goes to FIFO0 +2. The DroneCAN data type ID is in the "reject" filter (`accept_not_reject=0`) -> discarded +3. The DroneCAN data type ID is in neither -> goes to FIFO1 + +`canard_stm32g4fdcan_receive` reads one message at a time and prefers FIFO0. + +For instance, a ESC +1. would prioritize `1031.RawCommand` above anything else - so this ID should be accepted. +2. would reject spammy `1034.Status` messages from other ESCs - so this ID should be rejected. + +The filter functions are used by DroneCAN data type IDs, not CAN frame IDs. + +Note that all service requests will automatically go to FIFO1. + +## Config + +If `CANARD_ENABLE_CANFD` is defined, FDCAN will be configured in long frame mode + bitrate switch mode. diff --git a/drivers/stm32g4_fdcan/_fdcan_g4.h b/drivers/stm32g4_fdcan/_fdcan_g4.h new file mode 100644 index 0000000..ce942a8 --- /dev/null +++ b/drivers/stm32g4_fdcan/_fdcan_g4.h @@ -0,0 +1,189 @@ +/* + * Copyright (c) 2024-2026 Flytrex, by Grisha Revzin + * + * Distributed under the MIT License, available in the file LICENSE. + * + * Created on: Sep 10, 2024 + */ + +#ifndef INTERNAL_FDCAN_H +#define INTERNAL_FDCAN_H + +#include + +/* ------------------- SRAM layout --------------------------------------------------------------------------------- */ + +typedef struct __attribute__((packed, aligned(4))) { + volatile uint32_t CREL; + volatile uint32_t ENDN; + volatile uint32_t RESERVED1; + volatile uint32_t DBTP; + volatile uint32_t TEST; + volatile uint32_t RWD; + volatile uint32_t CCCR; + volatile uint32_t NBTP; + volatile uint32_t TSCC; + volatile uint32_t TSCV; + volatile uint32_t TOCC; + volatile uint32_t TOCV; + volatile uint32_t RESERVED2[4]; + volatile uint32_t ECR; + volatile uint32_t PSR; + volatile uint32_t TDCR; + volatile uint32_t RESERVED3; + volatile uint32_t IR; + volatile uint32_t IE; + volatile uint32_t ILS; + volatile uint32_t ILE; + volatile uint32_t RESERVED4[8]; + volatile uint32_t RXGFC; + volatile uint32_t XIDAM; + volatile uint32_t HPMS; + volatile uint32_t RESERVED5; + volatile uint32_t RXF0S; + volatile uint32_t RXF0A; + volatile uint32_t RXF1S; + volatile uint32_t RXF1A; + volatile uint32_t RESERVED6[8]; + volatile uint32_t TXBC; + volatile uint32_t TXFQS; + volatile uint32_t TXBRP; + volatile uint32_t TXBAR; + volatile uint32_t TXBCR; + volatile uint32_t TXBTO; + volatile uint32_t TXBCF; + volatile uint32_t TXBTIE; + volatile uint32_t TXBCIE; + volatile uint32_t TXEFS; + volatile uint32_t TXEFA; +} fdcan_registers; + +/* Helper to iterate over RXFxS/A */ +typedef struct __attribute__((packed, aligned(4))) { + volatile uint32_t RXFxS; + volatile uint32_t RXFxA; +} fdcan_rxfifo_regs; + +/* ------------------- SRAM layout --------------------------------------------------------------------------------- */ +/* Note: bitfields only to get a register-like view in GDB. Don't use them directly, it explodes. */ + +typedef struct __attribute__((packed, aligned(4))) { + union { + uint32_t t0; + struct { + uint32_t id : 29; + uint32_t rtr : 1; /* remote frame */ + uint32_t xtd : 1; /* extended frame */ + uint32_t esi : 1; /* Force error passive flag */ + }; + }; + + union { + uint32_t t1; + struct { + uint32_t reserved0 : 16; + uint32_t dlc : 4; /* DLC */ + uint32_t brs : 1; /* Bit Rate Switching Frame */ + uint32_t fdf : 1; /* CAN FD frame */ + uint32_t reserved1 : 1; + uint32_t efc : 1; /* store tx events */ + uint32_t mm : 8; /* message marker (tx events) */ + }; + }; + uint32_t data[64 / sizeof(uint32_t)]; +} fdcan_tx_buf_element; + +typedef struct __attribute__((packed, aligned(4))) { + union { + uint32_t r0; + struct { + uint32_t id : 29; + uint32_t rtr : 1; /* remote */ + uint32_t xtd : 1; /* extended */ + uint32_t esi : 1; /* transmitter error passive */ + }; + }; + + union { + uint32_t r1; + struct { + uint32_t rxts : 16;/* timestamp */ + uint32_t dlc : 4; + uint32_t brs : 1; /* Bit Rate Switching Frame */ + uint32_t fdf : 1; /* CAN FD frame */ + uint32_t reserved0 : 1; + uint32_t fidx : 7; /* filter index */ + uint32_t anmf : 1; /* no filter matched */ + }; + }; + uint32_t data[64 / sizeof(uint32_t)]; +} fdcan_rx_buf_element; + +typedef struct __attribute__((packed, aligned(4))) { + /* not detailed, standard IDs not used for DroneCAN */ + uint32_t stub; +} fdcan_stdid_filter_element; + +typedef struct __attribute__((packed, aligned(4))) { + /* not detailed, tx events are not used */ + uint32_t stub[2]; +} fdcan_tx_event_element; + +typedef struct __attribute__((packed, aligned(4))) { + union { + uint32_t f0; + struct { + uint32_t efid1 : 29; + enum { + FILT_ELEM_EFEC_DISABLE, + FILT_ELEM_EFEC_STORE_RXFIFO0, + FILT_ELEM_EFEC_STORE_RXFIFO1, + FILT_ELEM_EFEC_REJECT, + FILT_ELEM_EFEC_SET_PRIORITY_STORE_RXFIFO0, + FILT_ELEM_EFEC_SET_PRIORITY_STORE_RXFIFO1, + FILT_ELEM_EFEC_RESERVED, + } efec : 3; + }; + }; + union { + uint32_t f1; + struct { + uint32_t efid2 : 29; + uint32_t reserved0 : 1; + enum { + FILT_ELEM_EFT_RANGE, + FILT_ELEM_EFT_DUAL_ID, + FILT_ELEM_EFT_ID_MASK, + FILT_ELEM_EFT_RANGE_NO_XIDAM + } eft : 2; + }; + }; +} fdcan_extid_filter_element; + +/* Figure 669 RM0440 */ +#define FDCAN_NUM_STDID_FE 28 +#define FDCAN_NUM_EXTID_FE 8 +#define FDCAN_NUM_RXFIFO_ELEMENTS 3 +#define FDCAN_NUM_TX_EVENT_FIFOS 3 +#define FDCAN_NUM_TX_BUF 3 + +typedef struct __attribute__((packed, aligned(4))) { + fdcan_stdid_filter_element stdid_filter_element[FDCAN_NUM_STDID_FE]; // 28 // 28 + fdcan_extid_filter_element extid_filter_element[FDCAN_NUM_EXTID_FE]; // 16 // 44 + fdcan_rx_buf_element rxfifo0[FDCAN_NUM_RXFIFO_ELEMENTS]; // 54 // 98 + fdcan_rx_buf_element rxfifo1[FDCAN_NUM_RXFIFO_ELEMENTS]; // 54 // 152 + fdcan_tx_event_element txevent_fifo[FDCAN_NUM_TX_EVENT_FIFOS]; // 6 // 158 + fdcan_tx_buf_element txbuf[FDCAN_NUM_TX_BUF]; // 54 // 212 == 0x350 +} fdcan_sram; + +_Static_assert(sizeof(fdcan_sram) == 0x350, "fdcan_sram size must be 0x350"); + +#define FDCAN1 ((fdcan_registers *) FDCAN1_ADDR) +#define FDCAN2 ((fdcan_registers *) FDCAN2_ADDR) +#define FDCAN3 ((fdcan_registers *) FDCAN3_ADDR) + +#define IP_OFFSET (FDCAN2_ADDR - FDCAN1_ADDR) + +#define SRAMCAN_START 0x4000A400U + +#endif diff --git a/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c b/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c new file mode 100644 index 0000000..7b7ed29 --- /dev/null +++ b/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c @@ -0,0 +1,458 @@ +/* + * Copyright (c) 2024-2026 Flytrex, by Grisha Revzin + * + * Distributed under the MIT License, available in the file LICENSE. + * + * Created on: Sep 10, 2024 + */ + +#include "canard_stm32g4_fdcan.h" +#include +#include "_fdcan_g4.h" +#include "../stm32/canard_stm32.h" /* for CanardSTM32ComputeCANTimings */ + +/* Local */ +__attribute__((const)) +static inline uint32_t fdcan_ram(const fdcan_registers *r); +static void clear_and_handle_faults(canard_stm32g4_fdcan_driver *driver); +static void canard_frame_to_tx_buf_elem(const CanardCANFrame* const frame, fdcan_tx_buf_element *ele); +static int rxfifo_get_first_elem_index(fdcan_rxfifo_regs *rxf); +static void rxfifo_receive_frame(fdcan_registers *regs, CanardCANFrame* const out_frame, fdcan_rx_buf_element *ele); +static int rxfifo_get_fill_level(fdcan_rxfifo_regs *rxf); +static int rxfifo_get_read_index(fdcan_rxfifo_regs *rxf); +static void rxfifo_ack_frame(fdcan_rxfifo_regs *rxfifo_regs, int ack_index); + +__attribute__((const)) +static inline int dlc_decode(int dlc_value, int fd); + +__attribute__((const)) +static inline int dlc_encode(int data_len, int fd); + +#define EXT_ID_FILTER 0x1FFFFFFF + +/* Module */ + +int canard_stm32g4fdcan_init(canard_stm32g4_fdcan_driver *driver, int bitrate_bps, int fdbitrate_bps, int periph_clock_rate) +{ + (void) fdbitrate_bps; + + /* Calculate timings based on the provided bitrates */ + CanardSTM32CANTimings nominal_timings = {0}; + int rc = canardSTM32ComputeCANTimings(periph_clock_rate, bitrate_bps, &nominal_timings); + + if ((nominal_timings.bit_rate_prescaler < 1) || (nominal_timings.bit_rate_prescaler > 1024) || + (nominal_timings.max_resynchronization_jump_width < 1) || (nominal_timings.max_resynchronization_jump_width > 4) || + (nominal_timings.bit_segment_1 < 1) || (nominal_timings.bit_segment_1 > 16) || + (nominal_timings.bit_segment_2 < 1) || (nominal_timings.bit_segment_2 > 8)) + { + return -CANARD_ERROR_INVALID_ARGUMENT; + } + + if (rc < 0) + return rc; + +#if CANARD_ENABLE_CANFD + CanardSTM32CANTimings brs_timings = {0}; + rc = canardSTM32ComputeCANTimings(periph_clock_rate, fdbitrate_bps, &brs_timings); + + if ((brs_timings.bit_rate_prescaler < 1) || (brs_timings.bit_rate_prescaler > 1024) || + (brs_timings.max_resynchronization_jump_width < 1) || (brs_timings.max_resynchronization_jump_width > 4) || + (brs_timings.bit_segment_1 < 1) || (brs_timings.bit_segment_1 > 16) || + (brs_timings.bit_segment_2 < 1) || (brs_timings.bit_segment_2 > 8)) + { + return -CANARD_ERROR_INVALID_ARGUMENT; + } + + if (rc < 0) + return rc; +#endif + + switch ((uint32_t) driver->fdcan) { + case FDCAN1_ADDR: + case FDCAN2_ADDR: + case FDCAN3_ADDR: + break; + default: + CANARD_ASSERT(0); + return -CANARD_ERROR_INVALID_ARGUMENT; + } + + fdcan_registers *fdcan = driver->fdcan; + CANARD_ASSERT(fdcan->ENDN == 0x87654321); /* Also confirms that driver->fdcan + * is probably pointing at something sensible */ + + /* Enter configuration mode */ + fdcan->CCCR &= ~(1 << 4); /* Exit from sleep mode */ + while (fdcan->CCCR & (1 << 3)); /* Wait until exited from sleep mode */ + fdcan->CCCR |= (1 << 0); /* Get to INIT mode */ + while (!(fdcan->CCCR & (1 << 0))); /* Wait until init mode sets */ + fdcan->CCCR |= (1 << 1); /* CCE=1: config change enable */ + while (!(fdcan->CCCR & (1 << 1))); /* Wait intil CCE sets */ + + /* General config */ + fdcan->CCCR |= (1 << 6) | /* DAR=1: automatic retransmission should be disabled + * before DroneCAN Node ID is assigned */ + (1 << 13)| /* EFBI=1: edge filtering enabled */ + (1 << 14); /* TXP=1: Enable transmit pause -- should be generally beneficial. */ + +#if CANARD_ENABLE_CANFD + fdcan->CCCR |= (1 << 8) | (1 << 9); /* Enable FD and Bit Rate Switching */ +#endif + + /* Filter config */ + fdcan->RXGFC = + (8 << 24) | /* LSE = 8: number of Extended ID filters */ + (0 << 16) | /* LSS = 0: number of Standard ID filters */ + (0 << 9) | (0 << 8) | /* F0OM = F1OM = 0: FIFO to blocking mode */ + (2 << 4) | /* ANFS = 2: Reject non-matched standard frames */ + (1 << 2) | /* ANFE = 1: Non-matched extended frames go to FIFO1 */ + (1 << 1) | (1 << 0); /* RRFS = RRFE = 1: reject remote frames */ + + fdcan->XIDAM = 0xFFFF00; /* Only consider the DroneCAN Message type ID part in the CAN frame. + * This allows to setup each filter element as a dual ID filter + * and have 16 message rules in total. + * Matches only broadcast messages (serviceNotMessage bit + * will never match with anything in the filters). */ + /* Timing config */ + /* Set nominal timings */ + fdcan->NBTP = ((nominal_timings.max_resynchronization_jump_width - 1) << 25U) | + ((nominal_timings.bit_segment_2 - 1) << 0U) | + ((nominal_timings.bit_segment_1 - 1) << 8U) | + ((nominal_timings.bit_rate_prescaler - 1) << 16U); + +#if CANARD_ENABLE_CANFD + /* Set Bit Rate Switching timings */ + fdcan->DBTP = (0 << 23) | /* TDC = 0: disable transceiver delay compensation (same in AP_HAL_ChibiOS/CANFDIface.cpp) */ + ((brs_timings.max_resynchronization_jump_width - 1) << 0U) | + ((brs_timings.bit_segment_2 - 1) << 4U) | + ((brs_timings.bit_segment_1 - 1) << 8U) | + ((brs_timings.bit_rate_prescaler - 1) << 16U); +#endif + + /* Set TX buffer to queue mode (let it care about the priority inversion) */ + fdcan->TXBC = (1 << 24); /* TFQM = 1 */ + + /* Enable potentially useful interrupts: + * - on fifo0/1 received/lost + * - on bus-off */ + /* bus-off | rx1 lost | rx1 new | rx0 lost | rx0 new */ + fdcan->IE = (1 << 19) | (1 << 5) | (1 << 3) | (1 << 2) | (1 << 0); + driver->fdcan_sram = (void *) fdcan_ram(fdcan); + memset(driver->fdcan_sram, 0, sizeof(fdcan_sram)); + return 0; +} + +#define STM32G4_FDCAN_NUM_EXT_FILTER_ELEMENTS 8 +#define EFEC_OFFSET 29 +#define EFEC_MASK 0x7 +#define EFT_OFFSET 30 + +static int filt_elem_get_efec(fdcan_extid_filter_element *fe) +{ + return (fe->f0 & (EFEC_MASK << EFEC_OFFSET)) >> EFEC_OFFSET; +} + +int canard_stm32g4fdcan_type_id_filter(canard_stm32g4_fdcan_driver *driver, + int dronecan_type_id1, int dronecan_type_id2, int accept_not_reject) +{ + fdcan_sram *sram = driver->fdcan_sram; + for (int i = 0; i < STM32G4_FDCAN_NUM_EXT_FILTER_ELEMENTS; ++i) { + fdcan_extid_filter_element *filt = &sram->extid_filter_element[i]; + if (filt_elem_get_efec(filt) == FILT_ELEM_EFEC_DISABLE) { + filt->f0 = dronecan_type_id1 << 8; + filt->f1 = (FILT_ELEM_EFT_DUAL_ID << EFT_OFFSET) | (dronecan_type_id2 << 8); + filt->f0 |= accept_not_reject ? FILT_ELEM_EFEC_STORE_RXFIFO0 << EFEC_OFFSET : + FILT_ELEM_EFEC_REJECT << EFEC_OFFSET; + return 0; + } + } + return -CANARD_ERROR_STM32_FDCAN_OUT_OF_FILTER_SPACE; +} + +void canard_stm32g4fdcan_wipe_filters(canard_stm32g4_fdcan_driver *driver) +{ + fdcan_sram *sram = driver->fdcan_sram; + memset(sram->extid_filter_element, 0, sizeof(sram->extid_filter_element)); +} + +void canard_stm32g4fdcan_start(canard_stm32g4_fdcan_driver *driver) +{ + fdcan_registers *fdcan = driver->fdcan; + fdcan->CCCR &= ~((1 << 0) | (1 << 1)); /* Clear INIT and CCE */ + while ((fdcan->CCCR & (1 << 0))); /* Wait until we leave init mode */ +} + +int canard_stm32g4fdcan_transmit(canard_stm32g4_fdcan_driver *driver, const CanardCANFrame* const frame) +{ + clear_and_handle_faults(driver); + fdcan_registers *fdcan = driver->fdcan; + fdcan_sram *sram = driver->fdcan_sram; + if (fdcan->TXFQS & (1 << 21)) { + /* TFQF set: TX queue full */ + return 0; + } + int put_index = (fdcan->TXFQS & (3 << 16)) >> 16; + canard_frame_to_tx_buf_elem(frame, &sram->txbuf[put_index]); + fdcan->TXBAR = (1 << put_index); + driver->statistics.tx_frames++; + return 1; +} + +int canard_stm32g4fdcan_receive(canard_stm32g4_fdcan_driver *driver, CanardCANFrame* const out_frame) +{ + clear_and_handle_faults(driver); + fdcan_registers *fdcan = driver->fdcan; + fdcan_sram *sram = driver->fdcan_sram; + fdcan_rxfifo_regs *rxfifo[2] = {(fdcan_rxfifo_regs *) &fdcan->RXF0S, (fdcan_rxfifo_regs *) &fdcan->RXF1S}; + int index; + index = rxfifo_get_first_elem_index(rxfifo[0]); + if (index != -1) { + rxfifo_receive_frame(fdcan, out_frame, &sram->rxfifo0[index]); + rxfifo_ack_frame(rxfifo[0], index); + driver->statistics.rx_frames++; + return 1; + } + + index = rxfifo_get_first_elem_index(rxfifo[1]); + if (index != -1) { + rxfifo_receive_frame(fdcan, out_frame, &sram->rxfifo1[index]); + rxfifo_ack_frame(rxfifo[1], index); + driver->statistics.rx_frames++; + return 1; + } + + return 0; +} + +void canard_stm32g4fdcan_enable_automatic_retransmission(canard_stm32g4_fdcan_driver *driver) +{ + fdcan_registers *fdcan = driver->fdcan; + fdcan->CCCR |= (1 << 0); /* Get to INIT mode */ + while (!(fdcan->CCCR & (1 << 0))); /* Wait until init mode sets */ + fdcan->CCCR |= (1 << 1); /* CCE=1: config change enable */ + while (!(fdcan->CCCR & (1 << 1))); /* Wait intil CCE sets */ + fdcan->CCCR &= ~(1 << 6); /* DAR = 0 */ + fdcan->CCCR &= ~((1 << 0) | (1 << 1)); /* Clear INIT and CCE */ + while ((fdcan->CCCR & (1 << 0))); /* Wait until we leave init mode */ +} + +void canard_stm32g4fdcan_get_protocol_state(canard_stm32g4_fdcan_driver *driver, canard_stm32g4fdcan_protocol_state *s) +{ + fdcan_registers *fdcan = driver->fdcan; + s->bus_off = (fdcan->PSR & (1 << 7)) > 0; + s->warning = (fdcan->PSR & (1 << 6)) > 0; + s->error_passive = (fdcan->PSR & (1 << 5)) > 0; + s->tec = fdcan->ECR & 0xFF; + s->rec = (fdcan->ECR & 0x7F00) >> 8; +} + +void canard_stm32g4fdcan_get_statistics(canard_stm32g4_fdcan_driver *driver, uint32_t *num_rx_frames, + uint32_t *num_tx_frames, uint32_t *num_errors) +{ + *num_errors = driver->statistics.rx_fifo0_overruns + + driver->statistics.rx_fifo1_overruns + + driver->statistics.bus_off_events; + *num_rx_frames = driver->statistics.rx_frames; + *num_tx_frames = driver->statistics.tx_frames; +} + +/* Local */ + +static void canard_frame_to_tx_buf_elem(const CanardCANFrame* const frame, fdcan_tx_buf_element *ele) +{ + CANARD_ASSERT(frame->data_len); /* DLC = 0 not permitted in DroneCAN */ + + /* See Table 408 RM0440 */ + /* ESI = 0 29-bit id not remote CAN frame ID */ + ele->t0 = (0 << 31) | (1 << 30) | (0 << 29) | (frame->id & EXT_ID_FILTER); + +#if CANARD_ENABLE_CANFD + /* don't store events is this an FD frame use bit rate switch DLC */ + ele->t1 = (0 << 23) | (frame->canfd << 21) | (frame->canfd << 20) | (dlc_encode(frame->data_len, frame->canfd) << 16); +#else + ele->t1 = (0 << 23) | (0 << 21) | (0 << 20) | (dlc_encode(frame->data_len, 0) << 16); +#endif + + uint32_t *tx_data_frame = (uint32_t *) frame->data; + ele->data[0] = tx_data_frame[0]; + ele->data[1] = tx_data_frame[1]; +#if CANARD_ENABLE_CANFD + for (size_t i = 2; i < frame->data_len / sizeof(uint32_t); ++i) { + ele->data[i] = tx_data_frame[i]; + } +#endif +} + +static void clear_and_handle_faults(canard_stm32g4_fdcan_driver *driver) +{ + fdcan_registers *fdcan = driver->fdcan; + uint32_t psr = fdcan->PSR; /* reading clears PSR */ + + if (psr & (1 << 7)) { + /* Bus-off -- we need to clear INIT which is set by hardware in this case. + * See RM0440 44.4.13 */ + canard_stm32g4fdcan_start(driver); + driver->statistics.bus_off_events++; + } + if (psr & (1 << 14)) { /* PXE */ + driver->statistics.protocol_exceptions++; + } + + uint32_t ir = fdcan->IR; + if (ir & (1 << 2)) { + /* RF0L */ + driver->statistics.rx_fifo0_overruns++; + } + if (ir & (1 << 5)) { + /* RF1L */ + driver->statistics.rx_fifo1_overruns++; + } + if (ir & (1 << 12)) { + /* TEFL */ + driver->statistics.tx_fifo_overruns++; + } + fdcan->IR = 0xFFFFFF; /* clear IR */ +} + +#define DLC_OFFSET 16 +#define DLC_MASK 0xF + +static int rxfifo_get_dlc(fdcan_rx_buf_element *ele) +{ + return (ele->r1 & (DLC_MASK << DLC_OFFSET)) >> DLC_OFFSET; +} + +static void rxfifo_receive_frame(fdcan_registers *regs, CanardCANFrame* const out_frame, fdcan_rx_buf_element *ele) +{ + /* Don't need to check if this is a standard or remote frame (not needed for DroneCAN), + * they're rejected by the filter configuration */ + out_frame->id = ele->r0 & EXT_ID_FILTER; + out_frame->id |= CANARD_CAN_FRAME_EFF; /* canardHandleRxFrame() fails if this bit is not set. We set it manually, + * no standard frames are received by the filter configuration */ + out_frame->iface_id = ((uint32_t) regs - FDCAN1_ADDR) / IP_OFFSET; +#if CANARD_ENABLE_CANFD + out_frame->canfd = (ele->r1 & (1 << 21)) > 0; + out_frame->data_len = dlc_decode(rxfifo_get_dlc(ele), out_frame->canfd); + CANARD_ASSERT(out_frame->data_len <= 64); +#else + out_frame->data_len = dlc_decode(rxfifo_get_dlc(ele), 0); + CANARD_ASSERT(out_frame->data_len <= 8); +#endif + + uint32_t *out_data = (uint32_t *) out_frame->data; + /* It's faster to move two words around regardless of the actual DLC */ + out_data[0] = ele->data[0]; + out_data[1] = ele->data[1]; + + +#if CANARD_ENABLE_CANFD + if (out_frame->data_len >= 8) { + int limit = out_frame->data_len / sizeof(uint32_t); + for (int i = 0; i < limit; ++i) { + out_data[i] = ele->data[i]; + } + } +#endif +} + +__attribute__((const)) +static inline uint32_t fdcan_ram(const fdcan_registers *r) +{ + return SRAMCAN_START + ((uint32_t) r - FDCAN1_ADDR); +} + +static int rxfifo_get_first_elem_index(fdcan_rxfifo_regs *rxf) +{ + if (rxfifo_get_fill_level(rxf)) { + return rxfifo_get_read_index(rxf); + } + return -1; +} + +static int rxfifo_get_fill_level(fdcan_rxfifo_regs *rxf) +{ + return rxf->RXFxS & 0xF; +} + +static int rxfifo_get_read_index(fdcan_rxfifo_regs *rxf) +{ + return (rxf->RXFxS & (3 << 8)) >> 8; +} + +__attribute__((const)) +static inline int dlc_decode(int dlc_value, int fd) +{ + if (fd) { + if (dlc_value <= 8) { + return dlc_value; + } + else { + switch (dlc_value) { + case 9: + return 12; + case 10: + return 16; + case 11: + return 20; + case 12: + return 24; + case 13: + return 32; + case 14: + return 48; + case 15: + return 64; + default: + CANARD_ASSERT(0); + return 0; + } + } + } + else { + if (dlc_value <= 8) { + return dlc_value; + } + else { + CANARD_ASSERT(0); + return 0; + } + } +} + +__attribute__((const)) +static inline int dlc_encode(int data_len, int fd) +{ + if (data_len <= 8) { + return data_len; + } + else if (fd) { + switch (data_len) { + case 12: + return 9; + case 16: + return 10; + case 20: + return 11; + case 24: + return 12; + case 32: + return 13; + case 48: + return 14; + case 64: + return 15; + default: + CANARD_ASSERT(0); + return 0; + } + } + else { + CANARD_ASSERT(0); + return 0; + } +} + +static void rxfifo_ack_frame(fdcan_rxfifo_regs *rxfifo_regs, int ack_index) +{ + rxfifo_regs->RXFxA = ack_index; +} diff --git a/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.h b/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.h new file mode 100644 index 0000000..f1b2fe2 --- /dev/null +++ b/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.h @@ -0,0 +1,126 @@ +/* + * Copyright (c) 2024-2026 Flytrex, by Grisha Revzin + * + * Distributed under the MIT License, available in the file LICENSE. + * + * Created on: Sep 10, 2024 + */ + +#ifndef CANARD_STM32G4_FDCAN_H_ +#define CANARD_STM32G4_FDCAN_H_ + +#include + +#include + +#ifdef __cplusplus +extern "C" +{ +#endif + +#define CANARD_ERROR_STM32_FDCAN_OUT_OF_FILTER_SPACE 1200 + +#define FDCAN1_ADDR 0x40006400U +#define FDCAN2_ADDR 0x40006800U +#define FDCAN3_ADDR 0x40006C00U + +typedef struct { + void *fdcan; + void *fdcan_sram; + struct { + uint32_t rx_fifo0_overruns; + uint32_t rx_fifo1_overruns; + uint32_t tx_fifo_overruns; + uint32_t warning_events; + uint32_t bus_off_events; + uint32_t protocol_exceptions; + uint32_t tx_frames; + uint32_t rx_frames; + } statistics; +} canard_stm32g4_fdcan_driver; + +/** + * Initializes the driver and the hardware. Doesn't start FDCAN (you're expected + * to configure filters with `canard_stm32g4fdcan_type_id_filter` after it). + * To start the hardware, call canard_stm32g4fdcan_start after it. + * If CANARD_ENABLE_CANFD is defined, the hardware will be initialized to CAN FD mode + * (long frame + bit rate switch). + * + * @param[in] driver.fdcan Must be set to FDCAN1_ADDR, FDCAN2_ADDR or FDCAN3_ADDR. + * It's the user's responsibility to check if the hardware has them. + * @param[in] bitrate_bps Nominal bitrate + * @param[in] fdbitrate_bps FD bitrate (ignored if CANARD_ENABLE_CANFD is not defined). + * @param[in] periph_clock_rate The clock rate of the hardware (set to 80 MHz for best results). + * @retval 0 -- ok + * @retval negative -- error + */ +int canard_stm32g4fdcan_init(canard_stm32g4_fdcan_driver *driver, int bitrate_bps, int fdbitrate_bps, int periph_clock_rate); + +/** + * Starts the hardware after initialization and configuration. + */ +void canard_stm32g4fdcan_start(canard_stm32g4_fdcan_driver *driver); + +/** + * Creates a filter based on broadcast message IDs. Pass DroneCAN message IDs to create a filter. + * Due to internal organization, these filters come in pairs: just set to 0 if not needed. + * The messages that match the filter will either get into FIFO0 or rejected, depending on the value of + * @param[in] accept_not_reject + * + * The messages that don't match the filter end up in FIFO1. + * + */ +int canard_stm32g4fdcan_type_id_filter(canard_stm32g4_fdcan_driver *driver, + int dronecan_type_id1, int dronecan_type_id2, int accept_not_reject); +void canard_stm32g4fdcan_wipe_filters(canard_stm32g4_fdcan_driver *driver); + +/** + * Pushes one frame into the TX buffer, if there is space. + * This function doesn't block. + * + * @retval 1 Transmitted successfully + * @retval 0 No space in the buffer + * @retval negative Error + */ +int canard_stm32g4fdcan_transmit(canard_stm32g4_fdcan_driver *driver, const CanardCANFrame* const frame); + +/** + * Reads one frame from the hardware RX FIFO, unless all FIFO are empty. + * This function doesn't block. + * + * @retval 1 Read successfully + * @retval 0 The buffer is empty + * @retval negative Error + */ +int canard_stm32g4fdcan_receive(canard_stm32g4_fdcan_driver *driver, CanardCANFrame* const out_frame); + +/** + * Reads the instantaneous standard CAN state variables from the hardware. + */ +typedef struct { + uint8_t tec, rec; + uint8_t bus_off, error_passive, warning; +} canard_stm32g4fdcan_protocol_state; + +void canard_stm32g4fdcan_get_protocol_state(canard_stm32g4_fdcan_driver *driver, canard_stm32g4fdcan_protocol_state *s); + + +/** + * Enables automatic retransmission. + * Call this after the node gets its Node ID. + */ +void canard_stm32g4fdcan_enable_automatic_retransmission(canard_stm32g4_fdcan_driver *driver); + + +/** + * Gets basic statistics on this interface + */ +void canard_stm32g4fdcan_get_statistics(canard_stm32g4_fdcan_driver *driver, uint32_t *num_rx_frames, + uint32_t *num_tx_frames, uint32_t *num_errors); + + +#ifdef __cplusplus +} +#endif + +#endif /* CANARD_STM32G4_FDCAN_H_ */