Fix SPI driver

src/bmi2.rs

@@ -41,17 +41,21 @@ impl<I2C, D, const N: usize> Bmi2<I2cInterface<I2C>, D, N> {
     }
 }
 
-impl<SPI, D, const N: usize> Bmi2<SpiInterface<SPI>, D, N>
+impl<SPI, D, CommE, const N: usize> Bmi2<SpiInterface<SPI>, D, N>
 where
+    SPI: embedded_hal::spi::SpiDevice<Error = CommE>,
     D: DelayNs,
 {
     /// Create a new Bmi270 device with SPI communication.
     pub fn new_spi(spi: SPI, delay: D, burst: Burst) -> Self {
-        Bmi2 {
+        let mut bmi2 = Bmi2 {
             iface: SpiInterface { spi },
             max_burst: burst.val(),
             delay,
-        }
+        };
+        let _ = bmi2.get_chip_id();
+        bmi2.delay.delay_us(450);
+        bmi2
     }
 
     /// Release I2C and CS.
@@ -736,6 +740,9 @@ where
         self.send_cmd(Cmd::SoftReset)?;
         self.delay.delay_us(2000);
 
+        let _ = self.get_chip_id();
+        self.delay.delay_us(450);
+
         // Disable advanced power mode
         self.disable_power_save()?;
 

src/bmi2_async.rs

@@ -1,4 +1,4 @@
-use embedded_hal::delay::DelayNs;
+use embedded_hal_async::delay::DelayNs;
 use fixedvec::FixedVec;
 
 use crate::registers::Registers;
@@ -41,17 +41,21 @@ impl<I2C, D, const N: usize> Bmi2<I2cInterface<I2C>, D, N> {
     }
 }
 
-impl<SPI, D, const N: usize> Bmi2<SpiInterface<SPI>, D, N>
+impl<SPI, D, CommE, const N: usize> Bmi2<SpiInterface<SPI>, D, N>
 where
