fix 16bit offset

Author: Paval-from-Belarus

src/can/can.rs

@@ -57,7 +57,7 @@ impl<'d, T: Instance> Can<'d, T> {
         let Some(bit_timings) = util::calc_can_timings(T::frequency().0, bitrate) else {
             return Err(CanInitError::InvalidTimings);
         };
-        // .expect("Bit timing parameters weren't satisfied for CAN clock rate and desired bitrate.");
+
         Registers(T::regs()).set_bit_timing_and_mode(bit_timings, mode);
 
         Registers(T::regs()).leave_init_mode(); // Exit CAN initialization mode

src/can/filter/bit16.rs

@@ -11,8 +11,8 @@ pub struct Bit16MaskReg<'a> {
 
 impl Bit16IdReg<'_> {
     pub fn set(&mut self, id: StandardId, opts: FilterOptions) {
-        let bits = (id.as_raw() << 3) | ((opts.rtr_enabled as u16) << 2);
-        *self.0 &= 0x0000;
+        let bits = (id.as_raw() << 5) | ((opts.rtr_enabled as u16) << 4);
+        *self.0 = 0x0000;
         *self.0 |= bits;
     }
 }
@@ -22,8 +22,8 @@ impl Bit16MaskReg<'_> {
         *self.mask &= 0xFFFF;
         *self.id &= 0xFFFF;
 
-        *self.mask |= (id << 3) | ((opts.rtr_enabled as u16) << 2);
-        *self.id |= (mask << 3) | ((opts.rtr_enabled as u16) << 2);
+        *self.mask |= (id << 5) | ((opts.rtr_enabled as u16) << 4);
+        *self.id |= (mask << 5) | ((opts.rtr_enabled as u16) << 4);
     }
 }
 

src/can/filter/bit32.rs

@@ -13,28 +13,17 @@ impl<MODE: FilterMode> CanFilter<Bit32Mode, MODE> {
             id_value: 0,
             mode: self.mode,
             bank: self.bank,
-            bit_mode: PhantomData,
+            bit_mode: Bit16Mode,
         }
     }
 }
 
 impl CanFilter<Bit32Mode, ListMode> {
-    /// Creates a filter that accepts all frames
-    pub fn accept_all() -> Self {
-        CanFilter {
-            bank: 0,
-            mode: ListMode,
-            id_value: 0,
-            id_mask: 0,
-            bit_mode: PhantomData,
-        }
-    }
-
     /// Create a filter to configure id list
     pub fn new_id_list() -> Self {
         Self {
             bank: 0,
-            bit_mode: PhantomData,
+            bit_mode: Bit32Mode,
             mode: ListMode,
 
             id_mask: 0,
@@ -44,10 +33,21 @@ impl CanFilter<Bit32Mode, ListMode> {
 }
 
 impl CanFilter<Bit32Mode, MaskMode> {
+    /// Creates a filter that accepts all frames
+    pub fn accept_all() -> Self {
+        CanFilter {
+            bank: 0,
+            mode: MaskMode,
+            id_value: 0,
+            id_mask: 0,
+            bit_mode: Bit32Mode,
+        }
+    }
+
     pub fn new_id_mask() -> Self {
         Self {
             bank: 0,
-            bit_mode: PhantomData,
+            bit_mode: Bit32Mode,
             mode: MaskMode,
 
             id_mask: 0,

src/can/filter/mod.rs

@@ -24,13 +24,24 @@ impl FilterMode for ListMode {
     }
 }
 
-pub trait BitMode {}
+pub trait BitMode {
+    fn val_bool(&self) -> bool;
+}
 
 pub struct Bit16Mode;
 pub struct Bit32Mode;
 
-impl BitMode for Bit16Mode {}
-impl BitMode for Bit32Mode {}
+impl BitMode for Bit16Mode {
+    fn val_bool(&self) -> bool {
+        false
+    }
+}
+
+impl BitMode for Bit32Mode {
+    fn val_bool(&self) -> bool {
+        true
+    }
+}
 
 /// See table 24-1 of the reference manual for more details on filtering and modes.
 /// Each filter is applied for only one bank and for one register on it bank
@@ -43,7 +54,7 @@ pub struct CanFilter<BIT: BitMode, MODE: FilterMode> {
     /// Bit mask to be applied to incoming message before comparing it to a predefined value.
     /// In IdList mode, this is used in the same way as `id_value` is.
     pub id_mask: u32,
-    pub bit_mode: PhantomData<BIT>,
+    pub bit_mode: BIT,
     pub mode: MODE,
 }
 

src/can/registers.rs

@@ -53,7 +53,7 @@ impl Registers {
     ) {
         self.0.fctlr().modify(|w| w.set_finit(true)); // Enable filter init mode
         self.0.fwr().modify(|w| w.set_fact(filter.bank, true)); // Activate new filter in filter bank
-        self.0.fscfgr().modify(|w| w.set_fsc(filter.bank, true)); // Set filter scale config to single 32-bit (16-bit not implemented)
+        self.0.fscfgr().modify(|w| w.set_fsc(filter.bank, filter.bit_mode.val_bool())); // Set filter scale config (32bit or 16bit mode) 
         self.0
             .fr(filter.fr_id_value_reg())
             .write_value(crate::pac::can::regs::Fr(filter.id_value)); // Set filter's id value to match/mask
@@ -138,6 +138,11 @@ impl Registers {
         self.0.rfifo(fifo.val()).read().fmp() != 0
     }
 
+    pub fn reset_fifo(&self, fifo: &super::CanFifo) {
+        self.0.rfifo(fifo.val()).modify(|w| w.set_fmp(0));
+        self.0.rfifo(fifo.val()).modify(|w| w.set_fovr(false));
+    }
+
     pub fn read_frame_fifo(&self, fifo: &super::CanFifo) -> super::frame::CanFrame {
         let dlc = self.0.rxmdtr(fifo.val()).read().dlc() as usize;
         let raw_id = self.0.rxmir(fifo.val()).read().stid();