From 5e217cc5b34c69cc09d405a145feea195b258706 Mon Sep 17 00:00:00 2001
From: Grisha Revzin <grishar@flytrex.com>
Date: Thu, 12 Sep 2024 20:48:48 +0300
Subject: [PATCH 01/24] Initial draft, not yet tested

---
 drivers/stm32g4_fdcan/README.md              |  44 ++
 drivers/stm32g4_fdcan/_fdcan_g4.h            |  92 ++++
 drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c | 475 +++++++++++++++++++
 drivers/stm32g4_fdcan/canard_stm32g4_fdcan.h | 106 +++++
 4 files changed, 717 insertions(+)
 create mode 100644 drivers/stm32g4_fdcan/README.md
 create mode 100644 drivers/stm32g4_fdcan/_fdcan_g4.h
 create mode 100644 drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c
 create mode 100644 drivers/stm32g4_fdcan/canard_stm32g4_fdcan.h

diff --git a/drivers/stm32g4_fdcan/README.md b/drivers/stm32g4_fdcan/README.md
new file mode 100644
index 0000000..8557760
--- /dev/null
+++ b/drivers/stm32g4_fdcan/README.md
@@ -0,0 +1,44 @@
+# STM32G4 FDCAN driver for libcanard
+
+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)
+
+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
+
+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`.
+
+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:
+1. The DroneCAN data type ID is in the "accept" filter (`canard_stm32fdcan_accept_filter`) -> goes to FIFO0
+2. The DroneCAN data type ID is in the "reject" filter (`canard_stm32fdcan_reject_filter`) -> discarded
+3. The DroneCAN data type ID is in neither -> goes to FIFO1
+
+`canard_stm32fdcan_recieve` 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..3e4c9ec
--- /dev/null
+++ b/drivers/stm32g4_fdcan/_fdcan_g4.h
@@ -0,0 +1,92 @@
+#ifndef INTERNAL_FDCAN_H
+#define INTERNAL_FDCAN_H
+
+#include <stdint.h>
+
+typedef struct {
+	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;
+
+typedef struct {
+	volatile uint32_t RXFxS;
+	volatile uint32_t RXFxA;
+} fdcan_rxfifo_regs;
+
+typedef struct {
+	volatile uint32_t f0;
+	volatile uint32_t f1;
+} fdcan_ext_filt_elem;
+
+#define EFEC_OFFSET 29
+#define EFT_OFFSET 30
+
+#define FDCAN_RXFIFO_ELEM_LEN (64 / 4 + 2)
+typedef struct {
+	volatile uint32_t r[FDCAN_RXFIFO_ELEM_LEN];
+} fdcan_rxfifo_elem;
+
+#define FDCAN_TXFIFO_ELEM_LEN (64 / 4 + 2)
+typedef struct {
+	volatile uint32_t t[FDCAN_TXFIFO_ELEM_LEN];
+} fdcan_tx_buf_elem;
+
+#define DLC_OFFSET 16
+
+#define FDCAN1 ((fdcan_registers *) FDCAN1_ADDR)
+#define FDCAN2 ((fdcan_registers *) FDCAN2_ADDR)
+#define FDCAN3 ((fdcan_registers *) FDCAN3_ADDR)
+
+#define SRAMCAN_START 			0x4000A400U
+#define SRAMCAN_SIZE			0x0350U
+#define SRAMCAN_EXT_FILT_OFFSET 0x0070U
+#define SRAMCAN_RXFIFO0_OFFSET 	0x00B0U
+#define SRAMCAN_RXFIFO1_OFFSET 	0x0188U
+#define SRAMCAN_RXFIFO_SIZE 	(SRAMCAN_RXFIFO1_OFFSET - SRAMCAN_RXFIFO0_OFFSET)
+#define SRAMCAN_TXB_OFFSET		0x0278U
+
+#define DRONECAN_MESSAGEID_FILTER 0xFFFF80
+
+#define FDCAN_NUM_EXT_FILTERS 8
+
+#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..7d6ef7f
--- /dev/null
+++ b/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c
@@ -0,0 +1,475 @@
+/*
+ * canard_stm32g4_fdcan.c
+ *
+ *  Created on: Sep 10, 2024
+ *      Author: grishar
+ */
+
+#include "drivers/stm32g4_fdcan/canard_stm32g4_fdcan.h"
+
+#include "drivers/stm32g4_fdcan/_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 fdcan_tx_buf_elem *get_tx_buf_element(canard_stm32g4_fdcan_driver *driver, int index);
+static void canard_frame_to_tx_buf_elem(const CanardCANFrame* const frame, fdcan_tx_buf_elem *ele);
+
+static void rxfifo_receive_frame(fdcan_registers *regs, CanardCANFrame* const out_frame, fdcan_rxfifo_elem *ele);
+
+__attribute__((const))
+static inline fdcan_rxfifo_elem *get_rxfifo_elem(const canard_stm32g4_fdcan_driver *driver,
+								fdcan_rxfifo_regs *fifo_regs, size_t elemindex);
+static int rxfifo_elem_is_fd_frame(fdcan_rxfifo_elem *rxf);
+static int rxfifo_elem_get_ext_id(fdcan_rxfifo_elem *rxf);
+static volatile uint32_t *rxfifo_elem_get_payload(fdcan_rxfifo_elem *rxf);
+static int rxfifo_get_fill_level(fdcan_rxfifo_regs *rxf);
+static int rxfifo_get_read_index(fdcan_rxfifo_regs *rxf);
+static fdcan_rxfifo_elem *rxfifo_get_first_elem(canard_stm32g4_fdcan_driver *driver, fdcan_rxfifo_regs *rxf, int *elem_index);
+static int rxfifo_elem_get_dlc(fdcan_rxfifo_elem *rxf);
+static void rxfifo_ack_frame(fdcan_rxfifo_regs *rxfifo_regs, int ack_index);
+
+__attribute__((const))
+static inline fdcan_ext_filt_elem* get_ext_filt_elem(const fdcan_registers *r, size_t index);
+static int filt_elem_active(const fdcan_ext_filt_elem * const fe);
+static void filt_elem_accept_dual_id(fdcan_ext_filt_elem *fe, uint32_t frame_id1, uint32_t frame_id2);
+static void filt_elem_reject_dual_id(fdcan_ext_filt_elem *fe, uint32_t frame_id1, uint32_t frame_id2);
+
+__attribute__((const))
+static inline int dlc_decode(int dlc_value, int fd);
+
+__attribute__((const))
+static inline int dlc_encode(int data_len, int fd);
+
+
+/* Module */
+
+int canard_stm32g4fdcan_init(canard_stm32g4_fdcan_driver *driver, int bitrate_bps, int fdbitrate_bps, int periph_clock_rate)
+{
+	/* 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;
+
+#ifdef 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);
+	}
+
+	fdcan_registers *fdcan = driver->fdcan;
+	CANARD_ASSERT(fdcan->ENDN == 0x87654321);		/* Also confirms that driver->fdcan
+													 * is probably pointing at something sensible */
+
+	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 */
+
+	/* General config */
+	fdcan->CCCR |=  (1 << 1) | 						/* CCE=1: config change enable */
+					(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. */
+
+
+#ifdef 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 = 0xFFFF80 << 7;		/* 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.
+										 * Match 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);
+
+#ifdef CANARD_ENABLE_CANFD
+	/* Set Bit Rate Switching timings */
+	fdcan->DBTP = (1 << 23) | /* TDC = 1: enable transceiver delay compensation */
+			 	  ((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_base = fdcan_ram(fdcan);	/* Precalc this address to save on divisions later on */
+	return 0;
+}
+
+#define STM32G4_FDCAN_NUM_EXT_FILTER_ELEMENTS 8
+
+int canard_stm32g4fdcan_type_id_filter(canard_stm32g4_fdcan_driver *driver,
+										int dronecan_type_id1, int dronecan_type_id2, int accept_not_reject)
+{
+	for (int i = 0; i < STM32G4_FDCAN_NUM_EXT_FILTER_ELEMENTS; ++i) {
+		fdcan_ext_filt_elem *fe = get_ext_filt_elem(driver->fdcan, i);
+		if (!filt_elem_active(fe)) {
+			if (accept_not_reject) {
+				filt_elem_accept_dual_id(fe, dronecan_type_id1, dronecan_type_id2);
+			}
+			else {
+				filt_elem_reject_dual_id(fe, dronecan_type_id1, dronecan_type_id2);
+			}
+			return 0;
+		}
+	}
+	return -CANARD_ERROR_STM32_FDCAN_OUT_OF_FILTER_SPACE;
+}
+
+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;
+	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, get_tx_buf_element(driver, put_index));
+	fdcan->TXBAR = (1 << put_index);
+	return 0;
+}
+
+int canard_stm32g4fdcan_receive(canard_stm32g4_fdcan_driver *driver, CanardCANFrame* const out_frame)
+{
+	clear_and_handle_faults(driver);
+	fdcan_registers *fdcan = driver->fdcan;
+	fdcan_rxfifo_regs *rxfifo[2] = {(fdcan_rxfifo_regs *) &fdcan->RXF0S, (fdcan_rxfifo_regs *) &fdcan->RXF1S};
+	int ele_index = -1;
+	fdcan_rxfifo_elem *ele = NULL;
+	ele = rxfifo_get_first_elem(driver, rxfifo[0], &ele_index);
+	if (ele) {
+		rxfifo_receive_frame(fdcan, out_frame, ele);
+		rxfifo_ack_frame(rxfifo[0], ele_index);
+		return 1;
+	}
+	ele = rxfifo_get_first_elem(driver, rxfifo[1], &ele_index);
+	if (ele) {
+		rxfifo_receive_frame(fdcan, out_frame, ele);
+		rxfifo_ack_frame(rxfifo[1], ele_index);
+		return 1;
+	}
+	return 0;
+}
+
+void canard_stm32g4fdcan_enable_automatic_retransmission(canard_stm32g4_fdcan_driver *driver)
+{
+	fdcan_registers *fdcan = driver->fdcan;
+	fdcan->CCCR &= ~(1 << 6);
+}
+
+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 & 0xF00) >> 16;
+}
+
+/* Local */
+
+static fdcan_tx_buf_elem *get_tx_buf_element(canard_stm32g4_fdcan_driver *driver, int index)
+{
+	return (fdcan_tx_buf_elem *) driver->fdcan_sram_base + SRAMCAN_TXB_OFFSET + sizeof(fdcan_tx_buf_elem) * index;
+}
+
+static void canard_frame_to_tx_buf_elem(const CanardCANFrame* const frame, fdcan_tx_buf_elem *ele)
+{
+	/* See Table 408 RM0440 */
+	/* 			 ESI = 0   29-bit id   not remote	CAN frame ID */
+	ele->t[0] = (0 << 31) | (1 << 30) | (0 << 29) | (frame->id & 0x1FFFFFFF);
+#ifdef CANARD_ENABLE_CANFD
+	/* don't store events  is this an FD frame    use bit rate switch		DLC                        */
+	ele->t[1] = (0 << 23) | (frame->canfd << 21) |  (1 << 20) | (dlc_encode(frame->data_len, frame->canfd));
+#else
+	ele->t[1] = (0 << 23) | (0 << 21)            |  (0 << 20) | (dlc_encode(frame->data_len, 0));
+#endif
+
+	volatile uint32_t *tx_data_element = (volatile uint32_t *) &ele->t[2];
+	volatile uint32_t *tx_data_frame = (volatile uint32_t *) frame->data;
+
+	if (frame->data_len) {
+		tx_data_element[0] = tx_data_frame[0];
+		tx_data_element[1] = tx_data_frame[1];
+#ifdef CANARD_ENABLE_CANFD
+		for (int i = 2; i < frame->data_len / sizeof(uint32_t); ++i) {
+			tx_data_element[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) | (1 << 5))) { /* RF0L | RF1L */
+		driver->statistics.rx_fifo_overruns++;
+	}
+	if (ir & (1 << 12))  { /* TEFL */
+		driver->statistics.tx_fifo_overruns++;
+	}
+	fdcan->IR = 0xFFFFFF; /* clear IR */
+}
+
+static void rxfifo_receive_frame(fdcan_registers *regs, CanardCANFrame* const out_frame, fdcan_rxfifo_elem *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 = rxfifo_elem_get_ext_id(ele);
+	out_frame->data_len = rxfifo_elem_get_dlc(ele);
+	out_frame->iface_id = ((uint32_t) regs - FDCAN1_ADDR) / sizeof(fdcan_registers);
+#ifdef CANARD_ENABLE_CANFD
+	out_frame->canfd = rxfifo_elem_is_fd_frame(ele);
+	CANARD_ASSERT(out_frame->data_len <= 64);
+#else
+	CANARD_ASSERT(out_frame->data_len <= 8);
+#endif
+	volatile uint32_t *payload = rxfifo_elem_get_payload(ele);
+	if (out_frame->data_len) {
+		uint32_t *out_data = (uint32_t *) out_frame->data;
+		/* SRAMCAN is accessed in words, not bytes, so no memcpy().
+		 * For dlc <= 8, two words are always copied.*/
+		if (out_frame->data_len <= 8) {
+			out_data[0] = payload[0];
+			out_data[1] = payload[1];
+		}
+		else {
+			int limit = out_frame->data_len / sizeof(uint32_t);
+			for (int i = 0; i < limit; ++i) {
+				out_data[i] = payload[i];
+			}
+		}
+	}
+}
+
+__attribute__((const))
+static inline uint32_t fdcan_ram(const fdcan_registers *r)
+{
+	return SRAMCAN_START + ((uint32_t) r - FDCAN1_ADDR) / sizeof(fdcan_registers) * SRAMCAN_SIZE;
+}
+
+__attribute__((const))
+static inline fdcan_rxfifo_elem *get_rxfifo_elem(const canard_stm32g4_fdcan_driver *driver,
+								fdcan_rxfifo_regs *fifo_regs, size_t elemindex)
+{
+	fdcan_registers *fdcan = driver->fdcan;
+	int fifonr = ((uint32_t) fifo_regs) == ((uint32_t) &fdcan->RXF0S) ? 0 : 1;
+	return (fdcan_rxfifo_elem *)
+			(driver->fdcan_sram_base + SRAMCAN_RXFIFO0_OFFSET 	/* Base address */
+					+ SRAMCAN_RXFIFO_SIZE * fifonr 				/* FIFO0 or 1? */
+					+ sizeof(fdcan_rxfifo_elem) * elemindex); 	/* Which element */
+}
+
+static int rxfifo_elem_is_fd_frame(fdcan_rxfifo_elem *rxf)
+{
+	return (rxf->r[1] & (1 << 21)) > 0;
+}
+
+static int rxfifo_elem_get_ext_id(fdcan_rxfifo_elem *rxf)
+{
+	return rxf->r[0] & 0x1FFFFFFF;
+}
+
+static volatile uint32_t *rxfifo_elem_get_payload(fdcan_rxfifo_elem *rxf)
+{
+	return &rxf->r[2];
+}
+
+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;
+}
+
+static fdcan_rxfifo_elem *rxfifo_get_first_elem(canard_stm32g4_fdcan_driver *driver, fdcan_rxfifo_regs *rxf, int *elem_index)
+{
+	if (rxfifo_get_fill_level(rxf)) {
+		*elem_index = rxfifo_get_read_index(rxf);
+		return get_rxfifo_elem(driver, rxf, *elem_index);
+	}
+	*elem_index = -1;
+	return NULL;
+}
+
+__attribute__((const))
+static inline fdcan_ext_filt_elem* get_ext_filt_elem(const fdcan_registers *r, size_t index)
+{
+	return (fdcan_ext_filt_elem *) (fdcan_ram(r) + SRAMCAN_EXT_FILT_OFFSET + sizeof(fdcan_ext_filt_elem) * index);
+}
+
+static int filt_elem_active(const fdcan_ext_filt_elem * const fe)
+{
+	return (fe->f0 & (0x7 << 29)) > 0;
+}
+
+static void filt_elem_accept_dual_id(fdcan_ext_filt_elem *fe, uint32_t frame_id1, uint32_t frame_id2)
+{
+	fe->f0 = (1 << EFEC_OFFSET) | frame_id1;
+	fe->f1 = (1 << EFT_OFFSET) | frame_id2;
+}
+
+static void filt_elem_reject_dual_id(fdcan_ext_filt_elem *fe, uint32_t frame_id1, uint32_t frame_id2)
+{
+	fe->f0 = (3 << EFEC_OFFSET) | frame_id1;
+	fe->f1 = (1 << EFT_OFFSET) | frame_id2;
+}
+
+__attribute__((const))
+static inline int dlc_decode(int dlc_value, int fd)
+{
+	if (dlc_value <= 8) {
+		return dlc_value;
+	}
+	if (fd) {
+		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 {
+		return 8;
+	}
+}
+
+__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 int rxfifo_elem_get_dlc(fdcan_rxfifo_elem *rxf)
+{
+	return dlc_decode(((0xF << DLC_OFFSET) & rxf->r[1]) >> DLC_OFFSET, rxfifo_elem_is_fd_frame(rxf));
+}
+
+static void rxfifo_ack_frame(fdcan_rxfifo_regs *rxfifo_regs, int ack_index)
+{
+	rxfifo_regs->RXFxA |= 7 & 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..059a03b
--- /dev/null
+++ b/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.h
@@ -0,0 +1,106 @@
+/*
+ * canard_stm32g4_fdcan.h
+ *
+ *  Created on: Sep 10, 2024
+ *      Author: grishar
+ */
+
+#ifndef CANARD_STM32G4_FDCAN_H_
+#define CANARD_STM32G4_FDCAN_H_
+
+#include <stdint.h>
+
+#include <canard.h>
+
+#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;
+	struct {
+		uint32_t rx_fifo_overruns;
+		uint32_t tx_fifo_overruns;
+		uint32_t warning_events;
+		uint32_t bus_off_events;
+		uint32_t protocol_exceptions;
+	} statistics;
+	uint32_t fdcan_sram_base;
+} 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_ADDRESS, FDCAN2_ADDRESS or FDCAN3_ADDRESS.
+ *   		  					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  	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);
+
+/**
+ * Pushes one frame into the TX buffer, if there is space.
+ * Note that proper care is taken to ensure that no inner priority inversion is taking place.
+ * This function does never 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 does never block.
+ *
+ * @retval      1               Read successfully
+ * @retval      0               The buffer is empty
+ * @retval      negative        Error
+ */
+int canard_stm32g4fdcan_recieve(canard_stm32g4_fdcan_driver *driver, CanardCANFrame* const out_frame);
+
+/**
+ *	Reads the instant standard CAN state variables from the hardware.
+ */
+typedef struct {
+	uint8_t tec, rec;
+	uint8_t bus_off, error_passive, warning;
+	uint8_t delay_comp_value;
+} 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);
+
+
+#endif /* CANARD_STM32G4_FDCAN_H_ */

From 0d2bc4d4412481ae75eb60edd852d9859f4c29bc Mon Sep 17 00:00:00 2001
From: Grisha Revzin <grishar@flytrex.com>
Date: Thu, 12 Sep 2024 21:09:28 +0300
Subject: [PATCH 02/24] Doc updates

---
 drivers/stm32g4_fdcan/README.md | 12 ++++++++++--
 1 file changed, 10 insertions(+), 2 deletions(-)

diff --git a/drivers/stm32g4_fdcan/README.md b/drivers/stm32g4_fdcan/README.md
index 8557760..9b0edad 100644
--- a/drivers/stm32g4_fdcan/README.md
+++ b/drivers/stm32g4_fdcan/README.md
@@ -1,4 +1,4 @@
-# STM32G4 FDCAN driver for libcanard
+# STM32G4 FDCAN driver for libcanard/DroneCAN
 
 This is a bare-bones driver for the variety of FDCAN that's found in STM32G4 series.
 
@@ -6,6 +6,7 @@ This is a bare-bones driver for the variety of FDCAN that's found in STM32G4 ser
 * 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.
@@ -14,10 +15,17 @@ So this is only for G4, but can probably be extended to support everything.
 
 ### Initialization
 
-Store a `canard_stm32g4_fdcan_driver` somewhere and set the `fdcan` pointer 
+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

From eac645ba004994795b833bef0989ada4a8097b99 Mon Sep 17 00:00:00 2001
From: Grisha Revzin <grishar@flytrex.com>
Date: Sun, 15 Sep 2024 13:45:53 +0300
Subject: [PATCH 03/24] Import fixes

---
 drivers/stm32g4_fdcan/README.md              | 6 +++---
 drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c | 4 ++--
 2 files changed, 5 insertions(+), 5 deletions(-)

diff --git a/drivers/stm32g4_fdcan/README.md b/drivers/stm32g4_fdcan/README.md
index 9b0edad..f48d583 100644
--- a/drivers/stm32g4_fdcan/README.md
+++ b/drivers/stm32g4_fdcan/README.md
@@ -32,9 +32,9 @@ It's the user's responsibility to provide clock to the chosen FDCAN peripheral a
 
 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:
-1. The DroneCAN data type ID is in the "accept" filter (`canard_stm32fdcan_accept_filter`) -> goes to FIFO0
-2. The DroneCAN data type ID is in the "reject" filter (`canard_stm32fdcan_reject_filter`) -> discarded
+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_stm32fdcan_recieve` reads one message at a time and prefers FIFO0.
diff --git a/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c b/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c
index 7d6ef7f..f2024be 100644
--- a/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c
+++ b/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c
@@ -5,9 +5,9 @@
  *      Author: grishar
  */
 
