SPI improvements

va108xx/bootloader/src/lib.rs

@@ -1,10 +1,8 @@
 #![no_std]
 
-use core::convert::Infallible;
-
 /// Simple trait which makes swapping the NVM easier. NVMs only need to implement this interface.
 pub trait NvmInterface {
-    fn write(&mut self, address: usize, data: &[u8]) -> Result<(), Infallible>;
-    fn read(&mut self, address: usize, buf: &mut [u8]) -> Result<(), Infallible>;
-    fn verify(&mut self, address: usize, data: &[u8]) -> Result<bool, Infallible>;
+    fn write(&mut self, address: usize, data: &[u8]);
+    fn read(&mut self, address: usize, buf: &mut [u8]);
+    fn verify(&mut self, address: usize, data: &[u8]) -> bool;
 }

va108xx/bootloader/src/main.rs

@@ -84,15 +84,15 @@ pub struct NvmWrapper(pub M95M01);
 
 // Newtype pattern. We could now more easily swap the used NVM type.
 impl NvmInterface for NvmWrapper {
-    fn write(&mut self, address: usize, data: &[u8]) -> Result<(), core::convert::Infallible> {
+    fn write(&mut self, address: usize, data: &[u8]) {
         self.0.write(address, data)
     }
 
-    fn read(&mut self, address: usize, buf: &mut [u8]) -> Result<(), core::convert::Infallible> {
+    fn read(&mut self, address: usize, buf: &mut [u8]) {
         self.0.read(address, buf)
     }
 
-    fn verify(&mut self, address: usize, data: &[u8]) -> Result<bool, core::convert::Infallible> {
+    fn verify(&mut self, address: usize, data: &[u8]) -> bool {
         self.0.verify(address, data)
     }
 }
@@ -125,30 +125,20 @@ fn main() -> ! {
         digest.update(bootloader_data);
         let bootloader_crc = digest.finalize();
 
-        nvm.write(0x0, &first_four_bytes)
-            .expect("writing to NVM failed");
-        nvm.write(0x4, bootloader_data)
-            .expect("writing to NVM failed");
-        if let Err(e) = nvm.verify(0x0, &first_four_bytes) {
-            if DEFMT_PRINTOUT {
-                defmt::error!("verification of self-flash to NVM failed: {:?}", e);
-            }
+        nvm.write(0x0, &first_four_bytes);
+        nvm.write(0x4, bootloader_data);
+        if !nvm.verify(0x0, &first_four_bytes) && DEFMT_PRINTOUT {
+            defmt::error!("verification of self-flash to NVM failed");
         }
-        if let Err(e) = nvm.verify(0x4, bootloader_data) {
-            if DEFMT_PRINTOUT {
-                defmt::error!("verification of self-flash to NVM failed: {:?}", e);
-            }
+        if !nvm.verify(0x4, bootloader_data) && DEFMT_PRINTOUT {
+            defmt::error!("verification of self-flash to NVM failed");
         }
 
-        nvm.write(BOOTLOADER_CRC_ADDR as usize, &bootloader_crc.to_be_bytes())
-            .expect("writing CRC failed");
-        if let Err(e) = nvm.verify(BOOTLOADER_CRC_ADDR as usize, &bootloader_crc.to_be_bytes()) {
-            if DEFMT_PRINTOUT {
-                defmt::error!(
-                    "error: CRC verification for bootloader self-flash failed: {:?}",
-                    e
-                );
-            }
+        nvm.write(BOOTLOADER_CRC_ADDR as usize, &bootloader_crc.to_be_bytes());
+        if !nvm.verify(BOOTLOADER_CRC_ADDR as usize, &bootloader_crc.to_be_bytes())
+            && DEFMT_PRINTOUT
+        {
+            defmt::error!("error: CRC verification for bootloader self-flash failed",);
         }
     }
 
@@ -158,8 +148,7 @@ fn main() -> ! {
     check_own_crc(&dp.sysconfig, &cp, &mut nvm, &mut timer);
 
     let mut preferred_app_raw = [0; 1];
-    nvm.read(PREFERRED_SLOT_OFFSET as usize, &mut preferred_app_raw)
-        .expect("reading preferred slot failed");
+    nvm.read(PREFERRED_SLOT_OFFSET as usize, &mut preferred_app_raw);
     let preferred_app = AppSel::try_from(preferred_app_raw[0]).unwrap_or(AppSel::A);
     let other_app = if preferred_app == AppSel::A {
         AppSel::B
@@ -207,8 +196,7 @@ fn check_own_crc(
             defmt::info!("BL CRC blank - prog new CRC");
         }
         // Blank CRC, write it to NVM.
-        nvm.write(BOOTLOADER_CRC_ADDR as usize, &crc_calc.to_be_bytes())
-            .expect("writing CRC failed");
+        nvm.write(BOOTLOADER_CRC_ADDR as usize, &crc_calc.to_be_bytes());
         // The Vorago bootloader resets here. I am not sure why this is done but I think it is
         // necessary because somehow the boot will not work if we just continue as usual.
         // cortex_m::peripheral::SCB::sys_reset();

va108xx/examples/simple/examples/spi.rs

@@ -4,7 +4,7 @@
 use cortex_m_rt::entry;
 use embedded_hal::{
     delay::DelayNs,
-    spi::{Mode, SpiBus, MODE_0},
+    spi::{Mode, MODE_0},
 };
 // Import panic provider.
 use panic_probe as _;
@@ -107,14 +107,14 @@ fn main() -> ! {
     loop {
         let mut reply_buf: [u8; 8] = [0; 8];
         // Can't really verify correct reply here.
-        spi.write(&[0x42]).expect("write failed");
+        spi.write(&[0x42]);
         // Because of the loopback mode, we should get back the fill word here.
-        spi.read(&mut reply_buf[0..1]).unwrap();
+        spi.read(&mut reply_buf[0..1]);
         assert_eq!(reply_buf[0], FILL_WORD);
         delay.delay_ms(500_u32);
 
         let tx_buf: [u8; 3] = [0x01, 0x02, 0x03];
-        spi.transfer(&mut reply_buf[0..3], &tx_buf).unwrap();
+        spi.transfer(&mut reply_buf[0..3], &tx_buf);
         assert_eq!(tx_buf, reply_buf[0..3]);
         defmt::info!(
             "Received reply: {}, {}, {}",
@@ -125,7 +125,7 @@ fn main() -> ! {
         delay.delay_ms(500_u32);
 
         let mut tx_rx_buf: [u8; 3] = [0x03, 0x02, 0x01];
-        spi.transfer_in_place(&mut tx_rx_buf).unwrap();
+        spi.transfer_in_place(&mut tx_rx_buf);
         defmt::info!(
             "Received reply: {}, {}, {}",
             tx_rx_buf[0],

va108xx/flashloader/src/main.rs

@@ -299,15 +299,9 @@ mod app {
         if pus_tc.service_type_id() == PusServiceId::Action as u8 {
             let mut corrupt_image = |base_addr: u32| {
                 let mut buf = [0u8; 4];
-                cx.local
-                    .nvm
-                    .read(base_addr as usize + 32, &mut buf)
-                    .expect("reading from NVM failed");
+                cx.local.nvm.read(base_addr as usize + 32, &mut buf);
                 buf[0] += 1;
-                cx.local
-                    .nvm
-                    .write(base_addr as usize + 32, &buf)
-                    .expect("writing to NVM failed");
+                cx.local.nvm.write(base_addr as usize + 32, &buf);
                 let tm = cx
                     .local
                     .verif_reporter
@@ -337,8 +331,7 @@ mod app {
                 );
                 cx.local
                     .nvm
-                    .write(PREFERRED_SLOT_OFFSET as usize, &[pus_tc.app_data()[0]])
-                    .expect("writing to NVM failed");
+                    .write(PREFERRED_SLOT_OFFSET as usize, &[pus_tc.app_data()[0]]);
                 let tm = cx
                     .local
                     .verif_reporter
@@ -397,16 +390,8 @@ mod app {
                 }
                 let data = &app_data[10..10 + data_len as usize];
                 defmt::info!("writing {} bytes at offset {} to NVM", data_len, offset);
-                cx.local
-                    .nvm
-                    .write(offset as usize, data)
-                    .expect("writing to NVM failed");
-                let tm = if !cx
-                    .local
-                    .nvm
-                    .verify(offset as usize, data)
-                    .expect("NVM verification failed")
-                {
+                cx.local.nvm.write(offset as usize, data);
+                let tm = if !cx.local.nvm.verify(offset as usize, data) {
                     defmt::warn!("verification of data written to NVM failed");
                     cx.local
                         .verif_reporter

va108xx/vorago-reb1/examples/adxl343-accelerometer.rs

@@ -6,7 +6,7 @@
 #![no_std]
 use cortex_m_rt::entry;
 use embedded_hal::delay::DelayNs;
-use embedded_hal::spi::{SpiBus, MODE_3};
+use embedded_hal::spi::MODE_3;
 use panic_rtt_target as _;
 use rtt_target::{rprintln, rtt_init_print};
 use va108xx_hal::gpio::{Output, PinState};
@@ -51,14 +51,12 @@ fn main() -> ! {
 
     let mut tx_rx_buf: [u8; 3] = [0; 3];
     tx_rx_buf[0] = READ_MASK | DEVID_REG;
-    spi.transfer_in_place(&mut tx_rx_buf[0..2])
-        .expect("Reading DEVID register failed");
+    spi.transfer_in_place(&mut tx_rx_buf[0..2]);
     rprintln!("DEVID register: {}", tx_rx_buf[1]);
 
     tx_rx_buf[0] = POWER_CTL_REG;
     tx_rx_buf[1] = PWR_MEASUREMENT_MODE_MASK;
-    spi.write(&tx_rx_buf[0..2])
-        .expect("Enabling measurement mode failed");
+    spi.write(&tx_rx_buf[0..2]);
 
     loop {
         delay.delay_ms(500);

va108xx/vorago-reb1/examples/max11619-adc.rs

@@ -8,7 +8,7 @@
 use core::convert::Infallible;
 
 use cortex_m_rt::entry;
-use embedded_hal::spi::{SpiBus, SpiDevice, MODE_0};
+use embedded_hal::spi::{SpiDevice, MODE_0};
 use embedded_hal::{delay::DelayNs, spi};
 use max116xx_10bit::VoltageRefMode;
 use max116xx_10bit::{AveragingConversions, AveragingResults};
@@ -86,14 +86,10 @@ impl<Delay: DelayNs> SpiDevice for SpiWithHwCs<Delay> {
         self.inner.cfg_hw_cs(self.hw_cs_id);
         for operation in operations {
             match operation {
-                spi::Operation::Read(buf) => self.inner.read(buf).ok().unwrap(),
-                spi::Operation::Write(buf) => self.inner.write(buf).ok().unwrap(),
-                spi::Operation::Transfer(read, write) => {
-                    self.inner.transfer(read, write).ok().unwrap()
-                }
-                spi::Operation::TransferInPlace(buf) => {
-                    self.inner.transfer_in_place(buf).ok().unwrap()
-                }
+                spi::Operation::Read(buf) => self.inner.read(buf),
+                spi::Operation::Write(buf) => self.inner.write(buf),
+                spi::Operation::Transfer(read, write) => self.inner.transfer(read, write),
+                spi::Operation::TransferInPlace(buf) => self.inner.transfer_in_place(buf),
                 spi::Operation::DelayNs(delay) => self.delay_provider.delay_ns(*delay),
             };
         }

va108xx/vorago-reb1/examples/nvm.rs

@@ -20,7 +20,7 @@ fn main() -> ! {
     let mut delay = CountdownTimer::new(dp.tim0, CLOCK_FREQ);
     let clk_config = SpiClockConfig::new(2, 4);
     let mut nvm = M95M01::new(dp.spic, clk_config);
-    let status_reg = nvm.read_status_reg().expect("reading status reg failed");
+    let status_reg = nvm.read_status_reg();
     if status_reg.zero_segment().value() == 0b111 {
         panic!("status register unexpected values");
     }
@@ -31,26 +31,26 @@ fn main() -> ! {
     for (idx, val) in write_buf.iter_mut().enumerate() {
         *val = ((idx as u16) % (u8::MAX as u16 + 1)) as u8;
     }
-    nvm.read(0, &mut orig_content).unwrap();
+    nvm.read(0, &mut orig_content);
 
     nvm.write_page(0, 0, &[1, 2, 3, 4]).unwrap();
-    nvm.read(0, &mut read_buf[0..4]).unwrap();
+    nvm.read(0, &mut read_buf[0..4]);
 
     // Read the whole content. Write will internally be split across two page bounaries.
-    nvm.write(0, &write_buf).unwrap();
+    nvm.write(0, &write_buf);
     // Memory can be read in one go.
-    nvm.read(0, &mut read_buf).unwrap();
+    nvm.read(0, &mut read_buf);
     assert_eq!(&read_buf, &write_buf);
-    assert!(nvm.verify(0, &write_buf).unwrap());
+    assert!(nvm.verify(0, &write_buf));
     read_buf.fill(0);
 
     // Write along page boundary
-    nvm.write(PAGE_SIZE - 2, &write_buf[0..8]).unwrap();
-    nvm.read(PAGE_SIZE - 2, &mut read_buf[0..8]).unwrap();
+    nvm.write(PAGE_SIZE - 2, &write_buf[0..8]);
+    nvm.read(PAGE_SIZE - 2, &mut read_buf[0..8]);
     assert_eq!(&read_buf[0..8], &write_buf[0..8]);
-    assert!(nvm.verify(PAGE_SIZE - 2, &write_buf[0..8]).unwrap());
+    assert!(nvm.verify(PAGE_SIZE - 2, &write_buf[0..8]));
 
-    nvm.write(0, &orig_content).unwrap();
+    nvm.write(0, &orig_content);
     loop {
         delay.delay_ms(500);
     }

va108xx/vorago-reb1/src/m95m01.rs

@@ -9,7 +9,6 @@
 //! - [REB1 EEPROM example](https://egit.irs.uni-stuttgart.de/rust/va108xx-rs/src/branch/main/vorago-reb1/examples/nvm.rs)
 use arbitrary_int::{u2, u3};
 use core::convert::Infallible;
-use embedded_hal::spi::SpiBus;
 
 pub const PAGE_SIZE: usize = 256;
 
@@ -66,53 +65,53 @@ impl M95M01 {
     pub fn new(spi: pac::Spic, clk_config: SpiClockConfig) -> Self {
         let spi = RomSpi::new_for_rom(spi, SpiConfig::default().clk_cfg(clk_config));
         let mut spi_dev = Self { spi };
-        spi_dev.clear_block_protection().unwrap();
+        spi_dev.clear_block_protection();
         spi_dev
     }
 
     pub fn release(mut self) -> pac::Spic {
-        self.set_block_protection().unwrap();
+        self.set_block_protection();
         unsafe { pac::Spic::steal() }
     }
 
     // Wait until the write-in-progress state is cleared. This exposes a [nb] API, so this function
     // will return [nb::Error::WouldBlock] if the EEPROM is still busy.
     pub fn writes_are_done(&mut self) -> nb::Result<(), Infallible> {
-        let rdsr = self.read_status_reg()?;
+        let rdsr = self.read_status_reg();
         if rdsr.write_in_progress() {
             return Err(nb::Error::WouldBlock);
         }
         Ok(())
     }
 
-    pub fn read_status_reg(&mut self) -> Result<StatusReg, Infallible> {
+    pub fn read_status_reg(&mut self) -> StatusReg {
         let mut write_read: [u8; 2] = [regs::RDSR, 0x00];
-        self.spi.transfer_in_place(&mut write_read)?;
-        Ok(StatusReg::new_with_raw_value(write_read[1]))
+        self.spi.transfer_in_place(&mut write_read);
+        StatusReg::new_with_raw_value(write_read[1])
     }
 
-    pub fn write_enable(&mut self) -> Result<(), Infallible> {
+    pub fn write_enable(&mut self) {
         self.spi.write(&[regs::WREN])
     }
 
-    pub fn clear_block_protection(&mut self) -> Result<(), Infallible> {
+    pub fn clear_block_protection(&mut self) {
         // Has to be written separately.
-        self.write_enable()?;
+        self.write_enable();
         self.spi.write(&[WRSR, 0x00])
     }
 
-    pub fn set_block_protection(&mut self) -> Result<(), Infallible> {
+    pub fn set_block_protection(&mut self) {
         let mut reg = StatusReg::new_with_raw_value(0);
         reg.set_block_protection_bits(u2::new(0b11));
-        self.write_enable()?;
+        self.write_enable();
         self.spi.write(&[WRSR, reg.raw_value()])
     }
 
-    fn common_init_write_and_read(&mut self, address: usize, reg: u8) -> Result<(), Infallible> {
-        nb::block!(self.writes_are_done())?;
-        self.spi.flush()?;
+    fn common_init_write_and_read(&mut self, address: usize, reg: u8) {
+        nb::block!(self.writes_are_done()).unwrap();
+        self.spi.flush();
         if reg == WRITE {
-            self.write_enable()?;
+            self.write_enable();
             self.spi.write_fifo_unchecked(WRITE as u32);
         } else {
             self.spi.write_fifo_unchecked(READ as u32);
@@ -121,21 +120,19 @@ impl M95M01 {
         self.spi
             .write_fifo_unchecked((address as u32 & 0x00ff00) >> 8);
         self.spi.write_fifo_unchecked(address as u32 & 0xff);
-        Ok(())
     }
 
-    fn common_read(&mut self, address: usize) -> Result<(), Infallible> {
-        self.common_init_write_and_read(address, READ)?;
+    fn common_read(&mut self, address: usize) {
+        self.common_init_write_and_read(address, READ);
         for _ in 0..4 {
             // Pump the FIFO.
             self.spi.write_fifo_unchecked(0);
             // Ignore the first 4 bytes.
-            nb::block!(self.spi.read_fifo())?;
+            nb::block!(self.spi.read_fifo()).unwrap();
         }
-        Ok(())
     }
 
-    pub fn write(&mut self, mut address: usize, mut data: &[u8]) -> Result<(), Infallible> {
+    pub fn write(&mut self, mut address: usize, mut data: &[u8]) {
         // Loop until all data is written
         while !data.is_empty() {
             // Calculate the page and the offset within the page from the address
@@ -155,8 +152,6 @@ impl M95M01 {
             address += to_write;
             data = &data[to_write..];
         }
-
-        Ok(())
     }
 
     pub fn write_page(
@@ -170,8 +165,7 @@ impl M95M01 {
             return Err(PageBoundaryExceededError);
         }
 
-        self.common_init_write_and_read(page * PAGE_SIZE + offset, WRITE)
-            .unwrap();
+        self.common_init_write_and_read(page * PAGE_SIZE + offset, WRITE);
         for val in data.iter().take(data.len() - 1) {
             nb::block!(self.spi.write_fifo(*val as u32)).unwrap();
             nb::block!(self.spi.read_fifo()).unwrap();
@@ -180,33 +174,32 @@ impl M95M01 {
             .spi
             .write_fifo(*data.last().unwrap() as u32 | BMSTART_BMSTOP_MASK))
         .unwrap();
-        self.spi.flush().unwrap();
+        self.spi.flush();
         nb::block!(self.writes_are_done()).unwrap();
         Ok(())
     }
 
-    pub fn read(&mut self, address: usize, buf: &mut [u8]) -> Result<(), Infallible> {
-        self.common_read(address)?;
+    pub fn read(&mut self, address: usize, buf: &mut [u8]) {
+        self.common_read(address);
         for val in buf.iter_mut() {
             self.spi.write_fifo_unchecked(0);
             *val = (nb::block!(self.spi.read_fifo()).unwrap() & 0xff) as u8;
         }
-        nb::block!(self.spi.write_fifo(BMSTART_BMSTOP_MASK))?;
-        self.spi.flush()?;
-        Ok(())
+        nb::block!(self.spi.write_fifo(BMSTART_BMSTOP_MASK)).unwrap();
+        self.spi.flush();
     }
 
-    pub fn verify(&mut self, address: usize, data: &[u8]) -> Result<bool, Infallible> {
-        self.common_read(address)?;
+    pub fn verify(&mut self, address: usize, data: &[u8]) -> bool {
+        self.common_read(address);
         for val in data.iter() {
             self.spi.write_fifo_unchecked(0);
             let read_val = (nb::block!(self.spi.read_fifo()).unwrap() & 0xff) as u8;
             if read_val != *val {
-                return Ok(false);
+                return false;
             }
         }
-        nb::block!(self.spi.write_fifo(BMSTART_BMSTOP_MASK))?;
-        self.spi.flush()?;
-        Ok(true)
+        nb::block!(self.spi.write_fifo(BMSTART_BMSTOP_MASK)).unwrap();
+        self.spi.flush();
+        true
     }
 }

va416xx/examples/simple/examples/spi.rs

@@ -10,7 +10,7 @@ use panic_probe as _;
 use defmt_rtt as _;
 
 use cortex_m_rt::entry;
-use embedded_hal::spi::{Mode, SpiBus, MODE_0};
+use embedded_hal::spi::{Mode, MODE_0};
 use simple_examples::peb1;
 use va416xx_hal::clock::ClockConfigurator;
 use va416xx_hal::spi::{Spi, SpiClockConfig};
@@ -67,20 +67,18 @@ fn main() -> ! {
         let tx_buf: [u8; 4] = [1, 2, 3, 0];
         let mut rx_buf: [u8; 4] = [0; 4];
         // Can't really verify correct behaviour here. Just verify nothing crazy happens or it hangs up.
-        spi0.write(&[0x42, 0x43]).expect("write failed");
+        spi0.write(&[0x42, 0x43]);
 
         // Can't really verify correct behaviour here. Just verify nothing crazy happens or it hangs up.
-        spi0.read(&mut rx_buf[0..2]).unwrap();
+        spi0.read(&mut rx_buf[0..2]);
 
         // If the pins are tied together, we should received exactly what we send.
 
         let mut inplace_buf = tx_buf;
-        spi0.transfer_in_place(&mut inplace_buf)
-            .expect("SPI transfer_in_place failed");
+        spi0.transfer_in_place(&mut inplace_buf);
         assert_eq!([1, 2, 3, 0], inplace_buf);
 
-        spi0.transfer(&mut rx_buf, &tx_buf)
-            .expect("SPI transfer failed");
+        spi0.transfer(&mut rx_buf, &tx_buf);
         assert_eq!(rx_buf, [1, 2, 3, 0]);
         delay.delay_ms(500);
     }

vorago-shared-hal/src/spi/mod.rs

@@ -778,12 +778,11 @@ where
         }
     }
 
-    fn transfer_preparation(&mut self, words: &[Word]) -> Result<(), Infallible> {
+    fn transfer_preparation(&mut self, words: &[Word]) {
         if words.is_empty() {
-            return Ok(());
+            return;
         }
         self.flush_internal();
-        Ok(())
     }
 
     // The FIFO can hold a guaranteed amount of data, so we can pump it on transfer
@@ -844,6 +843,112 @@ where
         }
         current_write_idx
     }
+
+    pub fn read(&mut self, words: &mut [Word]) {
+        self.transfer_preparation(words);
+        let mut current_read_idx = 0;
+        let mut current_write_idx = self.initial_send_fifo_pumping_with_fill_words(words.len());
+        loop {
+            if current_read_idx < words.len() {
+                words[current_read_idx] = (nb::block!(self.read_fifo()).unwrap() & Word::MASK)
+                    .try_into()
+                    .unwrap();
+                current_read_idx += 1;
+            }
+            if current_write_idx < words.len() {
+                if current_write_idx == words.len() - 1 && self.bmstall {
+                    nb::block!(self.write_fifo(self.fill_word.into() | BMSTART_BMSTOP_MASK))
+                        .unwrap();
+                } else {
+                    nb::block!(self.write_fifo(self.fill_word.into())).unwrap();
+                }
+                current_write_idx += 1;
+            }
+            if current_read_idx >= words.len() && current_write_idx >= words.len() {
+                break;
+            }
+        }
+    }
+
+    pub fn write(&mut self, words: &[Word]) {
+        self.transfer_preparation(words);
+        let mut current_write_idx = self.initial_send_fifo_pumping_with_words(words);
+        while current_write_idx < words.len() {
+            if current_write_idx == words.len() - 1 && self.bmstall {
+                nb::block!(self.write_fifo(words[current_write_idx].into() | BMSTART_BMSTOP_MASK))
+                    .unwrap();
+            } else {
+                nb::block!(self.write_fifo(words[current_write_idx].into())).unwrap();
+            }
+            current_write_idx += 1;
+            // Ignore received words.
+            if self.regs.read_status().rx_not_empty() {
+                self.clear_rx_fifo();
+            }
+        }
+    }
+
+    pub fn transfer(&mut self, read: &mut [Word], write: &[Word]) {
+        self.transfer_preparation(write);
+        let mut current_read_idx = 0;
+        let mut current_write_idx = self.initial_send_fifo_pumping_with_words(write);
+        let max_idx = core::cmp::max(read.len(), write.len());
+        while current_read_idx < read.len() || current_write_idx < write.len() {
+            if current_write_idx < max_idx {
+                if current_write_idx == write.len() - 1 && self.bmstall {
+                    nb::block!(
+                        self.write_fifo(write[current_write_idx].into() | BMSTART_BMSTOP_MASK)
+                    )
+                    .unwrap();
+                } else if current_write_idx < write.len() {
+                    nb::block!(self.write_fifo(write[current_write_idx].into())).unwrap();
+                } else {
+                    nb::block!(self.write_fifo(0)).unwrap();
+                }
+                current_write_idx += 1;
+            }
+            if current_read_idx < max_idx {
+                if current_read_idx < read.len() {
+                    read[current_read_idx] = (nb::block!(self.read_fifo()).unwrap() & Word::MASK)
+                        .try_into()
+                        .unwrap();
+                } else {
+                    nb::block!(self.read_fifo()).unwrap();
+                }
+                current_read_idx += 1;
+            }
+        }
+    }
+
+    pub fn transfer_in_place(&mut self, words: &mut [Word]) {
+        self.transfer_preparation(words);
+        let mut current_read_idx = 0;
+        let mut current_write_idx = self.initial_send_fifo_pumping_with_words(words);
+
+        while current_read_idx < words.len() || current_write_idx < words.len() {
+            if current_write_idx < words.len() {
+                if current_write_idx == words.len() - 1 && self.bmstall {
+                    nb::block!(
+                        self.write_fifo(words[current_write_idx].into() | BMSTART_BMSTOP_MASK)
+                    )
+                    .unwrap();
+                } else {
+                    nb::block!(self.write_fifo(words[current_write_idx].into())).unwrap();
+                }
+                current_write_idx += 1;
+            }
+            if current_read_idx < words.len() && current_read_idx < current_write_idx {
+                words[current_read_idx] = (nb::block!(self.read_fifo()).unwrap() & Word::MASK)
+                    .try_into()
+                    .unwrap();
+                current_read_idx += 1;
+            }
+        }
+    }
+
+    pub fn flush(&mut self) {
+        self.flush_internal();
+    }
 }
 
 impl<W: SpiWord> SpiLowLevel for Spi<W>
@@ -887,110 +992,27 @@ where
     <Word as TryFrom<u32>>::Error: core::fmt::Debug,
 {
     fn read(&mut self, words: &mut [Word]) -> Result<(), Self::Error> {
-        self.transfer_preparation(words)?;
-        let mut current_read_idx = 0;
-        let mut current_write_idx = self.initial_send_fifo_pumping_with_fill_words(words.len());
-        loop {
-            if current_read_idx < words.len() {
-                words[current_read_idx] = (nb::block!(self.read_fifo())? & Word::MASK)
-                    .try_into()
-                    .unwrap();
-                current_read_idx += 1;
-            }
-            if current_write_idx < words.len() {
-                if current_write_idx == words.len() - 1 && self.bmstall {
-                    nb::block!(self.write_fifo(self.fill_word.into() | BMSTART_BMSTOP_MASK))?;
-                } else {
-                    nb::block!(self.write_fifo(self.fill_word.into()))?;
-                }
-                current_write_idx += 1;
-            }
-            if current_read_idx >= words.len() && current_write_idx >= words.len() {
-                break;
-            }
-        }
+        self.read(words);
         Ok(())
     }
 
     fn write(&mut self, words: &[Word]) -> Result<(), Self::Error> {
-        self.transfer_preparation(words)?;
-        let mut current_write_idx = self.initial_send_fifo_pumping_with_words(words);
-        while current_write_idx < words.len() {
-            if current_write_idx == words.len() - 1 && self.bmstall {
-                nb::block!(self.write_fifo(words[current_write_idx].into() | BMSTART_BMSTOP_MASK))?;
-            } else {
-                nb::block!(self.write_fifo(words[current_write_idx].into()))?;
-            }
-            current_write_idx += 1;
-            // Ignore received words.
-            if self.regs.read_status().rx_not_empty() {
-                self.clear_rx_fifo();
-            }
-        }
+        self.write(words);
         Ok(())
     }
 
     fn transfer(&mut self, read: &mut [Word], write: &[Word]) -> Result<(), Self::Error> {
-        self.transfer_preparation(write)?;
-        let mut current_read_idx = 0;
-        let mut current_write_idx = self.initial_send_fifo_pumping_with_words(write);
-        let max_idx = core::cmp::max(read.len(), write.len());
-        while current_read_idx < read.len() || current_write_idx < write.len() {
-            if current_write_idx < max_idx {
-                if current_write_idx == write.len() - 1 && self.bmstall {
-                    nb::block!(
-                        self.write_fifo(write[current_write_idx].into() | BMSTART_BMSTOP_MASK)
-                    )?;
-                } else if current_write_idx < write.len() {
-                    nb::block!(self.write_fifo(write[current_write_idx].into()))?;
-                } else {
-                    nb::block!(self.write_fifo(0))?;
-                }
-                current_write_idx += 1;
-            }
-            if current_read_idx < max_idx {
-                if current_read_idx < read.len() {
-                    read[current_read_idx] = (nb::block!(self.read_fifo())? & Word::MASK)
-                        .try_into()
-                        .unwrap();
-                } else {
-                    nb::block!(self.read_fifo()).unwrap();
-                }
-                current_read_idx += 1;
-            }
-        }
-
+        self.transfer(read, write);
         Ok(())
     }
 
     fn transfer_in_place(&mut self, words: &mut [Word]) -> Result<(), Self::Error> {
-        self.transfer_preparation(words)?;
-        let mut current_read_idx = 0;
-        let mut current_write_idx = self.initial_send_fifo_pumping_with_words(words);
-
-        while current_read_idx < words.len() || current_write_idx < words.len() {
-            if current_write_idx < words.len() {
-                if current_write_idx == words.len() - 1 && self.bmstall {
-                    nb::block!(
-                        self.write_fifo(words[current_write_idx].into() | BMSTART_BMSTOP_MASK)
-                    )?;
-                } else {
-                    nb::block!(self.write_fifo(words[current_write_idx].into()))?;
-                }
-                current_write_idx += 1;
-            }
-            if current_read_idx < words.len() && current_read_idx < current_write_idx {
-                words[current_read_idx] = (nb::block!(self.read_fifo())? & Word::MASK)
-                    .try_into()
-                    .unwrap();
-                current_read_idx += 1;
-            }
-        }
+        self.transfer_in_place(words);
         Ok(())
     }
 
     fn flush(&mut self) -> Result<(), Self::Error> {
-        self.flush_internal();
+        self.flush();
         Ok(())
     }
 }