i2cv2 timeouts

2 files changed, 174 insertions, 55 deletions

diff --git a/embassy-stm32/src/i2c/v2.rs b/embassy-stm32/src/i2c/v2.rs
index 89b52da98..40413b696 100644
--- a/embassy-stm32/src/i2c/v2.rs
+++ b/embassy-stm32/src/i2c/v2.rs
@@ -147,14 +147,23 @@ impl<'d, T: Instance, TXDMA, RXDMA> I2c<'d, T, TXDMA, RXDMA> {
         }
     }
 
-    unsafe fn master_read(address: u8, length: usize, stop: Stop, reload: bool, restart: bool) {
+    unsafe fn master_read(
+        address: u8,
+        length: usize,
+        stop: Stop,
+        reload: bool,
+        restart: bool,
+        check_timeout: impl Fn() -> Result<(), Error>,
+    ) -> Result<(), Error> {
         assert!(length < 256);
 
         if !restart {
             // Wait for any previous address sequence to end
             // automatically. This could be up to 50% of a bus
             // cycle (ie. up to 0.5/freq)
-            while T::regs().cr2().read().start() {}
+            while T::regs().cr2().read().start() {
+                check_timeout()?;
+            }
         }
 
         // Set START and prepare to receive bytes into
@@ -176,15 +185,19 @@ impl<'d, T: Instance, TXDMA, RXDMA> I2c<'d, T, TXDMA, RXDMA> {
             w.set_autoend(stop.autoend());
             w.set_reload(reload);
         });
+
+        Ok(())
     }
 