-#include "drivers/stm32g4_fdcan/canard_stm32g4_fdcan.h"
+#include "canard_stm32g4_fdcan.h"
 
-#include "drivers/stm32g4_fdcan/_fdcan_g4.h"
+#include "_fdcan_g4.h"
 #include "../stm32/canard_stm32.h" /* for CanardSTM32ComputeCANTimings */
 
 /* Local  */

From 560ab4d400743adbca829751f2676d736918cec9 Mon Sep 17 00:00:00 2001
From: Grisha Revzin <grishar@flytrex.com>
Date: Sun, 22 Sep 2024 21:00:33 +0300
Subject: [PATCH 04/24] Minor WIP changes

---
 drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c |  2 +-
 drivers/stm32g4_fdcan/canard_stm32g4_fdcan.h | 10 +++++-----
 2 files changed, 6 insertions(+), 6 deletions(-)

diff --git a/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c b/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c
index f2024be..5c819f3 100644
--- a/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c
+++ b/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c
@@ -193,7 +193,7 @@ int canard_stm32g4fdcan_transmit(canard_stm32g4_fdcan_driver *driver, const Cana
 	int put_index = fdcan->TXFQS & (3 << 16) >> 16;
 	canard_frame_to_tx_buf_elem(frame, get_tx_buf_element(driver, put_index));
 	fdcan->TXBAR = (1 << put_index);
-	return 0;
+	return 1;
 }
 
 int canard_stm32g4fdcan_receive(canard_stm32g4_fdcan_driver *driver, CanardCANFrame* const out_frame)
