From 09abd99cc37d0665b69cc134fa79d2027f8d2418 Mon Sep 17 00:00:00 2001
From: teamplayer3 <alex.teamplayer@gmail.com>
Date: Thu, 21 Mar 2024 16:09:00 +0100
Subject: [PATCH 01/11] bumped esp_idf version to 5.2

---
 .cargo/config.toml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/.cargo/config.toml b/.cargo/config.toml
index 197f5b94d5e..ed701c643f6 100644
--- a/.cargo/config.toml
+++ b/.cargo/config.toml
@@ -24,7 +24,7 @@ rustflags = ["--cfg", "espidf_time64"]
 
 [env]
 ESP_IDF_SDKCONFIG_DEFAULTS = ".github/configs/sdkconfig.defaults"
-ESP_IDF_VERSION = "v5.1.2"
+ESP_IDF_VERSION = "v5.2"
 
 [unstable]
 build-std = ["std", "panic_abort"]

From ce09ec5934be9331c9e330326a260d87aeeb54c2 Mon Sep 17 00:00:00 2001
From: teamplayer3 <alex.teamplayer@gmail.com>
Date: Thu, 21 Mar 2024 16:09:47 +0100
Subject: [PATCH 02/11] init new i2c driver

---
 src/i2c.rs | 2056 +++++++++++++++++++++++++++++++++++++++++-----------
 1 file changed, 1648 insertions(+), 408 deletions(-)

diff --git a/src/i2c.rs b/src/i2c.rs
index 8620ff94054..98e923d48e7 100644
--- a/src/i2c.rs
+++ b/src/i2c.rs
@@ -1,579 +1,1819 @@
-use core::marker::PhantomData;
-use core::time::Duration;
+#[cfg(all(not(esp_idf_version_major = "4"), not(esp_idf_version = "5.1")))]
+pub use i2c::*;
 
-use embedded_hal::i2c::{ErrorKind, NoAcknowledgeSource};
+#[cfg(any(esp_idf_version_major = "4", esp_idf_version = "5.1"))]
+pub use legacy::*;
 
 use esp_idf_sys::*;
 
