Implement i2cv1 timeout

4 files changed, 243 insertions, 23 deletions

diff --git a/embassy-stm32/src/i2c/mod.rs b/embassy-stm32/src/i2c/mod.rs
index 9d314f411..f4f64992c 100644
--- a/embassy-stm32/src/i2c/mod.rs
+++ b/embassy-stm32/src/i2c/mod.rs
@@ -7,6 +7,9 @@ use crate::interrupt::Interrupt;
 mod _version;
 pub use _version::*;
 
+mod timeout;
+pub use timeout::*;
+
 use crate::peripherals;
 
 #[derive(Debug)]
diff --git a/embassy-stm32/src/i2c/timeout.rs b/embassy-stm32/src/i2c/timeout.rs
new file mode 100644
index 000000000..12319af8a
--- /dev/null
+++ b/embassy-stm32/src/i2c/timeout.rs
@@ -0,0 +1,132 @@
+use embassy_time::{Duration, Instant};
+
+use super::{Error, I2c, Instance};
+
+pub struct TimeoutI2c<'d, T: Instance> {
+    i2c: &'d mut I2c<'d, T>,
+    timeout: Duration,
+}
+
+fn timeout_fn(timeout: Duration) -> impl Fn() -> Result<(), Error> {
+    let deadline = Instant::now() + timeout;
+    move || {
+        if Instant::now() > deadline {
+            Err(Error::Timeout)
+        } else {
+            Ok(())
+        }
+    }
+}
+
+impl<'d, T: Instance> TimeoutI2c<'d, T> {
+    pub fn new(i2c: &'d mut I2c<'d, T>, timeout: Duration) -> Self {
+        Self { i2c, timeout }
+    }
+
+    pub fn blocking_read_timeout(&mut self, addr: u8, buffer: &mut [u8], timeout: Duration) -> Result<(), Error> {
+        self.i2c.blocking_read_timeout(addr, buffer, timeout_fn(timeout))
+    }
+
+    pub fn blocking_read(&mut self, addr: u8, buffer: &mut [u8]) -> Result<(), Error> {
+        self.blocking_read_timeout(addr, buffer, self.timeout)
+    }
+
+    pub fn blocking_write_timeout(&mut self, addr: u8, bytes: &[u8], timeout: Duration) -> Result<(), Error> {
+        self.i2c.blocking_write_timeout(addr, bytes, timeout_fn(timeout))
+    }
+
+    pub fn blocking_write(&mut self, addr: u8, bytes: &[u8]) -> Result<(), Error> {
+        self.blocking_write_timeout(addr, bytes, self.timeout)
+    }
+
+    pub fn blocking_write_read_timeout(
+        &mut self,
+        addr: u8,
+        bytes: &[u8],
+        buffer: &mut [u8],
+        timeout: Duration,
+    ) -> Result<(), Error> {
+        self.i2c
+            .blocking_write_read_timeout(addr, bytes, buffer, timeout_fn(timeout))
+    }
+
+    pub fn blocking_write_read(&mut self, addr: u8, bytes: &[u8], buffer: &mut [u8]) -> Result<(), Error> {
+        self.blocking_write_read_timeout(addr, bytes, buffer, self.timeout)
+    }
+}
+
+impl<'d, T: Instance> embedded_hal_02::blocking::i2c::Read for TimeoutI2c<'d, T> {
+    type Error = Error;
+
+    fn read(&mut self, addr: u8, buffer: &mut [u8]) -> Result<(), Self::Error> {
+        self.blocking_read(addr, buffer)
+    }
+}
+
+impl<'d, T: Instance> embedded_hal_02::blocking::i2c::Write for TimeoutI2c<'d, T> {
+    type Error = Error;
+
+    fn write(&mut self, addr: u8, bytes: &[u8]) -> Result<(), Self::Error> {
+        self.blocking_write(addr, bytes)
+    }
+}
+
+impl<'d, T: Instance> embedded_hal_02::blocking::i2c::WriteRead for TimeoutI2c<'d, T> {
+    type Error = Error;
+
+    fn write_read(&mut self, addr: u8, bytes: &[u8], buffer: &mut [u8]) -> Result<(), Self::Error> {
+        self.blocking_write_read(addr, bytes, buffer)
+    }
+}
+
+#[cfg(feature = "unstable-traits")]
+mod eh1 {
+    use super::*;
+
+    impl<'d, T: Instance> embedded_hal_1::i2c::ErrorType for TimeoutI2c<'d, T> {
+        type Error = Error;
+    }
+
+    impl<'d, T: Instance> embedded_hal_1::i2c::I2c for TimeoutI2c<'d, T> {
+        fn read(&mut self, address: u8, buffer: &mut [u8]) -> Result<(), Self::Error> {
+            self.blocking_read(address, buffer)
+        }
+
+        fn write(&mut self, address: u8, buffer: &[u8]) -> Result<(), Self::Error> {
+            self.blocking_write(address, buffer)
+        }
+
+        fn write_iter<B>(&mut self, _address: u8, _bytes: B) -> Result<(), Self::Error>
+        where
+            B: IntoIterator<Item = u8>,
+        {
+            todo!();
+        }
+
+        fn write_iter_read<B>(&mut self, _address: u8, _bytes: B, _buffer: &mut [u8]) -> Result<(), Self::Error>
+        where
+            B: IntoIterator<Item = u8>,
+        {
+            todo!();
+        }
+
+        fn write_read(&mut self, address: u8, wr_buffer: &[u8], rd_buffer: &mut [u8]) -> Result<(), Self::Error> {
+            self.blocking_write_read(address, wr_buffer, rd_buffer)
+        }
+
+        fn transaction<'a>(
+            &mut self,
+            _address: u8,
+            _operations: &mut [embedded_hal_1::i2c::Operation<'a>],
+        ) -> Result<(), Self::Error> {
+            todo!();
+        }
+
+        fn transaction_iter<'a, O>(&mut self, _address: u8, _operations: O) -> Result<(), Self::Error>
+        where
+            O: IntoIterator<Item = embedded_hal_1::i2c::Operation<'a>>,
+        {
+            todo!();
+        }
+    }
+}
diff --git a/embassy-stm32/src/i2c/v1.rs b/embassy-stm32/src/i2c/v1.rs
index f39a37df6..92a898824 100644
--- a/embassy-stm32/src/i2c/v1.rs
+++ b/embassy-stm32/src/i2c/v1.rs
@@ -141,7 +141,12 @@ impl<'d, T: Instance> I2c<'d, T> {
         Ok(sr1)
     }
 
