_canio: Add listener matching

Lightly tested:
 * no matches (catch-all)
 * standard address single address matches (even and odd positions)
 * standard address mask matches
 * only tested that extended doesn't match non-extended

ports/atmel-samd/common-hal/_canio/CAN.c

@@ -197,7 +197,7 @@ void common_hal_canio_can_construct(canio_can_obj_t *self, mcu_pin_obj_t *rx, mc
     {
         CAN_GFC_Type gfc = {
             .bit.RRFE = 1,
-            .bit.ANFS = CAN_GFC_ANFS_RXF0_Val,
+            .bit.ANFS = CAN_GFC_ANFS_REJECT_Val,
             .bit.ANFE = CAN_GFC_ANFE_REJECT_Val,
         };
         hri_can_write_GFC_reg(self->hw, gfc.reg);
@@ -206,14 +206,15 @@ void common_hal_canio_can_construct(canio_can_obj_t *self, mcu_pin_obj_t *rx, mc
     {
         CAN_SIDFC_Type dfc = {
             .bit.LSS = COMMON_HAL_CANIO_RX_FILTER_SIZE,
-            .bit.FLSSA = (uint32_t)self->state->rx_filter
+            .bit.FLSSA = (uint32_t)self->state->standard_rx_filter
         };
         hri_can_write_SIDFC_reg(self->hw, dfc.reg);
     }
 
     {
         CAN_XIDFC_Type dfc = {
-            .bit.LSE = 0,
+            .bit.LSE = COMMON_HAL_CANIO_RX_FILTER_SIZE,
+            .bit.FLESA = (uint32_t)self->state->extended_rx_filter
         };
         hri_can_write_XIDFC_reg(self->hw, dfc.reg);
     }
@@ -337,10 +338,15 @@ void common_hal_canio_can_send(canio_can_obj_t *self, canio_message_obj_t *messa
 
     // We have just one dedicated TX buffer, use it!
     canio_can_fifo_t *ent = &self->state->tx_fifo[0];
+    
     ent->txb0.bit.ESI = false;
-    ent->txb0.bit.XTD = false;
+    ent->txb0.bit.XTD = message->extended;
     ent->txb0.bit.RTR = message->rtr;
+    if (message->extended) {
+        ent->txb0.bit.ID = message->id << 18;
+    } else {
         ent->txb0.bit.ID = message->id << 18; // short addresses are left-justified
+    }
 
     ent->txb1.bit.MM = 0; // "message marker"
     ent->txb1.bit.EFC = 0; // don't store fifo events to event queue

ports/atmel-samd/common-hal/_canio/Listener.c

@@ -34,12 +34,262 @@
 #include "common-hal/_canio/Listener.h"
 #include "shared-bindings/util.h"
 #include "supervisor/shared/tick.h"
+#include "component/can.h"
 
-void common_hal_canio_listener_construct(canio_listener_obj_t *self, canio_can_obj_t *can, size_t nmatch, canio_match_obj_t **matches, float timeout) {
+STATIC void allow_config_change(canio_can_obj_t *can) {
-    if (nmatch) {
+    can->hw->CCCR.bit.INIT = 1;
-        mp_raise_NotImplementedError(NULL);
+    while (!can->hw->CCCR.bit.INIT) {
+    }
+    can->hw->CCCR.bit.CCE = 1;
 }
 
+STATIC void prevent_config_change(canio_can_obj_t *can) {
+    can->hw->CCCR.bit.CCE = 0;
+    can->hw->CCCR.bit.INIT = 0;
+    while (can->hw->CCCR.bit.INIT) {
+    }
+}
+
+__attribute__((unused))
+STATIC void static_assertions(void) {
+    MP_STATIC_ASSERT(CAN_GFC_ANFE_RXF0_Val + 1 == CAN_GFC_ANFE_RXF1_Val);
+    MP_STATIC_ASSERT(CAN_GFC_ANFS_RXF0_Val + 1 == CAN_GFC_ANFS_RXF1_Val);
+    MP_STATIC_ASSERT(CAN_SIDFE_0_SFEC_STF0M_Val + 1 == CAN_SIDFE_0_SFEC_STF1M_Val);
+    MP_STATIC_ASSERT(CAN_XIDFE_0_EFEC_STF0M_Val + 1 == CAN_XIDFE_0_EFEC_STF1M_Val);
+}
+
+STATIC bool single_address_filter(canio_match_obj_t *match) {
+    return match->mask == 0 || match->mask == match->address;
+}
+
+STATIC bool standard_filter_in_use(CanMramSidfe *filter) {
+    return filter->SIDFE_0.bit.SFEC != CAN_SIDFE_0_SFEC_DISABLE_Val;
+}
+
+STATIC bool extended_filter_in_use(CanMramXidfe *filter) {
+    return filter->XIDFE_0.bit.EFEC != CAN_XIDFE_0_EFEC_DISABLE_Val;
+}
+
+STATIC size_t num_filters_needed(size_t nmatch, canio_match_obj_t **matches, bool extended) {
+    size_t num_half_filters_needed = 1;
+    for(size_t i=0; i<nmatch; i++) {
+        if (extended != matches[i]->extended) {
+            continue;
+        }
+        if (single_address_filter(matches[i])) {
+            num_half_filters_needed += 1;
+        } else {
+            num_half_filters_needed += 2;
+        }
+    }
+    return num_half_filters_needed / 2;
+}
+
+STATIC size_t num_filters_available(canio_can_obj_t *can, bool extended) {
+    size_t available = 0;
+    if (extended) {
+        for(size_t i = 0; i < MP_ARRAY_SIZE(can->state->extended_rx_filter); i++) {
+            if (!extended_filter_in_use(&can->state->extended_rx_filter[i])) {
+                available++;
+            }
+        }
+    } else {
+        for(size_t i = 0; i < MP_ARRAY_SIZE(can->state->standard_rx_filter); i++) {
+            if (!standard_filter_in_use(&can->state->standard_rx_filter[i])) {
+                available++;
+            }
+        }
+    }
+    return available;
+}
+
+STATIC void clear_filters(canio_listener_obj_t *self) {
+    canio_can_obj_t *can = self->can;
+    int fifo = self->fifo_idx;
+
+    // If it was a global accept, clear it
+    allow_config_change(can);
+    if (can->hw->GFC.bit.ANFS == CAN_GFC_ANFS_RXF0 + fifo) {
+        can->hw->GFC.bit.ANFS = CAN_GFC_ANFS_REJECT_Val;
+    }
+    if (can->hw->GFC.bit.ANFE == CAN_GFC_ANFE_RXF0 + fifo) {
+        can->hw->GFC.bit.ANFE = CAN_GFC_ANFE_REJECT_Val;
+    }
+    prevent_config_change(can);
+
+    // For each filter entry, if it pointed at this FIFO set it to DISABLE
+    for(size_t i = 0; i < MP_ARRAY_SIZE(can->state->extended_rx_filter); i++) {
+        int val = CAN_XIDFE_0_EFEC_STF0M_Val + fifo;
+        if (can->state->extended_rx_filter[i].XIDFE_0.bit.EFEC == val) {
+            can->state->extended_rx_filter[i].XIDFE_0.bit.EFEC = CAN_XIDFE_0_EFEC_DISABLE_Val;
+        }
+    }
+    for(size_t i = 0; i < MP_ARRAY_SIZE(can->state->standard_rx_filter); i++) {
+        int val = CAN_SIDFE_0_SFEC_STF1M_Val + fifo;
+        if (can->state->standard_rx_filter[i].SIDFE_0.bit.SFEC == val) {
+            can->state->standard_rx_filter[i].SIDFE_0.bit.SFEC = CAN_SIDFE_0_SFEC_DISABLE_Val;
+        }
+    }
+}
+
+STATIC CanMramXidfe *next_extended_filter(canio_listener_obj_t *self, CanMramXidfe *start) {
+    CanMramXidfe *end = &self->can->state->extended_rx_filter[MP_ARRAY_SIZE(self->can->state->extended_rx_filter)];
+    if (start == NULL) {
+        start = self->can->state->extended_rx_filter;
+    } else {
+        start = start + 1;
+    }
+    while (extended_filter_in_use(start)) {
+        if (start == end) {
+            return NULL;
+        }
+        start = start + 1;
+    }
+    return start;
+}
+
+STATIC CanMramSidfe *next_standard_filter(canio_listener_obj_t *self, CanMramSidfe *start) {
+    CanMramSidfe *end = &self->can->state->standard_rx_filter[MP_ARRAY_SIZE(self->can->state->standard_rx_filter)];
+    if (start == NULL) {
+        start = self->can->state->standard_rx_filter;
+    } else {
+        start = start + 1;
+    }
+    while (standard_filter_in_use(start)) {
+        if (start == end) {
+            return NULL;
+        }
+        start = start + 1;
+    }
+    return start;
+}
+
+STATIC void install_standard_filter(CanMramSidfe *standard, int id1, int id2, int sfec, int sft) {
+    assert(standard);
+    CAN_SIDFE_0_Type val = {
+        .bit.SFID1 = id1,
+        .bit.SFID2 = id2,
+        .bit.SFEC = sfec,
+        .bit.SFT = sft,
+    };
+    standard->SIDFE_0 = val;
+}
+
+STATIC void install_extended_filter(CanMramXidfe *extended, int id1, int id2, int efec, int eft) {
+    assert(extended);
+    CAN_XIDFE_0_Type val0 = {
+        .bit.EFID1 = id1,
+        .bit.EFEC = efec,
+    };
+    CAN_XIDFE_1_Type val1 = {
+        .bit.EFID2 = id2,
+        .bit.EFT = eft,
+    };
+    // Set entry 0 second, because it has the enable bits (XIDFE_0_EFEC)
+    extended->XIDFE_1 = val1;
+    extended->XIDFE_0 = val0;
+}
+
+
+#define NO_ADDRESS (-1)
+void set_filters(canio_listener_obj_t *self, size_t nmatch, canio_match_obj_t **matches) {
+    int fifo = self->fifo_idx;
+
+    if (!nmatch) {
+        allow_config_change(self->can);
+        self->can->hw->GFC.bit.ANFS = CAN_GFC_ANFS_RXF0_Val + fifo;
+        self->can->hw->GFC.bit.ANFE = CAN_GFC_ANFE_RXF0_Val + fifo;
+        self->can->hw->CCCR.bit.CCE = 0;
+        prevent_config_change(self->can);
+        return;
+    }
+
+    CanMramSidfe *standard = next_standard_filter(self, NULL);
+    CanMramXidfe *extended = next_extended_filter(self, NULL);
+
+    int first_address = NO_ADDRESS;
+
+    // step 1: single address standard matches
+    // we have to gather up pairs and stuff them in a single filter entry
+    for(size_t i = 0; i<nmatch; i++) {
+        canio_match_obj_t *match = matches[i];
+        if (match->extended) {
+            continue;
+        }
+        if (!single_address_filter(match)) {
+            continue;
+        }
+        if (first_address != NO_ADDRESS) {
+            install_standard_filter(standard, first_address, match->address, CAN_SIDFE_0_SFEC_STF0M_Val + fifo, CAN_SIDFE_0_SFT_DUAL_Val);
+            first_address = NO_ADDRESS;
+            standard = next_standard_filter(self, standard);
+        } else {
+            first_address = match->address;
+        }
+    }
+    // step 1.5. odd single address standard match
+    if (first_address != NO_ADDRESS) {
+            install_standard_filter(standard, first_address, first_address, CAN_SIDFE_0_SFEC_STF0M_Val + fifo, CAN_SIDFE_0_SFT_DUAL_Val);
+            standard = next_standard_filter(self, standard);
+            first_address = NO_ADDRESS;
+    }
+
+    // step 2: standard mask filter
+    for(size_t i = 0; i<nmatch; i++) {
+        canio_match_obj_t *match = matches[i];
+        if (match->extended) {
+            continue;
+        }
+        if (single_address_filter(match)) {
+            continue;
+        }
+        install_standard_filter(standard, match->address, match->mask, CAN_SIDFE_0_SFEC_STF0M_Val + fifo, CAN_SIDFE_0_SFT_CLASSIC_Val);
+        standard = next_standard_filter(self, standard);
+    }
+
+    // step 3: single address extended matches
+    // we have to gather up pairs and stuff them in a single filter entry
+    for(size_t i = 0; i<nmatch; i++) {
+        canio_match_obj_t *match = matches[i];
+        if (!match->extended) {
+            continue;
+        }
+        if (!single_address_filter(match)) {
+            continue;
+        }
+        if (first_address != NO_ADDRESS) {
+            install_extended_filter(extended, first_address, match->address, CAN_XIDFE_0_EFEC_STF0M_Val + fifo, CAN_XIDFE_1_EFT_DUAL_Val);
+            first_address = NO_ADDRESS;
+            extended = next_extended_filter(self, extended);
+        } else {
+            first_address = match->address;
+        }
+    }
+    // step 3.5. odd single address standard match
+    if (first_address != NO_ADDRESS) {
+            install_extended_filter(extended, first_address, first_address, CAN_XIDFE_0_EFEC_STF0M_Val + fifo, CAN_XIDFE_1_EFT_DUAL_Val);
+            extended = next_extended_filter(self, extended);
+            first_address = NO_ADDRESS;
+    }
+
+    // step 4: extended mask filters
+    for(size_t i = 0; i<nmatch; i++) {
+        canio_match_obj_t *match = matches[i];
+        if (!match->extended) {
+            continue;
+        }
+        if (single_address_filter(match)) {
+            continue;
+        }
+        install_extended_filter(extended, match->address, match->mask, CAN_XIDFE_0_EFEC_STF0M_Val + fifo, CAN_XIDFE_1_EFT_CLASSIC_Val);
+        extended = next_extended_filter(self, extended);
+    }
+
+    // phew, easy(!)
+}
+
+
+void common_hal_canio_listener_construct(canio_listener_obj_t *self, canio_can_obj_t *can, size_t nmatch, canio_match_obj_t **matches, float timeout) {
     if (!can->fifo0_in_use) {
         self->fifo_idx = 0;
         self->fifo = can->state->rx0_fifo;
@@ -56,7 +306,26 @@ void common_hal_canio_listener_construct(canio_listener_obj_t *self, canio_can_o
         mp_raise_ValueError(translate("All RX FIFOs in use"));
     }
 
+    if (!nmatch) {
+        if (can->hw->GFC.bit.ANFS == CAN_GFC_ANFS_RXF1_Val - self->fifo_idx) {
+            mp_raise_ValueError(translate("Already have all-matches listener"));
+        }
+        if (can->hw->GFC.bit.ANFE == CAN_GFC_ANFE_RXF1_Val - self->fifo_idx) {
+            mp_raise_ValueError(translate("Already have all-matches listener"));
+        }
+    }
+
+    if (num_filters_needed(nmatch, matches, false) > num_filters_available(can, false)) {
+        mp_raise_ValueError(translate("Filters too complex"));
+    }
+
+    if (num_filters_needed(nmatch, matches, true) > num_filters_available(can, true)) {
+        mp_raise_ValueError(translate("Filters too complex"));
+    }
+
+    // Nothing can fail now so it's safe to assign self->can
     self->can = can;
+    set_filters(self, nmatch, matches);
     common_hal_canio_listener_set_timeout(self, timeout);
 }
 
@@ -90,7 +359,12 @@ bool common_hal_canio_listener_readinto(canio_listener_obj_t *self, canio_messag
     }
     int index = self->hw->RXFS.bit.F0GI;
     canio_can_fifo_t *hw_message = &self->fifo[index];
+    message->extended = hw_message->rxb0.bit.XTD;
+    if (message->extended) {
+        message->id = hw_message->rxb0.bit.ID;
+    } else {
         message->id = hw_message->rxb0.bit.ID >> 18; // short addresses are left-justified
+    }
     message->rtr = hw_message->rxb0.bit.RTR;
     message->size = hw_message->rxb1.bit.DLC;
     if (!message->rtr) {
@@ -103,6 +377,7 @@ bool common_hal_canio_listener_readinto(canio_listener_obj_t *self, canio_messag
 void common_hal_canio_listener_deinit(canio_listener_obj_t *self) {
     // free our FIFO, clear our matches, SOMETHING
     if (self->can) {
+        clear_filters(self);
         if (self->fifo_idx == 0) {
             self->can->fifo0_in_use = false;
         }

ports/atmel-samd/common-hal/_canio/__init__.h

@@ -34,6 +34,10 @@
 #define COMMON_HAL_CANIO_RX_FILTER_SIZE (4)
 #define COMMON_HAL_CANIO_TX_FIFO_SIZE (1)
 
+// This appears to be a typo (transposition error) in the ASF4 headers
+// It's called the "Extended ID Filter Entry"
+typedef CanMramXifde CanMramXidfe;
+
 typedef struct canio_listener canio_listener_t;
 typedef struct canio_can canio_can_t;
 
@@ -58,5 +62,6 @@ typedef struct {
     canio_can_fifo_t tx_fifo[COMMON_HAL_CANIO_TX_FIFO_SIZE];
     canio_can_fifo_t rx0_fifo[COMMON_HAL_CANIO_RX_FIFO_SIZE];
     canio_can_fifo_t rx1_fifo[COMMON_HAL_CANIO_RX_FIFO_SIZE];
-    canio_can_filter_t rx_filter[COMMON_HAL_CANIO_RX_FILTER_SIZE];
+    CanMramSidfe standard_rx_filter[COMMON_HAL_CANIO_RX_FILTER_SIZE];
+    CanMramXifde extended_rx_filter[COMMON_HAL_CANIO_RX_FILTER_SIZE];
 } canio_can_state_t;

shared-bindings/_canio/CAN.c

@@ -258,15 +258,20 @@ STATIC mp_obj_t canio_can_restart(mp_obj_t self_in) {
 }
 STATIC MP_DEFINE_CONST_FUN_OBJ_1(canio_can_restart_obj, canio_can_restart);
 
-//|     def listen(self, filters: Optional[Sequence[Filter]]=None, *, timeout: float=10) -> Listener:
+//|     def listen(self, match: Optional[Sequence[Match]]=None, *, timeout: float=10) -> Listener:
 //|         """Start receiving messages that match any one of the filters.
+//|
 //|         Creating a listener is an expensive operation and can interfere with reception of messages by other listeners.
+//|
 //|         There is an implementation-defined maximum number of listeners and limit to the complexity of the filters.
-//|         If the hardware cannot support all the requested filters, a ValueError is raised.  Note that generally there are some number of hardware filters shared among all fifos.
+//|
-//|         A message can be received by at most one Listener.
+//|         If the hardware cannot support all the requested matches, a ValueError is raised.  Note that generally there are some number of hardware filters shared among all fifos.
+//|
+//|         A message can be received by at most one Listener.  If more than one listener matches a message, it is undefined which one actually receives it.
+//|
 //|         An empty filter list causes all messages to be accepted.
-//|         Timeout dictates how long readinto, read and next() will block.
+//|
-//|         Readinto will return false(), read will return None, and next() will raise StopIteration."""
+//|         Timeout dictates how long readinto, read and next() will block."""
 //|         ...
 //|
 STATIC mp_obj_t canio_can_listen(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) {