diff --git a/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.h b/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.h
index 059a03b..7ae08c4 100644
--- a/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.h
+++ b/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.h
@@ -61,12 +61,12 @@ void canard_stm32g4fdcan_start(canard_stm32g4_fdcan_driver *driver);
  * 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);
+int canard_stm32g4fdcan_type_id_filter(canard_stm32g4_fdcan_driver *driver,
+                                       int dronecan_type_id1, int dronecan_type_id2, int accept_not_reject);
 
 /**
  * Pushes one frame into the TX buffer, if there is space.
- * Note that proper care is taken to ensure that no inner priority inversion is taking place.
- * This function does never block.
+ * This function doesn't block.
  *
  * @retval      1               Transmitted successfully
  * @retval      0               No space in the buffer
@@ -76,13 +76,13 @@ int canard_stm32g4fdcan_transmit(canard_stm32g4_fdcan_driver *driver, const Cana
 
 /**
  * Reads one frame from the hardware RX FIFO, unless all FIFO are empty.
- * This function does never block.
+ * This function doesn't block.
  *
  * @retval      1               Read successfully
  * @retval      0               The buffer is empty
  * @retval      negative        Error
  */
-int canard_stm32g4fdcan_recieve(canard_stm32g4_fdcan_driver *driver, CanardCANFrame* const out_frame);
+int canard_stm32g4fdcan_receive(canard_stm32g4_fdcan_driver *driver, CanardCANFrame* const out_frame);
 
 /**
  *	Reads the instant standard CAN state variables from the hardware.

From 5a78e0947ce2348548dbfda4944245328d982461 Mon Sep 17 00:00:00 2001
From: Grisha Revzin <grishar@flytrex.com>
Date: Thu, 26 Sep 2024 10:45:39 +0300
Subject: [PATCH 05/24] Compiler warnings

---
 drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c b/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c
index 5c819f3..54527a4 100644
--- a/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c
+++ b/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c
@@ -260,7 +260,7 @@ static void canard_frame_to_tx_buf_elem(const CanardCANFrame* const frame, fdcan
 		tx_data_element[0] = tx_data_frame[0];
 		tx_data_element[1] = tx_data_frame[1];
 #ifdef CANARD_ENABLE_CANFD
-		for (int i = 2; i < frame->data_len / sizeof(uint32_t); ++i) {
+		for (size_t i = 2; i < frame->data_len / sizeof(uint32_t); ++i) {
 			tx_data_element[i] = tx_data_frame[i];
 		}
 #endif

From c3a21cba14f0786363d9fc6ca42409fe9806e58b Mon Sep 17 00:00:00 2001
From: Grisha Revzin <grishar@flytrex.com>
Date: Fri, 8 Nov 2024 20:39:40 +0200
Subject: [PATCH 06/24] aligned #ifdef -> #if CANARD_ENABLE_CANFD

---
 drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c | 12 ++++++------
 1 file changed, 6 insertions(+), 6 deletions(-)

diff --git a/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c b/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c
index 54527a4..14969e1 100644
--- a/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c
+++ b/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c
@@ -65,7 +65,7 @@ int canard_stm32g4fdcan_init(canard_stm32g4_fdcan_driver *driver, int bitrate_bp
 	if (rc < 0)
 		return rc;
 
-#ifdef CANARD_ENABLE_CANFD
+#if CANARD_ENABLE_CANFD
 	CanardSTM32CANTimings brs_timings = {0};
 	rc = canardSTM32ComputeCANTimings(periph_clock_rate, fdbitrate_bps, &brs_timings);
 
@@ -108,7 +108,7 @@ int canard_stm32g4fdcan_init(canard_stm32g4_fdcan_driver *driver, int bitrate_bp
 					(1 << 14);						/* TXP=1: Enable transmit pause -- should be generally beneficial. */
 
 
-#ifdef CANARD_ENABLE_CANFD
+#if CANARD_ENABLE_CANFD
 	fdcan->CCCR |= (1 << 8) | (1 << 9); 			/* Enable FD and Bit Rate Switching */
 #endif
 
