diff --git a/hal/src/stm32f2xx/can_hal.cpp b/hal/src/stm32f2xx/can_hal.cpp
index 2489559263..c09dcfbb6b 100644
--- a/hal/src/stm32f2xx/can_hal.cpp
+++ b/hal/src/stm32f2xx/can_hal.cpp
@@ -760,11 +760,22 @@ bool CANDriver::addFilter(uint32_t id, uint32_t mask, HAL_CAN_Filters type)
         return false;
     }
 
-    // Filter configuration - Register organization
-    // Lowest 11 bits of id are first, then next 18 bits of id, then
-    // standard/extended bit, then RTR bit, then 1 zero bit
-    uint32_t filterId = (id << 21) | ((id >> 8) & 0x1FFFF8) | (type == CAN_FILTER_EXTENDED ? 0x4 : 0);
-    uint32_t filterMask = (mask << 21) | ((mask >> 8) & 0x1FFFF8) | 0x6;
+    // Filter configuration
+    // See STM32F2 Reference Manual for register organization
+    uint32_t filterId, filterMask;
+    const uint32_t extendedBit = 0x4;
+    const uint32_t rtrBit = 0x2;
+
+    if(type == CAN_FILTER_STANDARD)
+    {
+        filterId = id << 21;
+        filterMask = (mask << 21) | extendedBit | rtrBit;
+    }
+    else
+    {
+        filterId = (id << 3) | extendedBit;
+        filterMask = (mask << 3) | extendedBit | rtrBit;
+    }
 
     CAN_FilterInitTypeDef   CAN_FilterInitStructure = {};
     CAN_FilterInitStructure.CAN_FilterNumber         = nextFilter;
diff --git a/user/tests/wiring/no_fixture/can.cpp b/user/tests/wiring/no_fixture/can.cpp
index 1a2a95f139..0a1ae0a2a3 100644
--- a/user/tests/wiring/no_fixture/can.cpp
+++ b/user/tests/wiring/no_fixture/can.cpp
@@ -28,7 +28,7 @@
 
 #ifdef HAL_HAS_CAN_D1_D2
 