-    unsafe fn master_write(address: u8, length: usize, stop: Stop, reload: bool) {
+    unsafe fn master_write(address: u8, length: usize, stop: Stop, reload: bool, check_timeout: impl Fn() -> Result<(), Error>) -> Result<(), Error> {
         assert!(length < 256);
 
         // Wait for any previous address sequence to end
         // automatically. This could be up to 50% of a bus
         // cycle (ie. up to 0.5/freq)
-        while T::regs().cr2().read().start() {}
+        while T::regs().cr2().read().start() {
+            check_timeout()?;
+        }
 
         let reload = if reload {
             i2c::vals::Reload::NOTCOMPLETED
@@ -204,12 +217,16 @@ impl<'d, T: Instance, TXDMA, RXDMA> I2c<'d, T, TXDMA, RXDMA> {
             w.set_autoend(stop.autoend());
             w.set_reload(reload);
         });
+
+        Ok(())
     }
 
-    unsafe fn master_continue(length: usize, reload: bool) {
+    unsafe fn master_continue(length: usize, reload: bool, check_timeout: impl Fn() -> Result<(), Error>) -> Result<(), Error> {
         assert!(length < 256 && length > 0);
 
-        while !T::regs().isr().read().tcr() {}
+        while !T::regs().isr().read().tcr() {
+            check_timeout()?;
+        }
 
         let reload = if reload {
             i2c::vals::Reload::NOTCOMPLETED
@@ -221,6 +238,8 @@ impl<'d, T: Instance, TXDMA, RXDMA> I2c<'d, T, TXDMA, RXDMA> {
             w.set_nbytes(length as u8);
             w.set_reload(reload);
         });
+
+        Ok(())
     }
 
     fn flush_txdr(&self) {
@@ -243,7 +262,7 @@ impl<'d, T: Instance, TXDMA, RXDMA> I2c<'d, T, TXDMA, RXDMA> {
         //}
     }
 
-    fn wait_txe(&self) -> Result<(), Error> {
+    fn wait_txe(&self, check_timeout: impl Fn() -> Result<(), Error>) -> Result<(), Error> {
         loop {
             unsafe {
                 let isr = T::regs().isr().read();
@@ -261,10 +280,12 @@ impl<'d, T: Instance, TXDMA, RXDMA> I2c<'d, T, TXDMA, RXDMA> {
                     return Err(Error::Nack);
                 }
             }
+
+            check_timeout()?;
         }
     }
 
-    fn wait_rxne(&self) -> Result<(), Error> {
+    fn wait_rxne(&self, check_timeout: impl Fn() -> Result<(), Error>) -> Result<(), Error> {
         loop {
             unsafe {
                 let isr = T::regs().isr().read();
@@ -282,10 +303,12 @@ impl<'d, T: Instance, TXDMA, RXDMA> I2c<'d, T, TXDMA, RXDMA> {
                     return Err(Error::Nack);
                 }
             }
+
+            check_timeout()?;
         }
     }
 
-    fn wait_tc(&self) -> Result<(), Error> {
+    fn wait_tc(&self, check_timeout: impl Fn() -> Result<(), Error>) -> Result<(), Error> {
         loop {
             unsafe {
                 let isr = T::regs().isr().read();
@@ -303,10 +326,12 @@ impl<'d, T: Instance, TXDMA, RXDMA> I2c<'d, T, TXDMA, RXDMA> {
                     return Err(Error::Nack);
                 }
             }
+
+            check_timeout()?;
         }
     }
 
-    fn read_internal(&mut self, address: u8, buffer: &mut [u8], restart: bool) -> Result<(), Error> {
+    fn read_internal(&mut self, address: u8, buffer: &mut [u8], restart: bool, check_timeout: impl Fn() -> Result<(), Error>) -> Result<(), Error> {
         let completed_chunks = buffer.len() / 255;
         let total_chunks = if completed_chunks * 255 == buffer.len() {
             completed_chunks
@@ -322,20 +347,21 @@ impl<'d, T: Instance, TXDMA, RXDMA> I2c<'d, T, TXDMA, RXDMA> {
                 Stop::Automatic,
                 last_chunk_idx != 0,
                 restart,
-            );
+                &check_timeout
+            )?;
         }
 
         for (number, chunk) in buffer.chunks_mut(255).enumerate() {
             if number != 0 {
                 // NOTE(unsafe) We have &mut self
                 unsafe {
-                    Self::master_continue(chunk.len(), number != last_chunk_idx);
+                    Self::master_continue(chunk.len(), number != last_chunk_idx, &check_timeout)?;
                 }
             }
 
             for byte in chunk {
                 // Wait until we have received something
-                self.wait_rxne()?;
+                self.wait_rxne(&check_timeout)?;
 
                 unsafe {
                     *byte = T::regs().rxdr().read().rxdata();
@@ -345,7 +371,7 @@ impl<'d, T: Instance, TXDMA, RXDMA> I2c<'d, T, TXDMA, RXDMA> {
         Ok(())
     }
 
-    fn write_internal(&mut self, address: u8, bytes: &[u8], send_stop: bool) -> Result<(), Error> {
+    fn write_internal(&mut self, address: u8, bytes: &[u8], send_stop: bool, check_timeout: impl Fn() -> Result<(), Error>) -> Result<(), Error> {
         let completed_chunks = bytes.len() / 255;
         let total_chunks = if completed_chunks * 255 == bytes.len() {
             completed_chunks
@@ -359,14 +385,14 @@ impl<'d, T: Instance, TXDMA, RXDMA> I2c<'d, T, TXDMA, RXDMA> {
         // ST SAD+W
         // NOTE(unsafe) We have &mut self
         unsafe {
-            Self::master_write(address, bytes.len().min(255), Stop::Software, last_chunk_idx != 0);
+            Self::master_write(address, bytes.len().min(255), Stop::Software, last_chunk_idx != 0, &check_timeout)?;
         }
 
         for (number, chunk) in bytes.chunks(255).enumerate() {
             if number != 0 {
                 // NOTE(unsafe) We have &mut self
                 unsafe {
-                    Self::master_continue(chunk.len(), number != last_chunk_idx);
+                    Self::master_continue(chunk.len(), number != last_chunk_idx, &check_timeout)?;
                 }
             }
 
@@ -374,7 +400,7 @@ impl<'d, T: Instance, TXDMA, RXDMA> I2c<'d, T, TXDMA, RXDMA> {
                 // Wait until we are allowed to send data
                 // (START has been ACKed or last byte when
                 // through)
-                self.wait_txe()?;
+                self.wait_txe(&check_timeout)?;
 
                 unsafe {
                     T::regs().txdr().write(|w| w.set_txdata(*byte));
@@ -382,7 +408,7 @@ impl<'d, T: Instance, TXDMA, RXDMA> I2c<'d, T, TXDMA, RXDMA> {
             }
         }
         // Wait until the write finishes
-        self.wait_tc()?;
+        self.wait_tc(&check_timeout)?;
 
         if send_stop {
             self.master_stop();
@@ -396,6 +422,7 @@ impl<'d, T: Instance, TXDMA, RXDMA> I2c<'d, T, TXDMA, RXDMA> {
         bytes: &[u8],
         first_slice: bool,
         last_slice: bool,
+        check_timeout: impl Fn() -> Result<(), Error>
     ) -> Result<(), Error>
     where
         TXDMA: crate::i2c::TxDma<T>,
@@ -447,11 +474,12 @@ impl<'d, T: Instance, TXDMA, RXDMA> I2c<'d, T, TXDMA, RXDMA> {
                     total_len.min(255),
                     Stop::Software,
                     (total_chunks != 1) || !last_slice,
-                );
+                    &check_timeout
+                )?;
             }
         } else {
             unsafe {
-                Self::master_continue(total_len.min(255), (total_chunks != 1) || !last_slice);
+                Self::master_continue(total_len.min(255), (total_chunks != 1) || !last_slice, &check_timeout)?;
                 T::regs().cr1().modify(|w| w.set_tcie(true));
             }
         }
@@ -461,32 +489,34 @@ impl<'d, T: Instance, TXDMA, RXDMA> I2c<'d, T, TXDMA, RXDMA> {
             let chunks_transferred = state.chunks_transferred.load(Ordering::Relaxed);
 
             if chunks_transferred == total_chunks {
-                return Poll::Ready(());
+                return Poll::Ready(Ok(()));
             } else if chunks_transferred != 0 {
                 remaining_len = remaining_len.saturating_sub(255);
                 let last_piece = (chunks_transferred + 1 == total_chunks) && last_slice;
 
                 // NOTE(unsafe) self.tx_dma does not fiddle with the i2c registers
                 unsafe {
-                    Self::master_continue(remaining_len.min(255), !last_piece);
+                    if let Err(e) = Self::master_continue(remaining_len.min(255), !last_piece, &check_timeout) {
+                        return Poll::Ready(Err(e));
+                    }
                     T::regs().cr1().modify(|w| w.set_tcie(true));
                 }
             }
             Poll::Pending
         })
-        .await;
+        .await?;
 
         dma_transfer.await;
 
         if last_slice {
             // This should be done already
-            self.wait_tc()?;
+            self.wait_tc(&check_timeout)?;
             self.master_stop();
         }
         Ok(())
     }
 
-    async fn read_dma_internal(&mut self, address: u8, buffer: &mut [u8], restart: bool) -> Result<(), Error>
+    async fn read_dma_internal(&mut self, address: u8, buffer: &mut [u8], restart: bool, check_timeout: impl Fn() -> Result<(), Error>) -> Result<(), Error>
     where
         RXDMA: crate::i2c::RxDma<T>,
     {
@@ -527,7 +557,7 @@ impl<'d, T: Instance, TXDMA, RXDMA> I2c<'d, T, TXDMA, RXDMA> {
 
         // NOTE(unsafe) self.rx_dma does not fiddle with the i2c registers
         unsafe {
-            Self::master_read(address, total_len.min(255), Stop::Software, total_chunks != 1, restart);
+            Self::master_read(address, total_len.min(255), Stop::Software, total_chunks != 1, restart, &check_timeout)?;
         }
 
         poll_fn(|cx| {
@@ -535,25 +565,27 @@ impl<'d, T: Instance, TXDMA, RXDMA> I2c<'d, T, TXDMA, RXDMA> {
             let chunks_transferred = state.chunks_transferred.load(Ordering::Relaxed);
 
             if chunks_transferred == total_chunks {
-                return Poll::Ready(());
+                return Poll::Ready(Ok(()));
             } else if chunks_transferred != 0 {
                 remaining_len = remaining_len.saturating_sub(255);
                 let last_piece = chunks_transferred + 1 == total_chunks;
 
                 // NOTE(unsafe) self.rx_dma does not fiddle with the i2c registers
                 unsafe {
-                    Self::master_continue(remaining_len.min(255), !last_piece);
+                    if let Err(e) = Self::master_continue(remaining_len.min(255), !last_piece, &check_timeout) {
+                        return Poll::Ready(Err(e))
+                    }
                     T::regs().cr1().modify(|w| w.set_tcie(true));
                 }
             }
             Poll::Pending
         })
-        .await;
+        .await?;
 
         dma_transfer.await;
 
         // This should be done already
-        self.wait_tc()?;
+        self.wait_tc(&check_timeout)?;
         self.master_stop();
         Ok(())
     }
@@ -561,18 +593,25 @@ impl<'d, T: Instance, TXDMA, RXDMA> I2c<'d, T, TXDMA, RXDMA> {
     // =========================
     //  Async public API
 
-    pub async fn write(&mut self, address: u8, bytes: &[u8]) -> Result<(), Error>
+    pub async fn write_timeout(&mut self, address: u8, bytes: &[u8], check_timeout: impl Fn() -> Result<(), Error>) -> Result<(), Error>
     where
         TXDMA: crate::i2c::TxDma<T>,
     {
         if bytes.is_empty() {
-            self.write_internal(address, bytes, true)
+            self.write_internal(address, bytes, true, &check_timeout)
         } else {
-            self.write_dma_internal(address, bytes, true, true).await
+            self.write_dma_internal(address, bytes, true, true, &check_timeout).await
         }
     }
 
-    pub async fn write_vectored(&mut self, address: u8, bytes: &[&[u8]]) -> Result<(), Error>
+    pub async fn write(&mut self, address: u8, bytes: &[u8]) -> Result<(), Error>
+    where
+        TXDMA: crate::i2c::TxDma<T>,
+    {
+        self.write_timeout(address, bytes, || Ok(())).await
+    }
+
+    pub async fn write_vectored_timeout(&mut self, address: u8, bytes: &[&[u8]], check_timeout: impl Fn() -> Result<(), Error>) -> Result<(), Error>
     where
         TXDMA: crate::i2c::TxDma<T>,
     {
@@ -587,63 +626,97 @@ impl<'d, T: Instance, TXDMA, RXDMA> I2c<'d, T, TXDMA, RXDMA> {
             let next = iter.next();
             let is_last = next.is_none();
 
-            self.write_dma_internal(address, c, first, is_last).await?;
+            self.write_dma_internal(address, c, first, is_last, &check_timeout).await?;
             first = false;
             current = next;
         }
         Ok(())
     }
 
-    pub async fn read(&mut self, address: u8, buffer: &mut [u8]) -> Result<(), Error>
+    pub async fn write_vectored(&mut self, address: u8, bytes: &[&[u8]]) -> Result<(), Error>
+    where
+        TXDMA: crate::i2c::TxDma<T>,
+    {
+        self.write_vectored_timeout(address, bytes, || Ok(())).await
+    }
+
+    pub async fn read_timeout(&mut self, address: u8, buffer: &mut [u8], check_timeout: impl Fn() -> Result<(), Error>) -> Result<(), Error>
     where
         RXDMA: crate::i2c::RxDma<T>,
     {
         if buffer.is_empty() {
-            self.read_internal(address, buffer, false)
+            self.read_internal(address, buffer, false, &check_timeout)
         } else {
-            self.read_dma_internal(address, buffer, false).await
+            self.read_dma_internal(address, buffer, false, &check_timeout).await
         }
     }
 
-    pub async fn write_read(&mut self, address: u8, bytes: &[u8], buffer: &mut [u8]) -> Result<(), Error>
+    pub async fn read(&mut self, address: u8, buffer: &mut [u8]) -> Result<(), Error>
+    where
+        RXDMA: crate::i2c::RxDma<T>,
+    {
+        self.read_timeout(address, buffer, || Ok(())).await
+    }
+
+    pub async fn write_read_timeout(&mut self, address: u8, bytes: &[u8], buffer: &mut [u8], check_timeout: impl Fn() -> Result<(), Error>) -> Result<(), Error>
     where
         TXDMA: super::TxDma<T>,
         RXDMA: super::RxDma<T>,
     {
         if bytes.is_empty() {
-            self.write_internal(address, bytes, false)?;
+            self.write_internal(address, bytes, false, &check_timeout)?;
         } else {
-            self.write_dma_internal(address, bytes, true, true).await?;
+            self.write_dma_internal(address, bytes, true, true, &check_timeout).await?;
         }
 
         if buffer.is_empty() {
-            self.read_internal(address, buffer, true)?;
+            self.read_internal(address, buffer, true, &check_timeout)?;
         } else {
-            self.read_dma_internal(address, buffer, true).await?;
+            self.read_dma_internal(address, buffer, true, &check_timeout).await?;
         }
 
         Ok(())
     }
 
+    pub async fn write_read(&mut self, address: u8, bytes: &[u8], buffer: &mut [u8]) -> Result<(), Error>
+    where
+        TXDMA: super::TxDma<T>,
+        RXDMA: super::RxDma<T>,
+    {
+        self.write_read_timeout(address, bytes, buffer, || Ok(())).await
+    }
+
     // =========================
     //  Blocking public API
 
-    pub fn blocking_read(&mut self, address: u8, buffer: &mut [u8]) -> Result<(), Error> {
-        self.read_internal(address, buffer, false)
+    pub fn blocking_read_timeout(&mut self, address: u8, buffer: &mut [u8], check_timeout: impl Fn() -> Result<(), Error>) -> Result<(), Error> {
+        self.read_internal(address, buffer, false, &check_timeout)
         // Automatic Stop
     }
 
+    pub fn blocking_read(&mut self, address: u8, buffer: &mut [u8]) -> Result<(), Error> {
+        self.blocking_read_timeout(address, buffer, || Ok(()))
+    }
+
+    pub fn blocking_write_timeout(&mut self, address: u8, bytes: &[u8], check_timeout: impl Fn() -> Result<(), Error>) -> Result<(), Error> {
+        self.write_internal(address, bytes, true, &check_timeout)
+    }
+
     pub fn blocking_write(&mut self, address: u8, bytes: &[u8]) -> Result<(), Error> {
-        self.write_internal(address, bytes, true)
+        self.blocking_write_timeout(address, bytes, || Ok(()))
     }
 
-    pub fn blocking_write_read(&mut self, address: u8, bytes: &[u8], buffer: &mut [u8]) -> Result<(), Error> {
-        self.write_internal(address, bytes, false)?;
-        self.read_internal(address, buffer, true)
+    pub fn blocking_write_read_timeout(&mut self, address: u8, bytes: &[u8], buffer: &mut [u8], check_timeout: impl Fn() -> Result<(), Error>) -> Result<(), Error> {
+        self.write_internal(address, bytes, false, &check_timeout)?;
+        self.read_internal(address, buffer, true, &check_timeout)
         // Automatic Stop
     }
 
-    pub fn blocking_write_vectored(&mut self, address: u8, bytes: &[&[u8]]) -> Result<(), Error> {
+    pub fn blocking_write_read(&mut self, address: u8, bytes: &[u8], buffer: &mut [u8]) -> Result<(), Error> {
+        self.blocking_write_read_timeout(address, bytes, buffer, || Ok(()))
+    }
+
+    pub fn blocking_write_vectored_timeout(&mut self, address: u8, bytes: &[&[u8]], check_timeout: impl Fn() -> Result<(), Error>) -> Result<(), Error> {
         if bytes.is_empty() {
             return Err(Error::ZeroLengthTransfer);
         }
@@ -657,7 +730,8 @@ impl<'d, T: Instance, TXDMA, RXDMA> I2c<'d, T, TXDMA, RXDMA> {
                 first_length.min(255),
                 Stop::Software,
                 (first_length > 255) || (last_slice_index != 0),
-            );
+                &check_timeout
+            )?;
         }
 
         for (idx, slice) in bytes.iter().enumerate() {
@@ -673,7 +747,7 @@ impl<'d, T: Instance, TXDMA, RXDMA> I2c<'d, T, TXDMA, RXDMA> {
             if idx != 0 {
                 // NOTE(unsafe) We have &mut self
                 unsafe {
-                    Self::master_continue(slice_len.min(255), (idx != last_slice_index) || (slice_len > 255));
+                    Self::master_continue(slice_len.min(255), (idx != last_slice_index) || (slice_len > 255), &check_timeout)?;
                 }
             }
 
@@ -681,7 +755,7 @@ impl<'d, T: Instance, TXDMA, RXDMA> I2c<'d, T, TXDMA, RXDMA> {
                 if number != 0 {
                     // NOTE(unsafe) We have &mut self
                     unsafe {
-                        Self::master_continue(chunk.len(), (number != last_chunk_idx) || (idx != last_slice_index));
+                        Self::master_continue(chunk.len(), (number != last_chunk_idx) || (idx != last_slice_index), &check_timeout)?;
                     }
                 }
 
@@ -689,7 +763,7 @@ impl<'d, T: Instance, TXDMA, RXDMA> I2c<'d, T, TXDMA, RXDMA> {
                     // Wait until we are allowed to send data
                     // (START has been ACKed or last byte when
                     // through)
-                    self.wait_txe()?;
+                    self.wait_txe(&check_timeout)?;
 
                     // Put byte on the wire
                     //self.i2c.txdr.write(|w| w.txdata().bits(*byte));
@@ -700,11 +774,15 @@ impl<'d, T: Instance, TXDMA, RXDMA> I2c<'d, T, TXDMA, RXDMA> {
             }
         }
         // Wait until the write finishes
-        self.wait_tc()?;
+        self.wait_tc(&check_timeout)?;
         self.master_stop();
 
         Ok(())
     }
+
+    pub fn blocking_write_vectored(&mut self, address: u8, bytes: &[&[u8]]) -> Result<(), Error> {
+        self.blocking_write_vectored_timeout(address, bytes, || Ok(()))
+    }
 }
 
 mod eh02 {
diff --git a/examples/stm32h7/src/bin/i2c.rs b/examples/stm32h7/src/bin/i2c.rs
new file mode 100644
index 000000000..7a314b996
--- /dev/null
+++ b/examples/stm32h7/src/bin/i2c.rs
@@ -0,0 +1,41 @@
+#![no_std]
+#![no_main]
+#![feature(type_alias_impl_trait)]
+
+use defmt::*;
+use embassy_executor::Spawner;
+use embassy_stm32::i2c::{Error, I2c, TimeoutI2c};
+use embassy_stm32::interrupt;
+use embassy_stm32::time::Hertz;
+use embassy_time::Duration;
+use {defmt_rtt as _, panic_probe as _};
+
+const ADDRESS: u8 = 0x5F;
+const WHOAMI: u8 = 0x0F;
+
+#[embassy_executor::main]
+async fn main(_spawner: Spawner) -> ! {
+    info!("Hello world!");
+    let p = embassy_stm32::init(Default::default());
+
+    let irq = interrupt::take!(I2C2_EV);
+    let mut i2c = I2c::new(
+        p.I2C2,
+        p.PB10,
+        p.PB11,
+        irq,
+        p.DMA1_CH4,
+        p.DMA1_CH5,
+        Hertz(100_000),
+        Default::default(),
+    );
+    let mut timeout_i2c = TimeoutI2c::new(&mut i2c, Duration::from_millis(1000));
+
+    let mut data = [0u8; 1];
+
+    match timeout_i2c.blocking_write_read(ADDRESS, &[WHOAMI], &mut data) {
+        Ok(()) => info!("Whoami: {}", data[0]),
+        Err(Error::Timeout) => error!("Operation timed out"),
+        Err(e) => error!("I2c Error: {:?}", e),
+    }
+}