@@ -133,7 +133,7 @@ int canard_stm32g4fdcan_init(canard_stm32g4_fdcan_driver *driver, int bitrate_bp
 				  ((nominal_timings.bit_segment_1 - 1) 						<< 8U) |
 				  ((nominal_timings.bit_rate_prescaler - 1)					<< 16U);
 
-#ifdef CANARD_ENABLE_CANFD
+#if CANARD_ENABLE_CANFD
 	/* Set Bit Rate Switching timings */
 	fdcan->DBTP = (1 << 23) | /* TDC = 1: enable transceiver delay compensation */
 			 	  ((brs_timings.max_resynchronization_jump_width - 1) 	<< 0U) |
@@ -246,7 +246,7 @@ static void canard_frame_to_tx_buf_elem(const CanardCANFrame* const frame, fdcan
 	/* See Table 408 RM0440 */
 	/* 			 ESI = 0   29-bit id   not remote	CAN frame ID */
 	ele->t[0] = (0 << 31) | (1 << 30) | (0 << 29) | (frame->id & 0x1FFFFFFF);
-#ifdef CANARD_ENABLE_CANFD
+#if CANARD_ENABLE_CANFD
 	/* don't store events  is this an FD frame    use bit rate switch		DLC                        */
 	ele->t[1] = (0 << 23) | (frame->canfd << 21) |  (1 << 20) | (dlc_encode(frame->data_len, frame->canfd));
 #else
@@ -259,7 +259,7 @@ static void canard_frame_to_tx_buf_elem(const CanardCANFrame* const frame, fdcan
 	if (frame->data_len) {
 		tx_data_element[0] = tx_data_frame[0];
 		tx_data_element[1] = tx_data_frame[1];
-#ifdef CANARD_ENABLE_CANFD
+#if CANARD_ENABLE_CANFD
 		for (size_t i = 2; i < frame->data_len / sizeof(uint32_t); ++i) {
 			tx_data_element[i] = tx_data_frame[i];
 		}
@@ -299,7 +299,7 @@ static void rxfifo_receive_frame(fdcan_registers *regs, CanardCANFrame* const ou
 	out_frame->id = rxfifo_elem_get_ext_id(ele);
 	out_frame->data_len = rxfifo_elem_get_dlc(ele);
 	out_frame->iface_id = ((uint32_t) regs - FDCAN1_ADDR) / sizeof(fdcan_registers);
-#ifdef CANARD_ENABLE_CANFD
+#if CANARD_ENABLE_CANFD
 	out_frame->canfd = rxfifo_elem_is_fd_frame(ele);
 	CANARD_ASSERT(out_frame->data_len <= 64);
 #else

From af119d1efa693f13bb3bda5ef9ede48a36cbe517 Mon Sep 17 00:00:00 2001
From: Grisha Revzin <grishar@flytrex.com>
Date: Sun, 10 Nov 2024 11:54:13 +0200
Subject: [PATCH 07/24] WIP: hardware works (messages are sent/received),
 cleaning code

---
 drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c | 74 ++++++++++++--------
 1 file changed, 43 insertions(+), 31 deletions(-)

diff --git a/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c b/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c
index 14969e1..1ef26c5 100644
--- a/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c
+++ b/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c
@@ -94,15 +94,16 @@ int canard_stm32g4fdcan_init(canard_stm32g4_fdcan_driver *driver, int bitrate_bp
 	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 << 1) | 						/* CCE=1: config change enable */
-					(1 << 6) |						/* DAR=1: automatic retransmission should be disabled
+	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. */
@@ -348,7 +349,11 @@ static int rxfifo_elem_is_fd_frame(fdcan_rxfifo_elem *rxf)
 
 static int rxfifo_elem_get_ext_id(fdcan_rxfifo_elem *rxf)
 {
-	return rxf->r[0] & 0x1FFFFFFF;
+    int ext_id = rxf->r[0] & 0x1FFFFFFF;
+    /* canardHandleRxFrame() wants bit 31 set if it's an extended frame.
+     * This driver filters out all non-extended frames, so we set it by default. */
+    ext_id |= CANARD_CAN_FRAME_EFF;
+    return ext_id;
 }
 
 static volatile uint32_t *rxfifo_elem_get_payload(fdcan_rxfifo_elem *rxf)
@@ -402,33 +407,40 @@ static void filt_elem_reject_dual_id(fdcan_ext_filt_elem *fe, uint32_t frame_id1
 __attribute__((const))
 static inline int dlc_decode(int dlc_value, int fd)
 {
-	if (dlc_value <= 8) {
-		return dlc_value;
-	}
-	if (fd) {
-		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 {
-		return 8;
-	}
+    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);
+        }
+    }
 }
 
 __attribute__((const))

From 2198a831bcb81116002a6d416406209a66b00dc9 Mon Sep 17 00:00:00 2001
From: Grisha Revzin <grishar@flytrex.com>
Date: Sun, 10 Nov 2024 18:07:37 +0200
Subject: [PATCH 08/24] Debug & cleanup; filters, transmission, reception are
 working

---
 drivers/stm32g4_fdcan/_fdcan_g4.h            | 141 ++++++++--
 drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c | 264 ++++++++-----------
 drivers/stm32g4_fdcan/canard_stm32g4_fdcan.h |   4 +-
 3 files changed, 221 insertions(+), 188 deletions(-)

diff --git a/drivers/stm32g4_fdcan/_fdcan_g4.h b/drivers/stm32g4_fdcan/_fdcan_g4.h
index 3e4c9ec..67a6b6f 100644
--- a/drivers/stm32g4_fdcan/_fdcan_g4.h
+++ b/drivers/stm32g4_fdcan/_fdcan_g4.h
@@ -3,7 +3,9 @@
 
 #include <stdint.h>
 
-typedef struct {
+/* ------------------- SRAM layout --------------------------------------------------------------------------------- */
+
+typedef struct __attribute__((packed, aligned(4))) {
 	volatile uint32_t CREL;
 	volatile uint32_t ENDN;
 	volatile uint32_t RESERVED1;
@@ -48,28 +50,125 @@ typedef struct {
 	volatile uint32_t TXEFA;
 } fdcan_registers;
 
-typedef struct {
+/* Helper to iterate over RXFxS/A */
+typedef struct __attribute__((packed, aligned(4))) {
 	volatile uint32_t RXFxS;
 	volatile uint32_t RXFxA;
 } fdcan_rxfifo_regs;
 
-typedef struct {
-	volatile uint32_t f0;
-	volatile uint32_t f1;
-} fdcan_ext_filt_elem;
+/* ------------------- 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;
 
-#define EFEC_OFFSET 29
-#define EFT_OFFSET 30
+typedef struct __attribute__((packed, aligned(4))) {
+    /* not detailed, standard IDs not used for DroneCAN */
+    uint32_t stub;
+} fdcan_stdid_filter_element;
 
-#define FDCAN_RXFIFO_ELEM_LEN (64 / 4 + 2)
-typedef struct {
-	volatile uint32_t r[FDCAN_RXFIFO_ELEM_LEN];
-} fdcan_rxfifo_elem;
+typedef struct __attribute__((packed, aligned(4))) {
+    /* not detailed, tx events are not used */
+    uint32_t stub[2];
+} fdcan_tx_event_element;
 
-#define FDCAN_TXFIFO_ELEM_LEN (64 / 4 + 2)
-typedef struct {
-	volatile uint32_t t[FDCAN_TXFIFO_ELEM_LEN];
-} fdcan_tx_buf_elem;
+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);
 
 #define DLC_OFFSET 16
 
@@ -78,15 +177,5 @@ typedef struct {
 #define FDCAN3 ((fdcan_registers *) FDCAN3_ADDR)
 
 #define SRAMCAN_START 			0x4000A400U
-#define SRAMCAN_SIZE			0x0350U
-#define SRAMCAN_EXT_FILT_OFFSET 0x0070U
-#define SRAMCAN_RXFIFO0_OFFSET 	0x00B0U
-#define SRAMCAN_RXFIFO1_OFFSET 	0x0188U
-#define SRAMCAN_RXFIFO_SIZE 	(SRAMCAN_RXFIFO1_OFFSET - SRAMCAN_RXFIFO0_OFFSET)
-#define SRAMCAN_TXB_OFFSET		0x0278U
-
-#define DRONECAN_MESSAGEID_FILTER 0xFFFF80
-
-#define FDCAN_NUM_EXT_FILTERS 8
 
 #endif
diff --git a/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c b/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c
index 1ef26c5..fe8123a 100644
--- a/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c
+++ b/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c
@@ -13,38 +13,21 @@
 /* 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 fdcan_tx_buf_elem *get_tx_buf_element(canard_stm32g4_fdcan_driver *driver, int index);
-static void canard_frame_to_tx_buf_elem(const CanardCANFrame* const frame, fdcan_tx_buf_elem *ele);
-
-static void rxfifo_receive_frame(fdcan_registers *regs, CanardCANFrame* const out_frame, fdcan_rxfifo_elem *ele);
-
-__attribute__((const))
-static inline fdcan_rxfifo_elem *get_rxfifo_elem(const canard_stm32g4_fdcan_driver *driver,
-								fdcan_rxfifo_regs *fifo_regs, size_t elemindex);
-static int rxfifo_elem_is_fd_frame(fdcan_rxfifo_elem *rxf);
-static int rxfifo_elem_get_ext_id(fdcan_rxfifo_elem *rxf);
-static volatile uint32_t *rxfifo_elem_get_payload(fdcan_rxfifo_elem *rxf);
+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 fdcan_rxfifo_elem *rxfifo_get_first_elem(canard_stm32g4_fdcan_driver *driver, fdcan_rxfifo_regs *rxf, int *elem_index);
-static int rxfifo_elem_get_dlc(fdcan_rxfifo_elem *rxf);
 static void rxfifo_ack_frame(fdcan_rxfifo_regs *rxfifo_regs, int ack_index);
 
-__attribute__((const))
-static inline fdcan_ext_filt_elem* get_ext_filt_elem(const fdcan_registers *r, size_t index);
-static int filt_elem_active(const fdcan_ext_filt_elem * const fe);
-static void filt_elem_accept_dual_id(fdcan_ext_filt_elem *fe, uint32_t frame_id1, uint32_t frame_id2);
-static void filt_elem_reject_dual_id(fdcan_ext_filt_elem *fe, uint32_t frame_id1, uint32_t frame_id2);
-
 __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 */
 
@@ -122,10 +105,10 @@ int canard_stm32g4fdcan_init(canard_stm32g4_fdcan_driver *driver, int bitrate_bp
 			(1 << 2)	| 				/* ANFE = 1: Non-matched extended frames go to FIFO1 */
 			(1 << 1) 	| (1 << 0);		/* RRFS = RRFE = 1: reject remote frames */
 
-	fdcan->XIDAM = 0xFFFF80 << 7;		/* Only consider the DroneCAN Message type ID part in the CAN frame.
+	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.
-										 * Match only broadcast messages (serviceNotMessage bit
+										 * Matches only broadcast messages (serviceNotMessage bit
 										 * will never match with anything in the filters). */
 	/* Timing config */
 	/* Set nominal timings */
@@ -151,27 +134,34 @@ int canard_stm32g4fdcan_init(canard_stm32g4_fdcan_driver *driver, int bitrate_bp
 	 * - 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_base = fdcan_ram(fdcan);	/* Precalc this address to save on divisions later on */
+	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_ext_filt_elem *fe = get_ext_filt_elem(driver->fdcan, i);
-		if (!filt_elem_active(fe)) {
-			if (accept_not_reject) {
-				filt_elem_accept_dual_id(fe, dronecan_type_id1, dronecan_type_id2);
-			}
-			else {
-				filt_elem_reject_dual_id(fe, dronecan_type_id1, dronecan_type_id2);
-			}
-			return 0;
-		}
+	    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;
 }
@@ -180,20 +170,21 @@ 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 */
+	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, get_tx_buf_element(driver, put_index));
-	fdcan->TXBAR = (1 << put_index);
+	canard_frame_to_tx_buf_elem(frame, &sram->txbuf[put_index]);
+	fdcan->TXBAR |= (1 << put_index);
 	return 1;
 }
 
@@ -201,21 +192,23 @@ int canard_stm32g4fdcan_receive(canard_stm32g4_fdcan_driver *driver, CanardCANFr
 {
 	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 ele_index = -1;
-	fdcan_rxfifo_elem *ele = NULL;
-	ele = rxfifo_get_first_elem(driver, rxfifo[0], &ele_index);
-	if (ele) {
-		rxfifo_receive_frame(fdcan, out_frame, ele);
-		rxfifo_ack_frame(rxfifo[0], ele_index);
-		return 1;
-	}
-	ele = rxfifo_get_first_elem(driver, rxfifo[1], &ele_index);
-	if (ele) {
-		rxfifo_receive_frame(fdcan, out_frame, ele);
-		rxfifo_ack_frame(rxfifo[1], ele_index);
-		return 1;
+	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);
+	    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);
+        return 1;
+    }
+
 	return 0;
 }
 
@@ -237,35 +230,29 @@ void canard_stm32g4fdcan_get_protocol_state(canard_stm32g4_fdcan_driver *driver,
 
 /* Local */
 
-static fdcan_tx_buf_elem *get_tx_buf_element(canard_stm32g4_fdcan_driver *driver, int index)
+static void canard_frame_to_tx_buf_elem(const CanardCANFrame* const frame, fdcan_tx_buf_element *ele)
 {
-	return (fdcan_tx_buf_elem *) driver->fdcan_sram_base + SRAMCAN_TXB_OFFSET + sizeof(fdcan_tx_buf_elem) * index;
-}
+    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);
 
-static void canard_frame_to_tx_buf_elem(const CanardCANFrame* const frame, fdcan_tx_buf_elem *ele)
-{
-	/* See Table 408 RM0440 */
-	/* 			 ESI = 0   29-bit id   not remote	CAN frame ID */
-	ele->t[0] = (0 << 31) | (1 << 30) | (0 << 29) | (frame->id & 0x1FFFFFFF);
 #if CANARD_ENABLE_CANFD
-	/* don't store events  is this an FD frame    use bit rate switch		DLC                        */
-	ele->t[1] = (0 << 23) | (frame->canfd << 21) |  (1 << 20) | (dlc_encode(frame->data_len, frame->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->t[1] = (0 << 23) | (0 << 21)            |  (0 << 20) | (dlc_encode(frame->data_len, 0));
+    ele->t1 = (0 << 23) | (0 << 21)            |  (0 << 20)             | (dlc_encode(frame->data_len, 0) << 16);
 #endif
 
-	volatile uint32_t *tx_data_element = (volatile uint32_t *) &ele->t[2];
-	volatile uint32_t *tx_data_frame = (volatile uint32_t *) frame->data;
-
-	if (frame->data_len) {
-		tx_data_element[0] = tx_data_frame[0];
-		tx_data_element[1] = tx_data_frame[1];
+    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) {
-			tx_data_element[i] = tx_data_frame[i];
-		}
+    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)
@@ -284,81 +271,74 @@ static void clear_and_handle_faults(canard_stm32g4_fdcan_driver *driver)
 	}
 
 	uint32_t ir = fdcan->IR;
-	if (ir & ((1 << 2) | (1 << 5))) { /* RF0L | RF1L */
-		driver->statistics.rx_fifo_overruns++;
+	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 */
+	if (ir & (1 << 12))  {
+	    /* TEFL */
 		driver->statistics.tx_fifo_overruns++;
 	}
 	fdcan->IR = 0xFFFFFF; /* clear IR */
 }
 
-static void rxfifo_receive_frame(fdcan_registers *regs, CanardCANFrame* const out_frame, fdcan_rxfifo_elem *ele)
+#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 = rxfifo_elem_get_ext_id(ele);
-	out_frame->data_len = rxfifo_elem_get_dlc(ele);
+	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) / sizeof(fdcan_registers);
 #if CANARD_ENABLE_CANFD
-	out_frame->canfd = rxfifo_elem_is_fd_frame(ele);
+	out_frame->canfd = (ele->r0 & (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
-	volatile uint32_t *payload = rxfifo_elem_get_payload(ele);
-	if (out_frame->data_len) {
-		uint32_t *out_data = (uint32_t *) out_frame->data;
-		/* SRAMCAN is accessed in words, not bytes, so no memcpy().
-		 * For dlc <= 8, two words are always copied.*/
-		if (out_frame->data_len <= 8) {
-			out_data[0] = payload[0];
-			out_data[1] = payload[1];
-		}
-		else {
-			int limit = out_frame->data_len / sizeof(uint32_t);
-			for (int i = 0; i < limit; ++i) {
-				out_data[i] = payload[i];
-			}
-		}
-	}
-}
 
-__attribute__((const))
-static inline uint32_t fdcan_ram(const fdcan_registers *r)
-{
-	return SRAMCAN_START + ((uint32_t) r - FDCAN1_ADDR) / sizeof(fdcan_registers) * SRAMCAN_SIZE;
-}
+    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];
 
-__attribute__((const))
-static inline fdcan_rxfifo_elem *get_rxfifo_elem(const canard_stm32g4_fdcan_driver *driver,
-								fdcan_rxfifo_regs *fifo_regs, size_t elemindex)
-{
-	fdcan_registers *fdcan = driver->fdcan;
-	int fifonr = ((uint32_t) fifo_regs) == ((uint32_t) &fdcan->RXF0S) ? 0 : 1;
-	return (fdcan_rxfifo_elem *)
-			(driver->fdcan_sram_base + SRAMCAN_RXFIFO0_OFFSET 	/* Base address */
-					+ SRAMCAN_RXFIFO_SIZE * fifonr 				/* FIFO0 or 1? */
-					+ sizeof(fdcan_rxfifo_elem) * elemindex); 	/* Which element */
-}
 
-static int rxfifo_elem_is_fd_frame(fdcan_rxfifo_elem *rxf)
-{
-	return (rxf->r[1] & (1 << 21)) > 0;
+#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
 }
 
-static int rxfifo_elem_get_ext_id(fdcan_rxfifo_elem *rxf)
+__attribute__((const))
+static inline uint32_t fdcan_ram(const fdcan_registers *r)
 {
-    int ext_id = rxf->r[0] & 0x1FFFFFFF;
-    /* canardHandleRxFrame() wants bit 31 set if it's an extended frame.
-     * This driver filters out all non-extended frames, so we set it by default. */
-    ext_id |= CANARD_CAN_FRAME_EFF;
-    return ext_id;
+	return SRAMCAN_START + ((uint32_t) r - FDCAN1_ADDR) / sizeof(fdcan_registers) * sizeof(fdcan_sram);
 }
 
-static volatile uint32_t *rxfifo_elem_get_payload(fdcan_rxfifo_elem *rxf)
+static int rxfifo_get_first_elem_index(fdcan_rxfifo_regs *rxf)
 {
-	return &rxf->r[2];
+    if (rxfifo_get_fill_level(rxf)) {
+        return rxfifo_get_read_index(rxf);
+    }
+    return -1;
 }
 
 static int rxfifo_get_fill_level(fdcan_rxfifo_regs *rxf)
@@ -371,39 +351,6 @@ static int rxfifo_get_read_index(fdcan_rxfifo_regs *rxf)
 	return (rxf->RXFxS & (3 << 8)) >> 8;
 }
 
-static fdcan_rxfifo_elem *rxfifo_get_first_elem(canard_stm32g4_fdcan_driver *driver, fdcan_rxfifo_regs *rxf, int *elem_index)
-{
-	if (rxfifo_get_fill_level(rxf)) {
-		*elem_index = rxfifo_get_read_index(rxf);
-		return get_rxfifo_elem(driver, rxf, *elem_index);
-	}
-	*elem_index = -1;
-	return NULL;
-}
-
-__attribute__((const))
-static inline fdcan_ext_filt_elem* get_ext_filt_elem(const fdcan_registers *r, size_t index)
-{
-	return (fdcan_ext_filt_elem *) (fdcan_ram(r) + SRAMCAN_EXT_FILT_OFFSET + sizeof(fdcan_ext_filt_elem) * index);
-}
-
-static int filt_elem_active(const fdcan_ext_filt_elem * const fe)
-{
-	return (fe->f0 & (0x7 << 29)) > 0;
-}
-
-static void filt_elem_accept_dual_id(fdcan_ext_filt_elem *fe, uint32_t frame_id1, uint32_t frame_id2)
-{
-	fe->f0 = (1 << EFEC_OFFSET) | frame_id1;
-	fe->f1 = (1 << EFT_OFFSET) | frame_id2;
-}
-
-static void filt_elem_reject_dual_id(fdcan_ext_filt_elem *fe, uint32_t frame_id1, uint32_t frame_id2)
-{
-	fe->f0 = (3 << EFEC_OFFSET) | frame_id1;
-	fe->f1 = (1 << EFT_OFFSET) | frame_id2;
-}
-
 __attribute__((const))
 static inline int dlc_decode(int dlc_value, int fd)
 {
@@ -476,11 +423,6 @@ static inline int dlc_encode(int data_len, int fd)
 	}
 }
 
-static int rxfifo_elem_get_dlc(fdcan_rxfifo_elem *rxf)
-{
-	return dlc_decode(((0xF << DLC_OFFSET) & rxf->r[1]) >> DLC_OFFSET, rxfifo_elem_is_fd_frame(rxf));
-}
-
 static void rxfifo_ack_frame(fdcan_rxfifo_regs *rxfifo_regs, int ack_index)
 {
 	rxfifo_regs->RXFxA |= 7 & ack_index;
diff --git a/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.h b/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.h
index 7ae08c4..4b0867e 100644
--- a/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.h
+++ b/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.h
@@ -20,8 +20,10 @@
 
 typedef struct {
 	void *fdcan;
+	void *fdcan_sram;
 	struct {
-		uint32_t rx_fifo_overruns;
+		uint32_t rx_fifo0_overruns;
+		uint32_t rx_fifo1_overruns;
 		uint32_t tx_fifo_overruns;
 		uint32_t warning_events;
 		uint32_t bus_off_events;

From a9d701a6cba7048b0758114b6fa262dabec86f05 Mon Sep 17 00:00:00 2001
From: Grisha Revzin <grishar@flytrex.com>
Date: Mon, 11 Nov 2024 12:39:09 +0200
Subject: [PATCH 09/24] Fixed put_index calculation in transmit()

---
 drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c b/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c
index fe8123a..daf714c 100644
--- a/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c
+++ b/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c
@@ -182,7 +182,7 @@ int canard_stm32g4fdcan_transmit(canard_stm32g4_fdcan_driver *driver, const Cana
 		/* TFQF set: TX queue full */
 		return 0;
 	}
-	int put_index = fdcan->TXFQS & (3 << 16) >> 16;
+	int put_index = (fdcan->TXFQS & (3 << 16)) >> 16;
 	canard_frame_to_tx_buf_elem(frame, &sram->txbuf[put_index]);
 	fdcan->TXBAR |= (1 << put_index);
 	return 1;

From e82eb76400c99ed7bf252584f4f99591be548aca Mon Sep 17 00:00:00 2001
From: Grisha Revzin <grishar@flytrex.com>
Date: Mon, 11 Nov 2024 12:39:34 +0200
Subject: [PATCH 10/24] silenced unused parameter warning for non-FD mode

---
 drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c b/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c
index daf714c..d217ab4 100644
--- a/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c
+++ b/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c
@@ -33,6 +33,8 @@ static inline int dlc_encode(int data_len, int fd);
 
 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);

From 4c1c14a5b28ebaa43adbdcea134442443c02e5b4 Mon Sep 17 00:00:00 2001
From: Grisha Revzin <grishar@flytrex.com>
Date: Mon, 11 Nov 2024 13:55:42 +0200
Subject: [PATCH 11/24] Fixed acknowledging received frames

---
 drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c b/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c
index d217ab4..69067c8 100644
--- a/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c
+++ b/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c
@@ -427,5 +427,5 @@ static inline int dlc_encode(int data_len, int fd)
 
 static void rxfifo_ack_frame(fdcan_rxfifo_regs *rxfifo_regs, int ack_index)
 {
-	rxfifo_regs->RXFxA |= 7 & ack_index;
+	rxfifo_regs->RXFxA = ack_index;
 }

From f08f41c7611ad470ff327dbcbffc21b3f2530909 Mon Sep 17 00:00:00 2001
From: Grisha Revzin <grishar@flytrex.com>
Date: Mon, 11 Nov 2024 14:15:03 +0200
Subject: [PATCH 12/24] tabs -> spaces

---
 drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c | 382 +++++++++----------
 1 file changed, 191 insertions(+), 191 deletions(-)

diff --git a/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c b/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c
index 69067c8..6aefe83 100644
--- a/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c
+++ b/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c
@@ -35,9 +35,9 @@ int canard_stm32g4fdcan_init(canard_stm32g4_fdcan_driver *driver, int bitrate_bp
 {
     (void) fdbitrate_bps;
 
-	/* Calculate timings based on the provided bitrates */
-	CanardSTM32CANTimings nominal_timings = {0};
-	int rc = canardSTM32ComputeCANTimings(periph_clock_rate, bitrate_bps, &nominal_timings);
+    /* 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) ||
@@ -47,12 +47,12 @@ int canard_stm32g4fdcan_init(canard_stm32g4_fdcan_driver *driver, int bitrate_bp
         return -CANARD_ERROR_INVALID_ARGUMENT;
     }
 
-	if (rc < 0)
-		return rc;
+    if (rc < 0)
+        return rc;
 
 #if CANARD_ENABLE_CANFD
-	CanardSTM32CANTimings brs_timings = {0};
-	rc = canardSTM32ComputeCANTimings(periph_clock_rate, fdbitrate_bps, &brs_timings);
+    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) ||
@@ -62,83 +62,83 @@ int canard_stm32g4fdcan_init(canard_stm32g4_fdcan_driver *driver, int bitrate_bp
         return -CANARD_ERROR_INVALID_ARGUMENT;
     }
 
-	if (rc < 0)
-		return rc;
+    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);
-	}
-
-	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. */
+    switch ((uint32_t) driver->fdcan) {
+    case FDCAN1_ADDR:
+    case FDCAN2_ADDR:
+    case FDCAN3_ADDR:
+        break;
+    default:
+        CANARD_ASSERT(0);
+    }
+
+    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 */
+    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);
+    /* 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 = (1 << 23) | /* TDC = 1: enable transceiver delay compensation */
-			 	  ((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);
+    /* Set Bit Rate Switching timings */
+    fdcan->DBTP = (1 << 23) | /* TDC = 1: enable transceiver delay compensation */
+                  ((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;
+    /* 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
@@ -152,82 +152,82 @@ static int filt_elem_get_efec(fdcan_extid_filter_element *fe)
 }
 
 int canard_stm32g4fdcan_type_id_filter(canard_stm32g4_fdcan_driver *driver,
-										int dronecan_type_id1, int dronecan_type_id2, int accept_not_reject)
+                                        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;
+    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_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 */
+    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);
-	return 1;
+    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);
+    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);
-	    return 1;
-	}
-
-	index = rxfifo_get_first_elem_index(rxfifo[1]);
+    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);
+        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);
         return 1;
     }
 
-	return 0;
+    return 0;
 }
 
 void canard_stm32g4fdcan_enable_automatic_retransmission(canard_stm32g4_fdcan_driver *driver)
 {
-	fdcan_registers *fdcan = driver->fdcan;
-	fdcan->CCCR &= ~(1 << 6);
+    fdcan_registers *fdcan = driver->fdcan;
+    fdcan->CCCR &= ~(1 << 6);
 }
 
 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 & 0xF00) >> 16;