-use crate::delay::*;
-use crate::gpio::*;
-use crate::interrupt::InterruptType;
-use crate::peripheral::Peripheral;
-use crate::units::*;
-
-pub use embedded_hal::i2c::Operation;
-
-crate::embedded_hal_error!(
-    I2cError,
-    embedded_hal::i2c::Error,
-    embedded_hal::i2c::ErrorKind
-);
-
-const APB_TICK_PERIOD_NS: u32 = 1_000_000_000 / 80_000_000;
-#[derive(Copy, Clone, Debug)]
-pub struct APBTickType(::core::ffi::c_int);
-impl From<Duration> for APBTickType {
-    fn from(duration: Duration) -> Self {
-        APBTickType(
-            ((duration.as_nanos() + APB_TICK_PERIOD_NS as u128 - 1) / APB_TICK_PERIOD_NS as u128)
-                as ::core::ffi::c_int,
-        )
-    }
+pub trait I2c: Send {
+    fn port() -> i2c_port_t;
 }
 
-pub type I2cConfig = config::Config;
-#[cfg(not(esp32c2))]
-pub type I2cSlaveConfig = config::SlaveConfig;
+#[cfg(all(not(esp_idf_version_major = "4"), not(esp_idf_version = "5.1")))]
+mod i2c {
 
-/// I2C configuration
-pub mod config {
-    use enumset::EnumSet;
+    use core::borrow::Borrow;
+    use core::ffi::c_void;
+    use core::marker::PhantomData;
+    use core::ptr;
 
-    use super::APBTickType;
-    use crate::{interrupt::InterruptType, units::*};
+    use embassy_sync::mutex::Mutex;
+    use embassy_sync::mutex::MutexGuard;
 
-    /// I2C Master configuration
-    #[derive(Debug, Clone)]
-    pub struct Config {
-        pub baudrate: Hertz,
-        pub sda_pullup_enabled: bool,
-        pub scl_pullup_enabled: bool,
-        pub timeout: Option<APBTickType>,
-        pub intr_flags: EnumSet<InterruptType>,
-    }
+    use embedded_hal::i2c::{ErrorKind, NoAcknowledgeSource};
+
+    use esp_idf_sys::*;
+
+    use crate::delay::*;
+    use crate::gpio::*;
+    use crate::interrupt::asynch::HalIsrNotification;
+    use crate::peripheral::Peripheral;
+    use crate::task::embassy_sync::EspRawMutex;
+    use crate::units::*;
+
+    pub use embedded_hal::i2c::Operation;
+
+    use super::I2c;
+
+    crate::embedded_hal_error!(
+        I2cError,
+        embedded_hal::i2c::Error,
+        embedded_hal::i2c::ErrorKind
+    );
+
+    pub type I2cConfig = config::Config;
+    #[cfg(not(esp32c2))]
+    pub type I2cSlaveConfig = config::SlaveDeviceConfig;
+
+    /// I2C configuration
+    pub mod config {
+        use esp_idf_sys::*;
+
+        use crate::units::*;
+
+        // TODO: in bindings its XTAL called and in doc its APB
+        const APB_SCLK: soc_periph_i2c_clk_src_t = soc_periph_i2c_clk_src_t_I2C_CLK_SRC_XTAL;
+        const FAST_SCLK: soc_periph_i2c_clk_src_t = soc_periph_i2c_clk_src_t_I2C_CLK_SRC_RC_FAST;
 
-    impl Config {
-        pub fn new() -> Self {
-            Default::default()
+        /// i2c source clock
+        #[derive(PartialEq, Eq, Copy, Clone, Debug)]
+        #[allow(non_camel_case_types)]
+        pub enum SourceClock {
+            APB,
+            RC_FAST,
         }
 
-        #[must_use]
-        pub fn baudrate(mut self, baudrate: Hertz) -> Self {
-            self.baudrate = baudrate;
-            self
+        impl SourceClock {
+            pub const fn default() -> Self {
+                Self::from_raw(soc_periph_i2c_clk_src_t_I2C_CLK_SRC_DEFAULT)
+            }
+
+            pub const fn from_raw(source_clock: soc_periph_i2c_clk_src_t) -> Self {
+                match source_clock {
+                    APB_SCLK => SourceClock::APB,
+                    FAST_SCLK => SourceClock::RC_FAST,
+                    _ => unreachable!(),
+                }
+            }
         }
 
-        #[must_use]
-        pub fn sda_enable_pullup(mut self, enable: bool) -> Self {
-            self.sda_pullup_enabled = enable;
-            self
+        impl Default for SourceClock {
+            fn default() -> Self {
+                SourceClock::default()
+            }
         }
 
-        #[must_use]
-        pub fn scl_enable_pullup(mut self, enable: bool) -> Self {
-            self.scl_pullup_enabled = enable;
-            self
+        impl From<SourceClock> for i2c_clock_source_t {
+            fn from(source_clock: SourceClock) -> Self {
+                match source_clock {
+                    SourceClock::RC_FAST => FAST_SCLK,
+                    SourceClock::APB => APB_SCLK,
+                }
+            }
         }
 
-        #[must_use]
-        pub fn timeout(mut self, timeout: APBTickType) -> Self {
-            self.timeout = Some(timeout);
-            self
+        impl From<uart_sclk_t> for SourceClock {
+            fn from(source_clock: i2c_clock_source_t) -> Self {
+                Self::from_raw(source_clock)
+            }
         }
 
-        #[must_use]
-        pub fn intr_flags(mut self, flags: EnumSet<InterruptType>) -> Self {
-            self.intr_flags = flags;
-            self
+        /// I2C Master configuration
+        #[derive(Debug, Clone)]
+        pub struct Config {
+            pub pullup_enabled: bool,
+            pub source_clock: SourceClock,
+            pub glitch_ignore_cnt: u8,
         }
-    }
 
-    impl Default for Config {
-        fn default() -> Self {
-            Self {
-                baudrate: Hertz(1_000_000),
-                sda_pullup_enabled: true,
-                scl_pullup_enabled: true,
-                timeout: None,
-                intr_flags: EnumSet::<InterruptType>::empty(),
+        impl Config {
+            pub fn new() -> Self {
+                Default::default()
+            }
+
+            #[must_use]
+            pub fn enable_pullup(mut self, enable: bool) -> Self {
+                self.pullup_enabled = enable;
+                self
+            }
+
+            #[must_use]
+            pub fn source_clock(mut self, source_clock: SourceClock) -> Self {
+                self.source_clock = source_clock;
+                self
+            }
+
+            #[must_use]
+            pub fn glitch_ignore_count(mut self, count: u8) -> Self {
+                self.glitch_ignore_cnt = count;
+                self
+            }
+        }
+
+        impl Default for Config {
+            fn default() -> Self {
+                Self {
+                    pullup_enabled: true,
+                    source_clock: SourceClock::default(),
+                    glitch_ignore_cnt: 7,
+                }
+            }
+        }
+
+        #[derive(Debug, Clone)]
+        pub enum DeviceAddress {
+            SevenBit(u8),
+            TenBit(u16),
+        }
+
+        impl DeviceAddress {
+            pub(super) fn address(&self) -> u16 {
+                match self {
+                    DeviceAddress::SevenBit(addr) => *addr as u16,
+                    // TODO: if cfg allows 10 bit address
+                    DeviceAddress::TenBit(addr) => *addr,
+                }
+            }
+        }
+
+        impl From<DeviceAddress> for i2c_addr_bit_len_t {
+            fn from(address: DeviceAddress) -> Self {
+                match address {
+                    DeviceAddress::SevenBit(_) => i2c_addr_bit_len_t_I2C_ADDR_BIT_LEN_7,
+                    DeviceAddress::TenBit(_) => i2c_addr_bit_len_t_I2C_ADDR_BIT_LEN_10,
+                }
+            }
+        }
+
+        #[derive(Debug, Clone)]
+        pub struct DeviceConfig {
+            pub address: DeviceAddress,
+            pub baudrate: Hertz,
+        }
+
+        impl DeviceConfig {
+            pub const fn new(address: DeviceAddress) -> Self {
+                Self {
+                    address,
+                    baudrate: Hertz(1_000_000),
+                }
+            }
+
+            #[must_use]
+            pub fn baudrate(mut self, baudrate: Hertz) -> Self {
+                self.baudrate = baudrate;
+                self
+            }
+        }
+
+        /// I2C Slave configuration
+        #[cfg(not(esp32c2))]
+        #[derive(Debug, Clone)]
+        pub struct SlaveDeviceConfig {
+            pub source_clock: SourceClock,
+            pub broadcast_enable: bool,
+            pub send_buffer_depth: u32,
+        }
+
+        #[cfg(not(esp32c2))]
+        impl SlaveDeviceConfig {
+            pub fn new() -> Self {
+                Default::default()
+            }
+
+            #[must_use]
+            pub fn source_clock(mut self, source_clock: SourceClock) -> Self {
+                self.source_clock = source_clock;
+                self
+            }
+
+            #[must_use]
+            pub fn enable_broadcast(mut self, enable: bool) -> Self {
+                self.broadcast_enable = enable;
+                self
+            }
+
+            #[must_use]
+            pub fn set_send_buffer_depth(mut self, depth: u32) -> Self {
+                self.send_buffer_depth = depth;
+                self
+            }
+        }
+
+        #[cfg(not(esp32c2))]
+        impl Default for SlaveDeviceConfig {
+            fn default() -> Self {
+                Self {
+                    source_clock: SourceClock::default(),
+                    broadcast_enable: false,
+                    send_buffer_depth: 0,
+                }
             }
         }
     }
 
-    /// I2C Slave configuration
-    #[cfg(not(esp32c2))]
-    #[derive(Debug, Clone)]
-    pub struct SlaveConfig {
-        pub sda_pullup_enabled: bool,
-        pub scl_pullup_enabled: bool,
-        pub rx_buf_len: usize,
-        pub tx_buf_len: usize,
-        pub intr_flags: EnumSet<InterruptType>,
+    pub struct I2cDriver<'d> {
+        port: u8,
+        handle: i2c_master_bus_handle_t,
+        _p: PhantomData<&'d mut ()>,
     }
 
-    #[cfg(not(esp32c2))]
-    impl SlaveConfig {
-        pub fn new() -> Self {
-            Default::default()
+    impl<'d> I2cDriver<'d> {
+        pub fn new<I2C: I2c>(
+            _i2c: impl Peripheral<P = I2C> + 'd,
+            sda: impl Peripheral<P = impl InputPin + OutputPin> + 'd,
+            scl: impl Peripheral<P = impl InputPin + OutputPin> + 'd,
+            config: &config::Config,
+        ) -> Result<Self, EspError> {
+            let handle = init_master_bus(_i2c, sda, scl, config, 0)?;
+
+            Ok(I2cDriver {
+                port: I2C::port() as u8,
+                handle,
+                _p: PhantomData,
+            })
         }
 
-        #[must_use]
-        pub fn sda_enable_pullup(mut self, enable: bool) -> Self {
-            self.sda_pullup_enabled = enable;
-            self
+        pub fn port(&self) -> u8 {
+            self.port
         }
 
-        #[must_use]
-        pub fn scl_enable_pullup(mut self, enable: bool) -> Self {
-            self.scl_pullup_enabled = enable;
-            self
+        fn bus_handle(&self) -> i2c_master_bus_handle_t {
+            self.handle
         }
 
-        #[must_use]
-        pub fn rx_buffer_length(mut self, len: usize) -> Self {
-            self.rx_buf_len = len;
-            self
+        /// Probe device on the bus.
+        pub fn probe_device(
+            &mut self,
+            address: config::DeviceAddress,
+            timeout: TickType_t,
+        ) -> Result<(), EspError> {
+            esp!(unsafe { i2c_master_probe(self.handle, address.address(), timeout as i32) })
         }
 
-        #[must_use]
-        pub fn tx_buffer_length(mut self, len: usize) -> Self {
-            self.tx_buf_len = len;
-            self
+        pub fn device(
+            &mut self,
+            config: &config::DeviceConfig,
+        ) -> Result<I2cDeviceDriver<'d, &mut I2cDriver<'d>>, EspError> {
+            I2cDeviceDriver::new(self, config)
         }
 
-        #[must_use]
-        pub fn intr_flags(mut self, flags: EnumSet<InterruptType>) -> Self {
-            self.intr_flags = flags;
-            self
+        // Helper to use the embedded_hal traits.
+        fn read(
+            &mut self,
+            addr: u8,
+            buffer: &mut [u8],
+            timeout: TickType_t,
+        ) -> Result<(), EspError> {
+            self.device(&config::DeviceConfig::new(config::DeviceAddress::SevenBit(
+                addr,
+            )))?
+            .read(buffer, timeout)
+        }
+
+        // Helper to use the embedded_hal traits.
+        fn write(&mut self, addr: u8, bytes: &[u8], timeout: TickType_t) -> Result<(), EspError> {
+            self.device(&config::DeviceConfig::new(config::DeviceAddress::SevenBit(
+                addr,
+            )))?
+            .write(bytes, timeout)
+        }
+
+        // Helper to use the embedded_hal traits.
+        fn write_read(
+            &mut self,
+            addr: u8,
+            bytes: &[u8],
+            buffer: &mut [u8],
+            timeout: TickType_t,
+        ) -> Result<(), EspError> {
+            self.device(&config::DeviceConfig::new(config::DeviceAddress::SevenBit(
+                addr,
+            )))?
+            .write_read(bytes, buffer, timeout)
         }
     }
 
-    #[cfg(not(esp32c2))]
-    impl Default for SlaveConfig {
-        fn default() -> Self {
-            Self {
-                sda_pullup_enabled: true,
-                scl_pullup_enabled: true,
-                rx_buf_len: 0,
-                tx_buf_len: 0,
-                intr_flags: EnumSet::<InterruptType>::empty(),
-            }
+    unsafe impl<'d> Send for I2cDriver<'d> {}
+
+    impl<'d> Drop for I2cDriver<'d> {
+        fn drop(&mut self) {
+            esp!(unsafe { i2c_del_master_bus(self.handle) }).unwrap();
         }
     }
-}
 
-pub trait I2c: Send {
-    fn port() -> i2c_port_t;
-}
+    impl<'d> embedded_hal_0_2::blocking::i2c::Read for I2cDriver<'d> {
+        type Error = I2cError;
 
-pub struct I2cDriver<'d> {
-    i2c: u8,
-    _p: PhantomData<&'d mut ()>,
-}
+        fn read(&mut self, addr: u8, buffer: &mut [u8]) -> Result<(), Self::Error> {
+            Self::read(self, addr, buffer, BLOCK).map_err(to_i2c_err)
+        }
+    }
 
-impl<'d> I2cDriver<'d> {
-    pub fn new<I2C: I2c>(
-        _i2c: impl Peripheral<P = I2C> + 'd,
-        sda: impl Peripheral<P = impl InputPin + OutputPin> + 'd,
-        scl: impl Peripheral<P = impl InputPin + OutputPin> + 'd,
-        config: &config::Config,
-    ) -> Result<Self, EspError> {
-        // i2c_config_t documentation says that clock speed must be no higher than 1 MHz
-        if config.baudrate > 1.MHz().into() {
-            return Err(EspError::from_infallible::<ESP_ERR_INVALID_ARG>());
+    impl<'d> embedded_hal_0_2::blocking::i2c::Write for I2cDriver<'d> {
+        type Error = I2cError;
+
+        fn write(&mut self, addr: u8, bytes: &[u8]) -> Result<(), Self::Error> {
+            Self::write(self, addr, bytes, BLOCK).map_err(to_i2c_err)
         }
+    }
 
-        crate::into_ref!(sda, scl);
+    impl<'d> embedded_hal_0_2::blocking::i2c::WriteRead for I2cDriver<'d> {
+        type Error = I2cError;
 
-        let sys_config = i2c_config_t {
-            mode: i2c_mode_t_I2C_MODE_MASTER,
-            sda_io_num: sda.pin(),
-            sda_pullup_en: config.sda_pullup_enabled,
-            scl_io_num: scl.pin(),
-            scl_pullup_en: config.scl_pullup_enabled,
-            __bindgen_anon_1: i2c_config_t__bindgen_ty_1 {
-                master: i2c_config_t__bindgen_ty_1__bindgen_ty_1 {
-                    clk_speed: config.baudrate.into(),
-                },
-            },
-            ..Default::default()
-        };
+        fn write_read(
+            &mut self,
+            addr: u8,
+            bytes: &[u8],
+            buffer: &mut [u8],
+        ) -> Result<(), Self::Error> {
+            Self::write_read(self, addr, bytes, buffer, BLOCK).map_err(to_i2c_err)
+        }
+    }
 
-        esp!(unsafe { i2c_param_config(I2C::port(), &sys_config) })?;
+    impl<'d> embedded_hal::i2c::ErrorType for I2cDriver<'d> {
+        type Error = I2cError;
+    }
 
-        esp!(unsafe {
-            i2c_driver_install(
-                I2C::port(),
-                i2c_mode_t_I2C_MODE_MASTER,
-                0, // Not used in master mode
-                0, // Not used in master mode
-                InterruptType::to_native(config.intr_flags) as _,
-            )
-        })?;
+    impl<'d> embedded_hal::i2c::I2c<embedded_hal::i2c::SevenBitAddress> for I2cDriver<'d> {
+        fn read(&mut self, addr: u8, buffer: &mut [u8]) -> Result<(), Self::Error> {
+            Self::read(self, addr, buffer, BLOCK).map_err(to_i2c_err)
+        }
 
-        if let Some(timeout) = config.timeout {
-            esp!(unsafe { i2c_set_timeout(I2C::port(), timeout.0) })?;
+        fn write(&mut self, addr: u8, bytes: &[u8]) -> Result<(), Self::Error> {
+            Self::write(self, addr, bytes, BLOCK).map_err(to_i2c_err)
         }
 
-        Ok(I2cDriver {
-            i2c: I2C::port() as _,
-            _p: PhantomData,
-        })
+        fn write_read(
+            &mut self,
+            addr: u8,
+            bytes: &[u8],
+            buffer: &mut [u8],
+        ) -> Result<(), Self::Error> {
+            Self::write_read(self, addr, bytes, buffer, BLOCK).map_err(to_i2c_err)
+        }
+
+        fn transaction(
+            &mut self,
+            _addr: u8,
+            _operations: &mut [embedded_hal::i2c::Operation<'_>],
+        ) -> Result<(), Self::Error> {
+            unimplemented!("transactional not implemented")
+        }
     }
 
-    pub fn read(
-        &mut self,
-        addr: u8,
-        buffer: &mut [u8],
-        timeout: TickType_t,
-    ) -> Result<(), EspError> {
-        let mut command_link = CommandLink::new()?;
+    pub struct I2cDeviceDriver<'d, T>
+    where
+        T: Borrow<I2cDriver<'d>>,
+    {
+        _driver: T,
+        handle: i2c_master_dev_handle_t,
+        _p: PhantomData<&'d mut ()>,
+    }
 
-        command_link.master_start()?;
-        command_link.master_write_byte((addr << 1) | (i2c_rw_t_I2C_MASTER_READ as u8), true)?;
+    impl<'d, T> I2cDeviceDriver<'d, T>
+    where
+        T: Borrow<I2cDriver<'d>>,
+    {
+        pub fn new(driver: T, config: &config::DeviceConfig) -> Result<Self, EspError> {
+            let handle = init_device(driver.borrow().bus_handle(), &config)?;
+
+            Ok(I2cDeviceDriver {
+                _driver: driver,
+                handle,
+                _p: PhantomData,
+            })
+        }
 
-        if !buffer.is_empty() {
-            command_link.master_read(buffer, AckType::LastNack)?;
+        pub fn read(&mut self, buffer: &mut [u8], timeout: TickType_t) -> Result<(), EspError> {
+            esp!(unsafe {
+                i2c_master_receive(
+                    self.handle,
+                    buffer.as_mut_ptr().cast(),
+                    buffer.len(),
+                    timeout as i32,
+                )
+            })
         }
 
-        command_link.master_stop()?;
+        pub fn write(&mut self, bytes: &[u8], timeout: TickType_t) -> Result<(), EspError> {
+            esp!(unsafe {
+                i2c_master_transmit(
+                    self.handle,
+                    bytes.as_ptr().cast(),
+                    bytes.len(),
+                    timeout as i32,
+                )
+            })
+        }
 
-        self.cmd_begin(&command_link, timeout)
+        pub fn write_read(
+            &mut self,
+            bytes: &[u8],
+            buffer: &mut [u8],
+            timeout: TickType_t,
+        ) -> Result<(), EspError> {
+            esp!(unsafe {
+                i2c_master_transmit_receive(
+                    self.handle,
+                    bytes.as_ptr().cast(),
+                    bytes.len(),
+                    buffer.as_mut_ptr().cast(),
+                    buffer.len(),
+                    timeout as i32,
+                )
+            })
+        }
     }
 
-    pub fn write(&mut self, addr: u8, bytes: &[u8], timeout: TickType_t) -> Result<(), EspError> {
-        let mut command_link = CommandLink::new()?;
+    impl<'d, T> Drop for I2cDeviceDriver<'d, T>
+    where
+        T: Borrow<I2cDriver<'d>>,
+    {
+        fn drop(&mut self) {
+            esp!(unsafe { i2c_master_bus_rm_device(self.handle) }).unwrap();
+        }
+    }
 
-        command_link.master_start()?;
-        command_link.master_write_byte((addr << 1) | (i2c_rw_t_I2C_MASTER_WRITE as u8), true)?;
+    impl<'d, T> embedded_hal_0_2::blocking::i2c::Read for I2cDeviceDriver<'d, T>
+    where
+        T: Borrow<I2cDriver<'d>>,
+    {
+        type Error = I2cError;
 
-        if !bytes.is_empty() {
-            command_link.master_write(bytes, true)?;
+        fn read(&mut self, _addr: u8, buffer: &mut [u8]) -> Result<(), Self::Error> {
+            I2cDeviceDriver::read(self, buffer, BLOCK).map_err(to_i2c_err)
         }
+    }
+
+    impl<'d, T> embedded_hal_0_2::blocking::i2c::Write for I2cDeviceDriver<'d, T>
+    where
+        T: Borrow<I2cDriver<'d>>,
+    {
+        type Error = I2cError;
 
-        command_link.master_stop()?;
+        fn write(&mut self, _addr: u8, bytes: &[u8]) -> Result<(), Self::Error> {
+            I2cDeviceDriver::write(self, bytes, BLOCK).map_err(to_i2c_err)
+        }
+    }
 
-        self.cmd_begin(&command_link, timeout)
+    impl<'d, T> embedded_hal_0_2::blocking::i2c::WriteRead for I2cDeviceDriver<'d, T>
+    where
+        T: Borrow<I2cDriver<'d>>,
+    {
+        type Error = I2cError;
+
+        fn write_read(
+            &mut self,
+            _addr: u8,
+            bytes: &[u8],
+            buffer: &mut [u8],
+        ) -> Result<(), Self::Error> {
+            I2cDeviceDriver::write_read(self, bytes, buffer, BLOCK).map_err(to_i2c_err)
+        }
     }
 
-    pub fn write_read(
-        &mut self,
-        addr: u8,
-        bytes: &[u8],
-        buffer: &mut [u8],
-        timeout: TickType_t,
-    ) -> Result<(), EspError> {
-        let mut command_link = CommandLink::new()?;
+    impl<'d, T> embedded_hal::i2c::ErrorType for I2cDeviceDriver<'d, T>
+    where
+        T: Borrow<I2cDriver<'d>>,
+    {
+        type Error = I2cError;
+    }
 
-        command_link.master_start()?;
-        command_link.master_write_byte((addr << 1) | (i2c_rw_t_I2C_MASTER_WRITE as u8), true)?;
+    impl<'d, T> embedded_hal::i2c::I2c<embedded_hal::i2c::SevenBitAddress> for I2cDeviceDriver<'d, T>
+    where
+        T: Borrow<I2cDriver<'d>>,
+    {
+        fn read(&mut self, _addr: u8, buffer: &mut [u8]) -> Result<(), Self::Error> {
+            I2cDeviceDriver::read(self, buffer, BLOCK).map_err(to_i2c_err)
+        }
 
-        if !bytes.is_empty() {
-            command_link.master_write(bytes, true)?;
+        fn write(&mut self, _addr: u8, bytes: &[u8]) -> Result<(), Self::Error> {
+            I2cDeviceDriver::write(self, bytes, BLOCK).map_err(to_i2c_err)
         }
 
-        command_link.master_start()?;
-        command_link.master_write_byte((addr << 1) | (i2c_rw_t_I2C_MASTER_READ as u8), true)?;
+        fn write_read(
+            &mut self,
+            _addr: u8,
+            bytes: &[u8],
+            buffer: &mut [u8],
+        ) -> Result<(), Self::Error> {
+            I2cDeviceDriver::write_read(self, bytes, buffer, BLOCK).map_err(to_i2c_err)
+        }
 
-        if !buffer.is_empty() {
-            command_link.master_read(buffer, AckType::LastNack)?;
+        fn transaction(
+            &mut self,
+            _addr: u8,
+            _operations: &mut [embedded_hal::i2c::Operation<'_>],
+        ) -> Result<(), Self::Error> {
+            unimplemented!("transactional not implemented")
         }
+    }
+
+    unsafe impl<'d, T> Send for I2cDeviceDriver<'d, T> where T: Send + Borrow<I2cDriver<'d>> + 'd {}
 
-        command_link.master_stop()?;
+    // ------------------------------------------------------------------------------------------------
+    // ------------------------------------- Async ----------------------------------------------------
+    // ------------------------------------------------------------------------------------------------
 
-        self.cmd_begin(&command_link, timeout)
+    pub struct AsyncI2cDriver<'d> {
+        bus_lock: Mutex<EspRawMutex, ()>,
+        handle: i2c_master_bus_handle_t,
+        i2c: u8,
+        _p: PhantomData<&'d mut ()>,
     }
 
-    pub fn transaction(
-        &mut self,
-        address: u8,
-        operations: &mut [Operation<'_>],
-        timeout: TickType_t,
-    ) -> Result<(), EspError> {
-        let mut command_link = CommandLink::new()?;
-
-        let last_op_index = operations.len() - 1;
-        let mut prev_was_read = None;
-
-        for (i, operation) in operations.iter_mut().enumerate() {
-            match operation {
-                Operation::Read(buf) => {
-                    if Some(true) != prev_was_read {
-                        command_link.master_start()?;
-                        command_link.master_write_byte(
-                            (address << 1) | (i2c_rw_t_I2C_MASTER_READ as u8),
-                            true,
-                        )?;
-                    }
-                    prev_was_read = Some(true);
+    impl<'d> AsyncI2cDriver<'d> {
+        pub fn new<I2C: I2c>(
+            _i2c: impl Peripheral<P = I2C> + 'd,
+            sda: impl Peripheral<P = impl InputPin + OutputPin> + 'd,
+            scl: impl Peripheral<P = impl InputPin + OutputPin> + 'd,
+            config: &config::Config,
+        ) -> Result<Self, EspError> {
+            let handle = init_master_bus(_i2c, sda, scl, config, 1)?;
+
+            Ok(AsyncI2cDriver {
+                bus_lock: Mutex::new(()),
+                handle,
+                i2c: I2C::port() as _,
+                _p: PhantomData,
+            })
+        }
 
-                    if !buf.is_empty() {
-                        let ack = if i == last_op_index {
-                            AckType::LastNack
-                        } else {
-                            AckType::Ack
-                        };
+        pub fn host(&self) -> u8 {
+            self.i2c
+        }
 
-                        command_link.master_read(buf, ack)?;
-                    }
-                }
-                Operation::Write(buf) => {
-                    if Some(false) != prev_was_read {
-                        command_link.master_start()?;
-                        command_link.master_write_byte(
-                            (address << 1) | (i2c_rw_t_I2C_MASTER_WRITE as u8),
-                            true,
-                        )?;
-                    }
-                    prev_was_read = Some(false);
+        fn bus_handle(&self) -> i2c_master_bus_handle_t {
+            self.handle
+        }
 
-                    if !buf.is_empty() {
-                        command_link.master_write(buf, true)?;
-                    }
+        async fn acquire_bus<'a>(&'a self) -> MutexGuard<'a, EspRawMutex, ()> {
+            self.bus_lock.lock().await
+        }
+
+        pub fn device(
+            &mut self,
+            config: &config::DeviceConfig,
+        ) -> Result<AsyncI2cDeviceDriver<'d, &AsyncI2cDriver<'d>>, EspError> {
+            AsyncI2cDeviceDriver::new(self, config)
+        }
+
+        pub fn owned_device(
+            self,
+            config: &config::DeviceConfig,
+        ) -> Result<OwnedAsyncI2cDeviceDriver<'d>, EspError> {
+            OwnedAsyncI2cDeviceDriver::wrap(self, config)
+        }
+
+        async fn read(&mut self, address: u8, buffer: &mut [u8]) -> Result<(), EspError> {
+            self.device(&config::DeviceConfig::new(config::DeviceAddress::SevenBit(
+                address,
+            )))?
+            .read(buffer, BLOCK)
+            .await
+        }
+
+        async fn write(&mut self, address: u8, bytes: &[u8]) -> Result<(), EspError> {
+            self.device(&config::DeviceConfig::new(config::DeviceAddress::SevenBit(
+                address,
+            )))?
+            .write(bytes, BLOCK)
+            .await
+        }
+
+        async fn write_read(
+            &mut self,
+            address: u8,
+            bytes: &[u8],
+            buffer: &mut [u8],
+        ) -> Result<(), EspError> {
+            self.device(&config::DeviceConfig::new(config::DeviceAddress::SevenBit(
+                address,
+            )))?
+            .write_read(bytes, buffer, BLOCK)
+            .await
+        }
+    }
+
+    impl<'d> embedded_hal::i2c::ErrorType for AsyncI2cDriver<'d> {
+        type Error = I2cError;
+    }
+
+    impl<'d> embedded_hal_async::i2c::I2c<embedded_hal::i2c::SevenBitAddress> for AsyncI2cDriver<'d> {
+        async fn read(&mut self, address: u8, buffer: &mut [u8]) -> Result<(), Self::Error> {
+            self.read(address, buffer).await.map_err(to_i2c_err)
+        }
+
+        async fn write(&mut self, address: u8, bytes: &[u8]) -> Result<(), Self::Error> {
+            self.write(address, bytes).await.map_err(to_i2c_err)
+        }
+
+        async fn write_read(
+            &mut self,
+            address: u8,
+            bytes: &[u8],
+            buffer: &mut [u8],
+        ) -> Result<(), Self::Error> {
+            self.write_read(address, bytes, buffer)
+                .await
+                .map_err(to_i2c_err)
+        }
+
+        async fn transaction(
+            &mut self,
+            _address: u8,
+            _operations: &mut [Operation<'_>],
+        ) -> Result<(), Self::Error> {
+            unimplemented!("transactional not implemented")
+        }
+    }
+
+    impl<'d> Drop for AsyncI2cDriver<'d> {
+        fn drop(&mut self) {
+            loop {
+                if let Ok(_lock_guard) = self.bus_lock.try_lock() {
+                    esp!(unsafe { i2c_del_master_bus(self.handle) }).unwrap();
+                    break;
                 }
             }
         }
+    }
 
-        command_link.master_stop()?;
+    #[cfg(not(esp_idf_i2c_isr_iram_safe))]
+    pub struct AsyncI2cDeviceDriver<'d, T>
+    where
+        T: Borrow<AsyncI2cDriver<'d>>,
+    {
+        driver: T,
+        handle: i2c_master_dev_handle_t,
+        _p: PhantomData<&'d mut ()>,
+    }
 
-        self.cmd_begin(&command_link, timeout)
+    #[cfg(not(esp_idf_i2c_isr_iram_safe))]
+    impl<'d, T> AsyncI2cDeviceDriver<'d, T>
+    where
+        T: Borrow<AsyncI2cDriver<'d>>,
+    {
+        fn new(driver: T, config: &config::DeviceConfig) -> Result<Self, EspError> {
+            let handle = init_device(driver.borrow().bus_handle(), config)?;
+
+            Ok(Self {
+                driver,
+                handle,
+                _p: PhantomData,
+            })
+        }
     }
 
-    fn cmd_begin(
-        &mut self,
-        command_link: &CommandLink,
-        timeout: TickType_t,
-    ) -> Result<(), EspError> {
-        esp!(unsafe { i2c_master_cmd_begin(self.port(), command_link.0, timeout) })
+    #[cfg(not(esp_idf_i2c_isr_iram_safe))]
+    impl<'d, T> AsyncI2cDeviceDriver<'d, T>
+    where
+        T: Borrow<AsyncI2cDriver<'d>>,
+    {
+        pub async fn read(
+            &mut self,
+            buffer: &mut [u8],
+            timeout: TickType_t,
+        ) -> Result<(), EspError> {
+            let handle = self.handle;
+            let host = self.driver.borrow().host();
+
+            let _lock_guard = self.driver.borrow().acquire_bus().await;
+            enable_master_isr_callback(handle, host)?;
+            esp!(unsafe {
+                i2c_master_receive(
+                    handle,
+                    buffer.as_mut_ptr().cast(),
+                    buffer.len(),
+                    timeout as i32,
+                )
+            })?;
+
+            NOTIFIER[host as usize].wait().await;
+            disable_master_isr_callback(handle)?;
+            Ok(())
+        }
+
+        pub async fn write(&mut self, bytes: &[u8], timeout: TickType_t) -> Result<(), EspError> {
+            let handle = self.handle;
+            let host = self.driver.borrow().host();
+
+            let _lock_guard = self.driver.borrow().acquire_bus().await;
+            enable_master_isr_callback(handle, host)?;
+            esp!(unsafe {
+                i2c_master_transmit(handle, bytes.as_ptr().cast(), bytes.len(), timeout as i32)
+            })?;
+
+            NOTIFIER[host as usize].wait().await;
+            disable_master_isr_callback(handle)?;
+            Ok(())
+        }
+
+        pub async fn write_read(
+            &mut self,
+            bytes: &[u8],
+            buffer: &mut [u8],
+            timeout: TickType_t,
+        ) -> Result<(), EspError> {
+            let handle = self.handle;
+            let host = self.driver.borrow().host();
+
+            let _lock_guard = self.driver.borrow().acquire_bus().await;
+            enable_master_isr_callback(handle, host)?;
+            esp!(unsafe {
+                i2c_master_transmit_receive(
+                    handle,
+                    bytes.as_ptr().cast(),
+                    bytes.len(),
+                    buffer.as_mut_ptr().cast(),
+                    buffer.len(),
+                    timeout as i32,
+                )
+            })?;
+
+            NOTIFIER[host as usize].wait().await;
+            disable_master_isr_callback(handle)?;
+            Ok(())
+        }
     }
 
-    pub fn port(&self) -> i2c_port_t {
-        self.i2c as _
+    #[cfg(not(esp_idf_i2c_isr_iram_safe))]
+    unsafe impl<'d, T> Send for AsyncI2cDeviceDriver<'d, T> where
+        T: Send + Borrow<AsyncI2cDriver<'d>> + 'd
+    {
     }
-}
 
-impl<'d> Drop for I2cDriver<'d> {
-    fn drop(&mut self) {
-        esp!(unsafe { i2c_driver_delete(self.port()) }).unwrap();
+    #[cfg(not(esp_idf_i2c_isr_iram_safe))]
+    impl<'d, T> embedded_hal::i2c::ErrorType for AsyncI2cDeviceDriver<'d, T>
+    where
+        T: Borrow<AsyncI2cDriver<'d>>,
+    {
+        type Error = I2cError;
     }
-}
 
-unsafe impl<'d> Send for I2cDriver<'d> {}
+    #[cfg(not(esp_idf_i2c_isr_iram_safe))]
+    impl<'d, T> embedded_hal_async::i2c::I2c<embedded_hal::i2c::SevenBitAddress>
+        for AsyncI2cDeviceDriver<'d, T>
+    where
+        T: Borrow<AsyncI2cDriver<'d>>,
+    {
+        async fn read(&mut self, _address: u8, buffer: &mut [u8]) -> Result<(), Self::Error> {
+            Self::read(self, buffer, BLOCK).await.map_err(to_i2c_err)
+        }
+
+        async fn write(&mut self, _address: u8, bytes: &[u8]) -> Result<(), Self::Error> {
+            Self::write(self, bytes, BLOCK).await.map_err(to_i2c_err)
+        }
 
-impl<'d> embedded_hal_0_2::blocking::i2c::Read for I2cDriver<'d> {
-    type Error = I2cError;
+        async fn write_read(
+            &mut self,
+            _address: u8,
+            bytes: &[u8],
+            buffer: &mut [u8],
+        ) -> Result<(), Self::Error> {
+            Self::write_read(self, bytes, buffer, BLOCK)
+                .await
+                .map_err(to_i2c_err)
+        }
 
-    fn read(&mut self, addr: u8, buffer: &mut [u8]) -> Result<(), Self::Error> {
-        I2cDriver::read(self, addr, buffer, BLOCK).map_err(to_i2c_err)
+        async fn transaction(
+            &mut self,
+            _address: u8,
+            _operations: &mut [Operation<'_>],
+        ) -> Result<(), Self::Error> {
+            unimplemented!("transactional not implemented")
+        }
     }
-}
 
-impl<'d> embedded_hal_0_2::blocking::i2c::Write for I2cDriver<'d> {
-    type Error = I2cError;
+    impl<'d, T> Drop for AsyncI2cDeviceDriver<'d, T>
+    where
+        T: Borrow<AsyncI2cDriver<'d>>,
+    {
+        fn drop(&mut self) {
+            loop {
+                if let Ok(_lock_guard) = self.driver.borrow().bus_lock.try_lock() {
+                    esp!(unsafe { i2c_master_bus_rm_device(self.handle) }).unwrap();
+                    break;
+                }
+            }
+        }
+    }
 
-    fn write(&mut self, addr: u8, bytes: &[u8]) -> Result<(), Self::Error> {
-        I2cDriver::write(self, addr, bytes, BLOCK).map_err(to_i2c_err)
+    #[cfg(not(esp_idf_i2c_isr_iram_safe))]
+    pub struct OwnedAsyncI2cDeviceDriver<'d> {
+        driver: Option<AsyncI2cDriver<'d>>,
+        handle: i2c_master_dev_handle_t,
+        _p: PhantomData<&'d mut ()>,
     }
-}
 
-impl<'d> embedded_hal_0_2::blocking::i2c::WriteRead for I2cDriver<'d> {
-    type Error = I2cError;
+    #[cfg(not(esp_idf_i2c_isr_iram_safe))]
+    impl<'d> OwnedAsyncI2cDeviceDriver<'d> {
+        pub fn new<I2C: I2c>(
+            _i2c: impl Peripheral<P = I2C> + 'd,
+            sda: impl Peripheral<P = impl InputPin + OutputPin> + 'd,
+            scl: impl Peripheral<P = impl InputPin + OutputPin> + 'd,
+            bus_config: &config::Config,
+            device_config: &config::DeviceConfig,
+        ) -> Result<Self, EspError> {
+            let driver = AsyncI2cDriver::new(_i2c, sda, scl, bus_config)?;
+            Self::wrap(driver, device_config)
+        }
+
+        pub fn wrap(
+            driver: AsyncI2cDriver<'d>,
+            device_config: &config::DeviceConfig,
+        ) -> Result<Self, EspError> {
+            let handle = init_device(driver.bus_handle(), device_config)?;
+
+            enable_master_isr_callback(handle, driver.host())?;
 
-    fn write_read(&mut self, addr: u8, bytes: &[u8], buffer: &mut [u8]) -> Result<(), Self::Error> {
-        I2cDriver::write_read(self, addr, bytes, buffer, BLOCK).map_err(to_i2c_err)
+            Ok(Self {
+                driver: Some(driver),
+                handle,
+                _p: PhantomData,
+            })
+        }
+
+        pub fn release(mut self) -> Result<AsyncI2cDriver<'d>, EspError> {
+            let driver = self.driver.take().unwrap();
+            drop(self);
+            Ok(driver)
+        }
     }
-}
 
-impl<'d> embedded_hal::i2c::ErrorType for I2cDriver<'d> {
-    type Error = I2cError;
-}
+    #[cfg(not(esp_idf_i2c_isr_iram_safe))]
+    impl<'d> OwnedAsyncI2cDeviceDriver<'d> {
+        pub async fn read(
+            &mut self,
+            buffer: &mut [u8],
+            timeout: TickType_t,
+        ) -> Result<(), EspError> {
+            esp!(unsafe {
+                i2c_master_receive(
+                    self.handle,
+                    buffer.as_mut_ptr().cast(),
+                    buffer.len(),
+                    timeout as i32,
+                )
+            })?;
+
+            NOTIFIER[self.driver.as_ref().unwrap().host() as usize]
+                .wait()
+                .await;
+            Ok(())
+        }
 
-impl<'d> embedded_hal::i2c::I2c<embedded_hal::i2c::SevenBitAddress> for I2cDriver<'d> {
-    fn read(&mut self, addr: u8, buffer: &mut [u8]) -> Result<(), Self::Error> {
-        I2cDriver::read(self, addr, buffer, BLOCK).map_err(to_i2c_err)
+        pub async fn write(&mut self, bytes: &[u8], timeout: TickType_t) -> Result<(), EspError> {
+            esp!(unsafe {
+                i2c_master_transmit(
+                    self.handle,
+                    bytes.as_ptr().cast(),
+                    bytes.len(),
+                    timeout as i32,
+                )
+            })?;
+
+            NOTIFIER[self.driver.as_ref().unwrap().host() as usize]
+                .wait()
+                .await;
+            Ok(())
+        }
+
+        pub async fn write_read(
+            &mut self,
+            bytes: &[u8],
+            buffer: &mut [u8],
+            timeout: TickType_t,
+        ) -> Result<(), EspError> {
+            esp!(unsafe {
+                i2c_master_transmit_receive(
+                    self.handle,
+                    bytes.as_ptr().cast(),
+                    bytes.len(),
+                    buffer.as_mut_ptr().cast(),
+                    buffer.len(),
+                    timeout as i32,
+                )
+            })?;
+
+            NOTIFIER[self.driver.as_ref().unwrap().host() as usize]
+                .wait()
+                .await;
+            Ok(())
+        }
     }
 
-    fn write(&mut self, addr: u8, bytes: &[u8]) -> Result<(), Self::Error> {
-        I2cDriver::write(self, addr, bytes, BLOCK).map_err(to_i2c_err)
+    #[cfg(not(esp_idf_i2c_isr_iram_safe))]
+    unsafe impl<'d> Send for OwnedAsyncI2cDeviceDriver<'d> {}
+
+    #[cfg(not(esp_idf_i2c_isr_iram_safe))]
+    impl<'d> Drop for OwnedAsyncI2cDeviceDriver<'d> {
+        fn drop(&mut self) {
+            disable_master_isr_callback(self.handle).unwrap();
+            esp!(unsafe { i2c_master_bus_rm_device(self.handle) }).unwrap();
+        }
     }
 
-    fn write_read(&mut self, addr: u8, bytes: &[u8], buffer: &mut [u8]) -> Result<(), Self::Error> {
-        I2cDriver::write_read(self, addr, bytes, buffer, BLOCK).map_err(to_i2c_err)
+    #[cfg(not(esp_idf_i2c_isr_iram_safe))]
+    impl<'d> embedded_hal::i2c::ErrorType for OwnedAsyncI2cDeviceDriver<'d> {
+        type Error = I2cError;
     }
 
-    fn transaction(
-        &mut self,
-        address: u8,
-        operations: &mut [embedded_hal::i2c::Operation<'_>],
-    ) -> Result<(), Self::Error> {
-        I2cDriver::transaction(self, address, operations, BLOCK).map_err(to_i2c_err)
+    #[cfg(not(esp_idf_i2c_isr_iram_safe))]
+    impl<'d> embedded_hal_async::i2c::I2c<embedded_hal::i2c::SevenBitAddress>
+        for OwnedAsyncI2cDeviceDriver<'d>
+    {
+        async fn read(&mut self, _address: u8, buffer: &mut [u8]) -> Result<(), Self::Error> {
+            Self::read(self, buffer, BLOCK).await.map_err(to_i2c_err)
+        }
+
+        async fn write(&mut self, _address: u8, bytes: &[u8]) -> Result<(), Self::Error> {
+            Self::write(self, bytes, BLOCK).await.map_err(to_i2c_err)
+        }
+
+        async fn write_read(
+            &mut self,
+            _address: u8,
+            bytes: &[u8],
+            buffer: &mut [u8],
+        ) -> Result<(), Self::Error> {
+            Self::write_read(self, bytes, buffer, BLOCK)
+                .await
+                .map_err(to_i2c_err)
+        }
+
+        async fn transaction(
+            &mut self,
+            _address: u8,
+            _operations: &mut [Operation<'_>],
+        ) -> Result<(), Self::Error> {
+            unimplemented!("transactional not implemented")
+        }
     }
-}
 
-fn to_i2c_err(err: EspError) -> I2cError {
-    if err.code() == ESP_FAIL {
-        I2cError::new(ErrorKind::NoAcknowledge(NoAcknowledgeSource::Unknown), err)
-    } else {
-        I2cError::other(err)
+    #[cfg(not(esp32c2))]
+    pub struct I2cSlaveDriver<'d> {
+        i2c: u8,
+        handle: i2c_slave_dev_handle_t,
+        _p: PhantomData<&'d mut ()>,
     }
-}
 
-#[cfg(not(esp32c2))]
-pub struct I2cSlaveDriver<'d> {
-    i2c: u8,
-    _p: PhantomData<&'d mut ()>,
-}
+    #[cfg(not(esp32c2))]
+    unsafe impl<'d> Send for I2cSlaveDriver<'d> {}
+
+    #[cfg(not(esp32c2))]
+    impl<'d> I2cSlaveDriver<'d> {
+        pub fn new<I2C: I2c>(
+            _i2c: impl Peripheral<P = I2C> + 'd,
+            sda: impl Peripheral<P = impl InputPin + OutputPin> + 'd,
+            scl: impl Peripheral<P = impl InputPin + OutputPin> + 'd,
+            address: config::DeviceAddress,
+            config: &config::SlaveDeviceConfig,
+        ) -> Result<Self, EspError> {
+            let handle = init_slave_device(_i2c, sda, scl, address, config)?;
+
+            enable_slave_isr_callback(handle, I2C::port() as _)?;
+
+            Ok(Self {
+                i2c: I2C::port() as _,
+                handle,
+                _p: PhantomData,
+            })
+        }
+
+        pub fn read(&mut self, buffer: &mut [u8], _timeout: TickType_t) -> Result<usize, EspError> {
+            esp!(unsafe { i2c_slave_receive(self.handle, buffer.as_mut_ptr(), buffer.len()) })?;
 
-#[cfg(not(esp32c2))]
-unsafe impl<'d> Send for I2cSlaveDriver<'d> {}
+            todo!("How to block?");
+        }
+
+        pub async fn async_read(&mut self, buffer: &mut [u8]) -> Result<(), EspError> {
+            esp!(unsafe { i2c_slave_receive(self.handle, buffer.as_mut_ptr(), buffer.len()) })?;
+
+            NOTIFIER[self.port() as usize].wait().await;
+            Ok(())
+        }
+
+        pub fn write(&mut self, bytes: &[u8], timeout: TickType_t) -> Result<(), EspError> {
+            esp!(unsafe {
+                i2c_slave_transmit(
+                    self.handle,
+                    bytes.as_ptr(),
+                    bytes.len() as i32,
+                    timeout as i32,
+                )
+            })
+        }
 
-#[cfg(not(esp32c2))]
-impl<'d> I2cSlaveDriver<'d> {
-    pub fn new<I2C: I2c>(
+        pub fn port(&self) -> i2c_port_t {
+            self.i2c as _
+        }
+    }
+
+    #[cfg(not(esp32c2))]
+    impl<'d> Drop for I2cSlaveDriver<'d> {
+        fn drop(&mut self) {
+            disable_slave_isr_callback(self.handle).unwrap();
+            esp!(unsafe { i2c_del_slave_device(self.handle) }).unwrap();
+        }
+    }
+
+    fn init_master_bus<'d, I2C: I2c>(
         _i2c: impl Peripheral<P = I2C> + 'd,
         sda: impl Peripheral<P = impl InputPin + OutputPin> + 'd,
         scl: impl Peripheral<P = impl InputPin + OutputPin> + 'd,
-        slave_addr: u8,
-        config: &config::SlaveConfig,
-    ) -> Result<Self, EspError> {
+        config: &config::Config,
+        max_device_count: usize,
+    ) -> Result<i2c_master_bus_handle_t, EspError> {
         crate::into_ref!(sda, scl);
 
-        #[cfg(not(esp_idf_version = "4.3"))]
-        let sys_config = i2c_config_t {
-            mode: i2c_mode_t_I2C_MODE_SLAVE,
+        let config = i2c_master_bus_config_t {
             sda_io_num: sda.pin(),
-            sda_pullup_en: config.sda_pullup_enabled,
             scl_io_num: scl.pin(),
-            scl_pullup_en: config.scl_pullup_enabled,
-            __bindgen_anon_1: i2c_config_t__bindgen_ty_1 {
-                slave: i2c_config_t__bindgen_ty_1__bindgen_ty_2 {
-                    slave_addr: slave_addr as u16,
-                    addr_10bit_en: 0, // For now; to become configurable with embedded-hal V1.0
-                    maximum_speed: 0,
-                },
+            clk_source: config.source_clock.into(),
+            flags: {
+                let mut flags = i2c_master_bus_config_t__bindgen_ty_1::default();
+                flags.set_enable_internal_pullup(config.pullup_enabled as _);
+                flags
             },
-            ..Default::default()
+            glitch_ignore_cnt: config.glitch_ignore_cnt,
+            i2c_port: I2C::port() as i32,
+            intr_priority: 0,
+            trans_queue_depth: max_device_count,
         };
 
-        #[cfg(esp_idf_version = "4.3")]
-        #[deprecated(
-            note = "Using ESP-IDF 4.3 is untested, please upgrade to 4.4 or newer. Support will be removed in the next major release."
-        )]
-        let sys_config = i2c_config_t {
-            mode: i2c_mode_t_I2C_MODE_SLAVE,
-            sda_io_num: pins.sda.pin(),
-            sda_pullup_en: config.sda_pullup_enabled,
-            scl_io_num: pins.scl.pin(),
-            scl_pullup_en: config.scl_pullup_enabled,
-            __bindgen_anon_1: i2c_config_t__bindgen_ty_1 {
-                slave: i2c_config_t__bindgen_ty_1__bindgen_ty_2 {
-                    slave_addr: slave_addr as u16,
-                    addr_10bit_en: 0, // For now; to become configurable with embedded-hal V1.0
-                },
+        let mut handle: i2c_master_bus_handle_t = ptr::null_mut();
+
+        esp!(unsafe { i2c_new_master_bus(&config, &mut handle as _) })?;
+
+        Ok(handle)
+    }
+
+    fn init_device(
+        bus_handle: i2c_master_bus_handle_t,
+        config: &config::DeviceConfig,
+    ) -> Result<i2c_master_dev_handle_t, EspError> {
+        // i2c_config_t documentation says that clock speed must be no higher than 1 MHz
+        if config.baudrate > 1.MHz().into() {
+            return Err(EspError::from_infallible::<ESP_ERR_INVALID_ARG>());
+        }
+
+        let config = i2c_device_config_t {
+            device_address: config.address.address(),
+            dev_addr_length: config.address.clone().into(),
+            scl_speed_hz: config.baudrate.into(),
+        };
+
+        let mut handle: i2c_master_dev_handle_t = ptr::null_mut();
+
+        esp!(unsafe { i2c_master_bus_add_device(bus_handle, &config, &mut handle as _) })?;
+
+        Ok(handle)
+    }
+
+    #[cfg(not(esp32c2))]
+    fn init_slave_device<'d, I2C: I2c>(
+        _i2c: impl Peripheral<P = I2C> + 'd,
+        sda: impl Peripheral<P = impl InputPin + OutputPin> + 'd,
+        scl: impl Peripheral<P = impl InputPin + OutputPin> + 'd,
+        address: config::DeviceAddress,
+        config: &config::SlaveDeviceConfig,
+    ) -> Result<i2c_slave_dev_handle_t, EspError> {
+        crate::into_ref!(sda, scl);
+
+        let config = i2c_slave_config_t {
+            sda_io_num: sda.pin(),
+            scl_io_num: scl.pin(),
+            clk_source: config.source_clock.into(),
+            flags: {
+                let mut flags = i2c_slave_config_t__bindgen_ty_1::default();
+                flags.set_stretch_en(0);
+                flags.set_broadcast_en(config.broadcast_enable as _);
+                flags
             },
-            ..Default::default()
+            i2c_port: I2C::port() as i32,
+            intr_priority: 0,
+            slave_addr: address.address(),
+            addr_bit_len: address.into(),
+            send_buf_depth: config.send_buffer_depth,
         };
 
-        esp!(unsafe { i2c_param_config(I2C::port(), &sys_config) })?;
+        let mut handle: i2c_slave_dev_handle_t = ptr::null_mut();
+
+        esp!(unsafe { i2c_new_slave_device(&config, &mut handle as _) })?;
+
+        Ok(handle)
+    }
 
+    fn to_i2c_err(err: EspError) -> I2cError {
+        if err.code() == ESP_FAIL {
+            I2cError::new(ErrorKind::NoAcknowledge(NoAcknowledgeSource::Unknown), err)
+        } else {
+            I2cError::other(err)
+        }
+    }
+
+    #[cfg(not(esp_idf_i2c_isr_iram_safe))]
+    fn enable_master_isr_callback(
+        handle: i2c_master_dev_handle_t,
+        host: u8,
+    ) -> Result<(), EspError> {
         esp!(unsafe {
-            i2c_driver_install(
-                I2C::port(),
-                i2c_mode_t_I2C_MODE_SLAVE,
-                config.rx_buf_len,
-                config.tx_buf_len,
-                InterruptType::to_native(config.intr_flags) as _,
+            i2c_master_register_event_callbacks(
+                handle,
+                &i2c_master_event_callbacks_t {
+                    on_trans_done: Some(master_isr),
+                },
+                &NOTIFIER[host as usize] as *const _ as *mut _,
             )
-        })?;
+        })
+    }
 
-        Ok(Self {
-            i2c: I2C::port() as _,
-            _p: PhantomData,
+    #[cfg(not(esp_idf_i2c_isr_iram_safe))]
+    fn disable_master_isr_callback(handle: i2c_master_dev_handle_t) -> Result<(), EspError> {
+        esp!(unsafe {
+            i2c_master_register_event_callbacks(
+                handle,
+                &i2c_master_event_callbacks_t::default(),
+                ptr::null_mut(),
+            )
         })
     }
 
-    pub fn read(&mut self, buffer: &mut [u8], timeout: TickType_t) -> Result<usize, EspError> {
-        let n = unsafe {
-            i2c_slave_read_buffer(self.port(), buffer.as_mut_ptr(), buffer.len(), timeout)
-        };
+    #[cfg(not(esp_idf_i2c_isr_iram_safe))]
+    extern "C" fn master_isr(
+        _handle: i2c_master_dev_handle_t,
+        _data: *const i2c_master_event_data_t,
+        user_data: *mut c_void,
+    ) -> bool {
+        let notifier: &HalIsrNotification =
+            unsafe { (user_data as *const HalIsrNotification).as_ref() }.unwrap();
 
-        if n > 0 {
-            Ok(n as usize)
-        } else {
-            Err(EspError::from_infallible::<ESP_ERR_TIMEOUT>())
+        notifier.notify_lsb()
+    }
+
+    #[cfg(all(not(esp32c2), not(esp_idf_i2c_isr_iram_safe)))]
+    fn enable_slave_isr_callback(handle: i2c_slave_dev_handle_t, host: u8) -> Result<(), EspError> {
+        esp!(unsafe {
+            i2c_slave_register_event_callbacks(
+                handle,
+                &i2c_slave_event_callbacks_t {
+                    on_recv_done: Some(slave_isr),
+                    on_stretch_occur: None,
+                },
+                &NOTIFIER[host as usize] as *const _ as *mut _,
+            )
+        })
+    }
+
+    #[cfg(all(not(esp32c2), not(esp_idf_i2c_isr_iram_safe)))]
+    fn disable_slave_isr_callback(handle: i2c_slave_dev_handle_t) -> Result<(), EspError> {
+        esp!(unsafe {
+            i2c_slave_register_event_callbacks(
+                handle,
+                &i2c_slave_event_callbacks_t::default(),
+                ptr::null_mut(),
+            )
+        })
+    }
+
+    #[cfg(all(not(esp32c2), not(esp_idf_i2c_isr_iram_safe)))]
+    extern "C" fn slave_isr(
+        _handle: i2c_slave_dev_handle_t,
+        _data: *const i2c_slave_rx_done_event_data_t,
+        user_data: *mut c_void,
+    ) -> bool {
+        let notifier: &HalIsrNotification =
+            unsafe { (user_data as *const HalIsrNotification).as_ref() }.unwrap();
+
+        notifier.notify_lsb()
+    }
+
+    #[cfg(any(esp32c3, esp32c2, esp32c6))]
+    static NOTIFIER: [HalIsrNotification; 1] = [HalIsrNotification::new()];
+
+    #[cfg(not(any(esp32c3, esp32c2, esp32c6)))]
+    static NOTIFIER: [HalIsrNotification; 2] =
+        [HalIsrNotification::new(), HalIsrNotification::new()];
+}
+
+#[cfg(any(esp_idf_version_major = "4", esp_idf_version = "5.1"))]
+mod legacy {
+
+    use core::marker::PhantomData;
+    use core::time::Duration;
+
+    use embedded_hal::i2c::{ErrorKind, NoAcknowledgeSource};
+
+    use esp_idf_sys::*;
+
+    use crate::delay::*;
+    use crate::gpio::*;
+    use crate::interrupt::InterruptType;
+    use crate::peripheral::Peripheral;
+    use crate::units::*;
+
+    pub use embedded_hal::i2c::Operation;
+
+    use super::I2c;
+
+    crate::embedded_hal_error!(
+        I2cError,
+        embedded_hal::i2c::Error,
+        embedded_hal::i2c::ErrorKind
+    );
+
+    const APB_TICK_PERIOD_NS: u32 = 1_000_000_000 / 80_000_000;
+    #[derive(Copy, Clone, Debug)]
+    pub struct APBTickType(::core::ffi::c_int);
+    impl From<Duration> for APBTickType {
+        fn from(duration: Duration) -> Self {
+            APBTickType(
+                ((duration.as_nanos() + APB_TICK_PERIOD_NS as u128 - 1)
+                    / APB_TICK_PERIOD_NS as u128) as ::core::ffi::c_int,
+            )
         }
     }
 
-    pub fn write(&mut self, bytes: &[u8], timeout: TickType_t) -> Result<usize, EspError> {
-        let n = unsafe {
-            i2c_slave_write_buffer(self.port(), bytes.as_ptr(), bytes.len() as i32, timeout)
-        };
+    pub type I2cConfig = config::Config;
+    #[cfg(not(esp32c2))]
+    pub type I2cSlaveConfig = config::SlaveConfig;
+
+    /// I2C configuration
+    pub mod config {
+        use enumset::EnumSet;
+
+        use super::APBTickType;
+        use crate::{interrupt::InterruptType, units::*};
+
+        /// I2C Master configuration
+        #[derive(Debug, Clone)]
+        pub struct Config {
+            pub baudrate: Hertz,
+            pub sda_pullup_enabled: bool,
+            pub scl_pullup_enabled: bool,
+            pub timeout: Option<APBTickType>,
+            pub intr_flags: EnumSet<InterruptType>,
+        }
 
-        if n > 0 {
-            Ok(n as usize)
-        } else {
-            Err(EspError::from_infallible::<ESP_ERR_TIMEOUT>())
+        impl Config {
+            pub fn new() -> Self {
+                Default::default()
+            }
+
+            #[must_use]
+            pub fn baudrate(mut self, baudrate: Hertz) -> Self {
+                self.baudrate = baudrate;
+                self
+            }
+
+            #[must_use]
+            pub fn sda_enable_pullup(mut self, enable: bool) -> Self {
+                self.sda_pullup_enabled = enable;
+                self
+            }
+
+            #[must_use]
+            pub fn scl_enable_pullup(mut self, enable: bool) -> Self {
+                self.scl_pullup_enabled = enable;
+                self
+            }
+
+            #[must_use]
+            pub fn timeout(mut self, timeout: APBTickType) -> Self {
+                self.timeout = Some(timeout);
+                self
+            }
+
+            #[must_use]
+            pub fn intr_flags(mut self, flags: EnumSet<InterruptType>) -> Self {
+                self.intr_flags = flags;
+                self
+            }
+        }
+
+        impl Default for Config {
+            fn default() -> Self {
+                Self {
+                    baudrate: Hertz(1_000_000),
+                    sda_pullup_enabled: true,
+                    scl_pullup_enabled: true,
+                    timeout: None,
+                    intr_flags: EnumSet::<InterruptType>::empty(),
+                }
+            }
+        }
+
+        /// I2C Slave configuration
+        #[cfg(not(esp32c2))]
+        #[derive(Debug, Clone)]
+        pub struct SlaveConfig {
+            pub sda_pullup_enabled: bool,
+            pub scl_pullup_enabled: bool,
+            pub rx_buf_len: usize,
+            pub tx_buf_len: usize,
+            pub intr_flags: EnumSet<InterruptType>,
+        }
+
+        #[cfg(not(esp32c2))]
+        impl SlaveConfig {
+            pub fn new() -> Self {
+                Default::default()
+            }
+
+            #[must_use]
+            pub fn sda_enable_pullup(mut self, enable: bool) -> Self {
+                self.sda_pullup_enabled = enable;
+                self
+            }
+
+            #[must_use]
+            pub fn scl_enable_pullup(mut self, enable: bool) -> Self {
+                self.scl_pullup_enabled = enable;
+                self
+            }
+
+            #[must_use]
+            pub fn rx_buffer_length(mut self, len: usize) -> Self {
+                self.rx_buf_len = len;
+                self
+            }
+
+            #[must_use]
+            pub fn tx_buffer_length(mut self, len: usize) -> Self {
+                self.tx_buf_len = len;
+                self
+            }
+
+            #[must_use]
+            pub fn intr_flags(mut self, flags: EnumSet<InterruptType>) -> Self {
+                self.intr_flags = flags;
+                self
+            }
+        }
+
+        #[cfg(not(esp32c2))]
+        impl Default for SlaveConfig {
+            fn default() -> Self {
+                Self {
+                    sda_pullup_enabled: true,
+                    scl_pullup_enabled: true,
+                    rx_buf_len: 0,
+                    tx_buf_len: 0,
+                    intr_flags: EnumSet::<InterruptType>::empty(),
+                }
+            }
         }
     }
 
-    pub fn port(&self) -> i2c_port_t {
-        self.i2c as _
+    pub struct I2cDriver<'d> {
+        i2c: u8,
+        _p: PhantomData<&'d mut ()>,
     }
-}
 
-#[cfg(not(esp32c2))]
-impl<'d> Drop for I2cSlaveDriver<'d> {
-    fn drop(&mut self) {
-        esp!(unsafe { i2c_driver_delete(self.port()) }).unwrap();
+    impl<'d> I2cDriver<'d> {
+        pub fn new<I2C: I2c>(
+            _i2c: impl Peripheral<P = I2C> + 'd,
+            sda: impl Peripheral<P = impl InputPin + OutputPin> + 'd,
+            scl: impl Peripheral<P = impl InputPin + OutputPin> + 'd,
+            config: &config::Config,
+        ) -> Result<Self, EspError> {
+            // i2c_config_t documentation says that clock speed must be no higher than 1 MHz
+            if config.baudrate > 1.MHz().into() {
+                return Err(EspError::from_infallible::<ESP_ERR_INVALID_ARG>());
+            }
+
+            crate::into_ref!(sda, scl);
+
+            let sys_config = i2c_config_t {
+                mode: i2c_mode_t_I2C_MODE_MASTER,
+                sda_io_num: sda.pin(),
+                sda_pullup_en: config.sda_pullup_enabled,
+                scl_io_num: scl.pin(),
+                scl_pullup_en: config.scl_pullup_enabled,
+                __bindgen_anon_1: i2c_config_t__bindgen_ty_1 {
+                    master: i2c_config_t__bindgen_ty_1__bindgen_ty_1 {
+                        clk_speed: config.baudrate.into(),
+                    },
+                },
+                ..Default::default()
+            };
+
+            esp!(unsafe { i2c_param_config(I2C::port(), &sys_config) })?;
+
+            esp!(unsafe {
+                i2c_driver_install(
+                    I2C::port(),
+                    i2c_mode_t_I2C_MODE_MASTER,
+                    0, // Not used in master mode
+                    0, // Not used in master mode
+                    InterruptType::to_native(config.intr_flags) as _,
+                )
+            })?;
+
+            if let Some(timeout) = config.timeout {
+                esp!(unsafe { i2c_set_timeout(I2C::port(), timeout.0) })?;
+            }
+
+            Ok(I2cDriver {
+                i2c: I2C::port() as _,
+                _p: PhantomData,
+            })
+        }
+
+        pub fn read(
+            &mut self,
+            addr: u8,
+            buffer: &mut [u8],
+            timeout: TickType_t,
+        ) -> Result<(), EspError> {
+            let mut command_link = CommandLink::new()?;
+
+            command_link.master_start()?;
+            command_link.master_write_byte((addr << 1) | (i2c_rw_t_I2C_MASTER_READ as u8), true)?;
+
+            if !buffer.is_empty() {
+                command_link.master_read(buffer, AckType::LastNack)?;
+            }
+
+            command_link.master_stop()?;
+
+            self.cmd_begin(&command_link, timeout)
+        }
+
+        pub fn write(
+            &mut self,
+            addr: u8,
+            bytes: &[u8],
+            timeout: TickType_t,
+        ) -> Result<(), EspError> {
+            let mut command_link = CommandLink::new()?;
+
+            command_link.master_start()?;
+            command_link
+                .master_write_byte((addr << 1) | (i2c_rw_t_I2C_MASTER_WRITE as u8), true)?;
+
+            if !bytes.is_empty() {
+                command_link.master_write(bytes, true)?;
+            }
+
+            command_link.master_stop()?;
+
+            self.cmd_begin(&command_link, timeout)
+        }
+
+        pub fn write_read(
+            &mut self,
+            addr: u8,
+            bytes: &[u8],
+            buffer: &mut [u8],
+            timeout: TickType_t,
+        ) -> Result<(), EspError> {
+            let mut command_link = CommandLink::new()?;
+
+            command_link.master_start()?;
+            command_link
+                .master_write_byte((addr << 1) | (i2c_rw_t_I2C_MASTER_WRITE as u8), true)?;
+
+            if !bytes.is_empty() {
+                command_link.master_write(bytes, true)?;
+            }
+
+            command_link.master_start()?;
+            command_link.master_write_byte((addr << 1) | (i2c_rw_t_I2C_MASTER_READ as u8), true)?;
+
+            if !buffer.is_empty() {
+                command_link.master_read(buffer, AckType::LastNack)?;
+            }
+
+            command_link.master_stop()?;
+
+            self.cmd_begin(&command_link, timeout)
+        }
+
+        pub fn transaction(
+            &mut self,
+            address: u8,
+            operations: &mut [Operation<'_>],
+            timeout: TickType_t,
+        ) -> Result<(), EspError> {
+            let mut command_link = CommandLink::new()?;
+
+            let last_op_index = operations.len() - 1;
+            let mut prev_was_read = None;
+
+            for (i, operation) in operations.iter_mut().enumerate() {
+                match operation {
+                    Operation::Read(buf) => {
+                        if Some(true) != prev_was_read {
+                            command_link.master_start()?;
+                            command_link.master_write_byte(
+                                (address << 1) | (i2c_rw_t_I2C_MASTER_READ as u8),
+                                true,
+                            )?;
+                        }
+                        prev_was_read = Some(true);
+
+                        if !buf.is_empty() {
+                            let ack = if i == last_op_index {
+                                AckType::LastNack
+                            } else {
+                                AckType::Ack
+                            };
+
+                            command_link.master_read(buf, ack)?;
+                        }
+                    }
+                    Operation::Write(buf) => {
+                        if Some(false) != prev_was_read {
+                            command_link.master_start()?;
+                            command_link.master_write_byte(
+                                (address << 1) | (i2c_rw_t_I2C_MASTER_WRITE as u8),
+                                true,
+                            )?;
+                        }
+                        prev_was_read = Some(false);
+
+                        if !buf.is_empty() {
+                            command_link.master_write(buf, true)?;
+                        }
+                    }
+                }
+            }
+
+            command_link.master_stop()?;
+
+            self.cmd_begin(&command_link, timeout)
+        }
+
+        fn cmd_begin(
+            &mut self,
+            command_link: &CommandLink,
+            timeout: TickType_t,
+        ) -> Result<(), EspError> {
+            esp!(unsafe { i2c_master_cmd_begin(self.port(), command_link.0, timeout) })
+        }
+
+        pub fn port(&self) -> i2c_port_t {
+            self.i2c as _
+        }
     }
-}
 
-#[repr(u32)]
-enum AckType {
-    Ack = i2c_ack_type_t_I2C_MASTER_ACK,
-    #[allow(dead_code)]
-    Nack = i2c_ack_type_t_I2C_MASTER_NACK,
-    LastNack = i2c_ack_type_t_I2C_MASTER_LAST_NACK,
-}
+    impl<'d> Drop for I2cDriver<'d> {
+        fn drop(&mut self) {
+            esp!(unsafe { i2c_driver_delete(self.port()) }).unwrap();
+        }
+    }
+
+    unsafe impl<'d> Send for I2cDriver<'d> {}
+
+    impl<'d> embedded_hal_0_2::blocking::i2c::Read for I2cDriver<'d> {
+        type Error = I2cError;
+
+        fn read(&mut self, addr: u8, buffer: &mut [u8]) -> Result<(), Self::Error> {
+            I2cDriver::read(self, addr, buffer, BLOCK).map_err(to_i2c_err)
+        }
+    }
+
+    impl<'d> embedded_hal_0_2::blocking::i2c::Write for I2cDriver<'d> {
+        type Error = I2cError;
 
-struct CommandLink<'buffers>(i2c_cmd_handle_t, PhantomData<&'buffers u8>);
+        fn write(&mut self, addr: u8, bytes: &[u8]) -> Result<(), Self::Error> {
+            I2cDriver::write(self, addr, bytes, BLOCK).map_err(to_i2c_err)
+        }
+    }
 
-impl<'buffers> CommandLink<'buffers> {
-    fn new() -> Result<Self, EspError> {
-        let handle = unsafe { i2c_cmd_link_create() };
+    impl<'d> embedded_hal_0_2::blocking::i2c::WriteRead for I2cDriver<'d> {
+        type Error = I2cError;
 
-        if handle.is_null() {
-            return Err(EspError::from_infallible::<ESP_ERR_NO_MEM>());
+        fn write_read(
+            &mut self,
+            addr: u8,
+            bytes: &[u8],
+            buffer: &mut [u8],
+        ) -> Result<(), Self::Error> {
+            I2cDriver::write_read(self, addr, bytes, buffer, BLOCK).map_err(to_i2c_err)
         }
+    }
 
-        Ok(CommandLink(handle, PhantomData))
+    impl<'d> embedded_hal::i2c::ErrorType for I2cDriver<'d> {
+        type Error = I2cError;
     }
 
-    fn master_start(&mut self) -> Result<(), EspError> {
-        esp!(unsafe { i2c_master_start(self.0) })
+    impl<'d> embedded_hal::i2c::I2c<embedded_hal::i2c::SevenBitAddress> for I2cDriver<'d> {
+        fn read(&mut self, addr: u8, buffer: &mut [u8]) -> Result<(), Self::Error> {
+            I2cDriver::read(self, addr, buffer, BLOCK).map_err(to_i2c_err)
+        }
+
+        fn write(&mut self, addr: u8, bytes: &[u8]) -> Result<(), Self::Error> {
+            I2cDriver::write(self, addr, bytes, BLOCK).map_err(to_i2c_err)
+        }
+
+        fn write_read(
+            &mut self,
+            addr: u8,
+            bytes: &[u8],
+            buffer: &mut [u8],
+        ) -> Result<(), Self::Error> {
+            I2cDriver::write_read(self, addr, bytes, buffer, BLOCK).map_err(to_i2c_err)
+        }
+
+        fn transaction(
+            &mut self,
+            address: u8,
+            operations: &mut [embedded_hal::i2c::Operation<'_>],
+        ) -> Result<(), Self::Error> {
+            I2cDriver::transaction(self, address, operations, BLOCK).map_err(to_i2c_err)
+        }
     }
 
-    fn master_stop(&mut self) -> Result<(), EspError> {
-        esp!(unsafe { i2c_master_stop(self.0) })
+    fn to_i2c_err(err: EspError) -> I2cError {
+        if err.code() == ESP_FAIL {
+            I2cError::new(ErrorKind::NoAcknowledge(NoAcknowledgeSource::Unknown), err)
+        } else {
+            I2cError::other(err)
+        }
+    }
+
+    #[cfg(not(esp32c2))]
+    pub struct I2cSlaveDriver<'d> {
+        i2c: u8,
+        _p: PhantomData<&'d mut ()>,
+    }
+
+    #[cfg(not(esp32c2))]
+    unsafe impl<'d> Send for I2cSlaveDriver<'d> {}
+
+    #[cfg(not(esp32c2))]
+    impl<'d> I2cSlaveDriver<'d> {
+        pub fn new<I2C: I2c>(
+            _i2c: impl Peripheral<P = I2C> + 'd,
+            sda: impl Peripheral<P = impl InputPin + OutputPin> + 'd,
+            scl: impl Peripheral<P = impl InputPin + OutputPin> + 'd,
+            slave_addr: u8,
+            config: &config::SlaveConfig,
+        ) -> Result<Self, EspError> {
+            crate::into_ref!(sda, scl);
+
+            #[cfg(not(esp_idf_version = "4.3"))]
+            let sys_config = i2c_config_t {
+                mode: i2c_mode_t_I2C_MODE_SLAVE,
+                sda_io_num: sda.pin(),
+                sda_pullup_en: config.sda_pullup_enabled,
+                scl_io_num: scl.pin(),
+                scl_pullup_en: config.scl_pullup_enabled,
+                __bindgen_anon_1: i2c_config_t__bindgen_ty_1 {
+                    slave: i2c_config_t__bindgen_ty_1__bindgen_ty_2 {
+                        slave_addr: slave_addr as u16,
+                        addr_10bit_en: 0, // For now; to become configurable with embedded-hal V1.0
+                        maximum_speed: 0,
+                    },
+                },
+                ..Default::default()
+            };
+
+            #[cfg(esp_idf_version = "4.3")]
+            #[deprecated(
+                note = "Using ESP-IDF 4.3 is untested, please upgrade to 4.4 or newer. Support will be removed in the next major release."
+            )]
+            let sys_config = i2c_config_t {
+                mode: i2c_mode_t_I2C_MODE_SLAVE,
+                sda_io_num: pins.sda.pin(),
+                sda_pullup_en: config.sda_pullup_enabled,
+                scl_io_num: pins.scl.pin(),
+                scl_pullup_en: config.scl_pullup_enabled,
+                __bindgen_anon_1: i2c_config_t__bindgen_ty_1 {
+                    slave: i2c_config_t__bindgen_ty_1__bindgen_ty_2 {
+                        slave_addr: slave_addr as u16,
+                        addr_10bit_en: 0, // For now; to become configurable with embedded-hal V1.0
+                    },
+                },
+                ..Default::default()
+            };
+
+            esp!(unsafe { i2c_param_config(I2C::port(), &sys_config) })?;
+
+            esp!(unsafe {
+                i2c_driver_install(
+                    I2C::port(),
+                    i2c_mode_t_I2C_MODE_SLAVE,
+                    config.rx_buf_len,
+                    config.tx_buf_len,
+                    InterruptType::to_native(config.intr_flags) as _,
+                )
+            })?;
+
+            Ok(Self {
+                i2c: I2C::port() as _,
+                _p: PhantomData,
+            })
+        }
+
+        pub fn read(&mut self, buffer: &mut [u8], timeout: TickType_t) -> Result<usize, EspError> {
+            let n = unsafe {
+                i2c_slave_read_buffer(self.port(), buffer.as_mut_ptr(), buffer.len(), timeout)
+            };
+
+            if n > 0 {
+                Ok(n as usize)
+            } else {
+                Err(EspError::from_infallible::<ESP_ERR_TIMEOUT>())
+            }
+        }
+
+        pub fn write(&mut self, bytes: &[u8], timeout: TickType_t) -> Result<usize, EspError> {
+            let n = unsafe {
+                i2c_slave_write_buffer(self.port(), bytes.as_ptr(), bytes.len() as i32, timeout)
+            };
+
+            if n > 0 {
+                Ok(n as usize)
+            } else {
+                Err(EspError::from_infallible::<ESP_ERR_TIMEOUT>())
+            }
+        }
+
+        pub fn port(&self) -> i2c_port_t {
+            self.i2c as _
+        }
     }
 
-    fn master_write_byte(&mut self, data: u8, ack_en: bool) -> Result<(), EspError> {
-        esp!(unsafe { i2c_master_write_byte(self.0, data, ack_en) })
+    #[cfg(not(esp32c2))]
+    impl<'d> Drop for I2cSlaveDriver<'d> {
+        fn drop(&mut self) {
+            esp!(unsafe { i2c_driver_delete(self.port()) }).unwrap();
+        }
     }
 
-    fn master_write(&mut self, buf: &'buffers [u8], ack_en: bool) -> Result<(), EspError> {
-        esp!(unsafe { i2c_master_write(self.0, buf.as_ptr(), buf.len(), ack_en,) })
+    #[repr(u32)]
+    enum AckType {
+        Ack = i2c_ack_type_t_I2C_MASTER_ACK,
+        #[allow(dead_code)]
+        Nack = i2c_ack_type_t_I2C_MASTER_NACK,
+        LastNack = i2c_ack_type_t_I2C_MASTER_LAST_NACK,
     }
 
-    fn master_read(&mut self, buf: &'buffers mut [u8], ack: AckType) -> Result<(), EspError> {
-        esp!(unsafe { i2c_master_read(self.0, buf.as_mut_ptr().cast(), buf.len(), ack as u32,) })
+    struct CommandLink<'buffers>(i2c_cmd_handle_t, PhantomData<&'buffers u8>);
+
+    impl<'buffers> CommandLink<'buffers> {
+        fn new() -> Result<Self, EspError> {
+            let handle = unsafe { i2c_cmd_link_create() };
+
+            if handle.is_null() {
+                return Err(EspError::from_infallible::<ESP_ERR_NO_MEM>());
+            }
+
+            Ok(CommandLink(handle, PhantomData))
+        }
+
+        fn master_start(&mut self) -> Result<(), EspError> {
+            esp!(unsafe { i2c_master_start(self.0) })
+        }
+
+        fn master_stop(&mut self) -> Result<(), EspError> {
+            esp!(unsafe { i2c_master_stop(self.0) })
+        }
+
+        fn master_write_byte(&mut self, data: u8, ack_en: bool) -> Result<(), EspError> {
+            esp!(unsafe { i2c_master_write_byte(self.0, data, ack_en) })
+        }
+
+        fn master_write(&mut self, buf: &'buffers [u8], ack_en: bool) -> Result<(), EspError> {
+            esp!(unsafe { i2c_master_write(self.0, buf.as_ptr(), buf.len(), ack_en,) })
+        }
+
+        fn master_read(&mut self, buf: &'buffers mut [u8], ack: AckType) -> Result<(), EspError> {
+            esp!(unsafe { i2c_master_read(self.0, buf.as_mut_ptr().cast(), buf.len(), ack as u32) })
+        }
     }
-}
 
-impl<'buffers> Drop for CommandLink<'buffers> {
-    fn drop(&mut self) {
-        unsafe {
-            i2c_cmd_link_delete(self.0);
+    impl<'buffers> Drop for CommandLink<'buffers> {
+        fn drop(&mut self) {
+            unsafe {
+                i2c_cmd_link_delete(self.0);
+            }
         }
     }
 }

From fab93205bc7b9b2ec3b66d1b10794381d258ed91 Mon Sep 17 00:00:00 2001
From: teamplayer3 <alex.teamplayer@gmail.com>
Date: Mon, 25 Mar 2024 12:30:32 +0100
Subject: [PATCH 03/11] refactoring

---
 src/i2c.rs | 70 +++++++++++++++++++++++++++---------------------------
 1 file changed, 35 insertions(+), 35 deletions(-)

diff --git a/src/i2c.rs b/src/i2c.rs
index 98e923d48e7..f4a739db9a8 100644
--- a/src/i2c.rs
+++ b/src/i2c.rs
@@ -531,8 +531,6 @@ mod i2c {
         }
     }
 
-    unsafe impl<'d, T> Send for I2cDeviceDriver<'d, T> where T: Send + Borrow<I2cDriver<'d>> + 'd {}
-
     // ------------------------------------------------------------------------------------------------
     // ------------------------------------- Async ----------------------------------------------------
     // ------------------------------------------------------------------------------------------------
@@ -540,7 +538,7 @@ mod i2c {
     pub struct AsyncI2cDriver<'d> {
         bus_lock: Mutex<EspRawMutex, ()>,
         handle: i2c_master_bus_handle_t,
-        i2c: u8,
+        port: u8,
         _p: PhantomData<&'d mut ()>,
     }
 
@@ -556,13 +554,13 @@ mod i2c {
             Ok(AsyncI2cDriver {
                 bus_lock: Mutex::new(()),
                 handle,
-                i2c: I2C::port() as _,
+                port: I2C::port() as _,
                 _p: PhantomData,
             })
         }
 
-        pub fn host(&self) -> u8 {
-            self.i2c
+        pub fn port(&self) -> u8 {
+            self.port
         }
 
         fn bus_handle(&self) -> i2c_master_bus_handle_t {
@@ -574,7 +572,7 @@ mod i2c {
         }
 
         pub fn device(
-            &mut self,
+            &self,
             config: &config::DeviceConfig,
         ) -> Result<AsyncI2cDeviceDriver<'d, &AsyncI2cDriver<'d>>, EspError> {
             AsyncI2cDeviceDriver::new(self, config)
@@ -650,6 +648,8 @@ mod i2c {
         }
     }
 
+    unsafe impl<'d> Send for AsyncI2cDriver<'d> {}
+
     impl<'d> Drop for AsyncI2cDriver<'d> {
         fn drop(&mut self) {
             loop {
@@ -698,10 +698,11 @@ mod i2c {
             timeout: TickType_t,
         ) -> Result<(), EspError> {
             let handle = self.handle;
-            let host = self.driver.borrow().host();
+            let driver = self.driver.borrow();
+            let port = driver.port();
 
-            let _lock_guard = self.driver.borrow().acquire_bus().await;
-            enable_master_isr_callback(handle, host)?;
+            let _lock_guard = driver.acquire_bus().await;
+            enable_master_dev_isr_callback(handle, port)?;
             esp!(unsafe {
                 i2c_master_receive(
                     handle,
@@ -711,23 +712,24 @@ mod i2c {
                 )
             })?;
 
-            NOTIFIER[host as usize].wait().await;
-            disable_master_isr_callback(handle)?;
+            NOTIFIER[port as usize].wait().await;
+            disable_master_dev_isr_callback(handle)?;
             Ok(())
         }
 
         pub async fn write(&mut self, bytes: &[u8], timeout: TickType_t) -> Result<(), EspError> {
             let handle = self.handle;
-            let host = self.driver.borrow().host();
+            let driver = self.driver.borrow();
+            let port = driver.port();
 
-            let _lock_guard = self.driver.borrow().acquire_bus().await;
-            enable_master_isr_callback(handle, host)?;
+            let _lock_guard = driver.acquire_bus().await;
+            enable_master_dev_isr_callback(handle, port)?;
             esp!(unsafe {
                 i2c_master_transmit(handle, bytes.as_ptr().cast(), bytes.len(), timeout as i32)
             })?;
 
-            NOTIFIER[host as usize].wait().await;
-            disable_master_isr_callback(handle)?;
+            NOTIFIER[port as usize].wait().await;
+            disable_master_dev_isr_callback(handle)?;
             Ok(())
         }
 
@@ -738,10 +740,11 @@ mod i2c {
             timeout: TickType_t,
         ) -> Result<(), EspError> {
             let handle = self.handle;
-            let host = self.driver.borrow().host();
+            let driver = self.driver.borrow();
+            let port = driver.port();
 
-            let _lock_guard = self.driver.borrow().acquire_bus().await;
-            enable_master_isr_callback(handle, host)?;
+            let _lock_guard = driver.acquire_bus().await;
+            enable_master_dev_isr_callback(handle, port)?;
             esp!(unsafe {
                 i2c_master_transmit_receive(
                     handle,
@@ -753,8 +756,8 @@ mod i2c {
                 )
             })?;
 
-            NOTIFIER[host as usize].wait().await;
-            disable_master_isr_callback(handle)?;
+            NOTIFIER[port as usize].wait().await;
+            disable_master_dev_isr_callback(handle)?;
             Ok(())
         }
     }
@@ -847,7 +850,7 @@ mod i2c {
         ) -> Result<Self, EspError> {
             let handle = init_device(driver.bus_handle(), device_config)?;
 
-            enable_master_isr_callback(handle, driver.host())?;
+            enable_master_dev_isr_callback(handle, driver.port())?;
 
             Ok(Self {
                 driver: Some(driver),
@@ -879,9 +882,8 @@ mod i2c {
                 )
             })?;
 
-            NOTIFIER[self.driver.as_ref().unwrap().host() as usize]
-                .wait()
-                .await;
+            let port = self.driver.as_ref().unwrap().port() as usize;
+            NOTIFIER[port].wait().await;
             Ok(())
         }
 
@@ -895,9 +897,8 @@ mod i2c {
                 )
             })?;
 
-            NOTIFIER[self.driver.as_ref().unwrap().host() as usize]
-                .wait()
-                .await;
+            let port = self.driver.as_ref().unwrap().port() as usize;
+            NOTIFIER[port].wait().await;
             Ok(())
         }
 
@@ -918,9 +919,8 @@ mod i2c {
                 )
             })?;
 
-            NOTIFIER[self.driver.as_ref().unwrap().host() as usize]
-                .wait()
-                .await;
+            let port = self.driver.as_ref().unwrap().port() as usize;
+            NOTIFIER[port].wait().await;
             Ok(())
         }
     }
@@ -931,7 +931,7 @@ mod i2c {
     #[cfg(not(esp_idf_i2c_isr_iram_safe))]
     impl<'d> Drop for OwnedAsyncI2cDeviceDriver<'d> {
         fn drop(&mut self) {
-            disable_master_isr_callback(self.handle).unwrap();
+            disable_master_dev_isr_callback(self.handle).unwrap();
             esp!(unsafe { i2c_master_bus_rm_device(self.handle) }).unwrap();
         }
     }
@@ -1136,7 +1136,7 @@ mod i2c {
     }
 
     #[cfg(not(esp_idf_i2c_isr_iram_safe))]
-    fn enable_master_isr_callback(
+    fn enable_master_dev_isr_callback(
         handle: i2c_master_dev_handle_t,
         host: u8,
     ) -> Result<(), EspError> {
@@ -1152,7 +1152,7 @@ mod i2c {
     }
 
     #[cfg(not(esp_idf_i2c_isr_iram_safe))]
-    fn disable_master_isr_callback(handle: i2c_master_dev_handle_t) -> Result<(), EspError> {
+    fn disable_master_dev_isr_callback(handle: i2c_master_dev_handle_t) -> Result<(), EspError> {
         esp!(unsafe {
             i2c_master_register_event_callbacks(
                 handle,

From 31150f997853a147c247c3847fe2b4802aaf4763 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Alexander=20H=C3=BCbener?= <alex.teamplayer@gmail.com>
Date: Wed, 5 Jun 2024 18:38:05 +0200
Subject: [PATCH 04/11] add driver version gate

---
 src/i2c.rs | 51 +++++++++++++++++++++++++++++++++++++++++++++------
 1 file changed, 45 insertions(+), 6 deletions(-)

diff --git a/src/i2c.rs b/src/i2c.rs
index f4a739db9a8..2fe3fba2525 100644
--- a/src/i2c.rs
+++ b/src/i2c.rs
@@ -1,7 +1,4 @@
-#[cfg(all(not(esp_idf_version_major = "4"), not(esp_idf_version = "5.1")))]
-pub use i2c::*;
-
-#[cfg(any(esp_idf_version_major = "4", esp_idf_version = "5.1"))]
+pub use i2c as beta;
 pub use legacy::*;
 
 use esp_idf_sys::*;
@@ -10,7 +7,38 @@ pub trait I2c: Send {
     fn port() -> i2c_port_t;
 }
 
-#[cfg(all(not(esp_idf_version_major = "4"), not(esp_idf_version = "5.1")))]
+#[repr(u8)]
+enum UsedDriver {
+    None = 0,
+    Legacy = 1,
+    Beta = 2,
+}
+
+// 0 -> no driver, 1 -> legacy driver, 2 -> beta driver
+static DRIVER_IN_USE: core::sync::atomic::AtomicU8 = core::sync::atomic::AtomicU8::new(0);
+
+fn check_and_set_beta_driver() {
+    if let Err(super::UsedDriver::Legacy) = DRIVER_IN_USE.compare_exchange(
+        super::UsedDriver::None,
+        super::UsedDriver::Beta,
+        core::sync::atomic::Ordering::Relaxed,
+        core::sync::atomic::Ordering::Relaxed,
+    ) {
+        panic!("Legacy I2C driver is already in use. Either legacy driver or beta driver can be used at a time.");
+    }
+}
+
+fn check_and_set_legacy_driver() {
+    if let Err(super::UsedDriver::Beta) = DRIVER_IN_USE.compare_exchange(
+        super::UsedDriver::None,
+        super::UsedDriver::Legacy,
+        core::sync::atomic::Ordering::Relaxed,
+        core::sync::atomic::Ordering::Relaxed,
+    ) {
+        panic!("Beta I2C driver is already in use. Either legacy driver or beta driver can be used at a time.");
+    }
+}
+
 mod i2c {
 
     use core::borrow::Borrow;
@@ -35,6 +63,8 @@ mod i2c {
     pub use embedded_hal::i2c::Operation;
 
     use super::I2c;
+    use super::BETA_DRIVER_IN_USE;
+    use super::DRIVER_IN_USE;
 
     crate::embedded_hal_error!(
         I2cError,
@@ -246,6 +276,8 @@ mod i2c {
             scl: impl Peripheral<P = impl InputPin + OutputPin> + 'd,
             config: &config::Config,
         ) -> Result<Self, EspError> {
+            super::check_and_set_beta_driver();
+
             let handle = init_master_bus(_i2c, sda, scl, config, 0)?;
 
             Ok(I2cDriver {
@@ -549,6 +581,8 @@ mod i2c {
             scl: impl Peripheral<P = impl InputPin + OutputPin> + 'd,
             config: &config::Config,
         ) -> Result<Self, EspError> {
+            super::check_and_set_beta_driver();
+
             let handle = init_master_bus(_i2c, sda, scl, config, 1)?;
 
             Ok(AsyncI2cDriver {
@@ -992,6 +1026,8 @@ mod i2c {
             address: config::DeviceAddress,
             config: &config::SlaveDeviceConfig,
         ) -> Result<Self, EspError> {
+            super::check_and_set_beta_driver();
+
             let handle = init_slave_device(_i2c, sda, scl, address, config)?;
 
             enable_slave_isr_callback(handle, I2C::port() as _)?;
@@ -1219,7 +1255,6 @@ mod i2c {
         [HalIsrNotification::new(), HalIsrNotification::new()];
 }
 
-#[cfg(any(esp_idf_version_major = "4", esp_idf_version = "5.1"))]
 mod legacy {
 
     use core::marker::PhantomData;
@@ -1400,6 +1435,8 @@ mod legacy {
             scl: impl Peripheral<P = impl InputPin + OutputPin> + 'd,
             config: &config::Config,
         ) -> Result<Self, EspError> {
+            super::check_and_set_legacy_driver();
+
             // i2c_config_t documentation says that clock speed must be no higher than 1 MHz
             if config.baudrate > 1.MHz().into() {
                 return Err(EspError::from_infallible::<ESP_ERR_INVALID_ARG>());
@@ -1675,6 +1712,8 @@ mod legacy {
             slave_addr: u8,
             config: &config::SlaveConfig,
         ) -> Result<Self, EspError> {
+            super::check_and_set_legacy_driver();
+
             crate::into_ref!(sda, scl);
 
             #[cfg(not(esp_idf_version = "4.3"))]

From e61b32ed1bbf64e8df335e310047211f35c26372 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Alexander=20H=C3=BCbener?= <alex.teamplayer@gmail.com>
Date: Wed, 5 Jun 2024 19:36:34 +0200
Subject: [PATCH 05/11] init with enum UsedDriver::None

---
 src/i2c.rs | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/i2c.rs b/src/i2c.rs
index 2fe3fba2525..5629535a20b 100644
--- a/src/i2c.rs
+++ b/src/i2c.rs
@@ -15,7 +15,7 @@ enum UsedDriver {
 }
 
 // 0 -> no driver, 1 -> legacy driver, 2 -> beta driver
-static DRIVER_IN_USE: core::sync::atomic::AtomicU8 = core::sync::atomic::AtomicU8::new(0);
+static DRIVER_IN_USE: core::sync::atomic::AtomicU8 = core::sync::atomic::AtomicU8::new(UsedDriver::None);
 
 fn check_and_set_beta_driver() {
     if let Err(super::UsedDriver::Legacy) = DRIVER_IN_USE.compare_exchange(

From 8866b4b66ea42b1a63b906a2c4f6252a8ab8106f Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Alexander=20H=C3=BCbener?= <alex.teamplayer@gmail.com>
Date: Thu, 6 Jun 2024 14:25:14 +0200
Subject: [PATCH 06/11] export new driver only if esp-idf version > 5.1

---
 src/i2c.rs | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/i2c.rs b/src/i2c.rs
index 5629535a20b..f44f03c1e16 100644
--- a/src/i2c.rs
+++ b/src/i2c.rs
@@ -1,4 +1,3 @@
-pub use i2c as beta;
 pub use legacy::*;
 
 use esp_idf_sys::*;
@@ -39,7 +38,8 @@ fn check_and_set_legacy_driver() {
     }
 }
 
-mod i2c {
+#[cfg(all(not(esp_idf_version_major = "4"), not(esp_idf_version = "5.1")))]
+pub mod beta {
 
     use core::borrow::Borrow;
     use core::ffi::c_void;

From ab80b661f2c9519bbd5e6b4229f6640677e0d68c Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Alexander=20H=C3=BCbener?= <alex.teamplayer@gmail.com>
Date: Thu, 6 Jun 2024 14:25:42 +0200
Subject: [PATCH 07/11] fix enum type usage

---
 src/i2c.rs | 22 +++++++++++++---------
 1 file changed, 13 insertions(+), 9 deletions(-)

diff --git a/src/i2c.rs b/src/i2c.rs
index f44f03c1e16..abd6caf22c0 100644
--- a/src/i2c.rs
+++ b/src/i2c.rs
@@ -14,27 +14,31 @@ enum UsedDriver {
 }
 
 // 0 -> no driver, 1 -> legacy driver, 2 -> beta driver
-static DRIVER_IN_USE: core::sync::atomic::AtomicU8 = core::sync::atomic::AtomicU8::new(UsedDriver::None);
+static DRIVER_IN_USE: core::sync::atomic::AtomicU8 =
+    core::sync::atomic::AtomicU8::new(UsedDriver::None as u8);
 
+#[cfg(all(not(esp_idf_version_major = "4"), not(esp_idf_version = "5.1")))]
 fn check_and_set_beta_driver() {
-    if let Err(super::UsedDriver::Legacy) = DRIVER_IN_USE.compare_exchange(
-        super::UsedDriver::None,
-        super::UsedDriver::Beta,
+    match DRIVER_IN_USE.compare_exchange(
+        UsedDriver::None as u8,
+        UsedDriver::Beta as u8,
         core::sync::atomic::Ordering::Relaxed,
         core::sync::atomic::Ordering::Relaxed,
     ) {
-        panic!("Legacy I2C driver is already in use. Either legacy driver or beta driver can be used at a time.");
+        Err(e) if e == UsedDriver::Legacy as u8 => panic!("Legacy I2C driver is already in use. Either legacy driver or beta driver can be used at a time."),
+        _ => ()
     }
 }
 
 fn check_and_set_legacy_driver() {
-    if let Err(super::UsedDriver::Beta) = DRIVER_IN_USE.compare_exchange(
-        super::UsedDriver::None,
-        super::UsedDriver::Legacy,
+    match DRIVER_IN_USE.compare_exchange(
+            UsedDriver::None as u8,
+            UsedDriver::Legacy as u8,
         core::sync::atomic::Ordering::Relaxed,
         core::sync::atomic::Ordering::Relaxed,
     ) {
-        panic!("Beta I2C driver is already in use. Either legacy driver or beta driver can be used at a time.");
+            Err(e) if e == UsedDriver::Beta as u8 => panic!("Beta I2C driver is already in use. Either legacy driver or beta driver can be used at a time."),
+            _ => ()
     }
 }
 

From a28c3fb0e9b011987c1c6fba06257213d78cf0d8 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Alexander=20H=C3=BCbener?= <alex.teamplayer@gmail.com>
Date: Thu, 6 Jun 2024 14:26:14 +0200
Subject: [PATCH 08/11] disable callback on esp err

---
 src/i2c.rs | 32 ++++++++++++++++++++++----------
 1 file changed, 22 insertions(+), 10 deletions(-)

diff --git a/src/i2c.rs b/src/i2c.rs
index abd6caf22c0..c7864b0c975 100644
--- a/src/i2c.rs
+++ b/src/i2c.rs
@@ -34,8 +34,8 @@ fn check_and_set_legacy_driver() {
     match DRIVER_IN_USE.compare_exchange(
             UsedDriver::None as u8,
             UsedDriver::Legacy as u8,
-        core::sync::atomic::Ordering::Relaxed,
-        core::sync::atomic::Ordering::Relaxed,
+            core::sync::atomic::Ordering::Relaxed,
+            core::sync::atomic::Ordering::Relaxed,
     ) {
             Err(e) if e == UsedDriver::Beta as u8 => panic!("Beta I2C driver is already in use. Either legacy driver or beta driver can be used at a time."),
             _ => ()
@@ -67,8 +67,6 @@ pub mod beta {
     pub use embedded_hal::i2c::Operation;
 
     use super::I2c;
-    use super::BETA_DRIVER_IN_USE;
-    use super::DRIVER_IN_USE;
 
     crate::embedded_hal_error!(
         I2cError,
@@ -76,6 +74,20 @@ pub mod beta {
         embedded_hal::i2c::ErrorKind
     );
 
+    macro_rules! on_err {
+        ($d:expr, $oe:expr) => {
+            {
+                match $d {
+                    Err(e) => {
+                        $oe
+                        Err(e)
+                    }
+                    v => v
+                }
+            }
+        };
+    }
+
     pub type I2cConfig = config::Config;
     #[cfg(not(esp32c2))]
     pub type I2cSlaveConfig = config::SlaveDeviceConfig;
@@ -741,14 +753,14 @@ pub mod beta {
 
             let _lock_guard = driver.acquire_bus().await;
             enable_master_dev_isr_callback(handle, port)?;
-            esp!(unsafe {
+            on_err!(esp!(unsafe {
                 i2c_master_receive(
                     handle,
                     buffer.as_mut_ptr().cast(),
                     buffer.len(),
                     timeout as i32,
                 )
-            })?;
+            }), { disable_master_dev_isr_callback(handle).unwrap(); })?;
 
             NOTIFIER[port as usize].wait().await;
             disable_master_dev_isr_callback(handle)?;
@@ -762,9 +774,9 @@ pub mod beta {
 
             let _lock_guard = driver.acquire_bus().await;
             enable_master_dev_isr_callback(handle, port)?;
-            esp!(unsafe {
+            on_err!(esp!(unsafe {
                 i2c_master_transmit(handle, bytes.as_ptr().cast(), bytes.len(), timeout as i32)
-            })?;
+            }), { disable_master_dev_isr_callback(handle).unwrap(); })?;
 
             NOTIFIER[port as usize].wait().await;
             disable_master_dev_isr_callback(handle)?;
@@ -783,7 +795,7 @@ pub mod beta {
 
             let _lock_guard = driver.acquire_bus().await;
             enable_master_dev_isr_callback(handle, port)?;
-            esp!(unsafe {
+            on_err!(esp!(unsafe {
                 i2c_master_transmit_receive(
                     handle,
                     bytes.as_ptr().cast(),
@@ -792,7 +804,7 @@ pub mod beta {
                     buffer.len(),
                     timeout as i32,
                 )
-            })?;
+            }), { disable_master_dev_isr_callback(handle).unwrap(); })?;
 
             NOTIFIER[port as usize].wait().await;
             disable_master_dev_isr_callback(handle)?;

From d08e2e58206b33ed9fd953b7abc4bcd22f7b8186 Mon Sep 17 00:00:00 2001
From: Alexander Huebener <alex.teamplayer@gmail.com>
Date: Fri, 20 Dec 2024 14:24:09 +0100
Subject: [PATCH 09/11] move new i2c driver to module `modern`

---
 src/i2c.rs        | 2097 +++++++++------------------------------------
 src/i2c/modern.rs | 1215 ++++++++++++++++++++++++++
 2 files changed, 1634 insertions(+), 1678 deletions(-)
 create mode 100644 src/i2c/modern.rs

diff --git a/src/i2c.rs b/src/i2c.rs
index 9f65a716381..6b195eee92a 100644
--- a/src/i2c.rs
+++ b/src/i2c.rs
@@ -1,11 +1,11 @@
+#[cfg(all(not(esp_idf_version_major = "4"), not(esp_idf_version = "5.1")))]
+mod modern;
+
 pub use legacy::*;
 
 use esp_idf_sys::*;
 
-pub trait I2c: Send {
-    fn port() -> i2c_port_t;
-}
-
+#[cfg(all(not(esp_idf_version_major = "4"), not(esp_idf_version = "5.1")))]
 #[repr(u8)]
 enum UsedDriver {
     None = 0,
@@ -13,6 +13,7 @@ enum UsedDriver {
     Beta = 2,
 }
 
+#[cfg(all(not(esp_idf_version_major = "4"), not(esp_idf_version = "5.1")))]
 // 0 -> no driver, 1 -> legacy driver, 2 -> beta driver
 static DRIVER_IN_USE: core::sync::atomic::AtomicU8 =
     core::sync::atomic::AtomicU8::new(UsedDriver::None as u8);
@@ -42,1848 +43,588 @@ fn check_and_set_legacy_driver() {
     }
 }
 
-#[cfg(all(not(esp_idf_version_major = "4"), not(esp_idf_version = "5.1")))]
-pub mod beta {
-
-    use core::borrow::Borrow;
-    use core::ffi::c_void;
-    use core::marker::PhantomData;
-    use core::ptr;
-
-    use embassy_sync::mutex::Mutex;
-    use embassy_sync::mutex::MutexGuard;
-
-    use embedded_hal::i2c::{ErrorKind, NoAcknowledgeSource};
-
-    use esp_idf_sys::*;
-
-    use crate::delay::*;
-    use crate::gpio::*;
-    use crate::interrupt::asynch::HalIsrNotification;
-    use crate::peripheral::Peripheral;
-    use crate::task::embassy_sync::EspRawMutex;
-    use crate::units::*;
+use core::marker::PhantomData;
+use core::time::Duration;
 
-    pub use embedded_hal::i2c::Operation;
+use embedded_hal::i2c::{ErrorKind, NoAcknowledgeSource};
 
-    use super::I2c;
-
-    crate::embedded_hal_error!(
-        I2cError,
-        embedded_hal::i2c::Error,
-        embedded_hal::i2c::ErrorKind
-    );
+use esp_idf_sys::*;
 
-    macro_rules! on_err {
-        ($d:expr, $oe:expr) => {
-            {
-                match $d {
-                    Err(e) => {
-                        $oe
-                        Err(e)
-                    }
-                    v => v
-                }
-            }
-        };
+use crate::delay::*;
+use crate::gpio::*;
+use crate::interrupt::InterruptType;
+use crate::peripheral::Peripheral;
+use crate::units::*;
+
+pub use embedded_hal::i2c::Operation;
+
+crate::embedded_hal_error!(
+    I2cError,
+    embedded_hal::i2c::Error,
+    embedded_hal::i2c::ErrorKind
+);
+
+const APB_TICK_PERIOD_NS: u32 = 1_000_000_000 / 80_000_000;
+#[derive(Copy, Clone, Debug)]
+pub struct APBTickType(::core::ffi::c_int);
+impl From<Duration> for APBTickType {
+    fn from(duration: Duration) -> Self {
+        APBTickType(
+            ((duration.as_nanos() + APB_TICK_PERIOD_NS as u128 - 1) / APB_TICK_PERIOD_NS as u128)
+                as ::core::ffi::c_int,
+        )
     }
+}
 
-    pub type I2cConfig = config::Config;
-    #[cfg(not(esp32c2))]
-    pub type I2cSlaveConfig = config::SlaveDeviceConfig;
-
-    /// I2C configuration
-    pub mod config {
-        use esp_idf_sys::*;
-
-        use crate::units::*;
-
-        // TODO: in bindings its XTAL called and in doc its APB
-        const APB_SCLK: soc_periph_i2c_clk_src_t = soc_periph_i2c_clk_src_t_I2C_CLK_SRC_XTAL;
-        const FAST_SCLK: soc_periph_i2c_clk_src_t = soc_periph_i2c_clk_src_t_I2C_CLK_SRC_RC_FAST;
-
-        /// i2c source clock
-        #[derive(PartialEq, Eq, Copy, Clone, Debug)]
-        #[allow(non_camel_case_types)]
-        pub enum SourceClock {
-            APB,
-            RC_FAST,
-        }
-
-        impl SourceClock {
-            pub const fn default() -> Self {
-                Self::from_raw(soc_periph_i2c_clk_src_t_I2C_CLK_SRC_DEFAULT)
-            }
-
-            pub const fn from_raw(source_clock: soc_periph_i2c_clk_src_t) -> Self {
-                match source_clock {
-                    APB_SCLK => SourceClock::APB,
-                    FAST_SCLK => SourceClock::RC_FAST,
-                    _ => unreachable!(),
-                }
-            }
-        }
-
-        impl Default for SourceClock {
-            fn default() -> Self {
-                SourceClock::default()
-            }
-        }
-
-        impl From<SourceClock> for i2c_clock_source_t {
-            fn from(source_clock: SourceClock) -> Self {
-                match source_clock {
-                    SourceClock::RC_FAST => FAST_SCLK,
-                    SourceClock::APB => APB_SCLK,
-                }
-            }
-        }
-
-        impl From<uart_sclk_t> for SourceClock {
-            fn from(source_clock: i2c_clock_source_t) -> Self {
-                Self::from_raw(source_clock)
-            }
-        }
-
-        /// I2C Master configuration
-        #[derive(Debug, Clone)]
-        pub struct Config {
-            pub pullup_enabled: bool,
-            pub source_clock: SourceClock,
-            pub glitch_ignore_cnt: u8,
-        }
-
-        impl Config {
-            pub fn new() -> Self {
-                Default::default()
-            }
-
-            #[must_use]
-            pub fn enable_pullup(mut self, enable: bool) -> Self {
-                self.pullup_enabled = enable;
-                self
-            }
-
-            #[must_use]
-            pub fn source_clock(mut self, source_clock: SourceClock) -> Self {
-                self.source_clock = source_clock;
-                self
-            }
+pub type I2cConfig = config::Config;
+#[cfg(not(esp32c2))]
+pub type I2cSlaveConfig = config::SlaveConfig;
 
-            #[must_use]
-            pub fn glitch_ignore_count(mut self, count: u8) -> Self {
-                self.glitch_ignore_cnt = count;
-                self
-            }
-        }
+/// I2C configuration
+pub mod config {
+    use enumset::EnumSet;
 
-        impl Default for Config {
-            fn default() -> Self {
-                Self {
-                    pullup_enabled: true,
-                    source_clock: SourceClock::default(),
-                    glitch_ignore_cnt: 7,
-                }
-            }
-        }
+    use super::APBTickType;
+    use crate::{interrupt::InterruptType, units::*};
 
-        #[derive(Debug, Clone)]
-        pub enum DeviceAddress {
-            SevenBit(u8),
-            TenBit(u16),
-        }
+    /// I2C Master configuration
+    #[derive(Debug, Clone)]
+    pub struct Config {
+        pub baudrate: Hertz,
+        pub sda_pullup_enabled: bool,
+        pub scl_pullup_enabled: bool,
+        pub timeout: Option<APBTickType>,
+        pub intr_flags: EnumSet<InterruptType>,
+    }
 
-        impl DeviceAddress {
-            pub(super) fn address(&self) -> u16 {
-                match self {
-                    DeviceAddress::SevenBit(addr) => *addr as u16,
-                    // TODO: if cfg allows 10 bit address
-                    DeviceAddress::TenBit(addr) => *addr,
-                }
-            }
+    impl Config {
+        pub fn new() -> Self {
+            Default::default()
         }
 
-        impl From<DeviceAddress> for i2c_addr_bit_len_t {
-            fn from(address: DeviceAddress) -> Self {
-                match address {
-                    DeviceAddress::SevenBit(_) => i2c_addr_bit_len_t_I2C_ADDR_BIT_LEN_7,
-                    DeviceAddress::TenBit(_) => i2c_addr_bit_len_t_I2C_ADDR_BIT_LEN_10,
-                }
-            }
+        #[must_use]
+        pub fn baudrate(mut self, baudrate: Hertz) -> Self {
+            self.baudrate = baudrate;
+            self
         }
 
-        #[derive(Debug, Clone)]
-        pub struct DeviceConfig {
-            pub address: DeviceAddress,
-            pub baudrate: Hertz,
+        #[must_use]
+        pub fn sda_enable_pullup(mut self, enable: bool) -> Self {
+            self.sda_pullup_enabled = enable;
+            self
         }
 
-        impl DeviceConfig {
-            pub const fn new(address: DeviceAddress) -> Self {
-                Self {
-                    address,
-                    baudrate: Hertz(1_000_000),
-                }
-            }
-
-            #[must_use]
-            pub fn baudrate(mut self, baudrate: Hertz) -> Self {
-                self.baudrate = baudrate;
-                self
-            }
+        #[must_use]
+        pub fn scl_enable_pullup(mut self, enable: bool) -> Self {
+            self.scl_pullup_enabled = enable;
+            self
         }
 
-        /// I2C Slave configuration
-        #[cfg(not(esp32c2))]
-        #[derive(Debug, Clone)]
-        pub struct SlaveDeviceConfig {
-            pub source_clock: SourceClock,
-            pub broadcast_enable: bool,
-            pub send_buffer_depth: u32,
+        #[must_use]
+        pub fn timeout(mut self, timeout: APBTickType) -> Self {
+            self.timeout = Some(timeout);
+            self
         }
 
-        #[cfg(not(esp32c2))]
-        impl SlaveDeviceConfig {
-            pub fn new() -> Self {
-                Default::default()
-            }
-
-            #[must_use]
-            pub fn source_clock(mut self, source_clock: SourceClock) -> Self {
-                self.source_clock = source_clock;
-                self
-            }
-
-            #[must_use]
-            pub fn enable_broadcast(mut self, enable: bool) -> Self {
-                self.broadcast_enable = enable;
-                self
-            }
-
-            #[must_use]
-            pub fn set_send_buffer_depth(mut self, depth: u32) -> Self {
-                self.send_buffer_depth = depth;
-                self
-            }
+        #[must_use]
+        pub fn intr_flags(mut self, flags: EnumSet<InterruptType>) -> Self {
+            self.intr_flags = flags;
+            self
         }
+    }
 
-        #[cfg(not(esp32c2))]
-        impl Default for SlaveDeviceConfig {
-            fn default() -> Self {
-                Self {
-                    source_clock: SourceClock::default(),
-                    broadcast_enable: false,
-                    send_buffer_depth: 0,
-                }
+    impl Default for Config {
+        fn default() -> Self {
+            Self {
+                baudrate: Hertz(1_000_000),
+                sda_pullup_enabled: true,
+                scl_pullup_enabled: true,
+                timeout: None,
+                intr_flags: EnumSet::<InterruptType>::empty(),
             }
         }
     }
 
-    pub struct I2cDriver<'d> {
-        port: u8,
-        handle: i2c_master_bus_handle_t,
-        _p: PhantomData<&'d mut ()>,
+    /// I2C Slave configuration
+    #[cfg(not(esp32c2))]
+    #[derive(Debug, Clone)]
+    pub struct SlaveConfig {
+        pub sda_pullup_enabled: bool,
+        pub scl_pullup_enabled: bool,
+        pub rx_buf_len: usize,
+        pub tx_buf_len: usize,
+        pub intr_flags: EnumSet<InterruptType>,
     }
 
-    impl<'d> I2cDriver<'d> {
-        pub fn new<I2C: I2c>(
-            _i2c: impl Peripheral<P = I2C> + 'd,
-            sda: impl Peripheral<P = impl InputPin + OutputPin> + 'd,
-            scl: impl Peripheral<P = impl InputPin + OutputPin> + 'd,
-            config: &config::Config,
-        ) -> Result<Self, EspError> {
-            super::check_and_set_beta_driver();
-
-            let handle = init_master_bus(_i2c, sda, scl, config, 0)?;
-
-            Ok(I2cDriver {
-                port: I2C::port() as u8,
-                handle,
-                _p: PhantomData,
-            })
-        }
-
-        pub fn port(&self) -> u8 {
-            self.port
-        }
-
-        fn bus_handle(&self) -> i2c_master_bus_handle_t {
-            self.handle
-        }
-
-        /// Probe device on the bus.
-        pub fn probe_device(
-            &mut self,
-            address: config::DeviceAddress,
-            timeout: TickType_t,
-        ) -> Result<(), EspError> {
-            esp!(unsafe { i2c_master_probe(self.handle, address.address(), timeout as i32) })
-        }
-
-        pub fn device(
-            &mut self,
-            config: &config::DeviceConfig,
-        ) -> Result<I2cDeviceDriver<'d, &mut I2cDriver<'d>>, EspError> {
-            I2cDeviceDriver::new(self, config)
+    #[cfg(not(esp32c2))]
+    impl SlaveConfig {
+        pub fn new() -> Self {
+            Default::default()
         }
 
-        // Helper to use the embedded_hal traits.
-        fn read(
-            &mut self,
-            addr: u8,
-            buffer: &mut [u8],
-            timeout: TickType_t,
-        ) -> Result<(), EspError> {
-            self.device(&config::DeviceConfig::new(config::DeviceAddress::SevenBit(
-                addr,
-            )))?
-            .read(buffer, timeout)
+        #[must_use]
+        pub fn sda_enable_pullup(mut self, enable: bool) -> Self {
+            self.sda_pullup_enabled = enable;
+            self
         }
 
-        // Helper to use the embedded_hal traits.
-        fn write(&mut self, addr: u8, bytes: &[u8], timeout: TickType_t) -> Result<(), EspError> {
-            self.device(&config::DeviceConfig::new(config::DeviceAddress::SevenBit(
-                addr,
-            )))?
-            .write(bytes, timeout)
+        #[must_use]
+        pub fn scl_enable_pullup(mut self, enable: bool) -> Self {
+            self.scl_pullup_enabled = enable;
+            self
         }
 
-        // Helper to use the embedded_hal traits.
-        fn write_read(
-            &mut self,
-            addr: u8,
-            bytes: &[u8],
-            buffer: &mut [u8],
-            timeout: TickType_t,
-        ) -> Result<(), EspError> {
-            self.device(&config::DeviceConfig::new(config::DeviceAddress::SevenBit(
-                addr,
-            )))?
-            .write_read(bytes, buffer, timeout)
+        #[must_use]
+        pub fn rx_buffer_length(mut self, len: usize) -> Self {
+            self.rx_buf_len = len;
+            self
         }
-    }
-
-    unsafe impl<'d> Send for I2cDriver<'d> {}
-
-    impl<'d> Drop for I2cDriver<'d> {
-        fn drop(&mut self) {
-            esp!(unsafe { i2c_del_master_bus(self.handle) }).unwrap();
-        }
-    }
-
-    impl<'d> embedded_hal_0_2::blocking::i2c::Read for I2cDriver<'d> {
-        type Error = I2cError;
 
-        fn read(&mut self, addr: u8, buffer: &mut [u8]) -> Result<(), Self::Error> {
-            Self::read(self, addr, buffer, BLOCK).map_err(to_i2c_err)
+        #[must_use]
+        pub fn tx_buffer_length(mut self, len: usize) -> Self {
+            self.tx_buf_len = len;
+            self
         }
-    }
-
-    impl<'d> embedded_hal_0_2::blocking::i2c::Write for I2cDriver<'d> {
-        type Error = I2cError;
 
-        fn write(&mut self, addr: u8, bytes: &[u8]) -> Result<(), Self::Error> {
-            Self::write(self, addr, bytes, BLOCK).map_err(to_i2c_err)
+        #[must_use]
+        pub fn intr_flags(mut self, flags: EnumSet<InterruptType>) -> Self {
+            self.intr_flags = flags;
+            self
         }
     }
 
-    impl<'d> embedded_hal_0_2::blocking::i2c::WriteRead for I2cDriver<'d> {
-        type Error = I2cError;
-
-        fn write_read(
-            &mut self,
-            addr: u8,
-            bytes: &[u8],
-            buffer: &mut [u8],
-        ) -> Result<(), Self::Error> {
-            Self::write_read(self, addr, bytes, buffer, BLOCK).map_err(to_i2c_err)
+    #[cfg(not(esp32c2))]
+    impl Default for SlaveConfig {
+        fn default() -> Self {
+            Self {
+                sda_pullup_enabled: true,
+                scl_pullup_enabled: true,
+                rx_buf_len: 0,
+                tx_buf_len: 0,
+                intr_flags: EnumSet::<InterruptType>::empty(),
+            }
         }
     }
+}
 
-    impl<'d> embedded_hal::i2c::ErrorType for I2cDriver<'d> {
-        type Error = I2cError;
-    }
-
-    impl<'d> embedded_hal::i2c::I2c<embedded_hal::i2c::SevenBitAddress> for I2cDriver<'d> {
-        fn read(&mut self, addr: u8, buffer: &mut [u8]) -> Result<(), Self::Error> {
-            Self::read(self, addr, buffer, BLOCK).map_err(to_i2c_err)
-        }
+pub trait I2c: Send {
+    fn port() -> i2c_port_t;
+}
 
-        fn write(&mut self, addr: u8, bytes: &[u8]) -> Result<(), Self::Error> {
-            Self::write(self, addr, bytes, BLOCK).map_err(to_i2c_err)
-        }
+pub struct I2cDriver<'d> {
+    i2c: u8,
+    _p: PhantomData<&'d mut ()>,
+}
 
-        fn write_read(
-            &mut self,
-            addr: u8,
-            bytes: &[u8],
-            buffer: &mut [u8],
-        ) -> Result<(), Self::Error> {
-            Self::write_read(self, addr, bytes, buffer, BLOCK).map_err(to_i2c_err)
-        }
+impl<'d> I2cDriver<'d> {
+    pub fn new<I2C: I2c>(
+        _i2c: impl Peripheral<P = I2C> + 'd,
+        sda: impl Peripheral<P = impl InputPin + OutputPin> + 'd,
+        scl: impl Peripheral<P = impl InputPin + OutputPin> + 'd,
+        config: &config::Config,
+    ) -> Result<Self, EspError> {
+        #[cfg(all(not(esp_idf_version_major = "4"), not(esp_idf_version = "5.1")))]
+        check_and_set_legacy_driver();
 
-        fn transaction(
-            &mut self,
-            _addr: u8,
-            _operations: &mut [embedded_hal::i2c::Operation<'_>],
-        ) -> Result<(), Self::Error> {
-            unimplemented!("transactional not implemented")
+        // i2c_config_t documentation says that clock speed must be no higher than 1 MHz
+        if config.baudrate > 1.MHz().into() {
+            return Err(EspError::from_infallible::<ESP_ERR_INVALID_ARG>());
         }
-    }
 
-    pub struct I2cDeviceDriver<'d, T>
-    where
-        T: Borrow<I2cDriver<'d>>,
-    {
-        _driver: T,
-        handle: i2c_master_dev_handle_t,
-        _p: PhantomData<&'d mut ()>,
-    }
+        crate::into_ref!(sda, scl);
 
-    impl<'d, T> I2cDeviceDriver<'d, T>
-    where
-        T: Borrow<I2cDriver<'d>>,
-    {
-        pub fn new(driver: T, config: &config::DeviceConfig) -> Result<Self, EspError> {
-            let handle = init_device(driver.borrow().bus_handle(), &config)?;
-
-            Ok(I2cDeviceDriver {
-                _driver: driver,
-                handle,
-                _p: PhantomData,
-            })
-        }
+        let sys_config = i2c_config_t {
+            mode: i2c_mode_t_I2C_MODE_MASTER,
+            sda_io_num: sda.pin(),
+            sda_pullup_en: config.sda_pullup_enabled,
+            scl_io_num: scl.pin(),
+            scl_pullup_en: config.scl_pullup_enabled,
+            __bindgen_anon_1: i2c_config_t__bindgen_ty_1 {
+                master: i2c_config_t__bindgen_ty_1__bindgen_ty_1 {
+                    clk_speed: config.baudrate.into(),
+                },
+            },
+            ..Default::default()
+        };
 
-        pub fn read(&mut self, buffer: &mut [u8], timeout: TickType_t) -> Result<(), EspError> {
-            esp!(unsafe {
-                i2c_master_receive(
-                    self.handle,
-                    buffer.as_mut_ptr().cast(),
-                    buffer.len(),
-                    timeout as i32,
-                )
-            })
-        }
+        esp!(unsafe { i2c_param_config(I2C::port(), &sys_config) })?;
 
-        pub fn write(&mut self, bytes: &[u8], timeout: TickType_t) -> Result<(), EspError> {
-            esp!(unsafe {
-                i2c_master_transmit(
-                    self.handle,
-                    bytes.as_ptr().cast(),
-                    bytes.len(),
-                    timeout as i32,
-                )
-            })
-        }
+        esp!(unsafe {
+            i2c_driver_install(
+                I2C::port(),
+                i2c_mode_t_I2C_MODE_MASTER,
+                0, // Not used in master mode
+                0, // Not used in master mode
+                InterruptType::to_native(config.intr_flags) as _,
+            )
+        })?;
 
-        pub fn write_read(
-            &mut self,
-            bytes: &[u8],
-            buffer: &mut [u8],
-            timeout: TickType_t,
-        ) -> Result<(), EspError> {
-            esp!(unsafe {
-                i2c_master_transmit_receive(
-                    self.handle,
-                    bytes.as_ptr().cast(),
-                    bytes.len(),
-                    buffer.as_mut_ptr().cast(),
-                    buffer.len(),
-                    timeout as i32,
-                )
-            })
+        if let Some(timeout) = config.timeout {
+            esp!(unsafe { i2c_set_timeout(I2C::port(), timeout.0) })?;
         }
-    }
 
-    impl<'d, T> Drop for I2cDeviceDriver<'d, T>
-    where
-        T: Borrow<I2cDriver<'d>>,
-    {
-        fn drop(&mut self) {
-            esp!(unsafe { i2c_master_bus_rm_device(self.handle) }).unwrap();
-        }
+        Ok(I2cDriver {
+            i2c: I2C::port() as _,
+            _p: PhantomData,
+        })
     }
 
-    impl<'d, T> embedded_hal_0_2::blocking::i2c::Read for I2cDeviceDriver<'d, T>
-    where
-        T: Borrow<I2cDriver<'d>>,
-    {
-        type Error = I2cError;
-
-        fn read(&mut self, _addr: u8, buffer: &mut [u8]) -> Result<(), Self::Error> {
-            I2cDeviceDriver::read(self, buffer, BLOCK).map_err(to_i2c_err)
-        }
-    }
+    pub fn read(
+        &mut self,
+        addr: u8,
+        buffer: &mut [u8],
+        timeout: TickType_t,
+    ) -> Result<(), EspError> {
+        let mut command_link = CommandLink::new()?;
 
-    impl<'d, T> embedded_hal_0_2::blocking::i2c::Write for I2cDeviceDriver<'d, T>
-    where
-        T: Borrow<I2cDriver<'d>>,
-    {
-        type Error = I2cError;
+        command_link.master_start()?;
+        command_link.master_write_byte((addr << 1) | (i2c_rw_t_I2C_MASTER_READ as u8), true)?;
 
-        fn write(&mut self, _addr: u8, bytes: &[u8]) -> Result<(), Self::Error> {
-            I2cDeviceDriver::write(self, bytes, BLOCK).map_err(to_i2c_err)
+        if !buffer.is_empty() {
+            command_link.master_read(buffer, AckType::LastNack)?;
         }
-    }
 
-    impl<'d, T> embedded_hal_0_2::blocking::i2c::WriteRead for I2cDeviceDriver<'d, T>
-    where
-        T: Borrow<I2cDriver<'d>>,
-    {
-        type Error = I2cError;
-
-        fn write_read(
-            &mut self,
-            _addr: u8,
-            bytes: &[u8],
-            buffer: &mut [u8],
-        ) -> Result<(), Self::Error> {
-            I2cDeviceDriver::write_read(self, bytes, buffer, BLOCK).map_err(to_i2c_err)
-        }
-    }
+        command_link.master_stop()?;
 
-    impl<'d, T> embedded_hal::i2c::ErrorType for I2cDeviceDriver<'d, T>
-    where
-        T: Borrow<I2cDriver<'d>>,
-    {
-        type Error = I2cError;
+        self.cmd_begin(&command_link, timeout)
     }
 
-    impl<'d, T> embedded_hal::i2c::I2c<embedded_hal::i2c::SevenBitAddress> for I2cDeviceDriver<'d, T>
-    where
-        T: Borrow<I2cDriver<'d>>,
-    {
-        fn read(&mut self, _addr: u8, buffer: &mut [u8]) -> Result<(), Self::Error> {
-            I2cDeviceDriver::read(self, buffer, BLOCK).map_err(to_i2c_err)
-        }
+    pub fn write(&mut self, addr: u8, bytes: &[u8], timeout: TickType_t) -> Result<(), EspError> {
+        let mut command_link = CommandLink::new()?;
 
-        fn write(&mut self, _addr: u8, bytes: &[u8]) -> Result<(), Self::Error> {
-            I2cDeviceDriver::write(self, bytes, BLOCK).map_err(to_i2c_err)
-        }
-
-        fn write_read(
-            &mut self,
-            _addr: u8,
-            bytes: &[u8],
-            buffer: &mut [u8],
-        ) -> Result<(), Self::Error> {
-            I2cDeviceDriver::write_read(self, bytes, buffer, BLOCK).map_err(to_i2c_err)
-        }
+        command_link.master_start()?;
+        command_link.master_write_byte((addr << 1) | (i2c_rw_t_I2C_MASTER_WRITE as u8), true)?;
 
-        fn transaction(
-            &mut self,
-            _addr: u8,
-            _operations: &mut [embedded_hal::i2c::Operation<'_>],
-        ) -> Result<(), Self::Error> {
-            unimplemented!("transactional not implemented")
+        if !bytes.is_empty() {
+            command_link.master_write(bytes, true)?;
         }
-    }
 
-    // ------------------------------------------------------------------------------------------------
-    // ------------------------------------- Async ----------------------------------------------------
-    // ------------------------------------------------------------------------------------------------
+        command_link.master_stop()?;
 
-    pub struct AsyncI2cDriver<'d> {
-        bus_lock: Mutex<EspRawMutex, ()>,
-        handle: i2c_master_bus_handle_t,
-        port: u8,
-        _p: PhantomData<&'d mut ()>,
+        self.cmd_begin(&command_link, timeout)
     }
 
-    impl<'d> AsyncI2cDriver<'d> {
-        pub fn new<I2C: I2c>(
-            _i2c: impl Peripheral<P = I2C> + 'd,
-            sda: impl Peripheral<P = impl InputPin + OutputPin> + 'd,
-            scl: impl Peripheral<P = impl InputPin + OutputPin> + 'd,
-            config: &config::Config,
-        ) -> Result<Self, EspError> {
-            super::check_and_set_beta_driver();
-
-            let handle = init_master_bus(_i2c, sda, scl, config, 1)?;
-
-            Ok(AsyncI2cDriver {
-                bus_lock: Mutex::new(()),
-                handle,
-                port: I2C::port() as _,
-                _p: PhantomData,
-            })
-        }
-
-        pub fn port(&self) -> u8 {
-            self.port
-        }
-
-        fn bus_handle(&self) -> i2c_master_bus_handle_t {
-            self.handle
-        }
-
-        async fn acquire_bus<'a>(&'a self) -> MutexGuard<'a, EspRawMutex, ()> {
-            self.bus_lock.lock().await
-        }
+    pub fn write_read(
+        &mut self,
+        addr: u8,
+        bytes: &[u8],
+        buffer: &mut [u8],
+        timeout: TickType_t,
+    ) -> Result<(), EspError> {
+        let mut command_link = CommandLink::new()?;
 
-        pub fn device(
-            &self,
-            config: &config::DeviceConfig,
-        ) -> Result<AsyncI2cDeviceDriver<'d, &AsyncI2cDriver<'d>>, EspError> {
-            AsyncI2cDeviceDriver::new(self, config)
-        }
+        command_link.master_start()?;
+        command_link.master_write_byte((addr << 1) | (i2c_rw_t_I2C_MASTER_WRITE as u8), true)?;
 
-        pub fn owned_device(
-            self,
-            config: &config::DeviceConfig,
-        ) -> Result<OwnedAsyncI2cDeviceDriver<'d>, EspError> {
-            OwnedAsyncI2cDeviceDriver::wrap(self, config)
+        if !bytes.is_empty() {
+            command_link.master_write(bytes, true)?;
         }
 
-        async fn read(&mut self, address: u8, buffer: &mut [u8]) -> Result<(), EspError> {
-            self.device(&config::DeviceConfig::new(config::DeviceAddress::SevenBit(
-                address,
-            )))?
-            .read(buffer, BLOCK)
-            .await
-        }
+        command_link.master_start()?;
+        command_link.master_write_byte((addr << 1) | (i2c_rw_t_I2C_MASTER_READ as u8), true)?;
 
-        async fn write(&mut self, address: u8, bytes: &[u8]) -> Result<(), EspError> {
-            self.device(&config::DeviceConfig::new(config::DeviceAddress::SevenBit(
-                address,
-            )))?
-            .write(bytes, BLOCK)
-            .await
+        if !buffer.is_empty() {
+            command_link.master_read(buffer, AckType::LastNack)?;
         }
 
-        async fn write_read(
-            &mut self,
-            address: u8,
-            bytes: &[u8],
-            buffer: &mut [u8],
-        ) -> Result<(), EspError> {
-            self.device(&config::DeviceConfig::new(config::DeviceAddress::SevenBit(
-                address,
-            )))?
-            .write_read(bytes, buffer, BLOCK)
-            .await
-        }
-    }
+        command_link.master_stop()?;
 
-    impl<'d> embedded_hal::i2c::ErrorType for AsyncI2cDriver<'d> {
-        type Error = I2cError;
+        self.cmd_begin(&command_link, timeout)
     }
 
-    impl<'d> embedded_hal_async::i2c::I2c<embedded_hal::i2c::SevenBitAddress> for AsyncI2cDriver<'d> {
-        async fn read(&mut self, address: u8, buffer: &mut [u8]) -> Result<(), Self::Error> {
-            self.read(address, buffer).await.map_err(to_i2c_err)
-        }
-
-        async fn write(&mut self, address: u8, bytes: &[u8]) -> Result<(), Self::Error> {
-            self.write(address, bytes).await.map_err(to_i2c_err)
-        }
-
-        async fn write_read(
-            &mut self,
-            address: u8,
-            bytes: &[u8],
-            buffer: &mut [u8],
-        ) -> Result<(), Self::Error> {
-            self.write_read(address, bytes, buffer)
-                .await
-                .map_err(to_i2c_err)
-        }
+    pub fn transaction(
+        &mut self,
+        address: u8,
+        operations: &mut [Operation<'_>],
+        timeout: TickType_t,
+    ) -> Result<(), EspError> {
+        let mut command_link = CommandLink::new()?;
+
+        let last_op_index = operations.len() - 1;
+        let mut prev_was_read = None;
+
+        for (i, operation) in operations.iter_mut().enumerate() {
+            match operation {
+                Operation::Read(buf) => {
+                    if Some(true) != prev_was_read {
+                        command_link.master_start()?;
+                        command_link.master_write_byte(
+                            (address << 1) | (i2c_rw_t_I2C_MASTER_READ as u8),
+                            true,
+                        )?;
+                    }
+                    prev_was_read = Some(true);
 
-        async fn transaction(
-            &mut self,
-            _address: u8,
-            _operations: &mut [Operation<'_>],
-        ) -> Result<(), Self::Error> {
-            unimplemented!("transactional not implemented")
-        }
-    }
+                    if !buf.is_empty() {
+                        let ack = if i == last_op_index {
+                            AckType::LastNack
+                        } else {
+                            AckType::Ack
+                        };
 
-    unsafe impl<'d> Send for AsyncI2cDriver<'d> {}
+                        command_link.master_read(buf, ack)?;
+                    }
+                }
+                Operation::Write(buf) => {
+                    if Some(false) != prev_was_read {
+                        command_link.master_start()?;
+                        command_link.master_write_byte(
+                            (address << 1) | (i2c_rw_t_I2C_MASTER_WRITE as u8),
+                            true,
+                        )?;
+                    }
+                    prev_was_read = Some(false);
 
-    impl<'d> Drop for AsyncI2cDriver<'d> {
-        fn drop(&mut self) {
-            loop {
-                if let Ok(_lock_guard) = self.bus_lock.try_lock() {
-                    esp!(unsafe { i2c_del_master_bus(self.handle) }).unwrap();
-                    break;
+                    if !buf.is_empty() {
+                        command_link.master_write(buf, true)?;
+                    }
                 }
             }
         }
-    }
 
-    #[cfg(not(esp_idf_i2c_isr_iram_safe))]
-    pub struct AsyncI2cDeviceDriver<'d, T>
-    where
-        T: Borrow<AsyncI2cDriver<'d>>,
-    {
-        driver: T,
-        handle: i2c_master_dev_handle_t,
-        _p: PhantomData<&'d mut ()>,
-    }
+        command_link.master_stop()?;
 
-    #[cfg(not(esp_idf_i2c_isr_iram_safe))]
-    impl<'d, T> AsyncI2cDeviceDriver<'d, T>
-    where
-        T: Borrow<AsyncI2cDriver<'d>>,
-    {
-        fn new(driver: T, config: &config::DeviceConfig) -> Result<Self, EspError> {
-            let handle = init_device(driver.borrow().bus_handle(), config)?;
-
-            Ok(Self {
-                driver,
-                handle,
-                _p: PhantomData,
-            })
-        }
+        self.cmd_begin(&command_link, timeout)
     }
 
-    #[cfg(not(esp_idf_i2c_isr_iram_safe))]
-    impl<'d, T> AsyncI2cDeviceDriver<'d, T>
-    where
-        T: Borrow<AsyncI2cDriver<'d>>,
-    {
-        pub async fn read(
-            &mut self,
-            buffer: &mut [u8],
-            timeout: TickType_t,
-        ) -> Result<(), EspError> {
-            let handle = self.handle;
-            let driver = self.driver.borrow();
-            let port = driver.port();
-
-            let _lock_guard = driver.acquire_bus().await;
-            enable_master_dev_isr_callback(handle, port)?;
-            on_err!(
-                esp!(unsafe {
-                    i2c_master_receive(
-                        handle,
-                        buffer.as_mut_ptr().cast(),
-                        buffer.len(),
-                        timeout as i32,
-                    )
-                }),
-                {
-                    disable_master_dev_isr_callback(handle).unwrap();
-                }
-            )?;
-
-            NOTIFIER[port as usize].wait().await;
-            disable_master_dev_isr_callback(handle)?;
-            Ok(())
-        }
-
-        pub async fn write(&mut self, bytes: &[u8], timeout: TickType_t) -> Result<(), EspError> {
-            let handle = self.handle;
-            let driver = self.driver.borrow();
-            let port = driver.port();
-
-            let _lock_guard = driver.acquire_bus().await;
-            enable_master_dev_isr_callback(handle, port)?;
-            on_err!(
-                esp!(unsafe {
-                    i2c_master_transmit(handle, bytes.as_ptr().cast(), bytes.len(), timeout as i32)
-                }),
-                {
-                    disable_master_dev_isr_callback(handle).unwrap();
-                }
-            )?;
-
-            NOTIFIER[port as usize].wait().await;
-            disable_master_dev_isr_callback(handle)?;
-            Ok(())
-        }
-
-        pub async fn write_read(
-            &mut self,
-            bytes: &[u8],
-            buffer: &mut [u8],
-            timeout: TickType_t,
-        ) -> Result<(), EspError> {
-            let handle = self.handle;
-            let driver = self.driver.borrow();
-            let port = driver.port();
-
-            let _lock_guard = driver.acquire_bus().await;
-            enable_master_dev_isr_callback(handle, port)?;
-            on_err!(
-                esp!(unsafe {
-                    i2c_master_transmit_receive(
-                        handle,
-                        bytes.as_ptr().cast(),
-                        bytes.len(),
-                        buffer.as_mut_ptr().cast(),
-                        buffer.len(),
-                        timeout as i32,
-                    )
-                }),
-                {
-                    disable_master_dev_isr_callback(handle).unwrap();
-                }
-            )?;
-
-            NOTIFIER[port as usize].wait().await;
-            disable_master_dev_isr_callback(handle)?;
-            Ok(())
-        }
+    fn cmd_begin(
+        &mut self,
+        command_link: &CommandLink,
+        timeout: TickType_t,
+    ) -> Result<(), EspError> {
+        esp!(unsafe { i2c_master_cmd_begin(self.port(), command_link.0, timeout) })
     }
 
-    #[cfg(not(esp_idf_i2c_isr_iram_safe))]
-    unsafe impl<'d, T> Send for AsyncI2cDeviceDriver<'d, T> where
-        T: Send + Borrow<AsyncI2cDriver<'d>> + 'd
-    {
+    pub fn port(&self) -> i2c_port_t {
+        self.i2c as _
     }
+}
 
-    #[cfg(not(esp_idf_i2c_isr_iram_safe))]
-    impl<'d, T> embedded_hal::i2c::ErrorType for AsyncI2cDeviceDriver<'d, T>
-    where
-        T: Borrow<AsyncI2cDriver<'d>>,
-    {
-        type Error = I2cError;
+impl Drop for I2cDriver<'_> {
+    fn drop(&mut self) {
+        esp!(unsafe { i2c_driver_delete(self.port()) }).unwrap();
     }
+}
 
-    #[cfg(not(esp_idf_i2c_isr_iram_safe))]
-    impl<'d, T> embedded_hal_async::i2c::I2c<embedded_hal::i2c::SevenBitAddress>
-        for AsyncI2cDeviceDriver<'d, T>
-    where
-        T: Borrow<AsyncI2cDriver<'d>>,
-    {
-        async fn read(&mut self, _address: u8, buffer: &mut [u8]) -> Result<(), Self::Error> {
-            Self::read(self, buffer, BLOCK).await.map_err(to_i2c_err)
-        }
+unsafe impl Send for I2cDriver<'_> {}
 
-        async fn write(&mut self, _address: u8, bytes: &[u8]) -> Result<(), Self::Error> {
-            Self::write(self, bytes, BLOCK).await.map_err(to_i2c_err)
-        }
+impl embedded_hal_0_2::blocking::i2c::Read for I2cDriver<'_> {
+    type Error = I2cError;
 
-        async fn write_read(
-            &mut self,
-            _address: u8,
-            bytes: &[u8],
-            buffer: &mut [u8],
-        ) -> Result<(), Self::Error> {
-            Self::write_read(self, bytes, buffer, BLOCK)
-                .await
-                .map_err(to_i2c_err)
-        }
-
-        async fn transaction(
-            &mut self,
-            _address: u8,
-            _operations: &mut [Operation<'_>],
-        ) -> Result<(), Self::Error> {
-            unimplemented!("transactional not implemented")
-        }
+    fn read(&mut self, addr: u8, buffer: &mut [u8]) -> Result<(), Self::Error> {
+        I2cDriver::read(self, addr, buffer, BLOCK).map_err(to_i2c_err)
     }
+}
 
-    impl<'d, T> Drop for AsyncI2cDeviceDriver<'d, T>
-    where
-        T: Borrow<AsyncI2cDriver<'d>>,
-    {
-        fn drop(&mut self) {
-            loop {
-                if let Ok(_lock_guard) = self.driver.borrow().bus_lock.try_lock() {
-                    esp!(unsafe { i2c_master_bus_rm_device(self.handle) }).unwrap();
-                    break;
-                }
-            }
-        }
-    }
+impl embedded_hal_0_2::blocking::i2c::Write for I2cDriver<'_> {
+    type Error = I2cError;
 
-    #[cfg(not(esp_idf_i2c_isr_iram_safe))]
-    pub struct OwnedAsyncI2cDeviceDriver<'d> {
-        driver: Option<AsyncI2cDriver<'d>>,
-        handle: i2c_master_dev_handle_t,
-        _p: PhantomData<&'d mut ()>,
+    fn write(&mut self, addr: u8, bytes: &[u8]) -> Result<(), Self::Error> {
+        I2cDriver::write(self, addr, bytes, BLOCK).map_err(to_i2c_err)
     }
+}
 
-    #[cfg(not(esp_idf_i2c_isr_iram_safe))]
-    impl<'d> OwnedAsyncI2cDeviceDriver<'d> {
-        pub fn new<I2C: I2c>(
-            _i2c: impl Peripheral<P = I2C> + 'd,
-            sda: impl Peripheral<P = impl InputPin + OutputPin> + 'd,
-            scl: impl Peripheral<P = impl InputPin + OutputPin> + 'd,
-            bus_config: &config::Config,
-            device_config: &config::DeviceConfig,
-        ) -> Result<Self, EspError> {
-            let driver = AsyncI2cDriver::new(_i2c, sda, scl, bus_config)?;
-            Self::wrap(driver, device_config)
-        }
-
-        pub fn wrap(
-            driver: AsyncI2cDriver<'d>,
-            device_config: &config::DeviceConfig,
-        ) -> Result<Self, EspError> {
-            let handle = init_device(driver.bus_handle(), device_config)?;
-
-            enable_master_dev_isr_callback(handle, driver.port())?;
-
-            Ok(Self {
-                driver: Some(driver),
-                handle,
-                _p: PhantomData,
-            })
-        }
+impl embedded_hal_0_2::blocking::i2c::WriteRead for I2cDriver<'_> {
+    type Error = I2cError;
 
-        pub fn release(mut self) -> Result<AsyncI2cDriver<'d>, EspError> {
-            let driver = self.driver.take().unwrap();
-            drop(self);
-            Ok(driver)
-        }
+    fn write_read(&mut self, addr: u8, bytes: &[u8], buffer: &mut [u8]) -> Result<(), Self::Error> {
+        I2cDriver::write_read(self, addr, bytes, buffer, BLOCK).map_err(to_i2c_err)
     }
+}
 
-    #[cfg(not(esp_idf_i2c_isr_iram_safe))]
-    impl<'d> OwnedAsyncI2cDeviceDriver<'d> {
-        pub async fn read(
-            &mut self,
-            buffer: &mut [u8],
-            timeout: TickType_t,
-        ) -> Result<(), EspError> {
-            esp!(unsafe {
-                i2c_master_receive(
-                    self.handle,
-                    buffer.as_mut_ptr().cast(),
-                    buffer.len(),
-                    timeout as i32,
-                )
-            })?;
-
-            let port = self.driver.as_ref().unwrap().port() as usize;
-            NOTIFIER[port].wait().await;
-            Ok(())
-        }
-
-        pub async fn write(&mut self, bytes: &[u8], timeout: TickType_t) -> Result<(), EspError> {
-            esp!(unsafe {
-                i2c_master_transmit(
-                    self.handle,
-                    bytes.as_ptr().cast(),
-                    bytes.len(),
-                    timeout as i32,
-                )
-            })?;
-
-            let port = self.driver.as_ref().unwrap().port() as usize;
-            NOTIFIER[port].wait().await;
-            Ok(())
-        }
+impl embedded_hal::i2c::ErrorType for I2cDriver<'_> {
+    type Error = I2cError;
+}
 
-        pub async fn write_read(
-            &mut self,
-            bytes: &[u8],
-            buffer: &mut [u8],
-            timeout: TickType_t,
-        ) -> Result<(), EspError> {
-            esp!(unsafe {
-                i2c_master_transmit_receive(
-                    self.handle,
-                    bytes.as_ptr().cast(),
-                    bytes.len(),
-                    buffer.as_mut_ptr().cast(),
-                    buffer.len(),
-                    timeout as i32,
-                )
-            })?;
-
-            let port = self.driver.as_ref().unwrap().port() as usize;
-            NOTIFIER[port].wait().await;
-            Ok(())
-        }
+impl embedded_hal::i2c::I2c<embedded_hal::i2c::SevenBitAddress> for I2cDriver<'_> {
+    fn read(&mut self, addr: u8, buffer: &mut [u8]) -> Result<(), Self::Error> {
+        I2cDriver::read(self, addr, buffer, BLOCK).map_err(to_i2c_err)
     }
 
-    #[cfg(not(esp_idf_i2c_isr_iram_safe))]
-    unsafe impl<'d> Send for OwnedAsyncI2cDeviceDriver<'d> {}
-
-    #[cfg(not(esp_idf_i2c_isr_iram_safe))]
-    impl<'d> Drop for OwnedAsyncI2cDeviceDriver<'d> {
-        fn drop(&mut self) {
-            disable_master_dev_isr_callback(self.handle).unwrap();
-            esp!(unsafe { i2c_master_bus_rm_device(self.handle) }).unwrap();
-        }
+    fn write(&mut self, addr: u8, bytes: &[u8]) -> Result<(), Self::Error> {
+        I2cDriver::write(self, addr, bytes, BLOCK).map_err(to_i2c_err)
     }
 
-    #[cfg(not(esp_idf_i2c_isr_iram_safe))]
-    impl<'d> embedded_hal::i2c::ErrorType for OwnedAsyncI2cDeviceDriver<'d> {
-        type Error = I2cError;
+    fn write_read(&mut self, addr: u8, bytes: &[u8], buffer: &mut [u8]) -> Result<(), Self::Error> {
+        I2cDriver::write_read(self, addr, bytes, buffer, BLOCK).map_err(to_i2c_err)
     }
 
-    #[cfg(not(esp_idf_i2c_isr_iram_safe))]
-    impl<'d> embedded_hal_async::i2c::I2c<embedded_hal::i2c::SevenBitAddress>
-        for OwnedAsyncI2cDeviceDriver<'d>
-    {
-        async fn read(&mut self, _address: u8, buffer: &mut [u8]) -> Result<(), Self::Error> {
-            Self::read(self, buffer, BLOCK).await.map_err(to_i2c_err)
-        }
-
-        async fn write(&mut self, _address: u8, bytes: &[u8]) -> Result<(), Self::Error> {
-            Self::write(self, bytes, BLOCK).await.map_err(to_i2c_err)
-        }
-
-        async fn write_read(
-            &mut self,
-            _address: u8,
-            bytes: &[u8],
-            buffer: &mut [u8],
-        ) -> Result<(), Self::Error> {
-            Self::write_read(self, bytes, buffer, BLOCK)
-                .await
-                .map_err(to_i2c_err)
-        }
-
-        async fn transaction(
-            &mut self,
-            _address: u8,
-            _operations: &mut [Operation<'_>],
-        ) -> Result<(), Self::Error> {
-            unimplemented!("transactional not implemented")
-        }
+    fn transaction(
+        &mut self,
+        address: u8,
+        operations: &mut [embedded_hal::i2c::Operation<'_>],
+    ) -> Result<(), Self::Error> {
+        I2cDriver::transaction(self, address, operations, BLOCK).map_err(to_i2c_err)
     }
+}
 
-    #[cfg(not(esp32c2))]
-    pub struct I2cSlaveDriver<'d> {
-        i2c: u8,
-        handle: i2c_slave_dev_handle_t,
-        _p: PhantomData<&'d mut ()>,
+fn to_i2c_err(err: EspError) -> I2cError {
+    if err.code() == ESP_FAIL {
+        I2cError::new(ErrorKind::NoAcknowledge(NoAcknowledgeSource::Unknown), err)
+    } else {
+        I2cError::other(err)
     }
+}
 
-    #[cfg(not(esp32c2))]
-    unsafe impl<'d> Send for I2cSlaveDriver<'d> {}
-
-    #[cfg(not(esp32c2))]
-    impl<'d> I2cSlaveDriver<'d> {
-        pub fn new<I2C: I2c>(
-            _i2c: impl Peripheral<P = I2C> + 'd,
-            sda: impl Peripheral<P = impl InputPin + OutputPin> + 'd,
-            scl: impl Peripheral<P = impl InputPin + OutputPin> + 'd,
-            address: config::DeviceAddress,
-            config: &config::SlaveDeviceConfig,
-        ) -> Result<Self, EspError> {
-            super::check_and_set_beta_driver();
-
-            let handle = init_slave_device(_i2c, sda, scl, address, config)?;
-
-            enable_slave_isr_callback(handle, I2C::port() as _)?;
-
-            Ok(Self {
-                i2c: I2C::port() as _,
-                handle,
-                _p: PhantomData,
-            })
-        }
-
-        pub fn read(&mut self, buffer: &mut [u8], _timeout: TickType_t) -> Result<usize, EspError> {
-            esp!(unsafe { i2c_slave_receive(self.handle, buffer.as_mut_ptr(), buffer.len()) })?;
-
-            todo!("How to block?");
-        }
-
-        pub async fn async_read(&mut self, buffer: &mut [u8]) -> Result<(), EspError> {
-            esp!(unsafe { i2c_slave_receive(self.handle, buffer.as_mut_ptr(), buffer.len()) })?;
-
-            NOTIFIER[self.port() as usize].wait().await;
-            Ok(())
-        }
-
-        pub fn write(&mut self, bytes: &[u8], timeout: TickType_t) -> Result<(), EspError> {
-            esp!(unsafe {
-                i2c_slave_transmit(
-                    self.handle,
-                    bytes.as_ptr(),
-                    bytes.len() as i32,
-                    timeout as i32,
-                )
-            })
-        }
-
-        pub fn port(&self) -> i2c_port_t {
-            self.i2c as _
-        }
-    }
+#[cfg(not(esp32c2))]
+pub struct I2cSlaveDriver<'d> {
+    i2c: u8,
+    _p: PhantomData<&'d mut ()>,
+}
 
-    #[cfg(not(esp32c2))]
-    impl<'d> Drop for I2cSlaveDriver<'d> {
-        fn drop(&mut self) {
-            disable_slave_isr_callback(self.handle).unwrap();
-            esp!(unsafe { i2c_del_slave_device(self.handle) }).unwrap();
-        }
-    }
+#[cfg(not(esp32c2))]
+unsafe impl Send for I2cSlaveDriver<'_> {}
 
-    fn init_master_bus<'d, I2C: I2c>(
+#[cfg(not(esp32c2))]
+impl<'d> I2cSlaveDriver<'d> {
+    pub fn new<I2C: I2c>(
         _i2c: impl Peripheral<P = I2C> + 'd,
         sda: impl Peripheral<P = impl InputPin + OutputPin> + 'd,
         scl: impl Peripheral<P = impl InputPin + OutputPin> + 'd,
-        config: &config::Config,
-        max_device_count: usize,
-    ) -> Result<i2c_master_bus_handle_t, EspError> {
-        crate::into_ref!(sda, scl);
-
-        let config = i2c_master_bus_config_t {
-            sda_io_num: sda.pin(),
-            scl_io_num: scl.pin(),
-            clk_source: config.source_clock.into(),
-            flags: {
-                let mut flags = i2c_master_bus_config_t__bindgen_ty_1::default();
-                flags.set_enable_internal_pullup(config.pullup_enabled as _);
-                flags
-            },
-            glitch_ignore_cnt: config.glitch_ignore_cnt,
-            i2c_port: I2C::port() as i32,
-            intr_priority: 0,
-            trans_queue_depth: max_device_count,
-        };
-
-        let mut handle: i2c_master_bus_handle_t = ptr::null_mut();
-
-        esp!(unsafe { i2c_new_master_bus(&config, &mut handle as _) })?;
-
-        Ok(handle)
-    }
-
-    fn init_device(
-        bus_handle: i2c_master_bus_handle_t,
-        config: &config::DeviceConfig,
-    ) -> Result<i2c_master_dev_handle_t, EspError> {
-        // i2c_config_t documentation says that clock speed must be no higher than 1 MHz
-        if config.baudrate > 1.MHz().into() {
-            return Err(EspError::from_infallible::<ESP_ERR_INVALID_ARG>());
-        }
-
-        let config = i2c_device_config_t {
-            device_address: config.address.address(),
-            dev_addr_length: config.address.clone().into(),
-            scl_speed_hz: config.baudrate.into(),
-        };
-
-        let mut handle: i2c_master_dev_handle_t = ptr::null_mut();
-
-        esp!(unsafe { i2c_master_bus_add_device(bus_handle, &config, &mut handle as _) })?;
-
-        Ok(handle)
-    }
+        slave_addr: u8,
+        config: &config::SlaveConfig,
+    ) -> Result<Self, EspError> {
+        #[cfg(all(not(esp_idf_version_major = "4"), not(esp_idf_version = "5.1")))]
+        check_and_set_legacy_driver();
 
-    #[cfg(not(esp32c2))]
-    fn init_slave_device<'d, I2C: I2c>(
-        _i2c: impl Peripheral<P = I2C> + 'd,
-        sda: impl Peripheral<P = impl InputPin + OutputPin> + 'd,
-        scl: impl Peripheral<P = impl InputPin + OutputPin> + 'd,
-        address: config::DeviceAddress,
-        config: &config::SlaveDeviceConfig,
-    ) -> Result<i2c_slave_dev_handle_t, EspError> {
         crate::into_ref!(sda, scl);
 
-        let config = i2c_slave_config_t {
+        #[cfg(not(esp_idf_version = "4.3"))]
+        let sys_config = i2c_config_t {
+            mode: i2c_mode_t_I2C_MODE_SLAVE,
             sda_io_num: sda.pin(),
+            sda_pullup_en: config.sda_pullup_enabled,
             scl_io_num: scl.pin(),
-            clk_source: config.source_clock.into(),
-            flags: {
-                let mut flags = i2c_slave_config_t__bindgen_ty_1::default();
-                flags.set_stretch_en(0);
-                flags.set_broadcast_en(config.broadcast_enable as _);
-                flags
+            scl_pullup_en: config.scl_pullup_enabled,
+            __bindgen_anon_1: i2c_config_t__bindgen_ty_1 {
+                slave: i2c_config_t__bindgen_ty_1__bindgen_ty_2 {
+                    slave_addr: slave_addr as u16,
+                    addr_10bit_en: 0, // For now; to become configurable with embedded-hal V1.0
+                    maximum_speed: 0,
+                },
             },
-            i2c_port: I2C::port() as i32,
-            intr_priority: 0,
-            slave_addr: address.address(),
-            addr_bit_len: address.into(),
-            send_buf_depth: config.send_buffer_depth,
+            ..Default::default()
         };
 
-        let mut handle: i2c_slave_dev_handle_t = ptr::null_mut();
-
-        esp!(unsafe { i2c_new_slave_device(&config, &mut handle as _) })?;
-
-        Ok(handle)
-    }
-
-    fn to_i2c_err(err: EspError) -> I2cError {
-        if err.code() == ESP_FAIL {
-            I2cError::new(ErrorKind::NoAcknowledge(NoAcknowledgeSource::Unknown), err)
-        } else {
-            I2cError::other(err)
-        }
-    }
-
-    #[cfg(not(esp_idf_i2c_isr_iram_safe))]
-    fn enable_master_dev_isr_callback(
-        handle: i2c_master_dev_handle_t,
-        host: u8,
-    ) -> Result<(), EspError> {
-        esp!(unsafe {
-            i2c_master_register_event_callbacks(
-                handle,
-                &i2c_master_event_callbacks_t {
-                    on_trans_done: Some(master_isr),
+        #[cfg(esp_idf_version = "4.3")]
+        #[deprecated(
+            note = "Using ESP-IDF 4.3 is untested, please upgrade to 4.4 or newer. Support will be removed in the next major release."
+        )]
+        let sys_config = i2c_config_t {
+            mode: i2c_mode_t_I2C_MODE_SLAVE,
+            sda_io_num: pins.sda.pin(),
+            sda_pullup_en: config.sda_pullup_enabled,
+            scl_io_num: pins.scl.pin(),
+            scl_pullup_en: config.scl_pullup_enabled,
+            __bindgen_anon_1: i2c_config_t__bindgen_ty_1 {
+                slave: i2c_config_t__bindgen_ty_1__bindgen_ty_2 {
+                    slave_addr: slave_addr as u16,
+                    addr_10bit_en: 0, // For now; to become configurable with embedded-hal V1.0
                 },
-                &NOTIFIER[host as usize] as *const _ as *mut _,
-            )
-        })
-    }
-
-    #[cfg(not(esp_idf_i2c_isr_iram_safe))]
-    fn disable_master_dev_isr_callback(handle: i2c_master_dev_handle_t) -> Result<(), EspError> {
-        esp!(unsafe {
-            i2c_master_register_event_callbacks(
-                handle,
-                &i2c_master_event_callbacks_t::default(),
-                ptr::null_mut(),
-            )
-        })
-    }
-
-    #[cfg(not(esp_idf_i2c_isr_iram_safe))]
-    extern "C" fn master_isr(
-        _handle: i2c_master_dev_handle_t,
-        _data: *const i2c_master_event_data_t,
-        user_data: *mut c_void,
-    ) -> bool {
-        let notifier: &HalIsrNotification =
-            unsafe { (user_data as *const HalIsrNotification).as_ref() }.unwrap();
+            },
+            ..Default::default()
+        };
 
-        notifier.notify_lsb()
-    }
+        esp!(unsafe { i2c_param_config(I2C::port(), &sys_config) })?;
 
-    #[cfg(all(not(esp32c2), not(esp_idf_i2c_isr_iram_safe)))]
-    fn enable_slave_isr_callback(handle: i2c_slave_dev_handle_t, host: u8) -> Result<(), EspError> {
         esp!(unsafe {
-            i2c_slave_register_event_callbacks(
-                handle,
-                &i2c_slave_event_callbacks_t {
-                    on_recv_done: Some(slave_isr),
-                    on_stretch_occur: None,
-                },
-                &NOTIFIER[host as usize] as *const _ as *mut _,
+            i2c_driver_install(
+                I2C::port(),
+                i2c_mode_t_I2C_MODE_SLAVE,
+                config.rx_buf_len,
+                config.tx_buf_len,
+                InterruptType::to_native(config.intr_flags) as _,
             )
-        })
-    }
+        })?;
 
-    #[cfg(all(not(esp32c2), not(esp_idf_i2c_isr_iram_safe)))]
-    fn disable_slave_isr_callback(handle: i2c_slave_dev_handle_t) -> Result<(), EspError> {
-        esp!(unsafe {
-            i2c_slave_register_event_callbacks(
-                handle,
-                &i2c_slave_event_callbacks_t::default(),
-                ptr::null_mut(),
-            )
+        Ok(Self {
+            i2c: I2C::port() as _,
+            _p: PhantomData,
         })
     }
 
-    #[cfg(all(not(esp32c2), not(esp_idf_i2c_isr_iram_safe)))]
-    extern "C" fn slave_isr(
-        _handle: i2c_slave_dev_handle_t,
-        _data: *const i2c_slave_rx_done_event_data_t,
-        user_data: *mut c_void,
-    ) -> bool {
-        let notifier: &HalIsrNotification =
-            unsafe { (user_data as *const HalIsrNotification).as_ref() }.unwrap();
-
-        notifier.notify_lsb()
-    }
-
-    #[cfg(any(esp32c3, esp32c2, esp32c6))]
-    static NOTIFIER: [HalIsrNotification; 1] = [HalIsrNotification::new()];
-
-    #[cfg(not(any(esp32c3, esp32c2, esp32c6)))]
-    static NOTIFIER: [HalIsrNotification; 2] =
-        [HalIsrNotification::new(), HalIsrNotification::new()];
-}
-
-mod legacy {
-
-    use core::marker::PhantomData;
-    use core::time::Duration;
-
-    use embedded_hal::i2c::{ErrorKind, NoAcknowledgeSource};
-
-    use esp_idf_sys::*;
-
-    use crate::delay::*;
-    use crate::gpio::*;
-    use crate::interrupt::InterruptType;
-    use crate::peripheral::Peripheral;
-    use crate::units::*;
-
-    pub use embedded_hal::i2c::Operation;
-
-    use super::I2c;
-
-    crate::embedded_hal_error!(
-        I2cError,
-        embedded_hal::i2c::Error,
-        embedded_hal::i2c::ErrorKind
-    );
-
-    const APB_TICK_PERIOD_NS: u32 = 1_000_000_000 / 80_000_000;
-    #[derive(Copy, Clone, Debug)]
-    pub struct APBTickType(::core::ffi::c_int);
-    impl From<Duration> for APBTickType {
-        fn from(duration: Duration) -> Self {
-            APBTickType(
-                ((duration.as_nanos() + APB_TICK_PERIOD_NS as u128 - 1)
-                    / APB_TICK_PERIOD_NS as u128) as ::core::ffi::c_int,
-            )
-        }
-    }
-
-    pub type I2cConfig = config::Config;
-    #[cfg(not(esp32c2))]
-    pub type I2cSlaveConfig = config::SlaveConfig;
-
-    /// I2C configuration
-    pub mod config {
-        use enumset::EnumSet;
-
-        use super::APBTickType;
-        use crate::{interrupt::InterruptType, units::*};
-
-        /// I2C Master configuration
-        #[derive(Debug, Clone)]
-        pub struct Config {
-            pub baudrate: Hertz,
-            pub sda_pullup_enabled: bool,
-            pub scl_pullup_enabled: bool,
-            pub timeout: Option<APBTickType>,
-            pub intr_flags: EnumSet<InterruptType>,
-        }
-
-        impl Config {
-            pub fn new() -> Self {
-                Default::default()
-            }
-
-            #[must_use]
-            pub fn baudrate(mut self, baudrate: Hertz) -> Self {
-                self.baudrate = baudrate;
-                self
-            }
-
-            #[must_use]
-            pub fn sda_enable_pullup(mut self, enable: bool) -> Self {
-                self.sda_pullup_enabled = enable;
-                self
-            }
-
-            #[must_use]
-            pub fn scl_enable_pullup(mut self, enable: bool) -> Self {
-                self.scl_pullup_enabled = enable;
-                self
-            }
-
-            #[must_use]
-            pub fn timeout(mut self, timeout: APBTickType) -> Self {
-                self.timeout = Some(timeout);
-                self
-            }
-
-            #[must_use]
-            pub fn intr_flags(mut self, flags: EnumSet<InterruptType>) -> Self {
-                self.intr_flags = flags;
-                self
-            }
-        }
-
-        impl Default for Config {
-            fn default() -> Self {
-                Self {
-                    baudrate: Hertz(1_000_000),
-                    sda_pullup_enabled: true,
-                    scl_pullup_enabled: true,
-                    timeout: None,
-                    intr_flags: EnumSet::<InterruptType>::empty(),
-                }
-            }
-        }
-
-        /// I2C Slave configuration
-        #[cfg(not(esp32c2))]
-        #[derive(Debug, Clone)]
-        pub struct SlaveConfig {
-            pub sda_pullup_enabled: bool,
-            pub scl_pullup_enabled: bool,
-            pub rx_buf_len: usize,
-            pub tx_buf_len: usize,
-            pub intr_flags: EnumSet<InterruptType>,
-        }
-
-        #[cfg(not(esp32c2))]
-        impl SlaveConfig {
-            pub fn new() -> Self {
-                Default::default()
-            }
-
-            #[must_use]
-            pub fn sda_enable_pullup(mut self, enable: bool) -> Self {
-                self.sda_pullup_enabled = enable;
-                self
-            }
-
-            #[must_use]
-            pub fn scl_enable_pullup(mut self, enable: bool) -> Self {
-                self.scl_pullup_enabled = enable;
-                self
-            }
-
-            #[must_use]
-            pub fn rx_buffer_length(mut self, len: usize) -> Self {
-                self.rx_buf_len = len;
-                self
-            }
-
-            #[must_use]
-            pub fn tx_buffer_length(mut self, len: usize) -> Self {
-                self.tx_buf_len = len;
-                self
-            }
-
-            #[must_use]
-            pub fn intr_flags(mut self, flags: EnumSet<InterruptType>) -> Self {
-                self.intr_flags = flags;
-                self
-            }
-        }
-
-        #[cfg(not(esp32c2))]
-        impl Default for SlaveConfig {
-            fn default() -> Self {
-                Self {
-                    sda_pullup_enabled: true,
-                    scl_pullup_enabled: true,
-                    rx_buf_len: 0,
-                    tx_buf_len: 0,
-                    intr_flags: EnumSet::<InterruptType>::empty(),
-                }
-            }
-        }
-    }
-
-    pub struct I2cDriver<'d> {
-        i2c: u8,
-        _p: PhantomData<&'d mut ()>,
-    }
-
-    impl<'d> I2cDriver<'d> {
-        pub fn new<I2C: I2c>(
-            _i2c: impl Peripheral<P = I2C> + 'd,
-            sda: impl Peripheral<P = impl InputPin + OutputPin> + 'd,
-            scl: impl Peripheral<P = impl InputPin + OutputPin> + 'd,
-            config: &config::Config,
-        ) -> Result<Self, EspError> {
-            super::check_and_set_legacy_driver();
-
-            // i2c_config_t documentation says that clock speed must be no higher than 1 MHz
-            if config.baudrate > 1.MHz().into() {
-                return Err(EspError::from_infallible::<ESP_ERR_INVALID_ARG>());
-            }
-
-            crate::into_ref!(sda, scl);
-
-            let sys_config = i2c_config_t {
-                mode: i2c_mode_t_I2C_MODE_MASTER,
-                sda_io_num: sda.pin(),
-                sda_pullup_en: config.sda_pullup_enabled,
-                scl_io_num: scl.pin(),
-                scl_pullup_en: config.scl_pullup_enabled,
-                __bindgen_anon_1: i2c_config_t__bindgen_ty_1 {
-                    master: i2c_config_t__bindgen_ty_1__bindgen_ty_1 {
-                        clk_speed: config.baudrate.into(),
-                    },
-                },
-                ..Default::default()
-            };
-
-            esp!(unsafe { i2c_param_config(I2C::port(), &sys_config) })?;
-
-            esp!(unsafe {
-                i2c_driver_install(
-                    I2C::port(),
-                    i2c_mode_t_I2C_MODE_MASTER,
-                    0, // Not used in master mode
-                    0, // Not used in master mode
-                    InterruptType::to_native(config.intr_flags) as _,
-                )
-            })?;
-
-            if let Some(timeout) = config.timeout {
-                esp!(unsafe { i2c_set_timeout(I2C::port(), timeout.0) })?;
-            }
-
-            Ok(I2cDriver {
-                i2c: I2C::port() as _,
-                _p: PhantomData,
-            })
-        }
-
-        pub fn read(
-            &mut self,
-            addr: u8,
-            buffer: &mut [u8],
-            timeout: TickType_t,
-        ) -> Result<(), EspError> {
-            let mut command_link = CommandLink::new()?;
-
-            command_link.master_start()?;
-            command_link.master_write_byte((addr << 1) | (i2c_rw_t_I2C_MASTER_READ as u8), true)?;
-
-            if !buffer.is_empty() {
-                command_link.master_read(buffer, AckType::LastNack)?;
-            }
-
-            command_link.master_stop()?;
-
-            self.cmd_begin(&command_link, timeout)
-        }
-
-        pub fn write(
-            &mut self,
-            addr: u8,
-            bytes: &[u8],
-            timeout: TickType_t,
-        ) -> Result<(), EspError> {
-            let mut command_link = CommandLink::new()?;
-
-            command_link.master_start()?;
-            command_link
-                .master_write_byte((addr << 1) | (i2c_rw_t_I2C_MASTER_WRITE as u8), true)?;
-
-            if !bytes.is_empty() {
-                command_link.master_write(bytes, true)?;
-            }
-
-            command_link.master_stop()?;
-
-            self.cmd_begin(&command_link, timeout)
-        }
-
-        pub fn write_read(
-            &mut self,
-            addr: u8,
-            bytes: &[u8],
-            buffer: &mut [u8],
-            timeout: TickType_t,
-        ) -> Result<(), EspError> {
-            let mut command_link = CommandLink::new()?;
-
-            command_link.master_start()?;
-            command_link
-                .master_write_byte((addr << 1) | (i2c_rw_t_I2C_MASTER_WRITE as u8), true)?;
-
-            if !bytes.is_empty() {
-                command_link.master_write(bytes, true)?;
-            }
-
-            command_link.master_start()?;
-            command_link.master_write_byte((addr << 1) | (i2c_rw_t_I2C_MASTER_READ as u8), true)?;
-
-            if !buffer.is_empty() {
-                command_link.master_read(buffer, AckType::LastNack)?;
-            }
-
-            command_link.master_stop()?;
-
-            self.cmd_begin(&command_link, timeout)
-        }
-
-        pub fn transaction(
-            &mut self,
-            address: u8,
-            operations: &mut [Operation<'_>],
-            timeout: TickType_t,
-        ) -> Result<(), EspError> {
-            let mut command_link = CommandLink::new()?;
-
-            let last_op_index = operations.len() - 1;
-            let mut prev_was_read = None;
-
-            for (i, operation) in operations.iter_mut().enumerate() {
-                match operation {
-                    Operation::Read(buf) => {
-                        if Some(true) != prev_was_read {
-                            command_link.master_start()?;
-                            command_link.master_write_byte(
-                                (address << 1) | (i2c_rw_t_I2C_MASTER_READ as u8),
-                                true,
-                            )?;
-                        }
-                        prev_was_read = Some(true);
-
-                        if !buf.is_empty() {
-                            let ack = if i == last_op_index {
-                                AckType::LastNack
-                            } else {
-                                AckType::Ack
-                            };
-
-                            command_link.master_read(buf, ack)?;
-                        }
-                    }
-                    Operation::Write(buf) => {
-                        if Some(false) != prev_was_read {
-                            command_link.master_start()?;
-                            command_link.master_write_byte(
-                                (address << 1) | (i2c_rw_t_I2C_MASTER_WRITE as u8),
-                                true,
-                            )?;
-                        }
-                        prev_was_read = Some(false);
-
-                        if !buf.is_empty() {
-                            command_link.master_write(buf, true)?;
-                        }
-                    }
-                }
-            }
-
-            command_link.master_stop()?;
-
-            self.cmd_begin(&command_link, timeout)
-        }
-
-        fn cmd_begin(
-            &mut self,
-            command_link: &CommandLink,
-            timeout: TickType_t,
-        ) -> Result<(), EspError> {
-            esp!(unsafe { i2c_master_cmd_begin(self.port(), command_link.0, timeout) })
-        }
-
-        pub fn port(&self) -> i2c_port_t {
-            self.i2c as _
-        }
-    }
-
-    impl<'d> Drop for I2cDriver<'d> {
-        fn drop(&mut self) {
-            esp!(unsafe { i2c_driver_delete(self.port()) }).unwrap();
-        }
-    }
-
-    unsafe impl<'d> Send for I2cDriver<'d> {}
-
-    impl<'d> embedded_hal_0_2::blocking::i2c::Read for I2cDriver<'d> {
-        type Error = I2cError;
+    pub fn read(&mut self, buffer: &mut [u8], timeout: TickType_t) -> Result<usize, EspError> {
+        let n = unsafe {
+            i2c_slave_read_buffer(self.port(), buffer.as_mut_ptr(), buffer.len(), timeout)
+        };
 
-        fn read(&mut self, addr: u8, buffer: &mut [u8]) -> Result<(), Self::Error> {
-            I2cDriver::read(self, addr, buffer, BLOCK).map_err(to_i2c_err)
+        if n > 0 {
+            Ok(n as usize)
+        } else {
+            Err(EspError::from_infallible::<ESP_ERR_TIMEOUT>())
         }
     }
 
-    impl<'d> embedded_hal_0_2::blocking::i2c::Write for I2cDriver<'d> {
-        type Error = I2cError;
+    pub fn write(&mut self, bytes: &[u8], timeout: TickType_t) -> Result<usize, EspError> {
+        let n = unsafe {
+            i2c_slave_write_buffer(self.port(), bytes.as_ptr(), bytes.len() as i32, timeout)
+        };
 
-        fn write(&mut self, addr: u8, bytes: &[u8]) -> Result<(), Self::Error> {
-            I2cDriver::write(self, addr, bytes, BLOCK).map_err(to_i2c_err)
+        if n > 0 {
+            Ok(n as usize)
+        } else {
+            Err(EspError::from_infallible::<ESP_ERR_TIMEOUT>())
         }
     }
 
-    impl<'d> embedded_hal_0_2::blocking::i2c::WriteRead for I2cDriver<'d> {
-        type Error = I2cError;
-
-        fn write_read(
-            &mut self,
-            addr: u8,
-            bytes: &[u8],
-            buffer: &mut [u8],
-        ) -> Result<(), Self::Error> {
-            I2cDriver::write_read(self, addr, bytes, buffer, BLOCK).map_err(to_i2c_err)
-        }
+    pub fn port(&self) -> i2c_port_t {
+        self.i2c as _
     }
+}
 
-    impl<'d> embedded_hal::i2c::ErrorType for I2cDriver<'d> {
-        type Error = I2cError;
+#[cfg(not(esp32c2))]
+impl Drop for I2cSlaveDriver<'_> {
+    fn drop(&mut self) {
+        esp!(unsafe { i2c_driver_delete(self.port()) }).unwrap();
     }
+}
 
-    impl<'d> embedded_hal::i2c::I2c<embedded_hal::i2c::SevenBitAddress> for I2cDriver<'d> {
-        fn read(&mut self, addr: u8, buffer: &mut [u8]) -> Result<(), Self::Error> {
-            I2cDriver::read(self, addr, buffer, BLOCK).map_err(to_i2c_err)
-        }
+#[repr(u32)]
+enum AckType {
+    Ack = i2c_ack_type_t_I2C_MASTER_ACK,
+    #[allow(dead_code)]
+    Nack = i2c_ack_type_t_I2C_MASTER_NACK,
+    LastNack = i2c_ack_type_t_I2C_MASTER_LAST_NACK,
+}
 
-        fn write(&mut self, addr: u8, bytes: &[u8]) -> Result<(), Self::Error> {
-            I2cDriver::write(self, addr, bytes, BLOCK).map_err(to_i2c_err)
-        }
+struct CommandLink<'buffers>(i2c_cmd_handle_t, PhantomData<&'buffers u8>);
 
-        fn write_read(
-            &mut self,
-            addr: u8,
-            bytes: &[u8],
-            buffer: &mut [u8],
-        ) -> Result<(), Self::Error> {
-            I2cDriver::write_read(self, addr, bytes, buffer, BLOCK).map_err(to_i2c_err)
-        }
+impl<'buffers> CommandLink<'buffers> {
+    fn new() -> Result<Self, EspError> {
+        let handle = unsafe { i2c_cmd_link_create() };
 
-        fn transaction(
-            &mut self,
-            address: u8,
-            operations: &mut [embedded_hal::i2c::Operation<'_>],
-        ) -> Result<(), Self::Error> {
-            I2cDriver::transaction(self, address, operations, BLOCK).map_err(to_i2c_err)
+        if handle.is_null() {
+            return Err(EspError::from_infallible::<ESP_ERR_NO_MEM>());
         }
-    }
 
-    fn to_i2c_err(err: EspError) -> I2cError {
-        if err.code() == ESP_FAIL {
-            I2cError::new(ErrorKind::NoAcknowledge(NoAcknowledgeSource::Unknown), err)
-        } else {
-            I2cError::other(err)
-        }
+        Ok(CommandLink(handle, PhantomData))
     }
 
-    #[cfg(not(esp32c2))]
-    pub struct I2cSlaveDriver<'d> {
-        i2c: u8,
-        _p: PhantomData<&'d mut ()>,
+    fn master_start(&mut self) -> Result<(), EspError> {
+        esp!(unsafe { i2c_master_start(self.0) })
     }
 
-    #[cfg(not(esp32c2))]
-    unsafe impl<'d> Send for I2cSlaveDriver<'d> {}
-
-    #[cfg(not(esp32c2))]
-    impl<'d> I2cSlaveDriver<'d> {
-        pub fn new<I2C: I2c>(
-            _i2c: impl Peripheral<P = I2C> + 'd,
-            sda: impl Peripheral<P = impl InputPin + OutputPin> + 'd,
-            scl: impl Peripheral<P = impl InputPin + OutputPin> + 'd,
-            slave_addr: u8,
-            config: &config::SlaveConfig,
-        ) -> Result<Self, EspError> {
-            super::check_and_set_legacy_driver();
-
-            crate::into_ref!(sda, scl);
-
-            #[cfg(not(esp_idf_version = "4.3"))]
-            let sys_config = i2c_config_t {
-                mode: i2c_mode_t_I2C_MODE_SLAVE,
-                sda_io_num: sda.pin(),
-                sda_pullup_en: config.sda_pullup_enabled,
-                scl_io_num: scl.pin(),
-                scl_pullup_en: config.scl_pullup_enabled,
-                __bindgen_anon_1: i2c_config_t__bindgen_ty_1 {
-                    slave: i2c_config_t__bindgen_ty_1__bindgen_ty_2 {
-                        slave_addr: slave_addr as u16,
-                        addr_10bit_en: 0, // For now; to become configurable with embedded-hal V1.0
-                        maximum_speed: 0,
-                    },
-                },
-                ..Default::default()
-            };
-
-            #[cfg(esp_idf_version = "4.3")]
-            #[deprecated(
-                note = "Using ESP-IDF 4.3 is untested, please upgrade to 4.4 or newer. Support will be removed in the next major release."
-            )]
-            let sys_config = i2c_config_t {
-                mode: i2c_mode_t_I2C_MODE_SLAVE,
-                sda_io_num: pins.sda.pin(),
-                sda_pullup_en: config.sda_pullup_enabled,
-                scl_io_num: pins.scl.pin(),
-                scl_pullup_en: config.scl_pullup_enabled,
-                __bindgen_anon_1: i2c_config_t__bindgen_ty_1 {
-                    slave: i2c_config_t__bindgen_ty_1__bindgen_ty_2 {
-                        slave_addr: slave_addr as u16,
-                        addr_10bit_en: 0, // For now; to become configurable with embedded-hal V1.0
-                    },
-                },
-                ..Default::default()
-            };
-
-            esp!(unsafe { i2c_param_config(I2C::port(), &sys_config) })?;
-
-            esp!(unsafe {
-                i2c_driver_install(
-                    I2C::port(),
-                    i2c_mode_t_I2C_MODE_SLAVE,
-                    config.rx_buf_len,
-                    config.tx_buf_len,
-                    InterruptType::to_native(config.intr_flags) as _,
-                )
-            })?;
-
-            Ok(Self {
-                i2c: I2C::port() as _,
-                _p: PhantomData,
-            })
-        }
-
-        pub fn read(&mut self, buffer: &mut [u8], timeout: TickType_t) -> Result<usize, EspError> {
-            let n = unsafe {
-                i2c_slave_read_buffer(self.port(), buffer.as_mut_ptr(), buffer.len(), timeout)
-            };
-
-            if n > 0 {
-                Ok(n as usize)
-            } else {
-                Err(EspError::from_infallible::<ESP_ERR_TIMEOUT>())
-            }
-        }
-
-        pub fn write(&mut self, bytes: &[u8], timeout: TickType_t) -> Result<usize, EspError> {
-            let n = unsafe {
-                i2c_slave_write_buffer(self.port(), bytes.as_ptr(), bytes.len() as i32, timeout)
-            };
-
-            if n > 0 {
-                Ok(n as usize)
-            } else {
-                Err(EspError::from_infallible::<ESP_ERR_TIMEOUT>())
-            }
-        }
-
-        pub fn port(&self) -> i2c_port_t {
-            self.i2c as _
-        }
+    fn master_stop(&mut self) -> Result<(), EspError> {
+        esp!(unsafe { i2c_master_stop(self.0) })
     }
 
-    #[cfg(not(esp32c2))]
-    impl<'d> Drop for I2cSlaveDriver<'d> {
-        fn drop(&mut self) {
-            esp!(unsafe { i2c_driver_delete(self.port()) }).unwrap();
-        }
+    fn master_write_byte(&mut self, data: u8, ack_en: bool) -> Result<(), EspError> {
+        esp!(unsafe { i2c_master_write_byte(self.0, data, ack_en) })
     }
 
-    #[repr(u32)]
-    enum AckType {
-        Ack = i2c_ack_type_t_I2C_MASTER_ACK,
-        #[allow(dead_code)]
-        Nack = i2c_ack_type_t_I2C_MASTER_NACK,
-        LastNack = i2c_ack_type_t_I2C_MASTER_LAST_NACK,
+    fn master_write(&mut self, buf: &'buffers [u8], ack_en: bool) -> Result<(), EspError> {
+        esp!(unsafe { i2c_master_write(self.0, buf.as_ptr(), buf.len(), ack_en,) })
     }
 
-    struct CommandLink<'buffers>(i2c_cmd_handle_t, PhantomData<&'buffers u8>);
-
-    impl<'buffers> CommandLink<'buffers> {
-        fn new() -> Result<Self, EspError> {
-            let handle = unsafe { i2c_cmd_link_create() };
-
-            if handle.is_null() {
-                return Err(EspError::from_infallible::<ESP_ERR_NO_MEM>());
-            }
-
-            Ok(CommandLink(handle, PhantomData))
-        }
-
-        fn master_start(&mut self) -> Result<(), EspError> {
-            esp!(unsafe { i2c_master_start(self.0) })
-        }
-
-        fn master_stop(&mut self) -> Result<(), EspError> {
-            esp!(unsafe { i2c_master_stop(self.0) })
-        }
-
-        fn master_write_byte(&mut self, data: u8, ack_en: bool) -> Result<(), EspError> {
-            esp!(unsafe { i2c_master_write_byte(self.0, data, ack_en) })
-        }
-
-        fn master_write(&mut self, buf: &'buffers [u8], ack_en: bool) -> Result<(), EspError> {
-            esp!(unsafe { i2c_master_write(self.0, buf.as_ptr(), buf.len(), ack_en,) })
-        }
-
-        fn master_read(&mut self, buf: &'buffers mut [u8], ack: AckType) -> Result<(), EspError> {
-            esp!(unsafe { i2c_master_read(self.0, buf.as_mut_ptr().cast(), buf.len(), ack as u32) })
-        }
+    fn master_read(&mut self, buf: &'buffers mut [u8], ack: AckType) -> Result<(), EspError> {
+        esp!(unsafe { i2c_master_read(self.0, buf.as_mut_ptr().cast(), buf.len(), ack as u32) })
     }
+}
 
-    impl<'buffers> Drop for CommandLink<'buffers> {
-        fn drop(&mut self) {
-            unsafe {
-                i2c_cmd_link_delete(self.0);
-            }
+impl Drop for CommandLink<'_> {
+    fn drop(&mut self) {
+        unsafe {
+            i2c_cmd_link_delete(self.0);
         }
     }
 }
diff --git a/src/i2c/modern.rs b/src/i2c/modern.rs
new file mode 100644
index 00000000000..a27db1a70e6
--- /dev/null
+++ b/src/i2c/modern.rs
@@ -0,0 +1,1215 @@
+use core::borrow::Borrow;
+use core::ffi::c_void;
+use core::marker::PhantomData;
+use core::ptr;
+
+use embassy_sync::mutex::Mutex;
+use embassy_sync::mutex::MutexGuard;
+
+use embedded_hal::i2c::{ErrorKind, NoAcknowledgeSource};
+
+use esp_idf_sys::*;
+
+use crate::delay::*;
+use crate::gpio::*;
+use crate::interrupt::asynch::HalIsrNotification;
+use crate::peripheral::Peripheral;
+use crate::task::embassy_sync::EspRawMutex;
+use crate::units::*;
+
+pub use embedded_hal::i2c::Operation;
+
+use super::I2c;
+
+crate::embedded_hal_error!(
+    I2cError,
+    embedded_hal::i2c::Error,
+    embedded_hal::i2c::ErrorKind
+);
+
+macro_rules! on_err {
+    ($d:expr, $oe:expr) => {
+        {
+            match $d {
+                Err(e) => {
+                    $oe
+                    Err(e)
+                }
+                v => v
+            }
+        }
+    };
+}
+
+pub type I2cConfig = config::Config;
+#[cfg(not(esp32c2))]
+pub type I2cSlaveConfig = config::SlaveDeviceConfig;
+
+/// I2C configuration
+pub mod config {
+    use esp_idf_sys::*;
+
+    use crate::units::*;
+
+    // TODO: in bindings its XTAL called and in doc its APB
+    const APB_SCLK: soc_periph_i2c_clk_src_t = soc_periph_i2c_clk_src_t_I2C_CLK_SRC_XTAL;
+    const FAST_SCLK: soc_periph_i2c_clk_src_t = soc_periph_i2c_clk_src_t_I2C_CLK_SRC_RC_FAST;
+
+    /// i2c source clock
+    #[derive(PartialEq, Eq, Copy, Clone, Debug)]
+    #[allow(non_camel_case_types)]
+    pub enum SourceClock {
+        APB,
+        RC_FAST,
+    }
+
+    impl SourceClock {
+        pub const fn default() -> Self {
+            Self::from_raw(soc_periph_i2c_clk_src_t_I2C_CLK_SRC_DEFAULT)
+        }
+
+        pub const fn from_raw(source_clock: soc_periph_i2c_clk_src_t) -> Self {
+            match source_clock {
+                APB_SCLK => SourceClock::APB,
+                FAST_SCLK => SourceClock::RC_FAST,
+                _ => unreachable!(),
+            }
+        }
+    }
+
+    impl Default for SourceClock {
+        fn default() -> Self {
+            SourceClock::default()
+        }
+    }
+
+    impl From<SourceClock> for i2c_clock_source_t {
+        fn from(source_clock: SourceClock) -> Self {
+            match source_clock {
+                SourceClock::RC_FAST => FAST_SCLK,
+                SourceClock::APB => APB_SCLK,
+            }
+        }
+    }
+
+    impl From<uart_sclk_t> for SourceClock {
+        fn from(source_clock: i2c_clock_source_t) -> Self {
+            Self::from_raw(source_clock)
+        }
+    }
+
+    /// I2C Master configuration
+    #[derive(Debug, Clone)]
+    pub struct Config {
+        pub pullup_enabled: bool,
+        pub source_clock: SourceClock,
+        pub glitch_ignore_cnt: u8,
+    }
+
+    impl Config {
+        pub fn new() -> Self {
+            Default::default()
+        }
+
+        #[must_use]
+        pub fn enable_pullup(mut self, enable: bool) -> Self {
+            self.pullup_enabled = enable;
+            self
+        }
+
+        #[must_use]
+        pub fn source_clock(mut self, source_clock: SourceClock) -> Self {
+            self.source_clock = source_clock;
+            self
+        }
+
+        #[must_use]
+        pub fn glitch_ignore_count(mut self, count: u8) -> Self {
+            self.glitch_ignore_cnt = count;
+            self
+        }
+    }
+
+    impl Default for Config {
+        fn default() -> Self {
+            Self {
+                pullup_enabled: true,
+                source_clock: SourceClock::default(),
+                glitch_ignore_cnt: 7,
+            }
+        }
+    }
+
+    #[derive(Debug, Clone)]
+    pub enum DeviceAddress {
+        SevenBit(u8),
+        TenBit(u16),
+    }
+
+    impl DeviceAddress {
+        pub(super) fn address(&self) -> u16 {
+            match self {
+                DeviceAddress::SevenBit(addr) => *addr as u16,
+                // TODO: if cfg allows 10 bit address
+                DeviceAddress::TenBit(addr) => *addr,
+            }
+        }
+    }
+
+    impl From<DeviceAddress> for i2c_addr_bit_len_t {
+        fn from(address: DeviceAddress) -> Self {
+            match address {
+                DeviceAddress::SevenBit(_) => i2c_addr_bit_len_t_I2C_ADDR_BIT_LEN_7,
+                DeviceAddress::TenBit(_) => i2c_addr_bit_len_t_I2C_ADDR_BIT_LEN_10,
+            }
+        }
+    }
+
+    #[derive(Debug, Clone)]
+    pub struct DeviceConfig {
+        pub address: DeviceAddress,
+        pub baudrate: Hertz,
+    }
+
+    impl DeviceConfig {
+        pub const fn new(address: DeviceAddress) -> Self {
+            Self {
+                address,
+                baudrate: Hertz(1_000_000),
+            }
+        }
+
+        #[must_use]
+        pub fn baudrate(mut self, baudrate: Hertz) -> Self {
+            self.baudrate = baudrate;
+            self
+        }
+    }
+
+    /// I2C Slave configuration
+    #[cfg(not(esp32c2))]
+    #[derive(Debug, Clone)]
+    pub struct SlaveDeviceConfig {
+        pub source_clock: SourceClock,
+        pub broadcast_enable: bool,
+        pub send_buffer_depth: u32,
+    }
+
+    #[cfg(not(esp32c2))]
+    impl SlaveDeviceConfig {
+        pub fn new() -> Self {
+            Default::default()
+        }
+
+        #[must_use]
+        pub fn source_clock(mut self, source_clock: SourceClock) -> Self {
+            self.source_clock = source_clock;
+            self
+        }
+
+        #[must_use]
+        pub fn enable_broadcast(mut self, enable: bool) -> Self {
+            self.broadcast_enable = enable;
+            self
+        }
+
+        #[must_use]
+        pub fn set_send_buffer_depth(mut self, depth: u32) -> Self {
+            self.send_buffer_depth = depth;
+            self
+        }
+    }
+
+    #[cfg(not(esp32c2))]
+    impl Default for SlaveDeviceConfig {
+        fn default() -> Self {
+            Self {
+                source_clock: SourceClock::default(),
+                broadcast_enable: false,
+                send_buffer_depth: 0,
+            }
+        }
+    }
+}
+
+pub struct I2cDriver<'d> {
+    port: u8,
+    handle: i2c_master_bus_handle_t,
+    _p: PhantomData<&'d mut ()>,
+}
+
+impl<'d> I2cDriver<'d> {
+    pub fn new<I2C: I2c>(
+        _i2c: impl Peripheral<P = I2C> + 'd,
+        sda: impl Peripheral<P = impl InputPin + OutputPin> + 'd,
+        scl: impl Peripheral<P = impl InputPin + OutputPin> + 'd,
+        config: &config::Config,
+    ) -> Result<Self, EspError> {
+        super::check_and_set_beta_driver();
+
+        let handle = init_master_bus(_i2c, sda, scl, config, 0)?;
+
+        Ok(I2cDriver {
+            port: I2C::port() as u8,
+            handle,
+            _p: PhantomData,
+        })
+    }
+
+    pub fn port(&self) -> u8 {
+        self.port
+    }
+
+    fn bus_handle(&self) -> i2c_master_bus_handle_t {
+        self.handle
+    }
+
+    /// Probe device on the bus.
+    pub fn probe_device(
+        &mut self,
+        address: config::DeviceAddress,
+        timeout: TickType_t,
+    ) -> Result<(), EspError> {
+        esp!(unsafe { i2c_master_probe(self.handle, address.address(), timeout as i32) })
+    }
+
+    pub fn device(
+        &mut self,
+        config: &config::DeviceConfig,
+    ) -> Result<I2cDeviceDriver<'d, &mut I2cDriver<'d>>, EspError> {
+        I2cDeviceDriver::new(self, config)
+    }
+
+    // Helper to use the embedded_hal traits.
+    fn read(&mut self, addr: u8, buffer: &mut [u8], timeout: TickType_t) -> Result<(), EspError> {
+        self.device(&config::DeviceConfig::new(config::DeviceAddress::SevenBit(
+            addr,
+        )))?
+        .read(buffer, timeout)
+    }
+
+    // Helper to use the embedded_hal traits.
+    fn write(&mut self, addr: u8, bytes: &[u8], timeout: TickType_t) -> Result<(), EspError> {
+        self.device(&config::DeviceConfig::new(config::DeviceAddress::SevenBit(
+            addr,
+        )))?
+        .write(bytes, timeout)
+    }
+
+    // Helper to use the embedded_hal traits.
+    fn write_read(
+        &mut self,
+        addr: u8,
+        bytes: &[u8],
+        buffer: &mut [u8],
+        timeout: TickType_t,
+    ) -> Result<(), EspError> {
+        self.device(&config::DeviceConfig::new(config::DeviceAddress::SevenBit(
+            addr,
+        )))?
+        .write_read(bytes, buffer, timeout)
+    }
+}
+
+unsafe impl<'d> Send for I2cDriver<'d> {}
+
+impl<'d> Drop for I2cDriver<'d> {
+    fn drop(&mut self) {
+        esp!(unsafe { i2c_del_master_bus(self.handle) }).unwrap();
+    }
+}
+
+impl<'d> embedded_hal_0_2::blocking::i2c::Read for I2cDriver<'d> {
+    type Error = I2cError;
+
+    fn read(&mut self, addr: u8, buffer: &mut [u8]) -> Result<(), Self::Error> {
+        Self::read(self, addr, buffer, BLOCK).map_err(to_i2c_err)
+    }
+}
+
+impl<'d> embedded_hal_0_2::blocking::i2c::Write for I2cDriver<'d> {
+    type Error = I2cError;
+
+    fn write(&mut self, addr: u8, bytes: &[u8]) -> Result<(), Self::Error> {
+        Self::write(self, addr, bytes, BLOCK).map_err(to_i2c_err)
+    }
+}
+
+impl<'d> embedded_hal_0_2::blocking::i2c::WriteRead for I2cDriver<'d> {
+    type Error = I2cError;
+
+    fn write_read(&mut self, addr: u8, bytes: &[u8], buffer: &mut [u8]) -> Result<(), Self::Error> {
+        Self::write_read(self, addr, bytes, buffer, BLOCK).map_err(to_i2c_err)
+    }
+}
+
+impl<'d> embedded_hal::i2c::ErrorType for I2cDriver<'d> {
+    type Error = I2cError;
+}
+
+impl<'d> embedded_hal::i2c::I2c<embedded_hal::i2c::SevenBitAddress> for I2cDriver<'d> {
+    fn read(&mut self, addr: u8, buffer: &mut [u8]) -> Result<(), Self::Error> {
+        Self::read(self, addr, buffer, BLOCK).map_err(to_i2c_err)
+    }
+
+    fn write(&mut self, addr: u8, bytes: &[u8]) -> Result<(), Self::Error> {
+        Self::write(self, addr, bytes, BLOCK).map_err(to_i2c_err)
+    }
+
+    fn write_read(&mut self, addr: u8, bytes: &[u8], buffer: &mut [u8]) -> Result<(), Self::Error> {
+        Self::write_read(self, addr, bytes, buffer, BLOCK).map_err(to_i2c_err)
+    }
+
+    fn transaction(
+        &mut self,
+        _addr: u8,
+        _operations: &mut [embedded_hal::i2c::Operation<'_>],
+    ) -> Result<(), Self::Error> {
+        unimplemented!("transactional not implemented")
+    }
+}
+
+pub struct I2cDeviceDriver<'d, T>
+where
+    T: Borrow<I2cDriver<'d>>,
+{
+    _driver: T,
+    handle: i2c_master_dev_handle_t,
+    _p: PhantomData<&'d mut ()>,
+}
+
+impl<'d, T> I2cDeviceDriver<'d, T>
+where
+    T: Borrow<I2cDriver<'d>>,
+{
+    pub fn new(driver: T, config: &config::DeviceConfig) -> Result<Self, EspError> {
+        let handle = init_device(driver.borrow().bus_handle(), &config)?;
+
+        Ok(I2cDeviceDriver {
+            _driver: driver,
+            handle,
+            _p: PhantomData,
+        })
+    }
+
+    pub fn read(&mut self, buffer: &mut [u8], timeout: TickType_t) -> Result<(), EspError> {
+        esp!(unsafe {
+            i2c_master_receive(
+                self.handle,
+                buffer.as_mut_ptr().cast(),
+                buffer.len(),
+                timeout as i32,
+            )
+        })
+    }
+
+    pub fn write(&mut self, bytes: &[u8], timeout: TickType_t) -> Result<(), EspError> {
+        esp!(unsafe {
+            i2c_master_transmit(
+                self.handle,
+                bytes.as_ptr().cast(),
+                bytes.len(),
+                timeout as i32,
+            )
+        })
+    }
+
+    pub fn write_read(
+        &mut self,
+        bytes: &[u8],
+        buffer: &mut [u8],
+        timeout: TickType_t,
+    ) -> Result<(), EspError> {
+        esp!(unsafe {
+            i2c_master_transmit_receive(
+                self.handle,
+                bytes.as_ptr().cast(),
+                bytes.len(),
+                buffer.as_mut_ptr().cast(),
+                buffer.len(),
+                timeout as i32,
+            )
+        })
+    }
+}
+
+impl<'d, T> Drop for I2cDeviceDriver<'d, T>
+where
+    T: Borrow<I2cDriver<'d>>,
+{
+    fn drop(&mut self) {
+        esp!(unsafe { i2c_master_bus_rm_device(self.handle) }).unwrap();
+    }
+}
+
+impl<'d, T> embedded_hal_0_2::blocking::i2c::Read for I2cDeviceDriver<'d, T>
+where
+    T: Borrow<I2cDriver<'d>>,
+{
+    type Error = I2cError;
+
+    fn read(&mut self, _addr: u8, buffer: &mut [u8]) -> Result<(), Self::Error> {
+        I2cDeviceDriver::read(self, buffer, BLOCK).map_err(to_i2c_err)
+    }
+}
+
+impl<'d, T> embedded_hal_0_2::blocking::i2c::Write for I2cDeviceDriver<'d, T>
+where
+    T: Borrow<I2cDriver<'d>>,
+{
+    type Error = I2cError;
+
+    fn write(&mut self, _addr: u8, bytes: &[u8]) -> Result<(), Self::Error> {
+        I2cDeviceDriver::write(self, bytes, BLOCK).map_err(to_i2c_err)
+    }
+}
+
+impl<'d, T> embedded_hal_0_2::blocking::i2c::WriteRead for I2cDeviceDriver<'d, T>
+where
+    T: Borrow<I2cDriver<'d>>,
+{
+    type Error = I2cError;
+
+    fn write_read(
+        &mut self,
+        _addr: u8,
+        bytes: &[u8],
+        buffer: &mut [u8],
+    ) -> Result<(), Self::Error> {
+        I2cDeviceDriver::write_read(self, bytes, buffer, BLOCK).map_err(to_i2c_err)
+    }
+}
+
+impl<'d, T> embedded_hal::i2c::ErrorType for I2cDeviceDriver<'d, T>
+where
+    T: Borrow<I2cDriver<'d>>,
+{
+    type Error = I2cError;
+}
+
+impl<'d, T> embedded_hal::i2c::I2c<embedded_hal::i2c::SevenBitAddress> for I2cDeviceDriver<'d, T>
+where
+    T: Borrow<I2cDriver<'d>>,
+{
+    fn read(&mut self, _addr: u8, buffer: &mut [u8]) -> Result<(), Self::Error> {
+        I2cDeviceDriver::read(self, buffer, BLOCK).map_err(to_i2c_err)
+    }
+
+    fn write(&mut self, _addr: u8, bytes: &[u8]) -> Result<(), Self::Error> {
+        I2cDeviceDriver::write(self, bytes, BLOCK).map_err(to_i2c_err)
+    }
+
+    fn write_read(
+        &mut self,
+        _addr: u8,
+        bytes: &[u8],
+        buffer: &mut [u8],
+    ) -> Result<(), Self::Error> {
+        I2cDeviceDriver::write_read(self, bytes, buffer, BLOCK).map_err(to_i2c_err)
+    }
+
+    fn transaction(
+        &mut self,
+        _addr: u8,
+        _operations: &mut [embedded_hal::i2c::Operation<'_>],
+    ) -> Result<(), Self::Error> {
+        unimplemented!("transactional not implemented")
+    }
+}
+
+// ------------------------------------------------------------------------------------------------
+// ------------------------------------- Async ----------------------------------------------------
+// ------------------------------------------------------------------------------------------------
+
+pub struct AsyncI2cDriver<'d> {
+    bus_lock: Mutex<EspRawMutex, ()>,
+    handle: i2c_master_bus_handle_t,
+    port: u8,
+    _p: PhantomData<&'d mut ()>,
+}
+
+impl<'d> AsyncI2cDriver<'d> {
+    pub fn new<I2C: I2c>(
+        _i2c: impl Peripheral<P = I2C> + 'd,
+        sda: impl Peripheral<P = impl InputPin + OutputPin> + 'd,
+        scl: impl Peripheral<P = impl InputPin + OutputPin> + 'd,
+        config: &config::Config,
+    ) -> Result<Self, EspError> {
+        super::check_and_set_beta_driver();
+
+        let handle = init_master_bus(_i2c, sda, scl, config, 1)?;
+
+        Ok(AsyncI2cDriver {
+            bus_lock: Mutex::new(()),
+            handle,
+            port: I2C::port() as _,
+            _p: PhantomData,
+        })
+    }
+
+    pub fn port(&self) -> u8 {
+        self.port
+    }
+
+    fn bus_handle(&self) -> i2c_master_bus_handle_t {
+        self.handle
+    }
+
+    async fn acquire_bus<'a>(&'a self) -> MutexGuard<'a, EspRawMutex, ()> {
+        self.bus_lock.lock().await
+    }
+
+    pub fn device(
+        &self,
+        config: &config::DeviceConfig,
+    ) -> Result<AsyncI2cDeviceDriver<'d, &AsyncI2cDriver<'d>>, EspError> {
+        AsyncI2cDeviceDriver::new(self, config)
+    }
+
+    pub fn owned_device(
+        self,
+        config: &config::DeviceConfig,
+    ) -> Result<OwnedAsyncI2cDeviceDriver<'d>, EspError> {
+        OwnedAsyncI2cDeviceDriver::wrap(self, config)
+    }
+
+    async fn read(&mut self, address: u8, buffer: &mut [u8]) -> Result<(), EspError> {
+        self.device(&config::DeviceConfig::new(config::DeviceAddress::SevenBit(
+            address,
+        )))?
+        .read(buffer, BLOCK)
+        .await
+    }
+
+    async fn write(&mut self, address: u8, bytes: &[u8]) -> Result<(), EspError> {
+        self.device(&config::DeviceConfig::new(config::DeviceAddress::SevenBit(
+            address,
+        )))?
+        .write(bytes, BLOCK)
+        .await
+    }
+
+    async fn write_read(
+        &mut self,
+        address: u8,
+        bytes: &[u8],
+        buffer: &mut [u8],
+    ) -> Result<(), EspError> {
+        self.device(&config::DeviceConfig::new(config::DeviceAddress::SevenBit(
+            address,
+        )))?
+        .write_read(bytes, buffer, BLOCK)
+        .await
+    }
+}
+
+impl<'d> embedded_hal::i2c::ErrorType for AsyncI2cDriver<'d> {
+    type Error = I2cError;
+}
+
+impl<'d> embedded_hal_async::i2c::I2c<embedded_hal::i2c::SevenBitAddress> for AsyncI2cDriver<'d> {
+    async fn read(&mut self, address: u8, buffer: &mut [u8]) -> Result<(), Self::Error> {
+        self.read(address, buffer).await.map_err(to_i2c_err)
+    }
+
+    async fn write(&mut self, address: u8, bytes: &[u8]) -> Result<(), Self::Error> {
+        self.write(address, bytes).await.map_err(to_i2c_err)
+    }
+
+    async fn write_read(
+        &mut self,
+        address: u8,
+        bytes: &[u8],
+        buffer: &mut [u8],
+    ) -> Result<(), Self::Error> {
+        self.write_read(address, bytes, buffer)
+            .await
+            .map_err(to_i2c_err)
+    }
+
+    async fn transaction(
+        &mut self,
+        _address: u8,
+        _operations: &mut [Operation<'_>],
+    ) -> Result<(), Self::Error> {
+        unimplemented!("transactional not implemented")
+    }
+}
+
+unsafe impl<'d> Send for AsyncI2cDriver<'d> {}
+
+impl<'d> Drop for AsyncI2cDriver<'d> {
+    fn drop(&mut self) {
+        loop {
+            if let Ok(_lock_guard) = self.bus_lock.try_lock() {
+                esp!(unsafe { i2c_del_master_bus(self.handle) }).unwrap();
+                break;
+            }
+        }
+    }
+}
+
+#[cfg(not(esp_idf_i2c_isr_iram_safe))]
+pub struct AsyncI2cDeviceDriver<'d, T>
+where
+    T: Borrow<AsyncI2cDriver<'d>>,
+{
+    driver: T,
+    handle: i2c_master_dev_handle_t,
+    _p: PhantomData<&'d mut ()>,
+}
+
+#[cfg(not(esp_idf_i2c_isr_iram_safe))]
+impl<'d, T> AsyncI2cDeviceDriver<'d, T>
+where
+    T: Borrow<AsyncI2cDriver<'d>>,
+{
+    fn new(driver: T, config: &config::DeviceConfig) -> Result<Self, EspError> {
+        let handle = init_device(driver.borrow().bus_handle(), config)?;
+
+        Ok(Self {
+            driver,
+            handle,
+            _p: PhantomData,
+        })
+    }
+}
+
+#[cfg(not(esp_idf_i2c_isr_iram_safe))]
+impl<'d, T> AsyncI2cDeviceDriver<'d, T>
+where
+    T: Borrow<AsyncI2cDriver<'d>>,
+{
+    pub async fn read(&mut self, buffer: &mut [u8], timeout: TickType_t) -> Result<(), EspError> {
+        let handle = self.handle;
+        let driver = self.driver.borrow();
+        let port = driver.port();
+
+        let _lock_guard = driver.acquire_bus().await;
+        enable_master_dev_isr_callback(handle, port)?;
+        on_err!(
+            esp!(unsafe {
+                i2c_master_receive(
+                    handle,
+                    buffer.as_mut_ptr().cast(),
+                    buffer.len(),
+                    timeout as i32,
+                )
+            }),
+            {
+                disable_master_dev_isr_callback(handle).unwrap();
+            }
+        )?;
+
+        NOTIFIER[port as usize].wait().await;
+        disable_master_dev_isr_callback(handle)?;
+        Ok(())
+    }
+
+    pub async fn write(&mut self, bytes: &[u8], timeout: TickType_t) -> Result<(), EspError> {
+        let handle = self.handle;
+        let driver = self.driver.borrow();
+        let port = driver.port();
+
+        let _lock_guard = driver.acquire_bus().await;
+        enable_master_dev_isr_callback(handle, port)?;
+        on_err!(
+            esp!(unsafe {
+                i2c_master_transmit(handle, bytes.as_ptr().cast(), bytes.len(), timeout as i32)
+            }),
+            {
+                disable_master_dev_isr_callback(handle).unwrap();
+            }
+        )?;
+
+        NOTIFIER[port as usize].wait().await;
+        disable_master_dev_isr_callback(handle)?;
+        Ok(())
+    }
+
+    pub async fn write_read(
+        &mut self,
+        bytes: &[u8],
+        buffer: &mut [u8],
+        timeout: TickType_t,
+    ) -> Result<(), EspError> {
+        let handle = self.handle;
+        let driver = self.driver.borrow();
+        let port = driver.port();
+
+        let _lock_guard = driver.acquire_bus().await;
+        enable_master_dev_isr_callback(handle, port)?;
+        on_err!(
+            esp!(unsafe {
+                i2c_master_transmit_receive(
+                    handle,
+                    bytes.as_ptr().cast(),
+                    bytes.len(),
+                    buffer.as_mut_ptr().cast(),
+                    buffer.len(),
+                    timeout as i32,
+                )
+            }),
+            {
+                disable_master_dev_isr_callback(handle).unwrap();
+            }
+        )?;
+
+        NOTIFIER[port as usize].wait().await;
+        disable_master_dev_isr_callback(handle)?;
+        Ok(())
+    }
+}
+
+#[cfg(not(esp_idf_i2c_isr_iram_safe))]
+unsafe impl<'d, T> Send for AsyncI2cDeviceDriver<'d, T> where
+    T: Send + Borrow<AsyncI2cDriver<'d>> + 'd
+{
+}
+
+#[cfg(not(esp_idf_i2c_isr_iram_safe))]
+impl<'d, T> embedded_hal::i2c::ErrorType for AsyncI2cDeviceDriver<'d, T>
+where
+    T: Borrow<AsyncI2cDriver<'d>>,
+{
+    type Error = I2cError;
+}
+
+#[cfg(not(esp_idf_i2c_isr_iram_safe))]
+impl<'d, T> embedded_hal_async::i2c::I2c<embedded_hal::i2c::SevenBitAddress>
+    for AsyncI2cDeviceDriver<'d, T>
+where
+    T: Borrow<AsyncI2cDriver<'d>>,
+{
+    async fn read(&mut self, _address: u8, buffer: &mut [u8]) -> Result<(), Self::Error> {
+        Self::read(self, buffer, BLOCK).await.map_err(to_i2c_err)
+    }
+
+    async fn write(&mut self, _address: u8, bytes: &[u8]) -> Result<(), Self::Error> {
+        Self::write(self, bytes, BLOCK).await.map_err(to_i2c_err)
+    }
+
+    async fn write_read(
+        &mut self,
+        _address: u8,
+        bytes: &[u8],
+        buffer: &mut [u8],
+    ) -> Result<(), Self::Error> {
+        Self::write_read(self, bytes, buffer, BLOCK)
+            .await
+            .map_err(to_i2c_err)
+    }
+
+    async fn transaction(
+        &mut self,
+        _address: u8,
+        _operations: &mut [Operation<'_>],
+    ) -> Result<(), Self::Error> {
+        unimplemented!("transactional not implemented")
+    }
+}
+
+impl<'d, T> Drop for AsyncI2cDeviceDriver<'d, T>
+where
+    T: Borrow<AsyncI2cDriver<'d>>,
+{
+    fn drop(&mut self) {
+        loop {
+            if let Ok(_lock_guard) = self.driver.borrow().bus_lock.try_lock() {
+                esp!(unsafe { i2c_master_bus_rm_device(self.handle) }).unwrap();
+                break;
+            }
+        }
+    }
+}
+
+#[cfg(not(esp_idf_i2c_isr_iram_safe))]
+pub struct OwnedAsyncI2cDeviceDriver<'d> {
+    driver: Option<AsyncI2cDriver<'d>>,
+    handle: i2c_master_dev_handle_t,
+    _p: PhantomData<&'d mut ()>,
+}
+
+#[cfg(not(esp_idf_i2c_isr_iram_safe))]
+impl<'d> OwnedAsyncI2cDeviceDriver<'d> {
+    pub fn new<I2C: I2c>(
+        _i2c: impl Peripheral<P = I2C> + 'd,
+        sda: impl Peripheral<P = impl InputPin + OutputPin> + 'd,
+        scl: impl Peripheral<P = impl InputPin + OutputPin> + 'd,
+        bus_config: &config::Config,
+        device_config: &config::DeviceConfig,
+    ) -> Result<Self, EspError> {
+        let driver = AsyncI2cDriver::new(_i2c, sda, scl, bus_config)?;
+        Self::wrap(driver, device_config)
+    }
+
+    pub fn wrap(
+        driver: AsyncI2cDriver<'d>,
+        device_config: &config::DeviceConfig,
+    ) -> Result<Self, EspError> {
+        let handle = init_device(driver.bus_handle(), device_config)?;
+
+        enable_master_dev_isr_callback(handle, driver.port())?;
+
+        Ok(Self {
+            driver: Some(driver),
+            handle,
+            _p: PhantomData,
+        })
+    }
+
+    pub fn release(mut self) -> Result<AsyncI2cDriver<'d>, EspError> {
+        let driver = self.driver.take().unwrap();
+        drop(self);
+        Ok(driver)
+    }
+}
+
+#[cfg(not(esp_idf_i2c_isr_iram_safe))]
+impl<'d> OwnedAsyncI2cDeviceDriver<'d> {
+    pub async fn read(&mut self, buffer: &mut [u8], timeout: TickType_t) -> Result<(), EspError> {
+        esp!(unsafe {
+            i2c_master_receive(
+                self.handle,
+                buffer.as_mut_ptr().cast(),
+                buffer.len(),
+                timeout as i32,
+            )
+        })?;
+
+        let port = self.driver.as_ref().unwrap().port() as usize;
+        NOTIFIER[port].wait().await;
+        Ok(())
+    }
+
+    pub async fn write(&mut self, bytes: &[u8], timeout: TickType_t) -> Result<(), EspError> {
+        esp!(unsafe {
+            i2c_master_transmit(
+                self.handle,
+                bytes.as_ptr().cast(),
+                bytes.len(),
+                timeout as i32,
+            )
+        })?;
+
+        let port = self.driver.as_ref().unwrap().port() as usize;
+        NOTIFIER[port].wait().await;
+        Ok(())
+    }
+
+    pub async fn write_read(
+        &mut self,
+        bytes: &[u8],
+        buffer: &mut [u8],
+        timeout: TickType_t,
+    ) -> Result<(), EspError> {
+        esp!(unsafe {
+            i2c_master_transmit_receive(
+                self.handle,
+                bytes.as_ptr().cast(),
+                bytes.len(),
+                buffer.as_mut_ptr().cast(),
+                buffer.len(),
+                timeout as i32,
+            )
+        })?;
+
+        let port = self.driver.as_ref().unwrap().port() as usize;
+        NOTIFIER[port].wait().await;
+        Ok(())
+    }
+}
+
+#[cfg(not(esp_idf_i2c_isr_iram_safe))]
+unsafe impl<'d> Send for OwnedAsyncI2cDeviceDriver<'d> {}
+
+#[cfg(not(esp_idf_i2c_isr_iram_safe))]
+impl<'d> Drop for OwnedAsyncI2cDeviceDriver<'d> {
+    fn drop(&mut self) {
+        disable_master_dev_isr_callback(self.handle).unwrap();
+        esp!(unsafe { i2c_master_bus_rm_device(self.handle) }).unwrap();
+    }
+}
+
+#[cfg(not(esp_idf_i2c_isr_iram_safe))]
+impl<'d> embedded_hal::i2c::ErrorType for OwnedAsyncI2cDeviceDriver<'d> {
+    type Error = I2cError;
+}
+
+#[cfg(not(esp_idf_i2c_isr_iram_safe))]
+impl<'d> embedded_hal_async::i2c::I2c<embedded_hal::i2c::SevenBitAddress>
+    for OwnedAsyncI2cDeviceDriver<'d>
+{
+    async fn read(&mut self, _address: u8, buffer: &mut [u8]) -> Result<(), Self::Error> {
+        Self::read(self, buffer, BLOCK).await.map_err(to_i2c_err)
+    }
+
+    async fn write(&mut self, _address: u8, bytes: &[u8]) -> Result<(), Self::Error> {
+        Self::write(self, bytes, BLOCK).await.map_err(to_i2c_err)
+    }
+
+    async fn write_read(
+        &mut self,
+        _address: u8,
+        bytes: &[u8],
+        buffer: &mut [u8],
+    ) -> Result<(), Self::Error> {
+        Self::write_read(self, bytes, buffer, BLOCK)
+            .await
+            .map_err(to_i2c_err)
+    }
+
+    async fn transaction(
+        &mut self,
+        _address: u8,
+        _operations: &mut [Operation<'_>],
+    ) -> Result<(), Self::Error> {
+        unimplemented!("transactional not implemented")
+    }
+}
+
+#[cfg(not(esp32c2))]
+pub struct I2cSlaveDriver<'d> {
+    i2c: u8,
+    handle: i2c_slave_dev_handle_t,
+    _p: PhantomData<&'d mut ()>,
+}
+
+#[cfg(not(esp32c2))]
+unsafe impl<'d> Send for I2cSlaveDriver<'d> {}
+
+#[cfg(not(esp32c2))]
+impl<'d> I2cSlaveDriver<'d> {
+    pub fn new<I2C: I2c>(
+        _i2c: impl Peripheral<P = I2C> + 'd,
+        sda: impl Peripheral<P = impl InputPin + OutputPin> + 'd,
+        scl: impl Peripheral<P = impl InputPin + OutputPin> + 'd,
+        address: config::DeviceAddress,
+        config: &config::SlaveDeviceConfig,
+    ) -> Result<Self, EspError> {
+        super::check_and_set_beta_driver();
+
+        let handle = init_slave_device(_i2c, sda, scl, address, config)?;
+
+        enable_slave_isr_callback(handle, I2C::port() as _)?;
+
+        Ok(Self {
+            i2c: I2C::port() as _,
+            handle,
+            _p: PhantomData,
+        })
+    }
+
+    pub fn read(&mut self, buffer: &mut [u8], _timeout: TickType_t) -> Result<usize, EspError> {
+        esp!(unsafe { i2c_slave_receive(self.handle, buffer.as_mut_ptr(), buffer.len()) })?;
+
+        todo!("How to block?");
+    }
+
+    pub async fn async_read(&mut self, buffer: &mut [u8]) -> Result<(), EspError> {
+        esp!(unsafe { i2c_slave_receive(self.handle, buffer.as_mut_ptr(), buffer.len()) })?;
+
+        NOTIFIER[self.port() as usize].wait().await;
+        Ok(())
+    }
+
+    pub fn write(&mut self, bytes: &[u8], timeout: TickType_t) -> Result<(), EspError> {
+        esp!(unsafe {
+            i2c_slave_transmit(
+                self.handle,
+                bytes.as_ptr(),
+                bytes.len() as i32,
+                timeout as i32,
+            )
+        })
+    }
+
+    pub fn port(&self) -> i2c_port_t {
+        self.i2c as _
+    }
+}
+
+#[cfg(not(esp32c2))]
+impl<'d> Drop for I2cSlaveDriver<'d> {
+    fn drop(&mut self) {
+        disable_slave_isr_callback(self.handle).unwrap();
+        esp!(unsafe { i2c_del_slave_device(self.handle) }).unwrap();
+    }
+}
+
+fn init_master_bus<'d, I2C: I2c>(
+    _i2c: impl Peripheral<P = I2C> + 'd,
+    sda: impl Peripheral<P = impl InputPin + OutputPin> + 'd,
+    scl: impl Peripheral<P = impl InputPin + OutputPin> + 'd,
+    config: &config::Config,
+    max_device_count: usize,
+) -> Result<i2c_master_bus_handle_t, EspError> {
+    crate::into_ref!(sda, scl);
+
+    let config = i2c_master_bus_config_t {
+        sda_io_num: sda.pin(),
+        scl_io_num: scl.pin(),
+        clk_source: config.source_clock.into(),
+        flags: {
+            let mut flags = i2c_master_bus_config_t__bindgen_ty_1::default();
+            flags.set_enable_internal_pullup(config.pullup_enabled as _);
+            flags
+        },
+        glitch_ignore_cnt: config.glitch_ignore_cnt,
+        i2c_port: I2C::port() as i32,
+        intr_priority: 0,
+        trans_queue_depth: max_device_count,
+    };
+
+    let mut handle: i2c_master_bus_handle_t = ptr::null_mut();
+
+    esp!(unsafe { i2c_new_master_bus(&config, &mut handle as _) })?;
+
+    Ok(handle)
+}
+
+fn init_device(
+    bus_handle: i2c_master_bus_handle_t,
+    config: &config::DeviceConfig,
+) -> Result<i2c_master_dev_handle_t, EspError> {
+    // i2c_config_t documentation says that clock speed must be no higher than 1 MHz
+    if config.baudrate > 1.MHz().into() {
+        return Err(EspError::from_infallible::<ESP_ERR_INVALID_ARG>());
+    }
+
+    let config = i2c_device_config_t {
+        device_address: config.address.address(),
+        dev_addr_length: config.address.clone().into(),
+        scl_speed_hz: config.baudrate.into(),
+    };
+
+    let mut handle: i2c_master_dev_handle_t = ptr::null_mut();
+
+    esp!(unsafe { i2c_master_bus_add_device(bus_handle, &config, &mut handle as _) })?;
+
+    Ok(handle)
+}
+
+#[cfg(not(esp32c2))]
+fn init_slave_device<'d, I2C: I2c>(
+    _i2c: impl Peripheral<P = I2C> + 'd,
+    sda: impl Peripheral<P = impl InputPin + OutputPin> + 'd,
+    scl: impl Peripheral<P = impl InputPin + OutputPin> + 'd,
+    address: config::DeviceAddress,
+    config: &config::SlaveDeviceConfig,
+) -> Result<i2c_slave_dev_handle_t, EspError> {
+    crate::into_ref!(sda, scl);
+
+    let config = i2c_slave_config_t {
+        sda_io_num: sda.pin(),
+        scl_io_num: scl.pin(),
+        clk_source: config.source_clock.into(),
+        flags: {
+            let mut flags = i2c_slave_config_t__bindgen_ty_1::default();
+            flags.set_stretch_en(0);
+            flags.set_broadcast_en(config.broadcast_enable as _);
+            flags
+        },
+        i2c_port: I2C::port() as i32,
+        intr_priority: 0,
+        slave_addr: address.address(),
+        addr_bit_len: address.into(),
+        send_buf_depth: config.send_buffer_depth,
+    };
+
+    let mut handle: i2c_slave_dev_handle_t = ptr::null_mut();
+
+    esp!(unsafe { i2c_new_slave_device(&config, &mut handle as _) })?;
+
+    Ok(handle)
+}
+
+fn to_i2c_err(err: EspError) -> I2cError {
+    if err.code() == ESP_FAIL {
+        I2cError::new(ErrorKind::NoAcknowledge(NoAcknowledgeSource::Unknown), err)
+    } else {
+        I2cError::other(err)
+    }
+}
+
+#[cfg(not(esp_idf_i2c_isr_iram_safe))]
+fn enable_master_dev_isr_callback(
+    handle: i2c_master_dev_handle_t,
+    host: u8,
+) -> Result<(), EspError> {
+    esp!(unsafe {
+        i2c_master_register_event_callbacks(
+            handle,
+            &i2c_master_event_callbacks_t {
+                on_trans_done: Some(master_isr),
+            },
+            &NOTIFIER[host as usize] as *const _ as *mut _,
+        )
+    })
+}
+
+#[cfg(not(esp_idf_i2c_isr_iram_safe))]
+fn disable_master_dev_isr_callback(handle: i2c_master_dev_handle_t) -> Result<(), EspError> {
+    esp!(unsafe {
+        i2c_master_register_event_callbacks(
+            handle,
+            &i2c_master_event_callbacks_t::default(),
+            ptr::null_mut(),
+        )
+    })
+}
+
+#[cfg(not(esp_idf_i2c_isr_iram_safe))]
+extern "C" fn master_isr(
+    _handle: i2c_master_dev_handle_t,
+    _data: *const i2c_master_event_data_t,
+    user_data: *mut c_void,
+) -> bool {
+    let notifier: &HalIsrNotification =
+        unsafe { (user_data as *const HalIsrNotification).as_ref() }.unwrap();
+
+    notifier.notify_lsb()
+}
+
+#[cfg(all(not(esp32c2), not(esp_idf_i2c_isr_iram_safe)))]
+fn enable_slave_isr_callback(handle: i2c_slave_dev_handle_t, host: u8) -> Result<(), EspError> {
+    esp!(unsafe {
+        i2c_slave_register_event_callbacks(
+            handle,
+            &i2c_slave_event_callbacks_t {
+                on_recv_done: Some(slave_isr),
+                on_stretch_occur: None,
+            },
+            &NOTIFIER[host as usize] as *const _ as *mut _,
+        )
+    })
+}
+
+#[cfg(all(not(esp32c2), not(esp_idf_i2c_isr_iram_safe)))]
+fn disable_slave_isr_callback(handle: i2c_slave_dev_handle_t) -> Result<(), EspError> {
+    esp!(unsafe {
+        i2c_slave_register_event_callbacks(
+            handle,
+            &i2c_slave_event_callbacks_t::default(),
+            ptr::null_mut(),
+        )
+    })
+}
+
+#[cfg(all(not(esp32c2), not(esp_idf_i2c_isr_iram_safe)))]
+extern "C" fn slave_isr(
+    _handle: i2c_slave_dev_handle_t,
+    _data: *const i2c_slave_rx_done_event_data_t,
+    user_data: *mut c_void,
+) -> bool {
+    let notifier: &HalIsrNotification =
+        unsafe { (user_data as *const HalIsrNotification).as_ref() }.unwrap();
+
+    notifier.notify_lsb()
+}
+
+#[cfg(any(esp32c3, esp32c2, esp32c6))]
+static NOTIFIER: [HalIsrNotification; 1] = [HalIsrNotification::new()];
+
+#[cfg(not(any(esp32c3, esp32c2, esp32c6)))]
+static NOTIFIER: [HalIsrNotification; 2] = [HalIsrNotification::new(), HalIsrNotification::new()];

From af2204f5c52ed55a9f50741f337a40abbfc68202 Mon Sep 17 00:00:00 2001
From: Alexander Huebener <alex.teamplayer@gmail.com>
Date: Tue, 24 Dec 2024 13:23:38 +0100
Subject: [PATCH 10/11] some cleanup

---
 src/i2c.rs | 42 +++++++++++++++++++-----------------------
 1 file changed, 19 insertions(+), 23 deletions(-)

diff --git a/src/i2c.rs b/src/i2c.rs
index 6b195eee92a..3a7037b28bb 100644
--- a/src/i2c.rs
+++ b/src/i2c.rs
@@ -1,32 +1,43 @@
 #[cfg(all(not(esp_idf_version_major = "4"), not(esp_idf_version = "5.1")))]
-mod modern;
+pub mod modern;
 
-pub use legacy::*;
+use core::marker::PhantomData;
+use core::time::Duration;
+
+use embedded_hal::i2c::{ErrorKind, NoAcknowledgeSource};
 
 use esp_idf_sys::*;
 
+use crate::delay::*;
+use crate::gpio::*;
+use crate::interrupt::InterruptType;
+use crate::peripheral::Peripheral;
+use crate::units::*;
+
+pub use embedded_hal::i2c::Operation;
+
 #[cfg(all(not(esp_idf_version_major = "4"), not(esp_idf_version = "5.1")))]
 #[repr(u8)]
 enum UsedDriver {
     None = 0,
     Legacy = 1,
-    Beta = 2,
+    Modern = 2,
 }
 
 #[cfg(all(not(esp_idf_version_major = "4"), not(esp_idf_version = "5.1")))]
-// 0 -> no driver, 1 -> legacy driver, 2 -> beta driver
+// 0 -> no driver, 1 -> legacy driver, 2 -> modern driver
 static DRIVER_IN_USE: core::sync::atomic::AtomicU8 =
     core::sync::atomic::AtomicU8::new(UsedDriver::None as u8);
 
 #[cfg(all(not(esp_idf_version_major = "4"), not(esp_idf_version = "5.1")))]
-fn check_and_set_beta_driver() {
+fn check_and_set_modern_driver() {
     match DRIVER_IN_USE.compare_exchange(
         UsedDriver::None as u8,
-        UsedDriver::Beta as u8,
+        UsedDriver::Modern as u8,
         core::sync::atomic::Ordering::Relaxed,
         core::sync::atomic::Ordering::Relaxed,
     ) {
-        Err(e) if e == UsedDriver::Legacy as u8 => panic!("Legacy I2C driver is already in use. Either legacy driver or beta driver can be used at a time."),
+        Err(e) if e == UsedDriver::Legacy as u8 => panic!("Legacy I2C driver is already in use. Either legacy driver or modern driver can be used at a time."),
         _ => ()
     }
 }
@@ -38,26 +49,11 @@ fn check_and_set_legacy_driver() {
             core::sync::atomic::Ordering::Relaxed,
             core::sync::atomic::Ordering::Relaxed,
     ) {
-            Err(e) if e == UsedDriver::Beta as u8 => panic!("Beta I2C driver is already in use. Either legacy driver or beta driver can be used at a time."),
+            Err(e) if e == UsedDriver::Modern as u8 => panic!("Modern I2C driver is already in use. Either legacy driver or modern driver can be used at a time."),
             _ => ()
     }
 }
 
-use core::marker::PhantomData;
-use core::time::Duration;
-
-use embedded_hal::i2c::{ErrorKind, NoAcknowledgeSource};
-
-use esp_idf_sys::*;
-
-use crate::delay::*;
-use crate::gpio::*;
-use crate::interrupt::InterruptType;
-use crate::peripheral::Peripheral;
-use crate::units::*;
-
-pub use embedded_hal::i2c::Operation;
-
 crate::embedded_hal_error!(
     I2cError,
     embedded_hal::i2c::Error,

From 33d40479da2b01b81f402788bca2abe4c8bda102 Mon Sep 17 00:00:00 2001
From: Alexander Huebener <alex.teamplayer@gmail.com>
Date: Tue, 24 Dec 2024 13:24:01 +0100
Subject: [PATCH 11/11] refactoring

---
 src/i2c/modern.rs | 921 +++++++++++++++++++++++-----------------------
 1 file changed, 470 insertions(+), 451 deletions(-)

diff --git a/src/i2c/modern.rs b/src/i2c/modern.rs
index a27db1a70e6..4aeccf8dadf 100644
--- a/src/i2c/modern.rs
+++ b/src/i2c/modern.rs
@@ -2,9 +2,9 @@ use core::borrow::Borrow;
 use core::ffi::c_void;
 use core::marker::PhantomData;
 use core::ptr;
-
-use embassy_sync::mutex::Mutex;
-use embassy_sync::mutex::MutexGuard;
+use core::sync::atomic::AtomicBool;
+use core::time::Duration;
+use std::sync::Condvar;
 
 use embedded_hal::i2c::{ErrorKind, NoAcknowledgeSource};
 
@@ -14,7 +14,6 @@ use crate::delay::*;
 use crate::gpio::*;
 use crate::interrupt::asynch::HalIsrNotification;
 use crate::peripheral::Peripheral;
-use crate::task::embassy_sync::EspRawMutex;
 use crate::units::*;
 
 pub use embedded_hal::i2c::Operation;
@@ -27,20 +26,6 @@ crate::embedded_hal_error!(
     embedded_hal::i2c::ErrorKind
 );
 
-macro_rules! on_err {
-    ($d:expr, $oe:expr) => {
-        {
-            match $d {
-                Err(e) => {
-                    $oe
-                    Err(e)
-                }
-                v => v
-            }
-        }
-    };
-}
-
 pub type I2cConfig = config::Config;
 #[cfg(not(esp32c2))]
 pub type I2cSlaveConfig = config::SlaveDeviceConfig;
@@ -232,6 +217,62 @@ pub mod config {
     }
 }
 
+#[cfg(any(esp32c3, esp32c2, esp32c6))]
+static ASYNC_DEVICE_IN_USE: [AtomicBool; 1] = [AtomicBool::new(false)];
+
+#[cfg(not(any(esp32c3, esp32c2, esp32c6)))]
+static ASYNC_DEVICE_IN_USE: [AtomicBool; 2] = [AtomicBool::new(false), AtomicBool::new(false)];
+
+mod private {
+    pub trait Sealed {}
+}
+
+// Should allow to construct a `I2cDeviceDriver` from a `I2cDriver` or `AsyncI2cDriver`.
+pub trait BorrowI2cDriver<'d>: private::Sealed {
+    fn borrowed_bus_handle(&self) -> i2c_master_bus_handle_t;
+    fn borrowed_port(&self) -> u8;
+}
+
+macro_rules! impl_borrow_trait {
+    ($( $accessor:ty ),*) => {
+        $(
+            impl<'d> BorrowI2cDriver<'d> for $accessor {
+                fn borrowed_bus_handle(&self) -> i2c_master_bus_handle_t {
+                    self.bus_handle()
+                }
+
+                fn borrowed_port(&self) -> u8 {
+                    self.port()
+                }
+            }
+
+            impl<'d> private::Sealed for $accessor {}
+        )*
+    };
+}
+
+impl_borrow_trait!(
+    I2cDriver<'d>,
+    &I2cDriver<'d>,
+    &mut I2cDriver<'d>,
+    std::rc::Rc<I2cDriver<'d>>,
+    std::sync::Arc<I2cDriver<'d>>,
+    std::boxed::Box<I2cDriver<'d>>,
+    AsyncI2cDriver<'d>,
+    &AsyncI2cDriver<'d>,
+    &mut AsyncI2cDriver<'d>,
+    std::rc::Rc<AsyncI2cDriver<'d>>,
+    std::sync::Arc<AsyncI2cDriver<'d>>,
+    std::boxed::Box<AsyncI2cDriver<'d>>
+);
+
+/// I2C Driver controlling the I2C bus.
+///
+/// # Example
+/// ```no_run
+/// let driver_config = config::Config::new();
+/// let driver = I2cDriver::new(..., &device_config).unwrap();
+/// ```
 pub struct I2cDriver<'d> {
     port: u8,
     handle: i2c_master_bus_handle_t,
@@ -245,11 +286,11 @@ impl<'d> I2cDriver<'d> {
         scl: impl Peripheral<P = impl InputPin + OutputPin> + 'd,
         config: &config::Config,
     ) -> Result<Self, EspError> {
-        super::check_and_set_beta_driver();
+        super::check_and_set_modern_driver();
 
         let handle = init_master_bus(_i2c, sda, scl, config, 0)?;
 
-        Ok(I2cDriver {
+        Ok(Self {
             port: I2C::port() as u8,
             handle,
             _p: PhantomData,
@@ -263,115 +304,212 @@ impl<'d> I2cDriver<'d> {
     fn bus_handle(&self) -> i2c_master_bus_handle_t {
         self.handle
     }
+}
 
-    /// Probe device on the bus.
-    pub fn probe_device(
-        &mut self,
-        address: config::DeviceAddress,
-        timeout: TickType_t,
-    ) -> Result<(), EspError> {
-        esp!(unsafe { i2c_master_probe(self.handle, address.address(), timeout as i32) })
-    }
+unsafe impl<'d> Send for I2cDriver<'d> {}
 
-    pub fn device(
-        &mut self,
-        config: &config::DeviceConfig,
-    ) -> Result<I2cDeviceDriver<'d, &mut I2cDriver<'d>>, EspError> {
-        I2cDeviceDriver::new(self, config)
+impl<'d> Drop for I2cDriver<'d> {
+    fn drop(&mut self) {
+        esp!(unsafe { i2c_del_master_bus(self.handle) }).unwrap();
     }
+}
+
+/// The `async` version of the I2C driver.
+///
+/// This driver allows to have one `AsyncI2cDeviceDriver` per bus with `async` support.
+///
+/// # Important
+/// I2C master asynchronous transaction is still an experimental feature. (The issue is when
+/// asynchronous transaction is very large, it will cause memory problem.)
+pub struct AsyncI2cDriver<'d> {
+    port: u8,
+    handle: i2c_master_bus_handle_t,
+    _p: PhantomData<&'d mut ()>,
+}
+
+impl<'d> AsyncI2cDriver<'d> {
+    /// Creates a new i2c driver with `async` support.
+    pub fn new<I2C: I2c>(
+        _i2c: impl Peripheral<P = I2C> + 'd,
+        sda: impl Peripheral<P = impl InputPin + OutputPin> + 'd,
+        scl: impl Peripheral<P = impl InputPin + OutputPin> + 'd,
+        config: &config::Config,
+    ) -> Result<Self, EspError> {
+        super::check_and_set_modern_driver();
 
-    // Helper to use the embedded_hal traits.
-    fn read(&mut self, addr: u8, buffer: &mut [u8], timeout: TickType_t) -> Result<(), EspError> {
-        self.device(&config::DeviceConfig::new(config::DeviceAddress::SevenBit(
-            addr,
-        )))?
-        .read(buffer, timeout)
+        let handle = init_master_bus(_i2c, sda, scl, config, 1)?;
+
+        Ok(Self {
+            port: I2C::port() as u8,
+            handle,
+            _p: PhantomData,
+        })
     }
 
-    // Helper to use the embedded_hal traits.
-    fn write(&mut self, addr: u8, bytes: &[u8], timeout: TickType_t) -> Result<(), EspError> {
-        self.device(&config::DeviceConfig::new(config::DeviceAddress::SevenBit(
-            addr,
-        )))?
-        .write(bytes, timeout)
+    pub fn port(&self) -> u8 {
+        self.port
     }
 
-    // Helper to use the embedded_hal traits.
-    fn write_read(
-        &mut self,
-        addr: u8,
-        bytes: &[u8],
-        buffer: &mut [u8],
-        timeout: TickType_t,
-    ) -> Result<(), EspError> {
-        self.device(&config::DeviceConfig::new(config::DeviceAddress::SevenBit(
-            addr,
-        )))?
-        .write_read(bytes, buffer, timeout)
+    fn bus_handle(&self) -> i2c_master_bus_handle_t {
+        self.handle
     }
 }
 
-unsafe impl<'d> Send for I2cDriver<'d> {}
+unsafe impl<'d> Send for AsyncI2cDriver<'d> {}
 
-impl<'d> Drop for I2cDriver<'d> {
+impl<'d> Drop for AsyncI2cDriver<'d> {
     fn drop(&mut self) {
         esp!(unsafe { i2c_del_master_bus(self.handle) }).unwrap();
     }
 }
 
-impl<'d> embedded_hal_0_2::blocking::i2c::Read for I2cDriver<'d> {
-    type Error = I2cError;
+macro_rules! impl_driver_blocking_functions {
+    ($driver:ident) => {
+        impl<'d> $driver<'d> {
+            /// Probe device on the bus.
+            pub fn probe_device(
+                &mut self,
+                address: config::DeviceAddress,
+                timeout: TickType_t,
+            ) -> Result<(), EspError> {
+                esp!(unsafe {
+                    i2c_master_probe(self.bus_handle(), address.address(), timeout as i32)
+                })
+            }
 
-    fn read(&mut self, addr: u8, buffer: &mut [u8]) -> Result<(), Self::Error> {
-        Self::read(self, addr, buffer, BLOCK).map_err(to_i2c_err)
-    }
-}
+            // Helper to use the embedded_hal traits.
+            fn read(
+                &self,
+                addr: u8,
+                buffer: &mut [u8],
+                timeout: TickType_t,
+            ) -> Result<(), EspError> {
+                let device = init_device(
+                    self.bus_handle(),
+                    &config::DeviceConfig::new(config::DeviceAddress::SevenBit(addr)),
+                )?;
+
+                esp!(unsafe {
+                    i2c_master_receive(
+                        device,
+                        buffer.as_mut_ptr().cast(),
+                        buffer.len(),
+                        timeout as i32,
+                    )
+                })
+            }
 
-impl<'d> embedded_hal_0_2::blocking::i2c::Write for I2cDriver<'d> {
-    type Error = I2cError;
+            // Helper to use the embedded_hal traits.
+            fn write(
+                &mut self,
+                addr: u8,
+                bytes: &[u8],
+                timeout: TickType_t,
+            ) -> Result<(), EspError> {
+                let device = init_device(
+                    self.bus_handle(),
+                    &config::DeviceConfig::new(config::DeviceAddress::SevenBit(addr)),
+                )?;
+
+                esp!(unsafe {
+                    i2c_master_transmit(device, bytes.as_ptr().cast(), bytes.len(), timeout as i32)
+                })
+            }
 
-    fn write(&mut self, addr: u8, bytes: &[u8]) -> Result<(), Self::Error> {
-        Self::write(self, addr, bytes, BLOCK).map_err(to_i2c_err)
-    }
-}
+            // Helper to use the embedded_hal traits.
+            fn write_read(
+                &mut self,
+                addr: u8,
+                bytes: &[u8],
+                buffer: &mut [u8],
+                timeout: TickType_t,
+            ) -> Result<(), EspError> {
+                let device = init_device(
+                    self.bus_handle(),
+                    &config::DeviceConfig::new(config::DeviceAddress::SevenBit(addr)),
+                )?;
+
+                esp!(unsafe {
+                    i2c_master_transmit_receive(
+                        device,
+                        bytes.as_ptr().cast(),
+                        bytes.len(),
+                        buffer.as_mut_ptr().cast(),
+                        buffer.len(),
+                        timeout as i32,
+                    )
+                })
+            }
+        }
 
-impl<'d> embedded_hal_0_2::blocking::i2c::WriteRead for I2cDriver<'d> {
-    type Error = I2cError;
+        impl<'d> embedded_hal_0_2::blocking::i2c::Read for $driver<'d> {
+            type Error = I2cError;
 
-    fn write_read(&mut self, addr: u8, bytes: &[u8], buffer: &mut [u8]) -> Result<(), Self::Error> {
-        Self::write_read(self, addr, bytes, buffer, BLOCK).map_err(to_i2c_err)
-    }
-}
+            fn read(&mut self, addr: u8, buffer: &mut [u8]) -> Result<(), Self::Error> {
+                Self::read(self, addr, buffer, BLOCK).map_err(to_i2c_err)
+            }
+        }
 
-impl<'d> embedded_hal::i2c::ErrorType for I2cDriver<'d> {
-    type Error = I2cError;
-}
+        impl<'d> embedded_hal_0_2::blocking::i2c::Write for $driver<'d> {
+            type Error = I2cError;
 
-impl<'d> embedded_hal::i2c::I2c<embedded_hal::i2c::SevenBitAddress> for I2cDriver<'d> {
-    fn read(&mut self, addr: u8, buffer: &mut [u8]) -> Result<(), Self::Error> {
-        Self::read(self, addr, buffer, BLOCK).map_err(to_i2c_err)
-    }
+            fn write(&mut self, addr: u8, bytes: &[u8]) -> Result<(), Self::Error> {
+                Self::write(self, addr, bytes, BLOCK).map_err(to_i2c_err)
+            }
+        }
 
-    fn write(&mut self, addr: u8, bytes: &[u8]) -> Result<(), Self::Error> {
-        Self::write(self, addr, bytes, BLOCK).map_err(to_i2c_err)
-    }
+        impl<'d> embedded_hal_0_2::blocking::i2c::WriteRead for $driver<'d> {
+            type Error = I2cError;
 
-    fn write_read(&mut self, addr: u8, bytes: &[u8], buffer: &mut [u8]) -> Result<(), Self::Error> {
-        Self::write_read(self, addr, bytes, buffer, BLOCK).map_err(to_i2c_err)
-    }
+            fn write_read(
+                &mut self,
+                addr: u8,
+                bytes: &[u8],
+                buffer: &mut [u8],
+            ) -> Result<(), Self::Error> {
+                Self::write_read(self, addr, bytes, buffer, BLOCK).map_err(to_i2c_err)
+            }
+        }
 
-    fn transaction(
-        &mut self,
-        _addr: u8,
-        _operations: &mut [embedded_hal::i2c::Operation<'_>],
-    ) -> Result<(), Self::Error> {
-        unimplemented!("transactional not implemented")
-    }
+        impl<'d> embedded_hal::i2c::ErrorType for $driver<'d> {
+            type Error = I2cError;
+        }
+
+        impl<'d> embedded_hal::i2c::I2c<embedded_hal::i2c::SevenBitAddress> for $driver<'d> {
+            fn read(&mut self, addr: u8, buffer: &mut [u8]) -> Result<(), Self::Error> {
+                Self::read(self, addr, buffer, BLOCK).map_err(to_i2c_err)
+            }
+
+            fn write(&mut self, addr: u8, bytes: &[u8]) -> Result<(), Self::Error> {
+                Self::write(self, addr, bytes, BLOCK).map_err(to_i2c_err)
+            }
+
+            fn write_read(
+                &mut self,
+                addr: u8,
+                bytes: &[u8],
+                buffer: &mut [u8],
+            ) -> Result<(), Self::Error> {
+                Self::write_read(self, addr, bytes, buffer, BLOCK).map_err(to_i2c_err)
+            }
+
+            fn transaction(
+                &mut self,
+                _addr: u8,
+                _operations: &mut [embedded_hal::i2c::Operation<'_>],
+            ) -> Result<(), Self::Error> {
+                unimplemented!("transactional not implemented")
+            }
+        }
+    };
 }
 
+impl_driver_blocking_functions!(I2cDriver);
+impl_driver_blocking_functions!(AsyncI2cDriver);
+
 pub struct I2cDeviceDriver<'d, T>
 where
-    T: Borrow<I2cDriver<'d>>,
+    T: BorrowI2cDriver<'d> + 'd,
 {
     _driver: T,
     handle: i2c_master_dev_handle_t,
@@ -380,10 +518,10 @@ where
 
 impl<'d, T> I2cDeviceDriver<'d, T>
 where
-    T: Borrow<I2cDriver<'d>>,
+    T: BorrowI2cDriver<'d> + 'd,
 {
     pub fn new(driver: T, config: &config::DeviceConfig) -> Result<Self, EspError> {
-        let handle = init_device(driver.borrow().bus_handle(), &config)?;
+        let handle = init_device(driver.borrowed_bus_handle(), &config)?;
 
         Ok(I2cDeviceDriver {
             _driver: driver,
@@ -392,10 +530,19 @@ where
         })
     }
 
+    fn device_handle(&self) -> i2c_master_dev_handle_t {
+        self.handle
+    }
+}
+
+impl<'d, T> I2cDeviceDriver<'d, T>
+where
+    T: BorrowI2cDriver<'d> + 'd,
+{
     pub fn read(&mut self, buffer: &mut [u8], timeout: TickType_t) -> Result<(), EspError> {
         esp!(unsafe {
             i2c_master_receive(
-                self.handle,
+                self.device_handle(),
                 buffer.as_mut_ptr().cast(),
                 buffer.len(),
                 timeout as i32,
@@ -406,7 +553,7 @@ where
     pub fn write(&mut self, bytes: &[u8], timeout: TickType_t) -> Result<(), EspError> {
         esp!(unsafe {
             i2c_master_transmit(
-                self.handle,
+                self.device_handle(),
                 bytes.as_ptr().cast(),
                 bytes.len(),
                 timeout as i32,
@@ -422,7 +569,7 @@ where
     ) -> Result<(), EspError> {
         esp!(unsafe {
             i2c_master_transmit_receive(
-                self.handle,
+                self.device_handle(),
                 bytes.as_ptr().cast(),
                 bytes.len(),
                 buffer.as_mut_ptr().cast(),
@@ -435,7 +582,7 @@ where
 
 impl<'d, T> Drop for I2cDeviceDriver<'d, T>
 where
-    T: Borrow<I2cDriver<'d>>,
+    T: BorrowI2cDriver<'d>,
 {
     fn drop(&mut self) {
         esp!(unsafe { i2c_master_bus_rm_device(self.handle) }).unwrap();
@@ -444,29 +591,29 @@ where
 
 impl<'d, T> embedded_hal_0_2::blocking::i2c::Read for I2cDeviceDriver<'d, T>
 where
-    T: Borrow<I2cDriver<'d>>,
+    T: BorrowI2cDriver<'d> + 'd,
 {
     type Error = I2cError;
 
     fn read(&mut self, _addr: u8, buffer: &mut [u8]) -> Result<(), Self::Error> {
-        I2cDeviceDriver::read(self, buffer, BLOCK).map_err(to_i2c_err)
+        Self::read(self, buffer, BLOCK).map_err(to_i2c_err)
     }
 }
 
 impl<'d, T> embedded_hal_0_2::blocking::i2c::Write for I2cDeviceDriver<'d, T>
 where
-    T: Borrow<I2cDriver<'d>>,
+    T: BorrowI2cDriver<'d> + 'd,
 {
     type Error = I2cError;
 
     fn write(&mut self, _addr: u8, bytes: &[u8]) -> Result<(), Self::Error> {
-        I2cDeviceDriver::write(self, bytes, BLOCK).map_err(to_i2c_err)
+        Self::write(self, bytes, BLOCK).map_err(to_i2c_err)
     }
 }
 
 impl<'d, T> embedded_hal_0_2::blocking::i2c::WriteRead for I2cDeviceDriver<'d, T>
 where
-    T: Borrow<I2cDriver<'d>>,
+    T: BorrowI2cDriver<'d> + 'd,
 {
     type Error = I2cError;
 
@@ -476,27 +623,27 @@ where
         bytes: &[u8],
         buffer: &mut [u8],
     ) -> Result<(), Self::Error> {
-        I2cDeviceDriver::write_read(self, bytes, buffer, BLOCK).map_err(to_i2c_err)
+        Self::write_read(self, bytes, buffer, BLOCK).map_err(to_i2c_err)
     }
 }
 
 impl<'d, T> embedded_hal::i2c::ErrorType for I2cDeviceDriver<'d, T>
 where
-    T: Borrow<I2cDriver<'d>>,
+    T: BorrowI2cDriver<'d> + 'd,
 {
     type Error = I2cError;
 }
 
 impl<'d, T> embedded_hal::i2c::I2c<embedded_hal::i2c::SevenBitAddress> for I2cDeviceDriver<'d, T>
 where
-    T: Borrow<I2cDriver<'d>>,
+    T: BorrowI2cDriver<'d> + 'd,
 {
     fn read(&mut self, _addr: u8, buffer: &mut [u8]) -> Result<(), Self::Error> {
-        I2cDeviceDriver::read(self, buffer, BLOCK).map_err(to_i2c_err)
+        Self::read(self, buffer, BLOCK).map_err(to_i2c_err)
     }
 
     fn write(&mut self, _addr: u8, bytes: &[u8]) -> Result<(), Self::Error> {
-        I2cDeviceDriver::write(self, bytes, BLOCK).map_err(to_i2c_err)
+        Self::write(self, bytes, BLOCK).map_err(to_i2c_err)
     }
 
     fn write_read(
@@ -505,7 +652,7 @@ where
         bytes: &[u8],
         buffer: &mut [u8],
     ) -> Result<(), Self::Error> {
-        I2cDeviceDriver::write_read(self, bytes, buffer, BLOCK).map_err(to_i2c_err)
+        Self::write_read(self, bytes, buffer, BLOCK).map_err(to_i2c_err)
     }
 
     fn transaction(
@@ -517,217 +664,91 @@ where
     }
 }
 
-// ------------------------------------------------------------------------------------------------
-// ------------------------------------- Async ----------------------------------------------------
-// ------------------------------------------------------------------------------------------------
-
-pub struct AsyncI2cDriver<'d> {
-    bus_lock: Mutex<EspRawMutex, ()>,
-    handle: i2c_master_bus_handle_t,
-    port: u8,
-    _p: PhantomData<&'d mut ()>,
-}
-
-impl<'d> AsyncI2cDriver<'d> {
-    pub fn new<I2C: I2c>(
-        _i2c: impl Peripheral<P = I2C> + 'd,
-        sda: impl Peripheral<P = impl InputPin + OutputPin> + 'd,
-        scl: impl Peripheral<P = impl InputPin + OutputPin> + 'd,
-        config: &config::Config,
-    ) -> Result<Self, EspError> {
-        super::check_and_set_beta_driver();
-
-        let handle = init_master_bus(_i2c, sda, scl, config, 1)?;
-
-        Ok(AsyncI2cDriver {
-            bus_lock: Mutex::new(()),
-            handle,
-            port: I2C::port() as _,
-            _p: PhantomData,
-        })
-    }
-
-    pub fn port(&self) -> u8 {
-        self.port
-    }
-
-    fn bus_handle(&self) -> i2c_master_bus_handle_t {
-        self.handle
-    }
-
-    async fn acquire_bus<'a>(&'a self) -> MutexGuard<'a, EspRawMutex, ()> {
-        self.bus_lock.lock().await
-    }
-
-    pub fn device(
-        &self,
-        config: &config::DeviceConfig,
-    ) -> Result<AsyncI2cDeviceDriver<'d, &AsyncI2cDriver<'d>>, EspError> {
-        AsyncI2cDeviceDriver::new(self, config)
-    }
-
-    pub fn owned_device(
-        self,
-        config: &config::DeviceConfig,
-    ) -> Result<OwnedAsyncI2cDeviceDriver<'d>, EspError> {
-        OwnedAsyncI2cDeviceDriver::wrap(self, config)
-    }
-
-    async fn read(&mut self, address: u8, buffer: &mut [u8]) -> Result<(), EspError> {
-        self.device(&config::DeviceConfig::new(config::DeviceAddress::SevenBit(
-            address,
-        )))?
-        .read(buffer, BLOCK)
-        .await
-    }
-
-    async fn write(&mut self, address: u8, bytes: &[u8]) -> Result<(), EspError> {
-        self.device(&config::DeviceConfig::new(config::DeviceAddress::SevenBit(
-            address,
-        )))?
-        .write(bytes, BLOCK)
-        .await
-    }
-
-    async fn write_read(
-        &mut self,
-        address: u8,
-        bytes: &[u8],
-        buffer: &mut [u8],
-    ) -> Result<(), EspError> {
-        self.device(&config::DeviceConfig::new(config::DeviceAddress::SevenBit(
-            address,
-        )))?
-        .write_read(bytes, buffer, BLOCK)
-        .await
-    }
-}
-
-impl<'d> embedded_hal::i2c::ErrorType for AsyncI2cDriver<'d> {
-    type Error = I2cError;
-}
-
-impl<'d> embedded_hal_async::i2c::I2c<embedded_hal::i2c::SevenBitAddress> for AsyncI2cDriver<'d> {
-    async fn read(&mut self, address: u8, buffer: &mut [u8]) -> Result<(), Self::Error> {
-        self.read(address, buffer).await.map_err(to_i2c_err)
-    }
-
-    async fn write(&mut self, address: u8, bytes: &[u8]) -> Result<(), Self::Error> {
-        self.write(address, bytes).await.map_err(to_i2c_err)
-    }
-
-    async fn write_read(
-        &mut self,
-        address: u8,
-        bytes: &[u8],
-        buffer: &mut [u8],
-    ) -> Result<(), Self::Error> {
-        self.write_read(address, bytes, buffer)
-            .await
-            .map_err(to_i2c_err)
-    }
-
-    async fn transaction(
-        &mut self,
-        _address: u8,
-        _operations: &mut [Operation<'_>],
-    ) -> Result<(), Self::Error> {
-        unimplemented!("transactional not implemented")
-    }
-}
-
-unsafe impl<'d> Send for AsyncI2cDriver<'d> {}
-
-impl<'d> Drop for AsyncI2cDriver<'d> {
-    fn drop(&mut self) {
-        loop {
-            if let Ok(_lock_guard) = self.bus_lock.try_lock() {
-                esp!(unsafe { i2c_del_master_bus(self.handle) }).unwrap();
-                break;
-            }
-        }
-    }
-}
-
-#[cfg(not(esp_idf_i2c_isr_iram_safe))]
 pub struct AsyncI2cDeviceDriver<'d, T>
 where
-    T: Borrow<AsyncI2cDriver<'d>>,
+    T: Borrow<AsyncI2cDriver<'d>> + 'd,
 {
     driver: T,
     handle: i2c_master_dev_handle_t,
+    _isr_callback_handle: IsrCallbackHandle,
     _p: PhantomData<&'d mut ()>,
 }
 
-#[cfg(not(esp_idf_i2c_isr_iram_safe))]
 impl<'d, T> AsyncI2cDeviceDriver<'d, T>
 where
-    T: Borrow<AsyncI2cDriver<'d>>,
+    T: Borrow<AsyncI2cDriver<'d>> + 'd,
 {
-    fn new(driver: T, config: &config::DeviceConfig) -> Result<Self, EspError> {
-        let handle = init_device(driver.borrow().bus_handle(), config)?;
+    pub fn new(driver: T, config: &config::DeviceConfig) -> Result<Self, EspError> {
+        // As documented here https://docs.espressif.com/projects/esp-idf/en/v5.2.3/esp32/api-reference/peripherals/i2c.html?highlight=i2c#_CPPv435i2c_master_register_event_callbacks23i2c_master_dev_handle_tPK28i2c_master_event_callbacks_tPv
+        if ASYNC_DEVICE_IN_USE[driver.borrow().port() as usize]
+            .swap(true, core::sync::atomic::Ordering::SeqCst)
+        {
+            // Should indicate that another async device is already in use
+            return Err(EspError::from(ESP_ERR_INVALID_STATE).unwrap());
+        }
+
+        let handle = init_device(driver.borrow().bus_handle(), &config)?;
+        let isr_callback_handle = enable_master_dev_isr_callback(handle, driver.borrow().port())?;
 
         Ok(Self {
             driver,
             handle,
+            _isr_callback_handle: isr_callback_handle,
             _p: PhantomData,
         })
     }
-}
 
-#[cfg(not(esp_idf_i2c_isr_iram_safe))]
-impl<'d, T> AsyncI2cDeviceDriver<'d, T>
-where
-    T: Borrow<AsyncI2cDriver<'d>>,
-{
-    pub async fn read(&mut self, buffer: &mut [u8], timeout: TickType_t) -> Result<(), EspError> {
+    /// Reads data from the device.
+    ///
+    /// # Safety
+    /// The buffer must be valid until the future returned by this function is resolved. Don't drop
+    /// the future before it's resolved. This can lead to corruption of the data.
+    pub async fn async_read(
+        &mut self,
+        buffer: &mut [u8],
+        timeout: TickType_t,
+    ) -> Result<(), EspError> {
         let handle = self.handle;
         let driver = self.driver.borrow();
         let port = driver.port();
 
-        let _lock_guard = driver.acquire_bus().await;
-        enable_master_dev_isr_callback(handle, port)?;
-        on_err!(
-            esp!(unsafe {
-                i2c_master_receive(
-                    handle,
-                    buffer.as_mut_ptr().cast(),
-                    buffer.len(),
-                    timeout as i32,
-                )
-            }),
-            {
-                disable_master_dev_isr_callback(handle).unwrap();
-            }
-        )?;
+        esp!(unsafe {
+            i2c_master_receive(
+                handle,
+                buffer.as_mut_ptr().cast(),
+                buffer.len(),
+                timeout as i32,
+            )
+        })?;
 
         NOTIFIER[port as usize].wait().await;
-        disable_master_dev_isr_callback(handle)?;
         Ok(())
     }
 
-    pub async fn write(&mut self, bytes: &[u8], timeout: TickType_t) -> Result<(), EspError> {
+    /// Writes data to the device.
+    ///
+    /// # Safety
+    /// The buffer must be valid until the future returned by this function is resolved. Don't drop
+    /// the future before it's resolved. This can lead to corruption of the data.
+    pub async fn async_write(&mut self, bytes: &[u8], timeout: TickType_t) -> Result<(), EspError> {
         let handle = self.handle;
         let driver = self.driver.borrow();
         let port = driver.port();
 
-        let _lock_guard = driver.acquire_bus().await;
-        enable_master_dev_isr_callback(handle, port)?;
-        on_err!(
-            esp!(unsafe {
-                i2c_master_transmit(handle, bytes.as_ptr().cast(), bytes.len(), timeout as i32)
-            }),
-            {
-                disable_master_dev_isr_callback(handle).unwrap();
-            }
-        )?;
+        esp!(unsafe {
+            i2c_master_transmit(handle, bytes.as_ptr().cast(), bytes.len(), timeout as i32)
+        })?;
 
         NOTIFIER[port as usize].wait().await;
-        disable_master_dev_isr_callback(handle)?;
         Ok(())
     }
 
-    pub async fn write_read(
+    /// Writes data to and reads data from the device.
+    ///
+    /// # Safety
+    /// The buffer must be valid until the future returned by this function is resolved. Don't drop
+    /// the future before it's resolved. This can lead to corruption of the data.
+    pub async fn async_write_read(
         &mut self,
         bytes: &[u8],
         buffer: &mut [u8],
@@ -737,167 +758,53 @@ where
         let driver = self.driver.borrow();
         let port = driver.port();
 
-        let _lock_guard = driver.acquire_bus().await;
-        enable_master_dev_isr_callback(handle, port)?;
-        on_err!(
-            esp!(unsafe {
-                i2c_master_transmit_receive(
-                    handle,
-                    bytes.as_ptr().cast(),
-                    bytes.len(),
-                    buffer.as_mut_ptr().cast(),
-                    buffer.len(),
-                    timeout as i32,
-                )
-            }),
-            {
-                disable_master_dev_isr_callback(handle).unwrap();
-            }
-        )?;
+        esp!(unsafe {
+            i2c_master_transmit_receive(
+                handle,
+                bytes.as_ptr().cast(),
+                bytes.len(),
+                buffer.as_mut_ptr().cast(),
+                buffer.len(),
+                timeout as i32,
+            )
+        })?;
 
         NOTIFIER[port as usize].wait().await;
-        disable_master_dev_isr_callback(handle)?;
         Ok(())
     }
-}
-
-#[cfg(not(esp_idf_i2c_isr_iram_safe))]
-unsafe impl<'d, T> Send for AsyncI2cDeviceDriver<'d, T> where
-    T: Send + Borrow<AsyncI2cDriver<'d>> + 'd
-{
-}
-
-#[cfg(not(esp_idf_i2c_isr_iram_safe))]
-impl<'d, T> embedded_hal::i2c::ErrorType for AsyncI2cDeviceDriver<'d, T>
-where
-    T: Borrow<AsyncI2cDriver<'d>>,
-{
-    type Error = I2cError;
-}
-
-#[cfg(not(esp_idf_i2c_isr_iram_safe))]
-impl<'d, T> embedded_hal_async::i2c::I2c<embedded_hal::i2c::SevenBitAddress>
-    for AsyncI2cDeviceDriver<'d, T>
-where
-    T: Borrow<AsyncI2cDriver<'d>>,
-{
-    async fn read(&mut self, _address: u8, buffer: &mut [u8]) -> Result<(), Self::Error> {
-        Self::read(self, buffer, BLOCK).await.map_err(to_i2c_err)
-    }
 
-    async fn write(&mut self, _address: u8, bytes: &[u8]) -> Result<(), Self::Error> {
-        Self::write(self, bytes, BLOCK).await.map_err(to_i2c_err)
-    }
-
-    async fn write_read(
-        &mut self,
-        _address: u8,
-        bytes: &[u8],
-        buffer: &mut [u8],
-    ) -> Result<(), Self::Error> {
-        Self::write_read(self, bytes, buffer, BLOCK)
-            .await
-            .map_err(to_i2c_err)
-    }
-
-    async fn transaction(
-        &mut self,
-        _address: u8,
-        _operations: &mut [Operation<'_>],
-    ) -> Result<(), Self::Error> {
-        unimplemented!("transactional not implemented")
+    fn device_handle(&self) -> i2c_master_dev_handle_t {
+        self.handle
     }
 }
 
-impl<'d, T> Drop for AsyncI2cDeviceDriver<'d, T>
+impl<'d, T> AsyncI2cDeviceDriver<'d, T>
 where
-    T: Borrow<AsyncI2cDriver<'d>>,
+    T: Borrow<AsyncI2cDriver<'d>> + 'd,
 {
-    fn drop(&mut self) {
-        loop {
-            if let Ok(_lock_guard) = self.driver.borrow().bus_lock.try_lock() {
-                esp!(unsafe { i2c_master_bus_rm_device(self.handle) }).unwrap();
-                break;
-            }
-        }
-    }
-}
-
-#[cfg(not(esp_idf_i2c_isr_iram_safe))]
-pub struct OwnedAsyncI2cDeviceDriver<'d> {
-    driver: Option<AsyncI2cDriver<'d>>,
-    handle: i2c_master_dev_handle_t,
-    _p: PhantomData<&'d mut ()>,
-}
-
-#[cfg(not(esp_idf_i2c_isr_iram_safe))]
-impl<'d> OwnedAsyncI2cDeviceDriver<'d> {
-    pub fn new<I2C: I2c>(
-        _i2c: impl Peripheral<P = I2C> + 'd,
-        sda: impl Peripheral<P = impl InputPin + OutputPin> + 'd,
-        scl: impl Peripheral<P = impl InputPin + OutputPin> + 'd,
-        bus_config: &config::Config,
-        device_config: &config::DeviceConfig,
-    ) -> Result<Self, EspError> {
-        let driver = AsyncI2cDriver::new(_i2c, sda, scl, bus_config)?;
-        Self::wrap(driver, device_config)
-    }
-
-    pub fn wrap(
-        driver: AsyncI2cDriver<'d>,
-        device_config: &config::DeviceConfig,
-    ) -> Result<Self, EspError> {
-        let handle = init_device(driver.bus_handle(), device_config)?;
-
-        enable_master_dev_isr_callback(handle, driver.port())?;
-
-        Ok(Self {
-            driver: Some(driver),
-            handle,
-            _p: PhantomData,
-        })
-    }
-
-    pub fn release(mut self) -> Result<AsyncI2cDriver<'d>, EspError> {
-        let driver = self.driver.take().unwrap();
-        drop(self);
-        Ok(driver)
-    }
-}
-
-#[cfg(not(esp_idf_i2c_isr_iram_safe))]
-impl<'d> OwnedAsyncI2cDeviceDriver<'d> {
-    pub async fn read(&mut self, buffer: &mut [u8], timeout: TickType_t) -> Result<(), EspError> {
+    pub fn read(&mut self, buffer: &mut [u8], timeout: TickType_t) -> Result<(), EspError> {
         esp!(unsafe {
             i2c_master_receive(
-                self.handle,
+                self.device_handle(),
                 buffer.as_mut_ptr().cast(),
                 buffer.len(),
                 timeout as i32,
             )
-        })?;
-
-        let port = self.driver.as_ref().unwrap().port() as usize;
-        NOTIFIER[port].wait().await;
-        Ok(())
+        })
     }
 
-    pub async fn write(&mut self, bytes: &[u8], timeout: TickType_t) -> Result<(), EspError> {
+    pub fn write(&mut self, bytes: &[u8], timeout: TickType_t) -> Result<(), EspError> {
         esp!(unsafe {
             i2c_master_transmit(
-                self.handle,
+                self.device_handle(),
                 bytes.as_ptr().cast(),
                 bytes.len(),
                 timeout as i32,
             )
-        })?;
-
-        let port = self.driver.as_ref().unwrap().port() as usize;
-        NOTIFIER[port].wait().await;
-        Ok(())
+        })
     }
 
-    pub async fn write_read(
+    pub fn write_read(
         &mut self,
         bytes: &[u8],
         buffer: &mut [u8],
@@ -905,47 +812,120 @@ impl<'d> OwnedAsyncI2cDeviceDriver<'d> {
     ) -> Result<(), EspError> {
         esp!(unsafe {
             i2c_master_transmit_receive(
-                self.handle,
+                self.device_handle(),
                 bytes.as_ptr().cast(),
                 bytes.len(),
                 buffer.as_mut_ptr().cast(),
                 buffer.len(),
                 timeout as i32,
             )
-        })?;
-
-        let port = self.driver.as_ref().unwrap().port() as usize;
-        NOTIFIER[port].wait().await;
-        Ok(())
+        })
     }
 }
 
-#[cfg(not(esp_idf_i2c_isr_iram_safe))]
-unsafe impl<'d> Send for OwnedAsyncI2cDeviceDriver<'d> {}
-
-#[cfg(not(esp_idf_i2c_isr_iram_safe))]
-impl<'d> Drop for OwnedAsyncI2cDeviceDriver<'d> {
+impl<'d, T> Drop for AsyncI2cDeviceDriver<'d, T>
+where
+    T: Borrow<AsyncI2cDriver<'d>>,
+{
     fn drop(&mut self) {
-        disable_master_dev_isr_callback(self.handle).unwrap();
         esp!(unsafe { i2c_master_bus_rm_device(self.handle) }).unwrap();
+        ASYNC_DEVICE_IN_USE[self.driver.borrow().port() as usize]
+            .store(false, core::sync::atomic::Ordering::SeqCst);
     }
 }
 
-#[cfg(not(esp_idf_i2c_isr_iram_safe))]
-impl<'d> embedded_hal::i2c::ErrorType for OwnedAsyncI2cDeviceDriver<'d> {
+impl<'d, T> embedded_hal_0_2::blocking::i2c::Read for AsyncI2cDeviceDriver<'d, T>
+where
+    T: Borrow<AsyncI2cDriver<'d>> + 'd,
+{
+    type Error = I2cError;
+
+    fn read(&mut self, _addr: u8, buffer: &mut [u8]) -> Result<(), Self::Error> {
+        Self::read(self, buffer, BLOCK).map_err(to_i2c_err)
+    }
+}
+
+impl<'d, T> embedded_hal_0_2::blocking::i2c::Write for AsyncI2cDeviceDriver<'d, T>
+where
+    T: Borrow<AsyncI2cDriver<'d>> + 'd,
+{
+    type Error = I2cError;
+
+    fn write(&mut self, _addr: u8, bytes: &[u8]) -> Result<(), Self::Error> {
+        Self::write(self, bytes, BLOCK).map_err(to_i2c_err)
+    }
+}
+
+impl<'d, T> embedded_hal_0_2::blocking::i2c::WriteRead for AsyncI2cDeviceDriver<'d, T>
+where
+    T: Borrow<AsyncI2cDriver<'d>> + 'd,
+{
+    type Error = I2cError;
+
+    fn write_read(
+        &mut self,
+        _addr: u8,
+        bytes: &[u8],
+        buffer: &mut [u8],
+    ) -> Result<(), Self::Error> {
+        Self::write_read(self, bytes, buffer, BLOCK).map_err(to_i2c_err)
+    }
+}
+
+impl<'d, T> embedded_hal::i2c::ErrorType for AsyncI2cDeviceDriver<'d, T>
+where
+    T: Borrow<AsyncI2cDriver<'d>> + 'd,
+{
     type Error = I2cError;
 }
 
+impl<'d, T> embedded_hal::i2c::I2c<embedded_hal::i2c::SevenBitAddress>
+    for AsyncI2cDeviceDriver<'d, T>
+where
+    T: Borrow<AsyncI2cDriver<'d>> + 'd,
+{
+    fn read(&mut self, _addr: u8, buffer: &mut [u8]) -> Result<(), Self::Error> {
+        Self::read(self, buffer, BLOCK).map_err(to_i2c_err)
+    }
+
+    fn write(&mut self, _addr: u8, bytes: &[u8]) -> Result<(), Self::Error> {
+        Self::write(self, bytes, BLOCK).map_err(to_i2c_err)
+    }
+
+    fn write_read(
+        &mut self,
+        _addr: u8,
+        bytes: &[u8],
+        buffer: &mut [u8],
+    ) -> Result<(), Self::Error> {
+        Self::write_read(self, bytes, buffer, BLOCK).map_err(to_i2c_err)
+    }
+
+    fn transaction(
+        &mut self,
+        _addr: u8,
+        _operations: &mut [embedded_hal::i2c::Operation<'_>],
+    ) -> Result<(), Self::Error> {
+        unimplemented!("transactional not implemented")
+    }
+}
+
 #[cfg(not(esp_idf_i2c_isr_iram_safe))]
-impl<'d> embedded_hal_async::i2c::I2c<embedded_hal::i2c::SevenBitAddress>
-    for OwnedAsyncI2cDeviceDriver<'d>
+impl<'d, T> embedded_hal_async::i2c::I2c<embedded_hal::i2c::SevenBitAddress>
+    for AsyncI2cDeviceDriver<'d, T>
+where
+    T: Borrow<AsyncI2cDriver<'d>>,
 {
     async fn read(&mut self, _address: u8, buffer: &mut [u8]) -> Result<(), Self::Error> {
-        Self::read(self, buffer, BLOCK).await.map_err(to_i2c_err)
+        Self::async_read(self, buffer, BLOCK)
+            .await
+            .map_err(to_i2c_err)
     }
 
     async fn write(&mut self, _address: u8, bytes: &[u8]) -> Result<(), Self::Error> {
-        Self::write(self, bytes, BLOCK).await.map_err(to_i2c_err)
+        Self::async_write(self, bytes, BLOCK)
+            .await
+            .map_err(to_i2c_err)
     }
 
     async fn write_read(
@@ -954,7 +934,7 @@ impl<'d> embedded_hal_async::i2c::I2c<embedded_hal::i2c::SevenBitAddress>
         bytes: &[u8],
         buffer: &mut [u8],
     ) -> Result<(), Self::Error> {
-        Self::write_read(self, bytes, buffer, BLOCK)
+        Self::async_write_read(self, bytes, buffer, BLOCK)
             .await
             .map_err(to_i2c_err)
     }
@@ -968,6 +948,13 @@ impl<'d> embedded_hal_async::i2c::I2c<embedded_hal::i2c::SevenBitAddress>
     }
 }
 
+// ------------------------------------------------------------------------------------------------
+// ------------------------------------- Slave ----------------------------------------------------
+// ------------------------------------------------------------------------------------------------
+
+static I2C_BLOCKING_READ_LOCK: (std::sync::Mutex<bool>, Condvar) =
+    (std::sync::Mutex::new(false), Condvar::new());
+
 #[cfg(not(esp32c2))]
 pub struct I2cSlaveDriver<'d> {
     i2c: u8,
@@ -987,7 +974,7 @@ impl<'d> I2cSlaveDriver<'d> {
         address: config::DeviceAddress,
         config: &config::SlaveDeviceConfig,
     ) -> Result<Self, EspError> {
-        super::check_and_set_beta_driver();
+        super::check_and_set_modern_driver();
 
         let handle = init_slave_device(_i2c, sda, scl, address, config)?;
 
@@ -1000,10 +987,23 @@ impl<'d> I2cSlaveDriver<'d> {
         })
     }
 
-    pub fn read(&mut self, buffer: &mut [u8], _timeout: TickType_t) -> Result<usize, EspError> {
+    pub fn read(&mut self, buffer: &mut [u8], timeout: TickType_t) -> Result<usize, EspError> {
         esp!(unsafe { i2c_slave_receive(self.handle, buffer.as_mut_ptr(), buffer.len()) })?;
+        let mut read_pending = I2C_BLOCKING_READ_LOCK.0.lock().unwrap();
+        *read_pending = true;
+        let (_guard, e) = Condvar::new()
+            .wait_timeout_while(
+                read_pending,
+                Duration::from_millis(timeout as u64),
+                |pending| *pending,
+            )
+            .unwrap();
 
-        todo!("How to block?");
+        if e.timed_out() {
+            Err(EspError::from(ESP_ERR_TIMEOUT).unwrap())
+        } else {
+            Ok(buffer.len())
+        }
     }
 
     pub async fn async_read(&mut self, buffer: &mut [u8]) -> Result<(), EspError> {
@@ -1042,7 +1042,7 @@ fn init_master_bus<'d, I2C: I2c>(
     sda: impl Peripheral<P = impl InputPin + OutputPin> + 'd,
     scl: impl Peripheral<P = impl InputPin + OutputPin> + 'd,
     config: &config::Config,
-    max_device_count: usize,
+    trans_queue_depth: usize,
 ) -> Result<i2c_master_bus_handle_t, EspError> {
     crate::into_ref!(sda, scl);
 
@@ -1058,7 +1058,7 @@ fn init_master_bus<'d, I2C: I2c>(
         glitch_ignore_cnt: config.glitch_ignore_cnt,
         i2c_port: I2C::port() as i32,
         intr_priority: 0,
-        trans_queue_depth: max_device_count,
+        trans_queue_depth,
     };
 
     let mut handle: i2c_master_bus_handle_t = ptr::null_mut();
@@ -1132,11 +1132,21 @@ fn to_i2c_err(err: EspError) -> I2cError {
     }
 }
 
+struct IsrCallbackHandle {
+    handle: i2c_master_dev_handle_t,
+}
+
+impl Drop for IsrCallbackHandle {
+    fn drop(&mut self) {
+        disable_master_dev_isr_callback(self.handle).unwrap();
+    }
+}
+
 #[cfg(not(esp_idf_i2c_isr_iram_safe))]
 fn enable_master_dev_isr_callback(
     handle: i2c_master_dev_handle_t,
     host: u8,
-) -> Result<(), EspError> {
+) -> Result<IsrCallbackHandle, EspError> {
     esp!(unsafe {
         i2c_master_register_event_callbacks(
             handle,
@@ -1145,7 +1155,9 @@ fn enable_master_dev_isr_callback(
             },
             &NOTIFIER[host as usize] as *const _ as *mut _,
         )
-    })
+    })?;
+
+    Ok(IsrCallbackHandle { handle })
 }
 
 #[cfg(not(esp_idf_i2c_isr_iram_safe))]
@@ -1202,9 +1214,16 @@ extern "C" fn slave_isr(
     _data: *const i2c_slave_rx_done_event_data_t,
     user_data: *mut c_void,
 ) -> bool {
+    // Notify for sync read
+    {
+        let mut read_pending = I2C_BLOCKING_READ_LOCK.0.lock().unwrap();
+        I2C_BLOCKING_READ_LOCK.1.notify_one();
+        *read_pending = false;
+    }
+
+    // Notify for async read
     let notifier: &HalIsrNotification =
         unsafe { (user_data as *const HalIsrNotification).as_ref() }.unwrap();
-
     notifier.notify_lsb()
 }