4 files changed, 177 insertions, 117 deletions

diff --git a/embassy-rp/src/i2c.rs b/embassy-rp/src/i2c.rs
index 74d015792..ac0eac96d 100644
--- a/embassy-rp/src/i2c.rs
+++ b/embassy-rp/src/i2c.rs
@@ -43,6 +43,18 @@ pub enum Error {
     AddressReserved(u16),
 }
 
+/// I2C Config error
+#[derive(Debug, PartialEq, Eq)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+pub enum ConfigError {
+    /// Max i2c speed is 1MHz
+    FrequencyTooHigh,
+    /// The sys clock is too slow to support given frequency
+    ClockTooSlow,
+    /// The sys clock is too fast to support given frequency
+    ClockTooFast,
+}
+
 /// I2C config.
 #[non_exhaustive]
 #[derive(Copy, Clone)]
@@ -365,37 +377,32 @@ impl<'d, T: Instance + 'd, M: Mode> I2c<'d, T, M> {
     ) -> Self {
         into_ref!(_peri);
 
-        assert!(config.frequency <= 1_000_000);
-        assert!(config.frequency > 0);
-
-        let p = T::regs();
-
         let reset = T::reset();
         crate::reset::reset(reset);
         crate::reset::unreset_wait(reset);
 
-        p.ic_enable().write(|w| w.set_enable(false));
-
-        // Select controller mode & speed
-        p.ic_con().modify(|w| {
-            // 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);
-            w.set_ic_restart_en(true);
-            w.set_tx_empty_ctrl(true);
-        });
-
-        // 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));
-
         // Configure SCL & SDA pins
         set_up_i2c_pin(&scl);
         set_up_i2c_pin(&sda);
 
+        let mut me = Self { phantom: PhantomData };
+
+        if let Err(e) = me.set_config_inner(&config) {
+            panic!("Error configuring i2c: {:?}", e);
+        }
+
+        me
+    }
+
+    fn set_config_inner(&mut self, config: &Config) -> Result<(), ConfigError> {
+        if config.frequency > 1_000_000 {
+            return Err(ConfigError::FrequencyTooHigh);
+        }
+
+        let p = T::regs();
+
+        p.ic_enable().write(|w| w.set_enable(false));
+
         // Configure baudrate
 
         // There are some subtleties to I2C timing which we are completely
@@ -408,10 +415,12 @@ impl<'d, T: Instance + 'd, M: Mode> I2c<'d, T, M> {
         let hcnt = period - lcnt; // and 2/5 (40%) of the period high
 
         // Check for out-of-range divisors:
-        assert!(hcnt <= 0xffff);
-        assert!(lcnt <= 0xffff);
-        assert!(hcnt >= 8);
-        assert!(lcnt >= 8);
+        if hcnt > 0xffff || lcnt > 0xffff {
+            return Err(ConfigError::ClockTooFast);
+        }
+        if hcnt < 8 || lcnt < 8 {
+            return Err(ConfigError::ClockTooSlow);
+        }
 
         // Per I2C-bus specification a device in standard or fast mode must
         // internally provide a hold time of at least 300ns for the SDA
@@ -424,14 +433,19 @@ impl<'d, T: Instance + 'd, M: Mode> I2c<'d, T, M> {
             ((clk_base * 3) / 10_000_000) + 1
         } else {
             // fast mode plus requires a clk_base > 32MHz
-            assert!(clk_base >= 32_000_000);
+            if clk_base <= 32_000_000 {
+                return Err(ConfigError::ClockTooSlow);
+            }
 
             // 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);
+
+        if sda_tx_hold_count > lcnt - 2 {
+            return Err(ConfigError::ClockTooSlow);
+        }
 
         p.ic_fs_scl_hcnt().write(|w| w.set_ic_fs_scl_hcnt(hcnt as u16));
         p.ic_fs_scl_lcnt().write(|w| w.set_ic_fs_scl_lcnt(lcnt as u16));
@@ -440,10 +454,9 @@ impl<'d, T: Instance + 'd, M: Mode> I2c<'d, T, M> {
         p.ic_sda_hold()
             .modify(|w| w.set_ic_sda_tx_hold(sda_tx_hold_count as u16));
 
-        // Enable I2C block
         p.ic_enable().write(|w| w.set_enable(true));
 
-        Self { phantom: PhantomData }
+        Ok(())
     }
 
     fn setup(addr: u16) -> Result<(), Error> {
@@ -757,6 +770,15 @@ where
     }
 }
 
