Adding CAN filter management

stmhal/can.c

@@ -43,6 +43,11 @@
 
 #if MICROPY_HW_ENABLE_CAN
 
+#define MASK16 (0)
+#define LIST16 (1)
+#define MASK32 (2)
+#define LIST32 (3)
+
 /// \moduleref pyb
 /// \class CAN - controller area network communication bus
 ///
@@ -71,6 +76,8 @@ typedef struct _pyb_can_obj_t {
     CAN_HandleTypeDef can;
 } pyb_can_obj_t;
 
+STATIC uint8_t can2_start_bank = 14;
+
 // assumes Init parameters have been set up correctly
 STATIC bool can_init(pyb_can_obj_t *can_obj) {
     CAN_TypeDef *CANx = NULL;
@@ -136,6 +143,23 @@ STATIC void can_deinit(pyb_can_obj_t *can_obj) {
     }
 }
 
+STATIC void can_clearfilter(uint32_t f) {
+    CAN_FilterConfTypeDef filter;
+
+    filter.FilterIdHigh         = 0;
+    filter.FilterIdLow          = 0;
+    filter.FilterMaskIdHigh     = 0;
+    filter.FilterMaskIdLow      = 0;
+    filter.FilterFIFOAssignment = CAN_FILTER_FIFO0;
+    filter.FilterNumber         = f;
+    filter.FilterMode           = CAN_FILTERMODE_IDMASK;
+    filter.FilterScale          = CAN_FILTERSCALE_16BIT;
+    filter.FilterActivation     = DISABLE;
+    filter.BankNumber           = can2_start_bank;
+
+    HAL_CAN_ConfigFilter(NULL, &filter);
+}
+
 /******************************************************************************/
 // Micro Python bindings
 
@@ -199,20 +223,6 @@ STATIC mp_obj_t pyb_can_init_helper(pyb_can_obj_t *self, mp_uint_t n_args, const
         nlr_raise(mp_obj_new_exception_msg_varg(&mp_type_ValueError, "CAN port %d does not exist", self->can_id));
     }
 
-    // set CAN filter to accept everything
-    CAN_FilterConfTypeDef filter;
-    filter.FilterIdHigh = 0;
-    filter.FilterIdLow = 0;
-    filter.FilterMaskIdHigh = 0;
-    filter.FilterMaskIdLow = 0;
-    filter.FilterFIFOAssignment = CAN_FILTER_FIFO0;
-    filter.FilterNumber = 0; // 0-27
-    filter.FilterMode = CAN_FILTERMODE_IDMASK;
-    filter.FilterScale = CAN_FILTERSCALE_32BIT;
-    filter.FilterActivation = ENABLE;
-    filter.BankNumber = 0; // what's this for?
-    HAL_CAN_ConfigFilter(&self->can, &filter);
-
     return mp_const_none;
 }
 
@@ -399,6 +409,126 @@ STATIC mp_obj_t pyb_can_recv(mp_uint_t n_args, const mp_obj_t *pos_args, mp_map_
 }
 STATIC MP_DEFINE_CONST_FUN_OBJ_KW(pyb_can_recv_obj, 1, pyb_can_recv);
 
