From bcd3ab4ba1541a098a74b5abffdb30a293c97f64 Mon Sep 17 00:00:00 2001
From: Mathias <mk@blackbird.online>
Date: Fri, 19 Aug 2022 14:15:43 +0200
Subject: [PATCH] Add blocking read & write for I2C

---
 embassy-rp/src/i2c.rs | 306 +++++++++++++++++++++++++++++++++++++++---
 1 file changed, 286 insertions(+), 20 deletions(-)

diff --git a/embassy-rp/src/i2c.rs b/embassy-rp/src/i2c.rs
index 4a27ee8df..f7e6a6f6b 100644
--- a/embassy-rp/src/i2c.rs
+++ b/embassy-rp/src/i2c.rs
@@ -39,12 +39,15 @@ impl Default for Config {
     }
 }
 
+const TX_FIFO_SIZE: u8 = 16;
+const RX_FIFO_SIZE: u8 = 16;
+
 pub struct I2c<'d, T: Instance, M: Mode> {
     phantom: PhantomData<(&'d mut T, M)>,
 }
 
-impl<'d, T: Instance> I2c<'d, T, Master> {
-    pub fn new_master(
+impl<'d, T: Instance> I2c<'d, T, Blocking> {
+    pub fn new_blocking(
         _peri: impl Peripheral<P = T> + 'd,
         scl: impl Peripheral<P = impl SclPin<T>> + 'd,
         sda: impl Peripheral<P = impl SdaPin<T>> + 'd,
@@ -60,9 +63,10 @@ impl<'d, T: Instance> I2c<'d, T, Master> {
         unsafe {
             p.ic_enable().write(|w| w.set_enable(false));
 
-            // select controller mode & speed
+            // Select controller mode & speed
             p.ic_con().write(|w| {
-                // Always use "fast" mode (<= 400 kHz, works fine for standard mode too)
+                // Always use "fast" mode (<= 400 kHz, works fine for standard
+                // mode too)
                 w.set_speed(i2c::vals::Speed::FAST);
                 w.set_master_mode(true);
                 w.set_ic_slave_disable(true);
@@ -70,7 +74,8 @@ impl<'d, T: Instance> I2c<'d, T, Master> {
                 w.set_tx_empty_ctrl(true);
             });
 
-            // Clear FIFO threshold
+            // Set FIFO watermarks to 1 to make things simpler. This is encoded
+            // by a register value of 0.
             p.ic_tx_tl().write(|w| w.set_tx_tl(0));
             p.ic_rx_tl().write(|w| w.set_rx_tl(0));
 
@@ -89,8 +94,9 @@ impl<'d, T: Instance> I2c<'d, T, Master> {
 
             // Configure baudrate
 
-            // There are some subtleties to I2C timing which we are completely ignoring here
-            // See: https://github.com/raspberrypi/pico-sdk/blob/bfcbefafc5d2a210551a4d9d80b4303d4ae0adf7/src/rp2_common/hardware_i2c/i2c.c#L69
+            // There are some subtleties to I2C timing which we are completely
+            // ignoring here See:
+            // https://github.com/raspberrypi/pico-sdk/blob/bfcbefafc5d2a210551a4d9d80b4303d4ae0adf7/src/rp2_common/hardware_i2c/i2c.c#L69
             let clk_base = crate::clocks::clk_sys_freq();
 
             let period = (clk_base + config.frequency / 2) / config.frequency;
@@ -104,21 +110,21 @@ impl<'d, T: Instance> I2c<'d, T, Master> {
             assert!(lcnt >= 8);
 
             // Per I2C-bus specification a device in standard or fast mode must
-            // internally provide a hold time of at least 300ns for the SDA signal to
-            // bridge the undefined region of the falling edge of SCL. A smaller hold
-            // time of 120ns is used for fast mode plus.
+            // internally provide a hold time of at least 300ns for the SDA
+            // signal to bridge the undefined region of the falling edge of SCL.
+            // A smaller hold time of 120ns is used for fast mode plus.
             let sda_tx_hold_count = if config.frequency < 1_000_000 {
-                // sda_tx_hold_count = clk_base [cycles/s] * 300ns * (1s / 1e9ns)
-                // Reduce 300/1e9 to 3/1e7 to avoid numbers that don't fit in uint.
-                // Add 1 to avoid division truncation.
+                // sda_tx_hold_count = clk_base [cycles/s] * 300ns * (1s /
+                // 1e9ns) Reduce 300/1e9 to 3/1e7 to avoid numbers that don't
+                // fit in uint. Add 1 to avoid division truncation.
                 ((clk_base * 3) / 10_000_000) + 1
             } else {
                 // fast mode plus requires a clk_base > 32MHz
                 assert!(clk_base >= 32_000_000);
 
-                // sda_tx_hold_count = clk_base [cycles/s] * 120ns * (1s / 1e9ns)
-                // Reduce 120/1e9 to 3/25e6 to avoid numbers that don't fit in uint.
-                // Add 1 to avoid division truncation.
+                // sda_tx_hold_count = clk_base [cycles/s] * 120ns * (1s /
+                // 1e9ns) Reduce 120/1e9 to 3/25e6 to avoid numbers that don't
+                // fit in uint. Add 1 to avoid division truncation.
                 ((clk_base * 3) / 25_000_000) + 1
             };
             assert!(sda_tx_hold_count <= lcnt - 2);
@@ -138,6 +144,266 @@ impl<'d, T: Instance> I2c<'d, T, Master> {
     }
 }
 
+impl<'d, T: Instance, M: Mode> I2c<'d, T, M> {
+    /// Number of bytes currently in the RX FIFO
+    #[inline]
+    pub fn rx_fifo_used(&self) -> u8 {
+        unsafe { T::regs().ic_rxflr().read().rxflr() }
+    }
+
+    /// Remaining capacity in the RX FIFO
+    #[inline]
+    pub fn rx_fifo_free(&self) -> u8 {
+        RX_FIFO_SIZE - self.rx_fifo_used()
+    }
+
+    /// RX FIFO is empty
+    #[inline]
+    pub fn rx_fifo_empty(&self) -> bool {
+        self.rx_fifo_used() == 0
+    }
+
+    /// Number of bytes currently in the TX FIFO
+    #[inline]
+    pub fn tx_fifo_used(&self) -> u8 {
+        unsafe { T::regs().ic_txflr().read().txflr() }
+    }
+
+    /// Remaining capacity in the TX FIFO
+    #[inline]
+    pub fn tx_fifo_free(&self) -> u8 {
+        TX_FIFO_SIZE - self.tx_fifo_used()
+    }
+
+    /// TX FIFO is at capacity
+    #[inline]
+    pub fn tx_fifo_full(&self) -> bool {
+        self.tx_fifo_free() == 0
+    }
+
+    fn setup(addr: u16) -> Result<(), Error> {
+        if addr >= 0x80 {
+            return Err(Error::AddressOutOfRange(addr));
+        }
+
+        if i2c_reserved_addr(addr) {
+            return Err(Error::AddressReserved(addr));
+        }
+
+        let p = T::regs();
+        unsafe {
+            p.ic_enable().write(|w| w.set_enable(false));
+            p.ic_tar().write(|w| w.set_ic_tar(addr));
+            p.ic_enable().write(|w| w.set_enable(true));
+        }
+        Ok(())
+    }
+
+    fn read_and_clear_abort_reason(&mut self) -> Option<u32> {
+        let p = T::regs();
+        unsafe {
+            let abort_reason = p.ic_tx_abrt_source().read().0;
+            if abort_reason != 0 {
+                // Note clearing the abort flag also clears the reason, and this
+                // instance of flag is clear-on-read! Note also the
+                // IC_CLR_TX_ABRT register always reads as 0.
+                p.ic_clr_tx_abrt().read();
+                Some(abort_reason)
+            } else {
+                None
+            }
+        }
+    }
+
+    fn read_blocking_internal(&mut self, buffer: &mut [u8], restart: bool, send_stop: bool) -> Result<(), Error> {
+        if buffer.is_empty() {
+            return Err(Error::InvalidReadBufferLength);
+        }
+
+        let p = T::regs();
+        let lastindex = buffer.len() - 1;
+        for (i, byte) in buffer.iter_mut().enumerate() {
+            let first = i == 0;
+            let last = i == lastindex;
+
+            // NOTE(unsafe) We have &mut self
+            unsafe {
+                // wait until there is space in the FIFO to write the next byte
+                while self.tx_fifo_full() {}
+
+                p.ic_data_cmd().write(|w| {
+                    if restart && first {
+                        w.set_restart(true);
+                    } else {
+                        w.set_restart(false);
+                    }
+
+                    if send_stop && last {
+                        w.set_stop(true);
+                    } else {
+                        w.set_stop(false);
+                    }
+
+                    w.cmd()
+                });
+
+                while p.ic_rxflr().read().rxflr() == 0 {
+                    if let Some(abort_reason) = self.read_and_clear_abort_reason() {
+                        return Err(Error::Abort(abort_reason));
+                    }
+                }
+
+                *byte = p.ic_data_cmd().read().dat();
+            }
+        }
+
+        Ok(())
+    }
+
+    fn write_blocking_internal(&mut self, bytes: &[u8], send_stop: bool) -> Result<(), Error> {
+        if bytes.is_empty() {
+            return Err(Error::InvalidWriteBufferLength);
+        }
+
+        let p = T::regs();
+
+        for (i, byte) in bytes.iter().enumerate() {
+            let last = i == bytes.len() - 1;
+
+            // NOTE(unsafe) We have &mut self
+            unsafe {
+                p.ic_data_cmd().write(|w| {
+                    if send_stop && last {
+                        w.set_stop(true);
+                    } else {
+                        w.set_stop(false);
+                    }
+                    w.set_dat(*byte);
+                });
+
+                // Wait until the transmission of the address/data from the
+                // internal shift register has completed. For this to function
+                // correctly, the TX_EMPTY_CTRL flag in IC_CON must be set. The
+                // TX_EMPTY_CTRL flag was set in i2c_init.
+                while !p.ic_raw_intr_stat().read().tx_empty() {}
+
+                let abort_reason = self.read_and_clear_abort_reason();
+
+                if abort_reason.is_some() || (send_stop && last) {
+                    // If the transaction was aborted or if it completed
+                    // successfully wait until the STOP condition has occured.
+
+                    while !p.ic_raw_intr_stat().read().stop_det() {}
+
+                    p.ic_clr_stop_det().read().clr_stop_det();
+                }
+
+                // Note the hardware issues a STOP automatically on an abort
+                // condition. Note also the hardware clears RX FIFO as well as
+                // TX on abort, ecause we set hwparam
+                // IC_AVOID_RX_FIFO_FLUSH_ON_TX_ABRT to 0.
+                if let Some(abort_reason) = abort_reason {
+                    return Err(Error::Abort(abort_reason));
+                }
+            }
+        }
+        Ok(())
+    }
+
+    // ========================= Blocking public API
+    // =========================
+
+    pub fn blocking_read(&mut self, address: u8, buffer: &mut [u8]) -> Result<(), Error> {
+        Self::setup(address.into())?;
+        self.read_blocking_internal(buffer, false, true)
+        // Automatic Stop
+    }
+
+    pub fn blocking_write(&mut self, address: u8, bytes: &[u8]) -> Result<(), Error> {
+        Self::setup(address.into())?;
+        self.write_blocking_internal(bytes, true)
+    }
+
+    pub fn blocking_write_read(&mut self, address: u8, bytes: &[u8], buffer: &mut [u8]) -> Result<(), Error> {
+        Self::setup(address.into())?;
+        self.write_blocking_internal(bytes, false)?;
+        self.read_blocking_internal(buffer, true, true)
+        // Automatic Stop
+    }
+}
+
+// impl<'d, T: Instance> I2c<'d, T, Async> { // ========================= //
+//     Async public API // =========================
+
+//     pub async fn write(&mut self, address: u8, bytes: &[u8]) -> Result<(),
+//         Error> { if bytes.is_empty() { self.write_blocking_internal(address,
+//             bytes, true) } else { self.write_dma_internal(address, bytes,
+//         true, true).await } }
+
+//     pub async fn write_vectored(&mut self, address: u8, bytes: &[&[u8]]) ->
+//         Result<(), Error> { if bytes.is_empty() { return
+//             Err(Error::ZeroLengthTransfer); } let mut iter = bytes.iter();
+
+//         let mut first = true; let mut current = iter.next(); while let
+//         Some(c) = current { let next = iter.next(); let is_last =
+//         next.is_none();
+
+//             self.write_dma_internal(address, c, first, is_last).await?;
+//             first = false;
+//             current = next;
+//         } Ok(())
+//     }
+
+//     pub async fn read(&mut self, address: u8, buffer: &mut [u8]) ->
+//         Result<(), Error> { if buffer.is_empty() {
+//             self.read_blocking_internal(address, buffer, false) } else {
+//         self.read_dma_internal(address, buffer, false).await } }
+
+//     pub async fn write_read(&mut self, address: u8, bytes: &[u8], buffer:
+//         &mut [u8]) -> Result<(), Error> { if bytes.is_empty() {
+//             self.write_blocking_internal(address, bytes, false)?; } else {
+//         self.write_dma_internal(address, bytes, true, true).await?; }
+
+//         if buffer.is_empty() { self.read_blocking_internal(address, buffer,
+//             true)?; } else { self.read_dma_internal(address, buffer,
+//         true).await?; }
+
+//         Ok(())
+//     }
+// }
+
+mod eh02 {
+    use super::*;
+
+    impl<'d, T: Instance, M: Mode> embedded_hal_02::blocking::i2c::Read for I2c<'d, T, M> {
+        type Error = Error;
+
+        fn read(&mut self, address: u8, buffer: &mut [u8]) -> Result<(), Self::Error> {
+            self.blocking_read(address, buffer)
+        }
+    }
+
+    impl<'d, T: Instance, M: Mode> embedded_hal_02::blocking::i2c::Write for I2c<'d, T, M> {
+        type Error = Error;
+
+        fn write(&mut self, address: u8, bytes: &[u8]) -> Result<(), Self::Error> {
+            self.blocking_write(address, bytes)
+        }
+    }
+
+    impl<'d, T: Instance, M: Mode> embedded_hal_02::blocking::i2c::WriteRead for I2c<'d, T, M> {
+        type Error = Error;
+
+        fn write_read(&mut self, address: u8, bytes: &[u8], buffer: &mut [u8]) -> Result<(), Self::Error> {
+            self.blocking_write_read(address, bytes, buffer)
+        }
+    }
+}
+
+fn i2c_reserved_addr(addr: u16) -> bool {
+    (addr & 0x78) == 0 || (addr & 0x78) == 0x78
+}
+
 mod sealed {
     pub trait Instance {}
     pub trait Mode {}
@@ -155,11 +421,11 @@ macro_rules! impl_mode {
     };
 }
 
-pub struct Master;
-pub struct Slave;
+pub struct Blocking;
+pub struct Async;
 
-impl_mode!(Master);
-impl_mode!(Slave);
+impl_mode!(Blocking);
+impl_mode!(Async);
 
 pub trait Instance: sealed::Instance {
     fn regs() -> pac::i2c::I2c;