diff --git a/stmhal/can.c b/stmhal/can.c
index eb52e982ff..005bd9e943 100644
--- a/stmhal/can.c
+++ b/stmhal/can.c
@@ -688,10 +688,10 @@ STATIC mp_obj_t pyb_can_setfilter(mp_uint_t n_args, const mp_obj_t *pos_args, mp
                 rtr_masks[1] = mp_obj_get_int(rtr_flags[1]) ? 0x02 : 0;
             }
         }
-        filter.FilterIdHigh     = (mp_obj_get_int(params[0]) & 0xFF00)  >> 13;
-        filter.FilterIdLow      = (((mp_obj_get_int(params[0]) & 0x00FF) << 3) | 4) | rtr_masks[0];
-        filter.FilterMaskIdHigh = (mp_obj_get_int(params[1]) & 0xFF00 ) >> 13;
-        filter.FilterMaskIdLow  = (((mp_obj_get_int(params[1]) & 0x00FF) << 3) | 4) | rtr_masks[1];
+        filter.FilterIdHigh     = (mp_obj_get_int(params[0]) & 0x1FFFE000)  >> 13;
+        filter.FilterIdLow      = (((mp_obj_get_int(params[0]) & 0x00001FFF) << 3) | 4) | rtr_masks[0];
+        filter.FilterMaskIdHigh = (mp_obj_get_int(params[1]) & 0x1FFFE000 ) >> 13;
+        filter.FilterMaskIdLow  = (((mp_obj_get_int(params[1]) & 0x00001FFF) << 3) | 4) | rtr_masks[1];
         if (args[1].u_int == MASK32) {
             filter.FilterMode  = CAN_FILTERMODE_IDMASK;
         }
diff --git a/tests/pyb/can.py b/tests/pyb/can.py
index 8c86349ea9..cf8089a3bd 100644
--- a/tests/pyb/can.py
+++ b/tests/pyb/can.py
@@ -58,6 +58,26 @@ else:
     else:
         print('failed, wrong data received')
 
+# Test filters
+for n in [0, 8, 16, 24]:
+    filter_id = 0b00001000 << n
+    filter_mask = 0b00011100 << n
+    id_ok = 0b00001010 << n
+    id_fail = 0b00011010 << n
+
+    can.clearfilter(0)
+    can.setfilter(0, pyb.CAN.MASK32, 0, (filter_id, filter_mask))
+
+    can.send('ok', id_ok, timeout=3)
+    if can.any(0):
+        msg = can.recv(0)
+        print((hex(filter_id), hex(filter_mask), hex(msg[0]), msg[3]))
+
+    can.send("fail", id_fail, timeout=3)
+    if can.any(0):
+        msg = can.recv(0)
+        print((hex(filter_id), hex(filter_mask), hex(msg[0]), msg[3]))
+
 del can
 
 # Test RxCallbacks
diff --git a/tests/pyb/can.py.exp b/tests/pyb/can.py.exp
index 9ec97bfa36..e25a0f406e 100644
--- a/tests/pyb/can.py.exp
+++ b/tests/pyb/can.py.exp
@@ -16,6 +16,10 @@ True
 passed
 CAN(1, CAN.LOOPBACK, extframe=True)
 passed
+('0x8', '0x1c', '0xa', b'ok')
+('0x800', '0x1c00', '0xa00', b'ok')
+('0x80000', '0x1c0000', '0xa0000', b'ok')
+('0x8000000', '0x1c000000', '0xa000000', b'ok')
 cb0
 pending
 cb0