3 files changed, 114 insertions, 67 deletions

diff --git a/embassy-rp/src/i2c.rs b/embassy-rp/src/i2c.rs
index 74d015792..85636f5fe 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
@@ -407,11 +414,14 @@ impl<'d, T: Instance + 'd, M: Mode> I2c<'d, T, M> {
         let lcnt = period * 3 / 5; // spend 3/5 (60%) of the period low
         let hcnt = period - lcnt; // and 2/5 (40%) of the period high
 
+        warn!("cb:{} h:{:x} l:{:x}", clk_base, hcnt, lcnt);
         // 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 +434,20 @@ 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::HoldCountOutOfRange);
+        }
+        */
 
         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 +456,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 +772,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/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();
+    }
 }