stm32/pyb_can: Handle timeout arg for FDCAN in pyb_can_send.

Following the documented pyb can_send behavior in pyb.CAN docs.

1 files changed, 14 insertions, 0 deletions

diff --git a/ports/stm32/pyb_can.c b/ports/stm32/pyb_can.c
index ad2efbef4..224b8f28b 100644
--- a/ports/stm32/pyb_can.c
+++ b/ports/stm32/pyb_can.c
@@ -432,6 +432,20 @@ STATIC mp_obj_t pyb_can_send(size_t n_args, const mp_obj_t *pos_args, mp_map_t *
 
     HAL_StatusTypeDef status;
     #if MICROPY_HW_ENABLE_FDCAN
+    uint32_t timeout_ms = args[ARG_timeout].u_int;
+    uint32_t start = HAL_GetTick();
+    while (HAL_FDCAN_GetTxFifoFreeLevel(&self->can) == 0) {
+        if (timeout_ms == 0) {
+            mp_raise_OSError(MP_ETIMEDOUT);
+        }
+        // Check for the Timeout
+        if (timeout_ms != HAL_MAX_DELAY) {
+            if (HAL_GetTick() - start >= timeout_ms) {
+                mp_raise_OSError(MP_ETIMEDOUT);
+            }
+        }
+        MICROPY_EVENT_POLL_HOOK
+    }
     status = HAL_FDCAN_AddMessageToTxFifoQ(&self->can, &tx_msg, tx_data);
     #else
     self->can.pTxMsg = &tx_msg;