+impl<'d, T: Instance, M: Mode> embassy_embedded_hal::SetConfig for I2c<'d, T, M> {
+    type Config = Config;
+    type ConfigError = ConfigError;
+
+    fn set_config(&mut self, config: &Self::Config) -> Result<(), Self::ConfigError> {
+        self.set_config_inner(config)
+    }
+}
+
 /// Check if address is reserved.
 pub fn i2c_reserved_addr(addr: u16) -> bool {
     ((addr & 0x78) == 0 || (addr & 0x78) == 0x78) && addr != 0
diff --git a/embassy-rp/src/i2c_slave.rs b/embassy-rp/src/i2c_slave.rs
index 979686627..97ca17295 100644
--- a/embassy-rp/src/i2c_slave.rs
+++ b/embassy-rp/src/i2c_slave.rs
@@ -83,6 +83,7 @@ impl Default for Config {
 pub struct I2cSlave<'d, T: Instance> {
     phantom: PhantomData<&'d mut T>,
     pending_byte: Option<u8>,
+    config: Config,
 }
 
 impl<'d, T: Instance> I2cSlave<'d, T> {
@@ -99,6 +100,25 @@ impl<'d, T: Instance> I2cSlave<'d, T> {
         assert!(!i2c_reserved_addr(config.addr));
         assert!(config.addr != 0);
 
+        // Configure SCL & SDA pins
+        set_up_i2c_pin(&scl);
+        set_up_i2c_pin(&sda);
+
+        let mut ret = Self {
+            phantom: PhantomData,
+            pending_byte: None,
+            config,
+        };
+
+        ret.reset();
+
+        ret
+    }
+
+    /// Reset the i2c peripheral. If you cancel a respond_to_read, you may stall the bus.
+    /// You can recover the bus by calling this function, but doing so will almost certainly cause
+    /// an i/o error in the master.
+    pub fn reset(&mut self) {
         let p = T::regs();
 
         let reset = T::reset();
@@ -107,7 +127,7 @@ impl<'d, T: Instance> I2cSlave<'d, T> {
 
         p.ic_enable().write(|w| w.set_enable(false));
 
-        p.ic_sar().write(|w| w.set_ic_sar(config.addr));
+        p.ic_sar().write(|w| w.set_ic_sar(self.config.addr));
         p.ic_con().modify(|w| {
             w.set_master_mode(false);
             w.set_ic_slave_disable(false);
@@ -121,10 +141,10 @@ impl<'d, T: Instance> I2cSlave<'d, T> {
             // Generate stop interrupts for general calls
             // This also causes stop interrupts for other devices on the bus but those will not be
             // propagated up to the application.
-            w.set_stop_det_ifaddressed(!config.general_call);
+            w.set_stop_det_ifaddressed(!self.config.general_call);
         });
         p.ic_ack_general_call()
-            .write(|w| w.set_ack_gen_call(config.general_call));
+            .write(|w| w.set_ack_gen_call(self.config.general_call));
 
         // Set FIFO watermarks to 1 to make things simpler. This is encoded
         // by a register value of 0. Rx watermark should never change, but Tx watermark will be
@@ -132,10 +152,6 @@ impl<'d, T: Instance> I2cSlave<'d, T> {
         p.ic_tx_tl().write(|w| w.set_tx_tl(0));
         p.ic_rx_tl().write(|w| w.set_rx_tl(0));
 
-        // Configure SCL & SDA pins
-        set_up_i2c_pin(&scl);
-        set_up_i2c_pin(&sda);
-
         // Clear interrupts
         p.ic_clr_intr().read();
 
@@ -146,11 +162,6 @@ impl<'d, T: Instance> I2cSlave<'d, T> {
         p.ic_intr_mask().write_value(i2c::regs::IcIntrMask(0));
         T::Interrupt::unpend();
         unsafe { T::Interrupt::enable() };
-
-        Self {
-            phantom: PhantomData,
-            pending_byte: None,
-        }
     }
 
     /// Calls `f` to check if we are ready or not.
@@ -178,15 +189,13 @@ impl<'d, T: Instance> I2cSlave<'d, T> {
     fn drain_fifo(&mut self, buffer: &mut [u8], offset: &mut usize) {
         let p = T::regs();
 
-        for b in &mut buffer[*offset..] {
-            if let Some(pending) = self.pending_byte.take() {
-                *b = pending;
-                *offset += 1;
-                continue;
-            }
+        if let Some(pending) = self.pending_byte.take() {
+            buffer[*offset] = pending;
+            *offset += 1;
+        }
 
-            let status = p.ic_status().read();
-            if !status.rfne() {
+        for b in &mut buffer[*offset..] {
+            if !p.ic_status().read().rfne() {
                 break;
             }
 
@@ -207,14 +216,6 @@ impl<'d, T: Instance> I2cSlave<'d, T> {
         }
     }
 
-    #[inline(always)]
-    fn write_to_fifo(&mut self, buffer: &[u8]) {
-        let p = T::regs();
-        for byte in buffer {
-            p.ic_data_cmd().write(|w| w.set_dat(*byte));
-        }
-    }
-
     /// Wait asynchronously for commands from an I2C master.
     /// `buffer` is provided in case master does a 'write', 'write read', or 'general call' and is unused for 'read'.
     pub async fn listen(&mut self, buffer: &mut [u8]) -> Result<Command, Error> {
@@ -227,8 +228,9 @@ impl<'d, T: Instance> I2cSlave<'d, T> {
         self.wait_on(
             |me| {
                 let stat = p.ic_raw_intr_stat().read();
+                trace!("ls:{:013b} len:{}", stat.0, len);
 
-                if p.ic_rxflr().read().rxflr() > 0 {
+                if p.ic_rxflr().read().rxflr() > 0 || me.pending_byte.is_some() {
                     me.drain_fifo(buffer, &mut len);
                     // we're recieving data, set rx fifo watermark to 12 bytes (3/4 full) to reduce interrupt noise
                     p.ic_rx_tl().write(|w| w.set_rx_tl(11));
@@ -241,6 +243,10 @@ impl<'d, T: Instance> I2cSlave<'d, T> {
                         return Poll::Ready(Err(Error::PartialWrite(buffer.len())));
                     }
                 }
+                trace!("len:{}, pend:{:?}", len, me.pending_byte);
+                if me.pending_byte.is_some() {
+                    warn!("pending")
+                }
 
                 if stat.restart_det() && stat.rd_req() {
                     p.ic_clr_restart_det().read();
@@ -257,12 +263,17 @@ impl<'d, T: Instance> I2cSlave<'d, T> {
                     p.ic_clr_restart_det().read();
                     p.ic_clr_gen_call().read();
                     Poll::Ready(Ok(Command::Read))
+                } else if stat.stop_det() {
+                    // clear stuck stop bit
+                    // This can happen if the SDA/SCL pullups are enabled after calling this func
+                    p.ic_clr_stop_det().read();
+                    Poll::Pending
                 } else {
                     Poll::Pending
                 }
             },
             |_me| {
-                p.ic_intr_mask().modify(|w| {
+                p.ic_intr_mask().write(|w| {
                     w.set_m_stop_det(true);
                     w.set_m_restart_det(true);
                     w.set_m_gen_call(true);
@@ -286,27 +297,30 @@ impl<'d, T: Instance> I2cSlave<'d, T> {
 
         self.wait_on(
             |me| {
-                if let Err(abort_reason) = me.read_and_clear_abort_reason() {
-                    if let Error::Abort(AbortReason::TxNotEmpty(bytes)) = abort_reason {
-                        p.ic_clr_intr().read();
-                        return Poll::Ready(Ok(ReadStatus::LeftoverBytes(bytes)));
-                    } else {
-                        return Poll::Ready(Err(abort_reason));
+                let stat = p.ic_raw_intr_stat().read();
+                trace!("rs:{:013b}", stat.0);
+
+                if stat.tx_abrt() {
+                    if let Err(abort_reason) = me.read_and_clear_abort_reason() {
+                        if let Error::Abort(AbortReason::TxNotEmpty(bytes)) = abort_reason {
+                            p.ic_clr_intr().read();
+                            return Poll::Ready(Ok(ReadStatus::LeftoverBytes(bytes)));
+                        } else {
+                            return Poll::Ready(Err(abort_reason));
+                        }
                     }
                 }
 
                 if let Some(chunk) = chunks.next() {
-                    me.write_to_fifo(chunk);
-
-                    p.ic_clr_rd_req().read();
+                    for byte in chunk {
+                        p.ic_clr_rd_req().read();
+                        p.ic_data_cmd().write(|w| w.set_dat(*byte));
+                    }
 
                     Poll::Pending
                 } else {
-                    let stat = p.ic_raw_intr_stat().read();
-
-                    if stat.rx_done() && stat.stop_det() {
+                    if stat.rx_done() {
                         p.ic_clr_rx_done().read();
-                        p.ic_clr_stop_det().read();
                         Poll::Ready(Ok(ReadStatus::Done))
                     } else if stat.rd_req() && stat.tx_empty() {
                         Poll::Ready(Ok(ReadStatus::NeedMoreBytes))
@@ -316,8 +330,7 @@ impl<'d, T: Instance> I2cSlave<'d, T> {
                 }
             },
             |_me| {
-                p.ic_intr_mask().modify(|w| {
-                    w.set_m_stop_det(true);
+                p.ic_intr_mask().write(|w| {
                     w.set_m_rx_done(true);
                     w.set_m_tx_empty(true);
                     w.set_m_tx_abrt(true);
@@ -329,9 +342,14 @@ impl<'d, T: Instance> I2cSlave<'d, T> {
 
     /// Respond to reads with the fill byte until the controller stops asking
     pub async fn respond_till_stop(&mut self, fill: u8) -> Result<(), Error> {
+        // Send fill bytes a full fifo at a time, to reduce interrupt noise.
+        // This does mean we'll almost certainly abort the write, but since these are fill bytes,
+        // we don't care.
+        let buff = [fill; FIFO_SIZE as usize];
         loop {
-            match self.respond_to_read(&[fill]).await {
+            match self.respond_to_read(&buff).await {
                 Ok(ReadStatus::NeedMoreBytes) => (),
+                Ok(ReadStatus::LeftoverBytes(_)) => break Ok(()),
                 Ok(_) => break Ok(()),
                 Err(e) => break Err(e),
             }
@@ -353,10 +371,7 @@ impl<'d, T: Instance> I2cSlave<'d, T> {
     #[inline(always)]
     fn read_and_clear_abort_reason(&mut self) -> Result<(), Error> {
         let p = T::regs();
-        let mut abort_reason = p.ic_tx_abrt_source().read();
-
-        // Mask off master_dis
-        abort_reason.set_abrt_master_dis(false);
+        let abort_reason = p.ic_tx_abrt_source().read();
 
         if abort_reason.0 != 0 {
             // Note clearing the abort flag also clears the reason, and this
diff --git a/tests/rp/Cargo.toml b/tests/rp/Cargo.toml
index 46e1e9a5f..e67f2117d 100644
--- a/tests/rp/Cargo.toml
+++ b/tests/rp/Cargo.toml
@@ -14,6 +14,7 @@ embassy-rp = { version = "0.1.0", path = "../../embassy-rp", features = [ "defmt
 embassy-futures = { version = "0.1.0", path = "../../embassy-futures" }
 embassy-net = { version = "0.4.0", path = "../../embassy-net", features = ["defmt",  "tcp", "udp", "dhcpv4", "medium-ethernet"] }
 embassy-net-wiznet = { version = "0.1.0", path = "../../embassy-net-wiznet", features = ["defmt"] }
+embassy-embedded-hal = { version = "0.1.0", path = "../../embassy-embedded-hal/"}
 cyw43 = { path = "../../cyw43", features = ["defmt", "firmware-logs"] }
 cyw43-pio = { path = "../../cyw43-pio", features = ["defmt", "overclock"] }
 perf-client = { path = "../perf-client" }
diff --git a/tests/rp/src/bin/i2c.rs b/tests/rp/src/bin/i2c.rs
index a0aed1a42..153b37999 100644
--- a/tests/rp/src/bin/i2c.rs
+++ b/tests/rp/src/bin/i2c.rs
@@ -3,7 +3,10 @@
 teleprobe_meta::target!(b"rpi-pico");
 
 use defmt::{assert_eq, info, panic, unwrap};
-use embassy_executor::Executor;
+use embassy_embedded_hal::SetConfig;
+use embassy_executor::{Executor, Spawner};
+use embassy_rp::clocks::{PllConfig, XoscConfig};
+use embassy_rp::config::Config as rpConfig;
 use embassy_rp::multicore::{spawn_core1, Stack};
 use embassy_rp::peripherals::{I2C0, I2C1};
 use embassy_rp::{bind_interrupts, i2c, i2c_slave};
@@ -13,7 +16,6 @@ use static_cell::StaticCell;
 use {defmt_rtt as _, panic_probe as _, panic_probe as _, panic_probe as _};
 
 static mut CORE1_STACK: Stack<1024> = Stack::new();
-static EXECUTOR0: StaticCell<Executor> = StaticCell::new();
 static EXECUTOR1: StaticCell<Executor> = StaticCell::new();
 
 use crate::i2c::AbortReason;
@@ -44,10 +46,7 @@ async fn device_task(mut dev: i2c_slave::I2cSlave<'static, I2C1>) -> ! {
                         Ok(x) => match x {
                             i2c_slave::ReadStatus::Done => break,
                             i2c_slave::ReadStatus::NeedMoreBytes => count += 1,
-                            i2c_slave::ReadStatus::LeftoverBytes(x) => {
-                                info!("tried to write {} extra bytes", x);
-                                break;
-                            }
+                            i2c_slave::ReadStatus::LeftoverBytes(x) => panic!("tried to write {} extra bytes", x),
                         },
                         Err(e) => match e {
                             embassy_rp::i2c_slave::Error::Abort(AbortReason::Other(n)) => panic!("Other {:b}", n),
@@ -92,6 +91,8 @@ async fn device_task(mut dev: i2c_slave::I2cSlave<'static, I2C1>) -> ! {
                             resp_buff[i] = i as u8;
                         }
                         dev.respond_to_read(&resp_buff).await.unwrap();
+                        // reset count for next round of tests
+                        count = 0xD0;
                     }
                     x => panic!("Invalid Write Read {:x}", x),
                 }
@@ -104,8 +105,7 @@ async fn device_task(mut dev: i2c_slave::I2cSlave<'static, I2C1>) -> ! {
     }
 }
 
-#[embassy_executor::task]
-async fn controller_task(mut con: i2c::I2c<'static, I2C0, i2c::Async>) {
+async fn controller_task(con: &mut i2c::I2c<'static, I2C0, i2c::Async>) {
     info!("Device start");
 
     {
@@ -179,33 +179,55 @@ async fn controller_task(mut con: i2c::I2c<'static, I2C0, i2c::Async>) {
         info!("large write_read - OK")
     }
 
-    info!("Test OK");
-    cortex_m::asm::bkpt();
-}
-
-#[cortex_m_rt::entry]
-fn main() -> ! {
-    let p = embassy_rp::init(Default::default());
-    info!("Hello World!");
-
-    let d_sda = p.PIN_19;
-    let d_scl = p.PIN_18;
-    let mut config = i2c_slave::Config::default();
-    config.addr = DEV_ADDR as u16;
-    let device = i2c_slave::I2cSlave::new(p.I2C1, d_sda, d_scl, Irqs, config);
-
-    spawn_core1(p.CORE1, unsafe { &mut CORE1_STACK }, move || {
-        let executor1 = EXECUTOR1.init(Executor::new());
-        executor1.run(|spawner| unwrap!(spawner.spawn(device_task(device))));
-    });
-
-    let executor0 = EXECUTOR0.init(Executor::new());
-
-    let c_sda = p.PIN_21;
-    let c_scl = p.PIN_20;
-    let mut config = i2c::Config::default();
-    config.frequency = 5_000;
-    let controller = i2c::I2c::new_async(p.I2C0, c_sda, c_scl, Irqs, config);
+    #[embassy_executor::main]
+    async fn main(_core0_spawner: Spawner) {
+        let mut config = rpConfig::default();
+        // Configure clk_sys to 48MHz to support 1kHz scl.
+        // In theory it can go lower, but we won't bother to test below 1kHz.
+        config.clocks.xosc = Some(XoscConfig {
+            hz: 12_000_000,
+            delay_multiplier: 128,
+            sys_pll: Some(PllConfig {
+                refdiv: 1,
+                fbdiv: 120,
+                post_div1: 6,
+                post_div2: 5,
+            }),
+            usb_pll: Some(PllConfig {
+                refdiv: 1,
+                fbdiv: 120,
+                post_div1: 6,
+                post_div2: 5,
+            }),
+        });
+
+        let p = embassy_rp::init(config);
+        info!("Hello World!");
+
+        let d_sda = p.PIN_19;
+        let d_scl = p.PIN_18;
+        let mut config = i2c_slave::Config::default();
+        config.addr = DEV_ADDR as u16;
+        let device = i2c_slave::I2cSlave::new(p.I2C1, d_sda, d_scl, Irqs, config);
+
+        spawn_core1(p.CORE1, unsafe { &mut CORE1_STACK }, move || {
+            let executor1 = EXECUTOR1.init(Executor::new());
+            executor1.run(|spawner| unwrap!(spawner.spawn(device_task(device))));
+        });
+
+        let c_sda = p.PIN_21;
+        let c_scl = p.PIN_20;
+        let mut controller = i2c::I2c::new_async(p.I2C0, c_sda, c_scl, Irqs, Default::default());
+
+        for freq in [1000, 100_000, 400_000, 1_000_000] {
+            info!("testing at {}hz", freq);
+            let mut config = i2c::Config::default();
+            config.frequency = freq;
+            controller.set_config(&config).unwrap();
+            controller_task(&mut controller).await;
+        }
 
-    executor0.run(|spawner| unwrap!(spawner.spawn(controller_task(controller))));
+        info!("Test OK");
+        cortex_m::asm::bkpt();
+    }
 }