+    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 & 0xF00) >> 16;
 }
 
 /* Local */
@@ -259,33 +259,33 @@ static void canard_frame_to_tx_buf_elem(const CanardCANFrame* const frame, fdcan
 
 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 */
+    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
@@ -298,19 +298,19 @@ static int rxfifo_get_dlc(fdcan_rx_buf_element *ele)
 
 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) / sizeof(fdcan_registers);
+    /* 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) / sizeof(fdcan_registers);
 #if CANARD_ENABLE_CANFD
-	out_frame->canfd = (ele->r0 & (1 << 21)) > 0;
-	out_frame->data_len = dlc_decode(rxfifo_get_dlc(ele), out_frame->canfd);
-	CANARD_ASSERT(out_frame->data_len <= 64);
+    out_frame->canfd = (ele->r0 & (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);
+    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;
@@ -332,7 +332,7 @@ static void rxfifo_receive_frame(fdcan_registers *regs, CanardCANFrame* const ou
 __attribute__((const))
 static inline uint32_t fdcan_ram(const fdcan_registers *r)
 {
-	return SRAMCAN_START + ((uint32_t) r - FDCAN1_ADDR) / sizeof(fdcan_registers) * sizeof(fdcan_sram);
+    return SRAMCAN_START + ((uint32_t) r - FDCAN1_ADDR) / sizeof(fdcan_registers) * sizeof(fdcan_sram);
 }
 
 static int rxfifo_get_first_elem_index(fdcan_rxfifo_regs *rxf)
@@ -345,12 +345,12 @@ static int rxfifo_get_first_elem_index(fdcan_rxfifo_regs *rxf)
 
 static int rxfifo_get_fill_level(fdcan_rxfifo_regs *rxf)
 {
-	return rxf->RXFxS & 0xF;
+    return rxf->RXFxS & 0xF;
 }
 
 static int rxfifo_get_read_index(fdcan_rxfifo_regs *rxf)
 {
-	return (rxf->RXFxS & (3 << 8)) >> 8;
+    return (rxf->RXFxS & (3 << 8)) >> 8;
 }
 
 __attribute__((const))
@@ -395,37 +395,37 @@ static inline int dlc_decode(int dlc_value, int fd)
 __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;
-	}
+    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;
+    rxfifo_regs->RXFxA = ack_index;
 }

From 989e47e5718aa076d141d52d47fe700040a20af4 Mon Sep 17 00:00:00 2001
From: Grisha Revzin <grishar@flytrex.com>
Date: Tue, 12 Nov 2024 14:57:50 +0200
Subject: [PATCH 13/24] Added canard_stm32g4fdcan_get_statistics()

---
 drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c | 13 +++++++++++++
 drivers/stm32g4_fdcan/canard_stm32g4_fdcan.h |  9 +++++++++
 2 files changed, 22 insertions(+)

diff --git a/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c b/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c
index 6aefe83..872736a 100644
--- a/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c
+++ b/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c
@@ -187,6 +187,7 @@ int canard_stm32g4fdcan_transmit(canard_stm32g4_fdcan_driver *driver, const Cana
     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;
 }
 
@@ -201,6 +202,7 @@ int canard_stm32g4fdcan_receive(canard_stm32g4_fdcan_driver *driver, CanardCANFr
     if (index != -1) {
         rxfifo_receive_frame(fdcan, out_frame, &sram->rxfifo0[index]);
         rxfifo_ack_frame(rxfifo[0], index);
+        driver->statistics.rx_frames++;
         return 1;
     }
 
@@ -208,6 +210,7 @@ int canard_stm32g4fdcan_receive(canard_stm32g4_fdcan_driver *driver, CanardCANFr
     if (index != -1) {
         rxfifo_receive_frame(fdcan, out_frame, &sram->rxfifo1[index]);
         rxfifo_ack_frame(rxfifo[1], index);
+        driver->statistics.rx_frames++;
         return 1;
     }
 
@@ -230,6 +233,16 @@ void canard_stm32g4fdcan_get_protocol_state(canard_stm32g4_fdcan_driver *driver,
     s->rec = (fdcan->ECR & 0xF00) >> 16;
 }
 
+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_tx_frames = driver->statistics.rx_frames;
+    *num_rx_frames = driver->statistics.tx_frames;
+}
+
 /* Local */
 
 static void canard_frame_to_tx_buf_elem(const CanardCANFrame* const frame, fdcan_tx_buf_element *ele)