+/// \class method initfilterbanks
+///
+/// Set up the filterbanks. All filter will be disabled and set to their reset states.
+///
+///   - `banks` is an integer that sets how many filter banks that are reserved for CAN1.
+///     0  -> no filters assigned for CAN1
+///     28 -> all filters are assigned to CAN1
+///     CAN2 will get the rest of the 28 available.
+///
+/// Return value: none.
+STATIC mp_obj_t pyb_can_initfilterbanks(mp_obj_t self, mp_obj_t bank_in) {
+    can2_start_bank = mp_obj_get_int(bank_in);
+
+    for (int f = 0; f < 28; f++) {
+        can_clearfilter(f);
+    }
+
+    return mp_const_none;
+}
+STATIC MP_DEFINE_CONST_FUN_OBJ_2(pyb_can_initfilterbanks_fun_obj, pyb_can_initfilterbanks);
+STATIC MP_DEFINE_CONST_CLASSMETHOD_OBJ(pyb_can_initfilterbanks_obj, (const mp_obj_t)&pyb_can_initfilterbanks_fun_obj);
+
+STATIC mp_obj_t pyb_can_clearfilter(mp_obj_t self_in, mp_obj_t bank_in) {
+    pyb_can_obj_t *self = self_in;
+    mp_int_t f = mp_obj_get_int(bank_in);
+    if (self->can_id == 2) {
+        f += can2_start_bank;
+    }
+    can_clearfilter(f);
+    return mp_const_none;
+}
+STATIC MP_DEFINE_CONST_FUN_OBJ_2(pyb_can_clearfilter_obj, pyb_can_clearfilter);
+
+/// Configures a filterbank
+/// Return value: `None`.
+#define EXTENDED_ID_TO_16BIT_FILTER(id) (((id & 0xC00000) >> 13) | ((id & 0x38000) >> 15)) | 8
+STATIC mp_obj_t pyb_can_setfilter(mp_uint_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) {
+    static const mp_arg_t allowed_args[] = {
+        { MP_QSTR_bank,     MP_ARG_REQUIRED | MP_ARG_INT, {.u_int = 0} },
+        { MP_QSTR_mode,     MP_ARG_REQUIRED | MP_ARG_INT, {.u_int = 0} },
+        { MP_QSTR_fifo,     MP_ARG_REQUIRED | MP_ARG_INT, {.u_int = CAN_FILTER_FIFO0} },
+        { MP_QSTR_params,   MP_ARG_REQUIRED | MP_ARG_OBJ, {.u_obj = MP_OBJ_NULL} },
+    };
+
+    // parse args
+    pyb_can_obj_t *self = pos_args[0];
+    mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)];
+    mp_arg_parse_all(n_args - 1, pos_args + 1, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args);
+
+    mp_uint_t len;
+    mp_obj_t *params;
+    mp_obj_get_array(args[3].u_obj, &len, &params);
+
+    CAN_FilterConfTypeDef filter;
+    if (args[1].u_int == MASK16 || args[1].u_int == LIST16) {
+        if (len != 4) {
+            goto error;
+        }
+        filter.FilterScale = CAN_FILTERSCALE_16BIT;
+        if (self->extframe) {
+            filter.FilterIdLow      = EXTENDED_ID_TO_16BIT_FILTER(mp_obj_get_int(params[0])); // id1
+            filter.FilterMaskIdLow  = EXTENDED_ID_TO_16BIT_FILTER(mp_obj_get_int(params[1])); // mask1
+            filter.FilterIdHigh     = EXTENDED_ID_TO_16BIT_FILTER(mp_obj_get_int(params[2])); // id2
+            filter.FilterMaskIdHigh = EXTENDED_ID_TO_16BIT_FILTER(mp_obj_get_int(params[3])); // mask2
+        } else {
+            filter.FilterIdLow      = mp_obj_get_int(params[0]) << 5; // id1
+            filter.FilterMaskIdLow  = mp_obj_get_int(params[1]) << 5; // mask1
+            filter.FilterIdHigh     = mp_obj_get_int(params[2]) << 5; // id2
+            filter.FilterMaskIdHigh = mp_obj_get_int(params[3]) << 5; // mask2
+        }
+        if (args[1].u_int == MASK16) {
+            filter.FilterMode  = CAN_FILTERMODE_IDMASK;
+        }
+        if (args[1].u_int == LIST16) {
+            filter.FilterMode  = CAN_FILTERMODE_IDLIST;
+        }
+    }
+    else if (args[1].u_int == MASK32 || args[1].u_int == LIST32) {
+        if (len != 2) {
+            goto error;
+        }
+        filter.FilterScale      = CAN_FILTERSCALE_32BIT;
+
+        filter.FilterIdHigh     = (mp_obj_get_int(params[0]) & 0xFF00)  >> 13;
+        filter.FilterIdLow      = ((mp_obj_get_int(params[0]) & 0x00FF) << 3) | 4;
+        filter.FilterMaskIdHigh = (mp_obj_get_int(params[1]) & 0xFF00 ) >> 13;
+        filter.FilterMaskIdLow  = ((mp_obj_get_int(params[1]) & 0x00FF) << 3) | 4;
+        if (args[1].u_int == MASK32) {
+            filter.FilterMode  = CAN_FILTERMODE_IDMASK;
+        }
+        if (args[1].u_int == LIST32) {
+            filter.FilterMode  = CAN_FILTERMODE_IDLIST;
+        }
+    } else {
+        goto error;
+    }
+
+    filter.FilterFIFOAssignment = args[2].u_int; // fifo
+    filter.FilterNumber = args[0].u_int; // bank
+    if (self->can_id == 1) {
+        if (filter.FilterNumber >= can2_start_bank) {
+            goto error;
+        }
+    } else {
+        filter.FilterNumber = filter.FilterNumber + can2_start_bank;
+        if (filter.FilterNumber > 27) {
+            goto error;
+        }
+    }
+    filter.FilterActivation = ENABLE;
+    filter.BankNumber = can2_start_bank;
+    HAL_CAN_ConfigFilter(&self->can, &filter);
+
+    return mp_const_none;
+
+error:
+    nlr_raise(mp_obj_new_exception_msg_varg(&mp_type_ValueError, "CAN filter parameter error"));
+}
+STATIC MP_DEFINE_CONST_FUN_OBJ_KW(pyb_can_setfilter_obj, 1, pyb_can_setfilter);
+
 STATIC const mp_map_elem_t pyb_can_locals_dict_table[] = {
     // instance methods
     { MP_OBJ_NEW_QSTR(MP_QSTR_init), (mp_obj_t)&pyb_can_init_obj },
@@ -406,6 +536,9 @@ STATIC const mp_map_elem_t pyb_can_locals_dict_table[] = {
     { MP_OBJ_NEW_QSTR(MP_QSTR_any), (mp_obj_t)&pyb_can_any_obj },
     { MP_OBJ_NEW_QSTR(MP_QSTR_send), (mp_obj_t)&pyb_can_send_obj },
     { MP_OBJ_NEW_QSTR(MP_QSTR_recv), (mp_obj_t)&pyb_can_recv_obj },
+    { MP_OBJ_NEW_QSTR(MP_QSTR_initfilterbanks), (mp_obj_t)&pyb_can_initfilterbanks_obj },
+    { MP_OBJ_NEW_QSTR(MP_QSTR_setfilter), (mp_obj_t)&pyb_can_setfilter_obj },
+    { MP_OBJ_NEW_QSTR(MP_QSTR_clearfilter), (mp_obj_t)&pyb_can_clearfilter_obj },
 
     // class constants
     // Note: we use the ST constants >> 4 so they fit in a small-int.  The
@@ -414,6 +547,11 @@ STATIC const mp_map_elem_t pyb_can_locals_dict_table[] = {
     { MP_OBJ_NEW_QSTR(MP_QSTR_LOOPBACK), MP_OBJ_NEW_SMALL_INT(CAN_MODE_LOOPBACK >> 4) },
     { MP_OBJ_NEW_QSTR(MP_QSTR_SILENT), MP_OBJ_NEW_SMALL_INT(CAN_MODE_SILENT >> 4) },
     { MP_OBJ_NEW_QSTR(MP_QSTR_SILENT_LOOPBACK), MP_OBJ_NEW_SMALL_INT(CAN_MODE_SILENT_LOOPBACK >> 4) },
+
+    { MP_OBJ_NEW_QSTR(MP_QSTR_MASK16), MP_OBJ_NEW_SMALL_INT(MASK16) },
+    { MP_OBJ_NEW_QSTR(MP_QSTR_LIST16), MP_OBJ_NEW_SMALL_INT(LIST16) },
+    { MP_OBJ_NEW_QSTR(MP_QSTR_MASK32), MP_OBJ_NEW_SMALL_INT(MASK32) },
+    { MP_OBJ_NEW_QSTR(MP_QSTR_LIST32), MP_OBJ_NEW_SMALL_INT(LIST32) },
 };
 
 STATIC MP_DEFINE_CONST_DICT(pyb_can_locals_dict, pyb_can_locals_dict_table);

stmhal/qstrdefsport.h

@@ -160,6 +160,7 @@ Q(RTS)
 Q(CTS)
 
 // for CAN class
+#if MICROPY_HW_ENABLE_CAN
 Q(CAN)
 Q(prescaler)
 Q(init)
@@ -174,10 +175,20 @@ Q(extframe)
 Q(sjw)
 Q(bs1)
 Q(bs2)
+Q(bank)
+Q(params)
+Q(initfilterbanks)
+Q(clearfilter)
+Q(setfilter)
 Q(NORMAL)
 Q(LOOPBACK)
 Q(SILENT)
 Q(SILENT_LOOPBACK)
+Q(MASK16)
+Q(LIST16)
+Q(MASK32)
+Q(LIST32)
+#endif
 
 // for Timer class
 Q(Timer)

tests/pyb/can.py

 from pyb import CAN
 
+CAN.initfilterbanks(14)
 can = CAN(1)
 print(can)
 
@@ -7,6 +8,9 @@ can.init(CAN.LOOPBACK)
 print(can)
 print(can.any(0))
 
+# Catch all filter
+can.setfilter(0, CAN.MASK16, 0, (0, 0, 0, 0))
+
 can.send('abcd', 123)
 print(can.any(0))
 print(can.recv(0))
@@ -17,7 +21,7 @@ print(can.recv(0))
 can.send('abcd', 0x7FF + 1)
 print(can.recv(0))
 
-#Test too long message
+# Test too long message
 try:
     can.send('abcdefghi', 0x7FF)
 except ValueError:
@@ -27,14 +31,17 @@ else:
 
 del can
 
-#Testing extended IDs
+# Testing extended IDs
 can = CAN(1, CAN.LOOPBACK, extframe = True)
+# Catch all filter
+can.setfilter(0, CAN.MASK32, 0, (0, 0))
+
 print(can)
 
 try:
     can.send('abcde', 0x7FF + 1)
 except ValueError:
-        print('failed')
+    print('failed')
 else:
     r = can.recv(0)
     if r[0] == 0x7FF+1 and r[3] == b'abcde':