-    unsafe fn write_bytes(&mut self, addr: u8, bytes: &[u8]) -> Result<(), Error> {
+    unsafe fn write_bytes(
+        &mut self,
+        addr: u8,
+        bytes: &[u8],
+        check_timeout: impl Fn() -> Result<(), Error>,
+    ) -> Result<(), Error> {
         // Send a START condition
 
         T::regs().cr1().modify(|reg| {
@@ -149,7 +154,9 @@ impl<'d, T: Instance> I2c<'d, T> {
         });
 
         // Wait until START condition was generated
-        while !self.check_and_clear_error_flags()?.start() {}
+        while !self.check_and_clear_error_flags()?.start() {
+            check_timeout()?;
+        }
 
         // Also wait until signalled we're master and everything is waiting for us
         while {
@@ -157,7 +164,9 @@ impl<'d, T: Instance> I2c<'d, T> {
 
             let sr2 = T::regs().sr2().read();
             !sr2.msl() && !sr2.busy()
-        } {}
+        } {
+            check_timeout()?;
+        }
 
         // Set up current address, we're trying to talk to
         T::regs().dr().write(|reg| reg.set_dr(addr << 1));
@@ -165,26 +174,30 @@ impl<'d, T: Instance> I2c<'d, T> {
         // Wait until address was sent
         // Wait for the address to be acknowledged
         // Check for any I2C errors. If a NACK occurs, the ADDR bit will never be set.
-        while !self.check_and_clear_error_flags()?.addr() {}
+        while !self.check_and_clear_error_flags()?.addr() {
+            check_timeout()?;
+        }
 
         // Clear condition by reading SR2
         let _ = T::regs().sr2().read();
 
         // Send bytes
         for c in bytes {
-            self.send_byte(*c)?;
+            self.send_byte(*c, &check_timeout)?;
         }
 
         // Fallthrough is success
         Ok(())
     }
 
-    unsafe fn send_byte(&self, byte: u8) -> Result<(), Error> {
+    unsafe fn send_byte(&self, byte: u8, check_timeout: impl Fn() -> Result<(), Error>) -> Result<(), Error> {
         // Wait until we're ready for sending
         while {
             // Check for any I2C errors. If a NACK occurs, the ADDR bit will never be set.
             !self.check_and_clear_error_flags()?.txe()
-        } {}
+        } {
+            check_timeout()?;
+        }
 
         // Push out a byte of data
         T::regs().dr().write(|reg| reg.set_dr(byte));
@@ -193,24 +206,33 @@ impl<'d, T: Instance> I2c<'d, T> {
         while {
             // Check for any potential error conditions.
             !self.check_and_clear_error_flags()?.btf()
-        } {}
+        } {
+            check_timeout()?;
+        }
 
         Ok(())
     }
 
-    unsafe fn recv_byte(&self) -> Result<u8, Error> {
+    unsafe fn recv_byte(&self, check_timeout: impl Fn() -> Result<(), Error>) -> Result<u8, Error> {
         while {
             // Check for any potential error conditions.
             self.check_and_clear_error_flags()?;
 
             !T::regs().sr1().read().rxne()
-        } {}
+        } {
+            check_timeout()?;
+        }
 
         let value = T::regs().dr().read().dr();
         Ok(value)
     }
 
-    pub fn blocking_read(&mut self, addr: u8, buffer: &mut [u8]) -> Result<(), Error> {
+    pub fn blocking_read_timeout(
+        &mut self,
+        addr: u8,
+        buffer: &mut [u8],
+        check_timeout: impl Fn() -> Result<(), Error>,
+    ) -> Result<(), Error> {
         if let Some((last, buffer)) = buffer.split_last_mut() {
             // Send a START condition and set ACK bit
             unsafe {
@@ -221,27 +243,33 @@ impl<'d, T: Instance> I2c<'d, T> {
             }
 
             // Wait until START condition was generated
-            while unsafe { !T::regs().sr1().read().start() } {}
+            while unsafe { !self.check_and_clear_error_flags()?.start() } {
+                check_timeout()?;
+            }
 
             // Also wait until signalled we're master and everything is waiting for us
             while {
                 let sr2 = unsafe { T::regs().sr2().read() };
                 !sr2.msl() && !sr2.busy()
-            } {}
+            } {
+                check_timeout()?;
+            }
 
             // Set up current address, we're trying to talk to
             unsafe { T::regs().dr().write(|reg| reg.set_dr((addr << 1) + 1)) }
 
             // Wait until address was sent
             // Wait for the address to be acknowledged
-            while unsafe { !self.check_and_clear_error_flags()?.addr() } {}
+            while unsafe { !self.check_and_clear_error_flags()?.addr() } {
+                check_timeout()?;
+            }
 
             // Clear condition by reading SR2
             let _ = unsafe { T::regs().sr2().read() };
 
             // Receive bytes into buffer
             for c in buffer {
-                *c = unsafe { self.recv_byte()? };
+                *c = unsafe { self.recv_byte(&check_timeout)? };
             }
 
             // Prepare to send NACK then STOP after next byte
@@ -253,10 +281,12 @@ impl<'d, T: Instance> I2c<'d, T> {
             }
 
             // Receive last byte
-            *last = unsafe { self.recv_byte()? };
+            *last = unsafe { self.recv_byte(&check_timeout)? };
 
             // Wait for the STOP to be sent.
-            while unsafe { T::regs().cr1().read().stop() } {}
+            while unsafe { T::regs().cr1().read().stop() } {
+                check_timeout()?;
+            }
 
             // Fallthrough is success
             Ok(())
@@ -265,25 +295,50 @@ impl<'d, T: Instance> I2c<'d, T> {
         }
     }
 
-    pub fn blocking_write(&mut self, addr: u8, bytes: &[u8]) -> Result<(), Error> {
+    pub fn blocking_read(&mut self, addr: u8, buffer: &mut [u8]) -> Result<(), Error> {
+        self.blocking_read_timeout(addr, buffer, || Ok(()))
+    }
+
+    pub fn blocking_write_timeout(
+        &mut self,
+        addr: u8,
+        bytes: &[u8],
+        check_timeout: impl Fn() -> Result<(), Error>,
+    ) -> Result<(), Error> {
         unsafe {
-            self.write_bytes(addr, bytes)?;
+            self.write_bytes(addr, bytes, &check_timeout)?;
             // Send a STOP condition
             T::regs().cr1().modify(|reg| reg.set_stop(true));
             // Wait for STOP condition to transmit.
-            while T::regs().cr1().read().stop() {}
+            while T::regs().cr1().read().stop() {
+                check_timeout()?;
+            }
         };
 
         // Fallthrough is success
         Ok(())
     }
 
-    pub fn blocking_write_read(&mut self, addr: u8, bytes: &[u8], buffer: &mut [u8]) -> Result<(), Error> {
-        unsafe { self.write_bytes(addr, bytes)? };
-        self.blocking_read(addr, buffer)?;
+    pub fn blocking_write(&mut self, addr: u8, bytes: &[u8]) -> Result<(), Error> {
+        self.blocking_write_timeout(addr, bytes, || Ok(()))
+    }
+
+    pub fn blocking_write_read_timeout(
+        &mut self,
+        addr: u8,
+        bytes: &[u8],
+        buffer: &mut [u8],
+        check_timeout: impl Fn() -> Result<(), Error>,
+    ) -> Result<(), Error> {
+        unsafe { self.write_bytes(addr, bytes, &check_timeout)? };
+        self.blocking_read_timeout(addr, buffer, &check_timeout)?;
 
         Ok(())
     }
+
+    pub fn blocking_write_read(&mut self, addr: u8, bytes: &[u8], buffer: &mut [u8]) -> Result<(), Error> {
+        self.blocking_write_read_timeout(addr, bytes, buffer, || Ok(()))
+    }
 }
 
 impl<'d, T: Instance> embedded_hal_02::blocking::i2c::Read for I2c<'d, T> {
diff --git a/examples/stm32f4/src/bin/i2c.rs b/examples/stm32f4/src/bin/i2c.rs
new file mode 100644
index 000000000..99e3cecfc
--- /dev/null
+++ b/examples/stm32f4/src/bin/i2c.rs
@@ -0,0 +1,30 @@
+#![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::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 mut i2c = I2c::new(p.I2C2, p.PB10, p.PB11, 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),
+    }
+}