diff --git a/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.h b/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.h
index 4b0867e..dee2e87 100644
--- a/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.h
+++ b/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.h
@@ -28,6 +28,8 @@ typedef struct {
 		uint32_t warning_events;
 		uint32_t bus_off_events;
 		uint32_t protocol_exceptions;
+		uint32_t tx_frames;
+		uint32_t rx_frames;
 	} statistics;
 	uint32_t fdcan_sram_base;
 } canard_stm32g4_fdcan_driver;
@@ -105,4 +107,11 @@ void canard_stm32g4fdcan_get_protocol_state(canard_stm32g4_fdcan_driver *driver,
 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);
+
+
 #endif /* CANARD_STM32G4_FDCAN_H_ */

From 35c70b1c6c6751865de06b773f57bd0a3ab5cf73 Mon Sep 17 00:00:00 2001
From: Grisha Revzin <grishar@flytrex.com>
Date: Wed, 1 Jan 2025 14:25:59 +0200
Subject: [PATCH 14/24] Fixed FD frame reception

---
 drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c b/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c
index 872736a..5fd5328 100644
--- a/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c
+++ b/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c
@@ -318,7 +318,7 @@ static void rxfifo_receive_frame(fdcan_registers *regs, CanardCANFrame* const ou
                                             * no standard frames are received by the filter configuration */
     out_frame->iface_id = ((uint32_t) regs - FDCAN1_ADDR) / sizeof(fdcan_registers);
 #if CANARD_ENABLE_CANFD
-    out_frame->canfd = (ele->r0 & (1 << 21)) > 0;
+    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

From 2982c7b0f0be957552870f00b0e5c6ca74e805d3 Mon Sep 17 00:00:00 2001
From: Grisha Revzin <grishar@flytrex.com>
Date: Wed, 1 Jan 2025 16:55:02 +0200
Subject: [PATCH 15/24] Set TDC = 0 for BRS

---
 drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c b/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c
index 5fd5328..3601049 100644
--- a/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c
+++ b/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c
@@ -121,7 +121,7 @@ int canard_stm32g4fdcan_init(canard_stm32g4_fdcan_driver *driver, int bitrate_bp
 
 #if CANARD_ENABLE_CANFD
     /* Set Bit Rate Switching timings */
-    fdcan->DBTP = (1 << 23) | /* TDC = 1: enable transceiver delay compensation */
+    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) |

From 71c504068c19c4a4c421d51040d4c7e887c37c69 Mon Sep 17 00:00:00 2001
From: Grisha Revzin <grishar@flytrex.com>
Date: Mon, 24 Feb 2025 18:27:28 +0200
Subject: [PATCH 16/24] Added wipe_filters()

---
 drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c | 6 ++++++
 drivers/stm32g4_fdcan/canard_stm32g4_fdcan.h | 1 +
 2 files changed, 7 insertions(+)

diff --git a/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c b/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c
index 3601049..fe030db 100644
--- a/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c
+++ b/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c
@@ -168,6 +168,12 @@ int canard_stm32g4fdcan_type_id_filter(canard_stm32g4_fdcan_driver *driver,
     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;
diff --git a/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.h b/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.h
index dee2e87..2e1d075 100644
--- a/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.h
+++ b/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.h
@@ -67,6 +67,7 @@ void canard_stm32g4fdcan_start(canard_stm32g4_fdcan_driver *driver);
  */
 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.

From c3f7fd5c29a01ddab9a91303db00cfd315423079 Mon Sep 17 00:00:00 2001
From: Grisha Revzin <grishar@flytrex.com>
Date: Fri, 11 Apr 2025 15:48:12 +0300
Subject: [PATCH 17/24] Fixed enable_automatic_retransmission

---
 drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c | 11 ++++++++---
 1 file changed, 8 insertions(+), 3 deletions(-)

diff --git a/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c b/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c
index fe030db..df66722 100644
--- a/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c
+++ b/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c
@@ -93,7 +93,6 @@ int canard_stm32g4fdcan_init(canard_stm32g4_fdcan_driver *driver, int bitrate_bp
                     (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
@@ -192,7 +191,7 @@ int canard_stm32g4fdcan_transmit(canard_stm32g4_fdcan_driver *driver, const Cana
     }
     int put_index = (fdcan->TXFQS & (3 << 16)) >> 16;
     canard_frame_to_tx_buf_elem(frame, &sram->txbuf[put_index]);
-    fdcan->TXBAR |= (1 << put_index);
+    fdcan->TXBAR = (1 << put_index);
     driver->statistics.tx_frames++;
     return 1;
 }
@@ -226,7 +225,13 @@ int canard_stm32g4fdcan_receive(canard_stm32g4_fdcan_driver *driver, CanardCANFr
 void canard_stm32g4fdcan_enable_automatic_retransmission(canard_stm32g4_fdcan_driver *driver)
 {
     fdcan_registers *fdcan = driver->fdcan;
-    fdcan->CCCR &= ~(1 << 6);
+    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)

From cebebe5b8677b1e1e052be56d156173a9f0ac9c6 Mon Sep 17 00:00:00 2001
From: grisharevzin <117658353+grisharevzin@users.noreply.github.com>
Date: Tue, 3 Mar 2026 10:03:08 +0200
Subject: [PATCH 18/24] CR, minor

Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com>
---
 drivers/stm32g4_fdcan/README.md              | 2 +-
 drivers/stm32g4_fdcan/_fdcan_g4.h            | 2 +-
 drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c | 8 +++++---
 drivers/stm32g4_fdcan/canard_stm32g4_fdcan.h | 4 ++--
 4 files changed, 9 insertions(+), 7 deletions(-)

diff --git a/drivers/stm32g4_fdcan/README.md b/drivers/stm32g4_fdcan/README.md
index f48d583..22c6d4c 100644
--- a/drivers/stm32g4_fdcan/README.md
+++ b/drivers/stm32g4_fdcan/README.md
@@ -37,7 +37,7 @@ Three things can happen to a message on reception (see `canard_stm32g4fdcan_type
 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_stm32fdcan_recieve` reads one message at a time and prefers FIFO0.
+`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.
diff --git a/drivers/stm32g4_fdcan/_fdcan_g4.h b/drivers/stm32g4_fdcan/_fdcan_g4.h
index 67a6b6f..9a6473f 100644
--- a/drivers/stm32g4_fdcan/_fdcan_g4.h
+++ b/drivers/stm32g4_fdcan/_fdcan_g4.h
@@ -168,7 +168,7 @@ typedef struct __attribute__((packed, aligned(4))) {
     fdcan_tx_buf_element txbuf[FDCAN_NUM_TX_BUF];                           // 54   // 212 == 0x350
 } fdcan_sram;
 
-static_assert(sizeof(fdcan_sram) == 0x350);
+_Static_assert(sizeof(fdcan_sram) == 0x350, "fdcan_sram size must be 0x350");
 
 #define DLC_OFFSET 16
 
diff --git a/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c b/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c
index df66722..2656fac 100644
--- a/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c
+++ b/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c
@@ -6,7 +6,7 @@
  */
 
 #include "canard_stm32g4_fdcan.h"
-
+#include <string.h>
 #include "_fdcan_g4.h"
 #include "../stm32/canard_stm32.h" /* for CanardSTM32ComputeCANTimings */
 
@@ -73,6 +73,7 @@ int canard_stm32g4fdcan_init(canard_stm32g4_fdcan_driver *driver, int bitrate_bp
         break;
     default:
         CANARD_ASSERT(0);
+        return -CANARD_ERROR_INVALID_ARGUMENT;
     }
 
     fdcan_registers *fdcan = driver->fdcan;
@@ -250,8 +251,8 @@ void canard_stm32g4fdcan_get_statistics(canard_stm32g4_fdcan_driver *driver, uin
     *num_errors = driver->statistics.rx_fifo0_overruns +
                   driver->statistics.rx_fifo1_overruns +
                   driver->statistics.bus_off_events;
-    *num_tx_frames = driver->statistics.rx_frames;
-    *num_rx_frames = driver->statistics.tx_frames;
+    *num_rx_frames = driver->statistics.rx_frames;
+    *num_tx_frames = driver->statistics.tx_frames;
 }
 
 /* Local */
@@ -412,6 +413,7 @@ static inline int dlc_decode(int dlc_value, int fd)
         }
         else {
             CANARD_ASSERT(0);
+            return 0;
         }
     }
 }
diff --git a/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.h b/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.h
index 2e1d075..c988749 100644
--- a/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.h
+++ b/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.h
@@ -12,7 +12,7 @@
 
 #include <canard.h>
 
-#define CANARD_ERROR_STM32_FDCAN_OUT_OF_FILTER_SPACE -1200
+#define CANARD_ERROR_STM32_FDCAN_OUT_OF_FILTER_SPACE 1200
 
 #define FDCAN1_ADDR 0x40006400U
 #define FDCAN2_ADDR 0x40006800U
@@ -41,7 +41,7 @@ typedef struct {
  * 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_ADDRESS, FDCAN2_ADDRESS or FDCAN3_ADDRESS.
+ * @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).

From 748b9f1a1d84a6ebef6187734486240425a63d10 Mon Sep 17 00:00:00 2001
From: Grisha Revzin <grishar@flytrex.com>
Date: Tue, 3 Mar 2026 10:13:27 +0200
Subject: [PATCH 19/24] Fixed incorrect ECR values

---
 drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c b/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c
index 2656fac..0875ccf 100644
--- a/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c
+++ b/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c
@@ -242,7 +242,7 @@ void canard_stm32g4fdcan_get_protocol_state(canard_stm32g4_fdcan_driver *driver,
     s->warning = (fdcan->PSR & (1 << 6)) > 0;
     s->error_passive = (fdcan->PSR & (1 << 5)) > 0;
     s->tec = fdcan->ECR & 0xFF;
-    s->rec = (fdcan->ECR & 0xF00) >> 16;
+    s->rec = (fdcan->ECR & 0x7F00) >> 8;
 }
 
 void canard_stm32g4fdcan_get_statistics(canard_stm32g4_fdcan_driver *driver, uint32_t *num_rx_frames,

From e1aeadb9c4393f6abbcd012b5aa473465d8f2323 Mon Sep 17 00:00:00 2001
From: Grisha Revzin <grishar@flytrex.com>
Date: Tue, 3 Mar 2026 11:18:03 +0200
Subject: [PATCH 20/24] Fixed incorrect IP offset calculation

---
 drivers/stm32g4_fdcan/_fdcan_g4.h            | 2 ++
 drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c | 6 +++---
 drivers/stm32g4_fdcan/canard_stm32g4_fdcan.h | 2 +-
 3 files changed, 6 insertions(+), 4 deletions(-)

diff --git a/drivers/stm32g4_fdcan/_fdcan_g4.h b/drivers/stm32g4_fdcan/_fdcan_g4.h
index 9a6473f..70fb904 100644
--- a/drivers/stm32g4_fdcan/_fdcan_g4.h
+++ b/drivers/stm32g4_fdcan/_fdcan_g4.h
@@ -176,6 +176,8 @@ _Static_assert(sizeof(fdcan_sram) == 0x350, "fdcan_sram size must be 0x350");
 #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
index 0875ccf..7f93581 100644
--- a/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c
+++ b/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c
@@ -230,7 +230,7 @@ void canard_stm32g4fdcan_enable_automatic_retransmission(canard_stm32g4_fdcan_dr
     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 << 6);                       /* DAR = 0 */
     fdcan->CCCR &= ~((1 << 0) | (1 << 1));          /* Clear INIT and CCE */
     while ((fdcan->CCCR & (1 << 0)));               /* Wait until we leave init mode */
 }
@@ -328,7 +328,7 @@ static void rxfifo_receive_frame(fdcan_registers *regs, CanardCANFrame* const ou
     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) / sizeof(fdcan_registers);
+    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);
@@ -357,7 +357,7 @@ static void rxfifo_receive_frame(fdcan_registers *regs, CanardCANFrame* const ou
 __attribute__((const))
 static inline uint32_t fdcan_ram(const fdcan_registers *r)
 {
-    return SRAMCAN_START + ((uint32_t) r - FDCAN1_ADDR) / sizeof(fdcan_registers) * sizeof(fdcan_sram);
+    return SRAMCAN_START + ((uint32_t) r - FDCAN1_ADDR);
 }
 
 static int rxfifo_get_first_elem_index(fdcan_rxfifo_regs *rxf)
diff --git a/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.h b/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.h
index c988749..02049ee 100644
--- a/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.h
+++ b/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.h
@@ -90,7 +90,7 @@ int canard_stm32g4fdcan_transmit(canard_stm32g4_fdcan_driver *driver, const Cana
 int canard_stm32g4fdcan_receive(canard_stm32g4_fdcan_driver *driver, CanardCANFrame* const out_frame);
 
 /**
- *	Reads the instant standard CAN state variables from the hardware.
+ *	Reads the instantaneous standard CAN state variables from the hardware.
  */
 typedef struct {
 	uint8_t tec, rec;

From c3a502c3282f6f2e9ac1e89400906221ae6ba64a Mon Sep 17 00:00:00 2001
From: Grisha Revzin <grishar@flytrex.com>
Date: Wed, 4 Mar 2026 20:40:33 +0200
Subject: [PATCH 21/24] CR, license notes

---
 drivers/stm32g4_fdcan/_fdcan_g4.h            | 10 ++++++++--
 drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c |  7 ++++---
 drivers/stm32g4_fdcan/canard_stm32g4_fdcan.h |  7 ++++---
 3 files changed, 16 insertions(+), 8 deletions(-)

diff --git a/drivers/stm32g4_fdcan/_fdcan_g4.h b/drivers/stm32g4_fdcan/_fdcan_g4.h
index 70fb904..ce942a8 100644
--- a/drivers/stm32g4_fdcan/_fdcan_g4.h
+++ b/drivers/stm32g4_fdcan/_fdcan_g4.h
@@ -1,3 +1,11 @@
+/*
+ * 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
 
@@ -170,8 +178,6 @@ typedef struct __attribute__((packed, aligned(4))) {
 
 _Static_assert(sizeof(fdcan_sram) == 0x350, "fdcan_sram size must be 0x350");
 
-#define DLC_OFFSET 16
-
 #define FDCAN1 ((fdcan_registers *) FDCAN1_ADDR)
 #define FDCAN2 ((fdcan_registers *) FDCAN2_ADDR)
 #define FDCAN3 ((fdcan_registers *) FDCAN3_ADDR)
diff --git a/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c b/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c
index 7f93581..7b7ed29 100644
--- a/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c
+++ b/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.c
@@ -1,8 +1,9 @@
 /*
- * canard_stm32g4_fdcan.c
+ * Copyright (c) 2024-2026 Flytrex, by Grisha Revzin
  *
- *  Created on: Sep 10, 2024
- *      Author: grishar
+ * Distributed under the MIT License, available in the file LICENSE.
+ *
+ * Created on: Sep 10, 2024
  */
 
 #include "canard_stm32g4_fdcan.h"
diff --git a/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.h b/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.h
index 02049ee..e81e9d8 100644
--- a/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.h
+++ b/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.h
@@ -1,8 +1,9 @@
 /*
- * canard_stm32g4_fdcan.h
+ * Copyright (c) 2024-2026 Flytrex, by Grisha Revzin
  *
- *  Created on: Sep 10, 2024
- *      Author: grishar
+ * Distributed under the MIT License, available in the file LICENSE.
+ *
+ * Created on: Sep 10, 2024
  */
 
 #ifndef CANARD_STM32G4_FDCAN_H_

From 6c86ab187f58bcd06807866384e7978f0792286f Mon Sep 17 00:00:00 2001
From: Grisha Revzin <grishar@flytrex.com>
Date: Wed, 4 Mar 2026 20:55:50 +0200
Subject: [PATCH 22/24] CR, minor

---
 drivers/stm32g4_fdcan/canard_stm32g4_fdcan.h | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.h b/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.h
index e81e9d8..cf69023 100644
--- a/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.h
+++ b/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.h
@@ -32,7 +32,6 @@ typedef struct {
 		uint32_t tx_frames;
 		uint32_t rx_frames;
 	} statistics;
-	uint32_t fdcan_sram_base;
 } canard_stm32g4_fdcan_driver;
 
 /**
@@ -46,7 +45,7 @@ typedef struct {
  *   		  					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  	The clock rate of the hardware (set to 80 MHz for best results).
+ * @param[in] periph_clock_rate The clock rate of the hardware (set to 80 MHz for best results).
  * @retval 0 -- ok
  * @retval negative -- error
  */

From 2d48474c3b736c443d70f25e3983fef7785040e8 Mon Sep 17 00:00:00 2001
From: Grisha Revzin <grishar@flytrex.com>
Date: Wed, 4 Mar 2026 20:59:44 +0200
Subject: [PATCH 23/24] extern 'C'

---
 drivers/stm32g4_fdcan/canard_stm32g4_fdcan.h | 9 +++++++++
 1 file changed, 9 insertions(+)

diff --git a/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.h b/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.h
index cf69023..be5651b 100644
--- a/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.h
+++ b/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.h
@@ -13,6 +13,11 @@
 
 #include <canard.h>
 
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
 #define CANARD_ERROR_STM32_FDCAN_OUT_OF_FILTER_SPACE 1200
 
 #define FDCAN1_ADDR 0x40006400U
@@ -115,4 +120,8 @@ void canard_stm32g4fdcan_get_statistics(canard_stm32g4_fdcan_driver *driver, uin
                                         uint32_t *num_tx_frames, uint32_t *num_errors);
 
 
+#ifdef __cplusplus
+}
+#endif
+
 #endif /* CANARD_STM32G4_FDCAN_H_ */

From 2e696cbf999f5e0c31489bd3f5263da46da72edb Mon Sep 17 00:00:00 2001
From: Grisha Revzin <grishar@flytrex.com>
Date: Wed, 4 Mar 2026 21:02:21 +0200
Subject: [PATCH 24/24] CR, minor

---
 drivers/stm32g4_fdcan/canard_stm32g4_fdcan.h | 1 -
 1 file changed, 1 deletion(-)

diff --git a/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.h b/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.h
index be5651b..f1b2fe2 100644
--- a/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.h
+++ b/drivers/stm32g4_fdcan/canard_stm32g4_fdcan.h
@@ -100,7 +100,6 @@ int canard_stm32g4fdcan_receive(canard_stm32g4_fdcan_driver *driver, CanardCANFr
 typedef struct {
 	uint8_t tec, rec;
 	uint8_t bus_off, error_passive, warning;
-	uint8_t delay_comp_value;
 } canard_stm32g4fdcan_protocol_state;
 
 void canard_stm32g4fdcan_get_protocol_state(canard_stm32g4_fdcan_driver *driver, canard_stm32g4fdcan_protocol_state *s);