stm32: canio: When Tx mailboxes are full, cancel an old message

.. also add the 8ms wait for transmission that the atmel sam port
has

2 files changed, 19 insertions, 0 deletions

diff --git a/ports/stm/common-hal/canio/CAN.c b/ports/stm/common-hal/canio/CAN.c
index 244cc037c..8ff91aaca 100644
--- a/ports/stm/common-hal/canio/CAN.c
+++ b/ports/stm/common-hal/canio/CAN.c
@@ -225,10 +225,28 @@ void common_hal_canio_can_send(canio_can_obj_t *self, mp_obj_t message_in)
         .RTR = rtr ? CAN_RTR_REMOTE : CAN_RTR_DATA,
         .DLC = message->size,
     };
+    uint32_t free_level = HAL_CAN_GetTxMailboxesFreeLevel(&self->handle);
+    if (free_level == 0) {
+        // There's no free Tx mailbox.  We need to cancel some message without
+        // transmitting it, because once the bus returns to active state it's
+        // preferable to transmit the newest messages instead of older messages.
+        //
+        // We don't strictly guarantee that we abort the oldest Tx request,
+        // rather we just abort a different index each time.  This permits us
+        // to avoid tracking this information altogether.
+        HAL_CAN_AbortTxRequest(&self->handle, 1 << (self->cancel_mailbox));
+        self->cancel_mailbox = (self->cancel_mailbox + 1) % 3;
+    }
     HAL_StatusTypeDef status = HAL_CAN_AddTxMessage(&self->handle, &header, message->data, &mailbox);
     if (status != HAL_OK) {
         mp_raise_OSError(MP_ENOMEM);
     }
+
+    // wait 8ms (hard coded for now) for TX to occur
+    uint64_t deadline = port_get_raw_ticks(NULL) + 8;
+    while (port_get_raw_ticks(NULL) < deadline && HAL_CAN_IsTxMessagePending(&self->handle, 1 << mailbox)) {
+        RUN_BACKGROUND_TASKS;
+    }
 }
 
 bool common_hal_canio_can_silent_get(canio_can_obj_t *self) {
diff --git a/ports/stm/common-hal/canio/CAN.h b/ports/stm/common-hal/canio/CAN.h
index bffc0f65f..94d7e99a8 100644
--- a/ports/stm/common-hal/canio/CAN.h
+++ b/ports/stm/common-hal/canio/CAN.h
@@ -54,6 +54,7 @@ typedef struct canio_can_obj {
     bool fifo0_in_use:1;
     bool fifo1_in_use:1;
     uint8_t periph_index:2;
+    uint8_t cancel_mailbox;
     uint8_t start_filter_bank;
     uint8_t end_filter_bank;
     long filter_in_use; // bitmask for the 28 filter banks