+    SPI: embedded_hal_async::spi::SpiDevice<Error = CommE>,
     D: DelayNs,
 {
     /// Create a new Bmi270 device with SPI communication.
-    pub fn new_spi(spi: SPI, delay: D, burst: Burst) -> Self {
-        Bmi2 {
+    pub async fn new_spi(spi: SPI, delay: D, burst: Burst) -> Self {
+        let mut bmi2 = Bmi2 {
             iface: SpiInterface { spi },
             max_burst: burst.val(),
             delay,
-        }
+        };
+        let _ = bmi2.get_chip_id().await;
+        bmi2.delay.delay_us(450).await;
+        bmi2
     }
 
     /// Release I2C and CS.
@@ -63,7 +67,7 @@ where
 impl<I, D, CommE, const N: usize> Bmi2<I, D, N>
 where
     I: ReadData<Error = Error<CommE>> + WriteData<Error = Error<CommE>>,
-    D: embedded_hal::delay::DelayNs, // Add constraint for D
+    D: DelayNs, // Add constraint for D
 {
     /// Get the chip id.
     pub async fn get_chip_id(&mut self) -> Result<u8, Error<CommE>> {
@@ -744,7 +748,7 @@ where
         pwr_conf.power_save = false;
         self.set_pwr_conf(pwr_conf).await?;
         // Critical delay after disabling power save
-        self.delay.delay_us(450);
+        self.delay.delay_us(450).await;
         Ok(())
     }
 
@@ -754,7 +758,7 @@ where
         pwr_conf.power_save = true;
         self.set_pwr_conf(pwr_conf).await?;
         // Critical delay after enabling power save
-        self.delay.delay_us(450);
+        self.delay.delay_us(450).await;
         Ok(())
     }
 
@@ -768,7 +772,10 @@ where
 
         // Reset Chip, mandatory per datasheet
         self.send_cmd(Cmd::SoftReset).await?;
-        self.delay.delay_us(2000);
+        self.delay.delay_us(2000).await;
+
+        let _ = self.get_chip_id().await?;
+        self.delay.delay_us(450).await;
 
         // Disable advanced power mode
         self.disable_power_save().await?;
@@ -792,7 +799,7 @@ where
 
         let init_ctrl = self.get_init_ctrl().await?;
         self.set_init_ctrl(init_ctrl & 0b1111_1110).await?;
-        self.delay.delay_us(450);
+        self.delay.delay_us(450).await;
 
         while offset < max_len {
             // INIT_ADDR should point to 16-bit words
@@ -822,17 +829,17 @@ where
             self.iface.write(vec.as_mut_slice()).await?;
 
             offset += chunk_size;
-            self.delay.delay_us(2);
+            self.delay.delay_us(2).await;
         }
 
         // This operation must not be performed more than once after POR or soft reset.
         self.set_init_ctrl(1).await?;
-        self.delay.delay_us(2);
+        self.delay.delay_us(2).await;
 
         self.enable_power_save().await?;
 
         // Initialization takes at most 20ms per datasheet
-        self.delay.delay_us(20_000);
+        self.delay.delay_us(20_000).await;
 
         let internal_status = self.iface.read_reg(Registers::INTERNAL_STATUS).await?;
 

src/interface.rs

@@ -37,15 +37,12 @@ where
 {
     type Error = Error<CommE>;
     fn write(&mut self, payload: &mut [u8]) -> Result<(), Self::Error> {
-        payload[0] += 0x80;
-
         // `write` asserts and deasserts CS for us. No need to do it manually!
-
         self.spi.write(payload).map_err(Error::Comm)
     }
 
     fn write_reg(&mut self, register: u8, data: u8) -> Result<(), Self::Error> {
-        let payload: [u8; 2] = [register + 0x80, data];
+        let payload: [u8; 2] = [register, data];
 
         // `write` asserts and deasserts CS for us. No need to do it manually!
 
@@ -87,13 +84,13 @@ where
     }
 
     fn read_reg(&mut self, register: u8) -> Result<u8, Self::Error> {
-        let mut payload = [register, 0];
+        let mut payload = [register + 0x80, 0, 0];
 
         // `read` asserts and deasserts CS for us. No need to do it manually!
-        let res = self.spi.read(&mut payload).map_err(Error::Comm);
+        let res = self.spi.transfer_in_place(&mut payload).map_err(Error::Comm);
 
         match res {
-            Ok(_) => Ok(payload[1]),
+            Ok(_) => Ok(payload[2]),
             Err(e) => Err(e),
         }
     }

src/interface_async.rs

@@ -1,6 +1,7 @@
 pub use crate::interface_common::{I2cAddr, I2cInterface, SpiInterface};
 
 use crate::types::Error;
+use embedded_hal::spi::Operation;
 use embedded_hal_async::i2c::I2c;
 use embedded_hal_async::spi::SpiDevice;
 
@@ -45,15 +46,12 @@ where
 {
     type Error = Error<CommE>;
     async fn write(&mut self, payload: &mut [u8]) -> Result<(), Self::Error> {
-        payload[0] += 0x80;
-
         // `write` asserts and deasserts CS for us. No need to do it manually!
-
         self.spi.write(payload).await.map_err(Error::Comm)
     }
 
     async fn write_reg(&mut self, register: u8, data: u8) -> Result<(), Self::Error> {
-        let payload: [u8; 2] = [register + 0x80, data];
+        let payload: [u8; 2] = [register, data];
 
         // `write` asserts and deasserts CS for us. No need to do it manually!
 
@@ -89,21 +87,32 @@ where
 {
     type Error = Error<CommE>;
     async fn read(&mut self, payload: &mut [u8]) -> Result<(), Self::Error> {
-        // `read` asserts and deasserts CS for us. No need to do it manually!
-        let res = self.spi.read(payload).await.map_err(Error::Comm);
+        if payload.is_empty() {
+            return Ok(());
+        }
+        let addr = payload[0] | 0x80;
+        let mut dummy = [0u8; 1];
+
+        self.spi
+            .transaction(&mut [
+                Operation::Write(&[addr]),          // send address with R bit
+                Operation::Read(&mut dummy),        // consume 1 dummy byte
+                Operation::Read(&mut payload[1..]), // read real data directly into caller buffer
+            ])
+            .await
+            .map_err(Error::Comm)?;
 
-        res?;
         Ok(())
     }
 
     async fn read_reg(&mut self, register: u8) -> Result<u8, Self::Error> {
-        let mut payload = [register, 0];
+        let mut payload = [register + 0x80, 00, 00];
 
         // `read` asserts and deasserts CS for us. No need to do it manually!
-        let res = self.spi.read(&mut payload).await.map_err(Error::Comm);
+        let res = self.spi.transfer_in_place(&mut payload).await.map_err(Error::Comm);
 
         match res {
-            Ok(_) => Ok(payload[1]),
+            Ok(_) => Ok(payload[2]),
             Err(e) => Err(e),
         }
     }