-test(CAN_D1D2_ReceivesTransmittedMessage) {
+test(CAN_D1_D2_ReceivesTransmittedMessage) {
     CANChannel can(CAN_D1_D2);
     can.begin(500000, CAN_TEST_MODE);
 
@@ -48,10 +48,29 @@ test(CAN_D1D2_ReceivesTransmittedMessage) {
     assertEqual(rx.data[0], 10);
 }
 
-test(CAN_D1D2_DoesntReceiveFilteredMessage) {
+test(CAN_D1_D2_ReceivesAcceptedMessage) {
     CANChannel can(CAN_D1_D2);
     can.begin(500000, CAN_TEST_MODE);
-    // Accept only message 0x100
+    // Accept only standard message 0x100
+    can.addFilter(0x100, 0x7FF);
+
+    CANMessage tx;
+    tx.id = 0x100;
+    tx.len = 1;
+    tx.data[0] = 10;
+    can.transmit(tx);
+    delay(1);
+    CANMessage rx;
+    bool hasMessage = can.receive(rx);
+    can.end();
+
+    assertEqual(hasMessage, true);
+}
+
+test(CAN_D1_D2_DoesntReceiveFilteredMessage) {
+    CANChannel can(CAN_D1_D2);
+    can.begin(500000, CAN_TEST_MODE);
+    // Accept only standard message 0x100
     can.addFilter(0x100, 0x7FF);
 
     CANMessage tx;
@@ -67,11 +86,73 @@ test(CAN_D1D2_DoesntReceiveFilteredMessage) {
     assertEqual(hasMessage, false);
 }
 
+test(CAN_D1_D2_ReceivesTransmittedExtendedMessage) {
+    CANChannel can(CAN_D1_D2);
+    can.begin(500000, CAN_TEST_MODE);
+
+    CANMessage tx;
+    tx.extended = true;
+    tx.id = 0x3000;
+    tx.len = 1;
+    tx.data[0] = 10;
+    can.transmit(tx);
+    delay(1);
+    CANMessage rx;
+    bool hasMessage = can.receive(rx);
+    can.end();
+
+    assertEqual(hasMessage, true);
+    assertEqual(rx.extended, true);
+    assertEqual(rx.id, 0x3000);
+    assertEqual(rx.len, 1);
+    assertEqual(rx.data[0], 10);
+}
+
+test(CAN_D1_D2_ReceivesAcceptedExtendedMessage) {
+    CANChannel can(CAN_D1_D2);
+    can.begin(500000, CAN_TEST_MODE);
+    // Accept only extended message 0x3000
+    can.addFilter(0x3000, 0x7FF, CAN_FILTER_EXTENDED);
+
+    CANMessage tx;
+    tx.extended = true;
+    tx.id = 0x3000;
+    tx.len = 1;
+    tx.data[0] = 10;
+    can.transmit(tx);
+    delay(1);
+    CANMessage rx;
+    bool hasMessage = can.receive(rx);
+    can.end();
+
+    assertEqual(hasMessage, true);
+}
+
+test(CAN_D1_D2_DoesntReceiveFilteredExtendedMessage) {
+    CANChannel can(CAN_D1_D2);
+    can.begin(500000, CAN_TEST_MODE);
+    // Accept only extended message 0x3000
+    can.addFilter(0x3000, 0x7FF, CAN_FILTER_EXTENDED);
+
+    CANMessage tx;
+    tx.extended = true;
+    tx.id = 0x3001;
+    tx.len = 1;
+    tx.data[0] = 10;
+    can.transmit(tx);
+    delay(1);
+    CANMessage rx;
+    bool hasMessage = can.receive(rx);
+    can.end();
+
+    assertEqual(hasMessage, false);
+}
+
 #endif // HAL_HAS_CAN_D1_D2
 
 #ifdef HAL_HAS_CAN_C4_C5
 
-test(CAN_C4C5_ReceivesTransmittedMessage) {
+test(CAN_C4_C5_ReceivesTransmittedMessage) {
     CANChannel can(CAN_C4_C5);
     can.begin(500000, CAN_TEST_MODE);
 
@@ -91,10 +172,29 @@ test(CAN_C4C5_ReceivesTransmittedMessage) {
     assertEqual(rx.data[0], 10);
 }
 
-test(CAN_C4C5_DoesntReceiveFilteredMessage) {
+test(CAN_C4_C5_ReceivesAcceptedMessage) {
     CANChannel can(CAN_C4_C5);
     can.begin(500000, CAN_TEST_MODE);
-    // Accept only message 0x100
+    // Accept only standard message 0x100
+    can.addFilter(0x100, 0x7FF);
+
+    CANMessage tx;
+    tx.id = 0x100;
+    tx.len = 1;
+    tx.data[0] = 10;
+    can.transmit(tx);
+    delay(1);
+    CANMessage rx;
+    bool hasMessage = can.receive(rx);
+    can.end();
+
+    assertEqual(hasMessage, true);
+}
+
+test(CAN_C4_C5_DoesntReceiveFilteredMessage) {
+    CANChannel can(CAN_C4_C5);
+    can.begin(500000, CAN_TEST_MODE);
+    // Accept only standard message 0x100
     can.addFilter(0x100, 0x7FF);
 
     CANMessage tx;
@@ -110,4 +210,66 @@ test(CAN_C4C5_DoesntReceiveFilteredMessage) {
     assertEqual(hasMessage, false);
 }
 
+test(CAN_C4_C5_ReceivesTransmittedExtendedMessage) {
+    CANChannel can(CAN_C4_C5);
+    can.begin(500000, CAN_TEST_MODE);
+
+    CANMessage tx;
+    tx.extended = true;
+    tx.id = 0x3000;
+    tx.len = 1;
+    tx.data[0] = 10;
+    can.transmit(tx);
+    delay(1);
+    CANMessage rx;
+    bool hasMessage = can.receive(rx);
+    can.end();
+
+    assertEqual(hasMessage, true);
+    assertEqual(rx.extended, true);
+    assertEqual(rx.id, 0x3000);
+    assertEqual(rx.len, 1);
+    assertEqual(rx.data[0], 10);
+}
+
+test(CAN_C4_C5_ReceivesAcceptedExtendedMessage) {
+    CANChannel can(CAN_C4_C5);
+    can.begin(500000, CAN_TEST_MODE);
+    // Accept only extended message 0x3000
+    can.addFilter(0x3000, 0x7FF, CAN_FILTER_EXTENDED);
+
+    CANMessage tx;
+    tx.extended = true;
+    tx.id = 0x3000;
+    tx.len = 1;
+    tx.data[0] = 10;
+    can.transmit(tx);
+    delay(1);
+    CANMessage rx;
+    bool hasMessage = can.receive(rx);
+    can.end();
+
+    assertEqual(hasMessage, true);
+}
+
+test(CAN_C4_C5_DoesntReceiveFilteredExtendedMessage) {
+    CANChannel can(CAN_C4_C5);
+    can.begin(500000, CAN_TEST_MODE);
+    // Accept only extended message 0x3000
+    can.addFilter(0x3000, 0x7FF, CAN_FILTER_EXTENDED);
+
+    CANMessage tx;
+    tx.extended = true;
+    tx.id = 0x3001;
+    tx.len = 1;
+    tx.data[0] = 10;
+    can.transmit(tx);
+    delay(1);
+    CANMessage rx;
+    bool hasMessage = can.receive(rx);
+    can.end();
+
+    assertEqual(hasMessage, false);
+}
+
 #endif // HAL_HAS_CAN_C4_C5