From 3efc3eee5700d2a39e397f1b1b821885301c0862 Mon Sep 17 00:00:00 2001
From: Barnaby Walters <barnaby@waterpigs.co.uk>
Date: Mon, 20 Nov 2023 01:29:02 +0100
Subject: [PATCH] stm32/i2c: implement async i2c v1.

---
 embassy-stm32/src/i2c/v1.rs                | 404 +++++++++++++++++++--
 examples/stm32f4/src/bin/i2c_async.rs      |  62 ++++
 examples/stm32f4/src/bin/i2c_comparison.rs | 135 +++++++
 3 files changed, 578 insertions(+), 23 deletions(-)
 create mode 100644 examples/stm32f4/src/bin/i2c_async.rs
 create mode 100644 examples/stm32f4/src/bin/i2c_comparison.rs

diff --git a/embassy-stm32/src/i2c/v1.rs b/embassy-stm32/src/i2c/v1.rs
index 03f07c4fe..b62ee8246 100644
--- a/embassy-stm32/src/i2c/v1.rs
+++ b/embassy-stm32/src/i2c/v1.rs
@@ -1,10 +1,14 @@
+use core::future::poll_fn;
 use core::marker::PhantomData;
+use core::task::Poll;
 
 use embassy_embedded_hal::SetConfig;
+use embassy_futures::select::{select, Either};
+use embassy_hal_internal::drop::OnDrop;
 use embassy_hal_internal::{into_ref, PeripheralRef};
 
 use super::*;
-use crate::dma::NoDma;
+use crate::dma::{NoDma, Transfer};
 use crate::gpio::sealed::AFType;
 use crate::gpio::Pull;
 use crate::interrupt::typelevel::Interrupt;
@@ -13,7 +17,17 @@ use crate::time::Hertz;
 use crate::{interrupt, Peripheral};
 
 pub unsafe fn on_interrupt<T: Instance>() {
-    // todo
+    let regs = T::regs();
+    // i2c v2 only woke the task on transfer complete interrupts. v1 uses interrupts for a bunch of
+    // other stuff, so we wake the task on every interrupt.
+    T::state().waker.wake();
+    critical_section::with(|_| {
+        // Clear event interrupt flag.
+        regs.cr2().modify(|w| {
+            w.set_itevten(false);
+            w.set_iterren(false);
+        });
+    });
 }
 
 #[non_exhaustive]
@@ -98,40 +112,58 @@ impl<'d, T: Instance, TXDMA, RXDMA> I2c<'d, T, TXDMA, RXDMA> {
         }
     }
 
-    fn check_and_clear_error_flags(&self) -> Result<i2c::regs::Sr1, Error> {
+    fn check_and_clear_error_flags() -> Result<i2c::regs::Sr1, Error> {
         // Note that flags should only be cleared once they have been registered. If flags are
         // cleared otherwise, there may be an inherent race condition and flags may be missed.
         let sr1 = T::regs().sr1().read();
 
         if sr1.timeout() {
-            T::regs().sr1().modify(|reg| reg.set_timeout(false));
+            T::regs().sr1().write(|reg| {
+                reg.0 = !0;
+                reg.set_timeout(false);
+            });
             return Err(Error::Timeout);
         }
 
         if sr1.pecerr() {
-            T::regs().sr1().modify(|reg| reg.set_pecerr(false));
+            T::regs().sr1().write(|reg| {
+                reg.0 = !0;
+                reg.set_pecerr(false);
+            });
             return Err(Error::Crc);
         }
 
         if sr1.ovr() {
-            T::regs().sr1().modify(|reg| reg.set_ovr(false));
+            T::regs().sr1().write(|reg| {
+                reg.0 = !0;
+                reg.set_ovr(false);
+            });
             return Err(Error::Overrun);
         }
 
         if sr1.af() {
-            T::regs().sr1().modify(|reg| reg.set_af(false));
+            T::regs().sr1().write(|reg| {
+                reg.0 = !0;
+                reg.set_af(false);
+            });
             return Err(Error::Nack);
         }
 
         if sr1.arlo() {
-            T::regs().sr1().modify(|reg| reg.set_arlo(false));
+            T::regs().sr1().write(|reg| {
+                reg.0 = !0;
+                reg.set_arlo(false);
+            });
             return Err(Error::Arbitration);
         }
 
         // The errata indicates that BERR may be incorrectly detected. It recommends ignoring and
         // clearing the BERR bit instead.
         if sr1.berr() {
-            T::regs().sr1().modify(|reg| reg.set_berr(false));
+            T::regs().sr1().write(|reg| {
+                reg.0 = !0;
+                reg.set_berr(false);
+            });
         }
 
         Ok(sr1)
@@ -150,13 +182,13 @@ impl<'d, T: Instance, TXDMA, RXDMA> I2c<'d, T, TXDMA, RXDMA> {
         });
 
         // Wait until START condition was generated
-        while !self.check_and_clear_error_flags()?.start() {
+        while !Self::check_and_clear_error_flags()?.start() {
             check_timeout()?;
         }
 
         // Also wait until signalled we're master and everything is waiting for us
         while {
-            self.check_and_clear_error_flags()?;
+            Self::check_and_clear_error_flags()?;
 
             let sr2 = T::regs().sr2().read();
             !sr2.msl() && !sr2.busy()
@@ -170,7 +202,7 @@ impl<'d, T: Instance, TXDMA, RXDMA> I2c<'d, T, TXDMA, RXDMA> {
         // Wait until address was sent
         // Wait for the address to be acknowledged
         // Check for any I2C errors. If a NACK occurs, the ADDR bit will never be set.
-        while !self.check_and_clear_error_flags()?.addr() {
+        while !Self::check_and_clear_error_flags()?.addr() {
             check_timeout()?;
         }
 
@@ -190,7 +222,7 @@ impl<'d, T: Instance, TXDMA, RXDMA> I2c<'d, T, TXDMA, RXDMA> {
         // Wait until we're ready for sending
         while {
             // Check for any I2C errors. If a NACK occurs, the ADDR bit will never be set.
-            !self.check_and_clear_error_flags()?.txe()
+            !Self::check_and_clear_error_flags()?.txe()
         } {
             check_timeout()?;
         }
@@ -201,7 +233,7 @@ impl<'d, T: Instance, TXDMA, RXDMA> I2c<'d, T, TXDMA, RXDMA> {
         // Wait until byte is transferred
         while {
             // Check for any potential error conditions.
-            !self.check_and_clear_error_flags()?.btf()
+            !Self::check_and_clear_error_flags()?.btf()
         } {
             check_timeout()?;
         }
@@ -212,7 +244,7 @@ impl<'d, T: Instance, TXDMA, RXDMA> I2c<'d, T, TXDMA, RXDMA> {
     fn recv_byte(&self, check_timeout: impl Fn() -> Result<(), Error>) -> Result<u8, Error> {
         while {
             // Check for any potential error conditions.
-            self.check_and_clear_error_flags()?;
+            Self::check_and_clear_error_flags()?;
 
             !T::regs().sr1().read().rxne()
         } {
@@ -237,7 +269,7 @@ impl<'d, T: Instance, TXDMA, RXDMA> I2c<'d, T, TXDMA, RXDMA> {
             });
 
             // Wait until START condition was generated
-            while !self.check_and_clear_error_flags()?.start() {
+            while !Self::check_and_clear_error_flags()?.start() {
                 check_timeout()?;
             }
 
@@ -254,7 +286,7 @@ impl<'d, T: Instance, TXDMA, RXDMA> I2c<'d, T, TXDMA, RXDMA> {
 
             // Wait until address was sent
             // Wait for the address to be acknowledged
-            while !self.check_and_clear_error_flags()?.addr() {
+            while !Self::check_and_clear_error_flags()?.addr() {
                 check_timeout()?;
             }
 
@@ -332,26 +364,352 @@ impl<'d, T: Instance, TXDMA, RXDMA> I2c<'d, T, TXDMA, RXDMA> {
 
     // Async
 
-    pub async fn write(&mut self, _address: u8, _write: &[u8]) -> Result<(), Error>
+    #[inline] // pretty sure this should always be inlined
+    fn enable_interrupts() -> () {
+        T::regs().cr2().modify(|w| {
+            w.set_iterren(true);
+            w.set_itevten(true);
+        });
+    }
+
+    async fn write_with_stop(&mut self, address: u8, write: &[u8], send_stop: bool) -> Result<(), Error>
     where
         TXDMA: crate::i2c::TxDma<T>,
     {
-        todo!()
+        let dma_transfer = unsafe {
+            let regs = T::regs();
+            regs.cr2().modify(|w| {
+                // DMA mode can be enabled for transmission by setting the DMAEN bit in the I2C_CR2 register.
+                w.set_dmaen(true);
+                w.set_itbufen(false);
+            });
+            // Set the I2C_DR register address in the DMA_SxPAR register. The data will be moved to this address from the memory after each TxE event.
+            let dst = regs.dr().as_ptr() as *mut u8;
+
+            let ch = &mut self.tx_dma;
+            let request = ch.request();
+            Transfer::new_write(ch, request, write, dst, Default::default())
+        };
+
+        let on_drop = OnDrop::new(|| {
+            let regs = T::regs();
+            regs.cr2().modify(|w| {
+                w.set_dmaen(false);
+                w.set_iterren(false);
+                w.set_itevten(false);
+            })
+        });
+
+        Self::enable_interrupts();
+
+        // Send a START condition
+        T::regs().cr1().modify(|reg| {
+            reg.set_start(true);
+        });
+
+        let state = T::state();
+
+        // Wait until START condition was generated
+        poll_fn(|cx| {
+            state.waker.register(cx.waker());
+
+            match Self::check_and_clear_error_flags() {
+                Err(e) => Poll::Ready(Err(e)),
+                Ok(sr1) => {
+                    if sr1.start() {
+                        Poll::Ready(Ok(()))
+                    } else {
+                        Poll::Pending
+                    }
+                }
+            }
+        })
+        .await?;
+
+        // Also wait until signalled we're master and everything is waiting for us
+        Self::enable_interrupts();
+        poll_fn(|cx| {
+            state.waker.register(cx.waker());
+
+            match Self::check_and_clear_error_flags() {
+                Err(e) => Poll::Ready(Err(e)),
+                Ok(_) => {
+                    let sr2 = T::regs().sr2().read();
+                    if !sr2.msl() && !sr2.busy() {
+                        Poll::Pending
+                    } else {
+                        Poll::Ready(Ok(()))
+                    }
+                }
+            }
+        })
+        .await?;
+
+        // Set up current address, we're trying to talk to
+        Self::enable_interrupts();
+        T::regs().dr().write(|reg| reg.set_dr(address << 1));
+
+        poll_fn(|cx| {
+            state.waker.register(cx.waker());
+            match Self::check_and_clear_error_flags() {
+                Err(e) => Poll::Ready(Err(e)),
+                Ok(sr1) => {
+                    if sr1.addr() {
+                        // Clear the ADDR condition by reading SR2.
+                        T::regs().sr2().read();
+                        Poll::Ready(Ok(()))
+                    } else {
+                        Poll::Pending
+                    }
+                }
+            }
+        })
+        .await?;
+        Self::enable_interrupts();
+        let poll_error = poll_fn(|cx| {
+            state.waker.register(cx.waker());
+
+            match Self::check_and_clear_error_flags() {
+                // Unclear why the Err turbofish is necessary here? The compiler didn’t require it in the other
+                // identical poll_fn check_and_clear matches.
+                Err(e) => Poll::Ready(Err::<T, Error>(e)),
+                Ok(_) => Poll::Pending,
+            }
+        });
+
+        // Wait for either the DMA transfer to successfully finish, or an I2C error to occur.
+        match select(dma_transfer, poll_error).await {
+            Either::Second(Err(e)) => Err(e),
+            _ => Ok(()),
+        }?;
+
+        // The I2C transfer itself will take longer than the DMA transfer, so wait for that to finish too.
+
+        // 18.3.8 “Master transmitter: In the interrupt routine after the EOT interrupt, disable DMA
+        // requests then wait for a BTF event before programming the Stop condition.”
+
+        // TODO: If this has to be done “in the interrupt routine after the EOT interrupt”, where to put it?
+        T::regs().cr2().modify(|w| {
+            w.set_dmaen(false);
+        });
+
+        Self::enable_interrupts();
+        poll_fn(|cx| {
+            state.waker.register(cx.waker());
+
+            match Self::check_and_clear_error_flags() {
+                Err(e) => Poll::Ready(Err(e)),
+                Ok(sr1) => {
+                    if sr1.btf() {
+                        if send_stop {
+                            T::regs().cr1().modify(|w| {
+                                w.set_stop(true);
+                            });
+                        }
+
+                        Poll::Ready(Ok(()))
+                    } else {
+                        Poll::Pending
+                    }
+                }
+            }
+        })
+        .await?;
+
+        drop(on_drop);
+
+        // Fallthrough is success
+        Ok(())
     }
 
-    pub async fn read(&mut self, _address: u8, _buffer: &mut [u8]) -> Result<(), Error>
+    pub async fn write(&mut self, address: u8, write: &[u8]) -> Result<(), Error>
+    where
+        TXDMA: crate::i2c::TxDma<T>,
+    {
+        self.write_with_stop(address, write, true).await?;
+
+        // Wait for STOP condition to transmit.
+        Self::enable_interrupts();
+        poll_fn(|cx| {
+            T::state().waker.register(cx.waker());
+            // TODO: error interrupts are enabled here, should we additional check for and return errors?
+            if T::regs().cr1().read().stop() {
+                Poll::Pending
+            } else {
+                Poll::Ready(Ok(()))
+            }
+        })
+        .await?;
+
+        Ok(())
+    }
+
+    pub async fn read(&mut self, address: u8, buffer: &mut [u8]) -> Result<(), Error>
     where
         RXDMA: crate::i2c::RxDma<T>,
     {
-        todo!()
+        let state = T::state();
+        let buffer_len = buffer.len();
+
+        let dma_transfer = unsafe {
+            let regs = T::regs();
+            regs.cr2().modify(|w| {
+                // DMA mode can be enabled for transmission by setting the DMAEN bit in the I2C_CR2 register.
+                w.set_itbufen(false);
+                w.set_dmaen(true);
+            });
+            // Set the I2C_DR register address in the DMA_SxPAR register. The data will be moved to this address from the memory after each TxE event.
+            let src = regs.dr().as_ptr() as *mut u8;
+
+            let ch = &mut self.rx_dma;
+            let request = ch.request();
+            Transfer::new_read(ch, request, src, buffer, Default::default())
+        };
+
+        let on_drop = OnDrop::new(|| {
+            let regs = T::regs();
+            regs.cr2().modify(|w| {
+                w.set_dmaen(false);
+                w.set_iterren(false);
+                w.set_itevten(false);
+            })
+        });
+
+        Self::enable_interrupts();
+
+        // Send a START condition and set ACK bit
+        T::regs().cr1().modify(|reg| {
+            reg.set_start(true);
+            reg.set_ack(true);
+        });
+
+        // Wait until START condition was generated
+        poll_fn(|cx| {
+            state.waker.register(cx.waker());
+
+            match Self::check_and_clear_error_flags() {
+                Err(e) => Poll::Ready(Err(e)),
+                Ok(sr1) => {
+                    if sr1.start() {
+                        Poll::Ready(Ok(()))
+                    } else {
+                        Poll::Pending
+                    }
+                }
+            }
+        })
+        .await?;
+
+        // Also wait until signalled we're master and everything is waiting for us
+        Self::enable_interrupts();
+        poll_fn(|cx| {
+            state.waker.register(cx.waker());
+
+            // blocking read didn’t have a check_and_clear call here, but blocking write did so
+            // I’m adding it here in case that was an oversight.
+            match Self::check_and_clear_error_flags() {
+                Err(e) => Poll::Ready(Err(e)),
+                Ok(_) => {
+                    let sr2 = T::regs().sr2().read();
+                    if !sr2.msl() && !sr2.busy() {
+                        Poll::Pending
+                    } else {
+                        Poll::Ready(Ok(()))
+                    }
+                }
+            }
+        })
+        .await?;
+
+        // Set up current address, we're trying to talk to
+        T::regs().dr().write(|reg| reg.set_dr((address << 1) + 1));
+
+        // Wait for the address to be acknowledged
+
+        Self::enable_interrupts();
+        poll_fn(|cx| {
+            state.waker.register(cx.waker());
+
+            match Self::check_and_clear_error_flags() {
+                Err(e) => Poll::Ready(Err(e)),
+                Ok(sr1) => {
+                    if sr1.addr() {
+                        // 18.3.8: When a single byte must be received: the NACK must be programmed during EV6
+                        // event, i.e. program ACK=0 when ADDR=1, before clearing ADDR flag.
+                        if buffer_len == 1 {
+                            T::regs().cr1().modify(|w| {
+                                w.set_ack(false);
+                            });
+                        }
+                        Poll::Ready(Ok(()))
+                    } else {
+                        Poll::Pending
+                    }
+                }
+            }
+        })
+        .await?;
+
+        // Clear ADDR condition by reading SR2
+        T::regs().sr2().read();
+
+        // 18.3.8: When a single byte must be received: [snip] Then the
+        // user can program the STOP condition either after clearing ADDR flag, or in the
+        // DMA Transfer Complete interrupt routine.
+        if buffer_len == 1 {
+            T::regs().cr1().modify(|w| {
+                w.set_stop(true);
+            });
+        } else {
+            // If, in the I2C_CR2 register, the LAST bit is set, I2C
+            // automatically sends a NACK after the next byte following EOT_1. The user can
+            // generate a Stop condition in the DMA Transfer Complete interrupt routine if enabled.
+            T::regs().cr2().modify(|w| {
+                w.set_last(true);
+            })
+        }
+
+        // Wait for bytes to be received, or an error to occur.
+        Self::enable_interrupts();
+        let poll_error = poll_fn(|cx| {
+            state.waker.register(cx.waker());
+
+            match Self::check_and_clear_error_flags() {
+                Err(e) => Poll::Ready(Err::<T, Error>(e)),
+                _ => Poll::Pending,
+            }
+        });
+
+        match select(dma_transfer, poll_error).await {
+            Either::Second(Err(e)) => Err(e),
+            _ => Ok(()),
+        }?;
+
+        // Wait for the STOP to be sent (STOP bit cleared).
+        Self::enable_interrupts();
+        poll_fn(|cx| {
+            state.waker.register(cx.waker());
+            // TODO: error interrupts are enabled here, should we additional check for and return errors?
+            if T::regs().cr1().read().stop() {
+                Poll::Pending
+            } else {
+                Poll::Ready(Ok(()))
+            }
+        })
+        .await?;
+        drop(on_drop);
+
+        // Fallthrough is success
+        Ok(())
     }
 
-    pub async fn write_read(&mut self, _address: u8, _write: &[u8], _read: &mut [u8]) -> Result<(), Error>
+    pub async fn write_read(&mut self, address: u8, write: &[u8], read: &mut [u8]) -> Result<(), Error>
     where
         RXDMA: crate::i2c::RxDma<T>,
         TXDMA: crate::i2c::TxDma<T>,
     {
-        todo!()
+        self.write_with_stop(address, write, false).await?;
+        self.read(address, read).await
     }
 }
 
diff --git a/examples/stm32f4/src/bin/i2c_async.rs b/examples/stm32f4/src/bin/i2c_async.rs
new file mode 100644
index 000000000..9f59e4d41
--- /dev/null
+++ b/examples/stm32f4/src/bin/i2c_async.rs
@@ -0,0 +1,62 @@
+#![no_std]
+#![no_main]
+#![feature(type_alias_impl_trait)]
+
+// Example originally designed for stm32f411ceu6 reading an A1454 hall effect sensor on I2C1
+// DMA peripherals changed to compile for stm32f429zi, for the CI.
+
+use defmt::*;
+use embassy_executor::Spawner;
+use embassy_stm32::i2c::I2c;
+use embassy_stm32::time::Hertz;
+use embassy_stm32::{bind_interrupts, i2c, peripherals};
+use {defmt_rtt as _, panic_probe as _};
+
+const ADDRESS: u8 = 96;
+
+bind_interrupts!(struct Irqs {
+    I2C1_EV => i2c::EventInterruptHandler<peripherals::I2C1>;
+    I2C1_ER => i2c::ErrorInterruptHandler<peripherals::I2C1>;
+});
+
+#[embassy_executor::main]
+async fn main(_spawner: Spawner) {
+    info!("Hello world!");
+    let p = embassy_stm32::init(Default::default());
+
+    let mut i2c = I2c::new(
+        p.I2C1,
+        p.PB8,
+        p.PB7,
+        Irqs,
+        p.DMA1_CH6,
+        p.DMA1_CH0,
+        Hertz(100_000),
+        Default::default(),
+    );
+
+    loop {
+        let a1454_read_sensor_command = [0x1F];
+        let mut sensor_data_buffer: [u8; 4] = [0, 0, 0, 0];
+
+        match i2c
+            .write_read(ADDRESS, &a1454_read_sensor_command, &mut sensor_data_buffer)
+            .await
+        {
+            Ok(()) => {
+                // Convert 12-bit signed integer into 16-bit signed integer.
+                // Is the 12 bit number negative?
+                if (sensor_data_buffer[2] & 0b00001000) == 0b0001000 {
+                    sensor_data_buffer[2] = sensor_data_buffer[2] | 0b11110000;
+                }
+
+                let mut sensor_value_raw: u16 = sensor_data_buffer[3].into();
+                sensor_value_raw |= (sensor_data_buffer[2] as u16) << 8;
+                let sensor_value: u16 = sensor_value_raw.into();
+                let sensor_value = sensor_value as i16;
+                info!("Data: {}", sensor_value);
+            }
+            Err(e) => error!("I2C Error during read: {:?}", e),
+        }
+    }
+}
diff --git a/examples/stm32f4/src/bin/i2c_comparison.rs b/examples/stm32f4/src/bin/i2c_comparison.rs
new file mode 100644
index 000000000..6d23c0ed8
--- /dev/null
+++ b/examples/stm32f4/src/bin/i2c_comparison.rs
@@ -0,0 +1,135 @@
+#![no_std]
+#![no_main]
+#![feature(type_alias_impl_trait)]
+
+// Example originally designed for stm32f411ceu6 with three A1454 hall effect sensors, connected to I2C1, 2 and 3
+// on the pins referenced in the peripheral definitions.
+// Pins and DMA peripherals changed to compile for stm32f429zi, to work with the CI.
+// MUST be compiled in release mode to see actual performance, otherwise the async transactions take 2x
+// as long to complete as the blocking ones!
+
+use defmt::*;
+use embassy_executor::Spawner;
+use embassy_stm32::i2c::I2c;
+use embassy_stm32::time::Hertz;
+use embassy_stm32::{bind_interrupts, i2c, peripherals};
+use embassy_time::Instant;
+use futures::future::try_join3;
+use {defmt_rtt as _, panic_probe as _};
+
+const ADDRESS: u8 = 96;
+
+bind_interrupts!(struct Irqs {
+    I2C1_EV => i2c::EventInterruptHandler<peripherals::I2C1>;
+    I2C1_ER => i2c::ErrorInterruptHandler<peripherals::I2C1>;
+    I2C2_EV => i2c::EventInterruptHandler<peripherals::I2C2>;
+    I2C2_ER => i2c::ErrorInterruptHandler<peripherals::I2C2>;
+    I2C3_EV => i2c::EventInterruptHandler<peripherals::I2C3>;
+    I2C3_ER => i2c::ErrorInterruptHandler<peripherals::I2C3>;
+});
+
+/// Convert 12-bit signed integer within a 4 byte long buffer into 16-bit signed integer.
+fn a1454_buf_to_i16(buffer: &[u8; 4]) -> i16 {
+    let lower = buffer[3];
+    let mut upper = buffer[2];
+    // Fill in additional 1s if the 12 bit number is negative.
+    if (upper & 0b00001000) == 0b0001000 {
+        upper = upper | 0b11110000;
+    }
+
+    let mut sensor_value_raw: u16 = lower.into();
+    sensor_value_raw |= (upper as u16) << 8;
+    let sensor_value: u16 = sensor_value_raw.into();
+    let sensor_value = sensor_value as i16;
+    sensor_value
+}
+
+#[embassy_executor::main]
+async fn main(_spawner: Spawner) {
+    info!("Setting up peripherals.");
+    let p = embassy_stm32::init(Default::default());
+
+    let mut i2c1 = I2c::new(
+        p.I2C1,
+        p.PB8,
+        p.PB7,
+        Irqs,
+        p.DMA1_CH6,
+        p.DMA1_CH0,
+        Hertz(100_000),
+        Default::default(),
+    );
+
+    let mut i2c2 = I2c::new(
+        p.I2C2,
+        p.PB10,
+        p.PB11,
+        Irqs,
+        p.DMA1_CH7,
+        p.DMA1_CH3,
+        Hertz(100_000),
+        Default::default(),
+    );
+
+    let mut i2c3 = I2c::new(
+        p.I2C3,
+        p.PA8,
+        p.PC9,
+        Irqs,
+        p.DMA1_CH4,
+        p.DMA1_CH2,
+        Hertz(100_000),
+        Default::default(),
+    );
+
+    let a1454_read_sensor_command = [0x1F];
+    let mut i2c1_buffer: [u8; 4] = [0, 0, 0, 0];
+    let mut i2c2_buffer: [u8; 4] = [0, 0, 0, 0];
+    let mut i2c3_buffer: [u8; 4] = [0, 0, 0, 0];
+    loop {
+        // Blocking reads one after the other. Completes in about 2000us.
+        let blocking_read_start_us = Instant::now().as_micros();
+        match i2c1.blocking_write_read(ADDRESS, &a1454_read_sensor_command, &mut i2c1_buffer) {
+            Ok(()) => {}
+            Err(e) => error!("I2C Error: {:?}", e),
+        }
+        match i2c2.blocking_write_read(ADDRESS, &a1454_read_sensor_command, &mut i2c2_buffer) {
+            Ok(()) => {}
+            Err(e) => error!("I2C Error: {:?}", e),
+        }
+        match i2c3.blocking_write_read(ADDRESS, &a1454_read_sensor_command, &mut i2c3_buffer) {
+            Ok(()) => {}
+            Err(e) => error!("I2C Error: {:?}", e),
+        }
+        let blocking_read_total_us = Instant::now().as_micros() - blocking_read_start_us;
+        info!(
+            "Blocking reads completed in {}us: i2c1: {} i2c2: {} i2c3: {}",
+            blocking_read_total_us,
+            a1454_buf_to_i16(&i2c1_buffer),
+            a1454_buf_to_i16(&i2c2_buffer),
+            a1454_buf_to_i16(&i2c3_buffer)
+        );
+
+        // Async reads overlapping. Completes in about 1000us.
+        let async_read_start_us = Instant::now().as_micros();
+
+        let i2c1_result = i2c1.write_read(ADDRESS, &a1454_read_sensor_command, &mut i2c1_buffer);
+        let i2c2_result = i2c2.write_read(ADDRESS, &a1454_read_sensor_command, &mut i2c2_buffer);
+        let i2c3_result = i2c3.write_read(ADDRESS, &a1454_read_sensor_command, &mut i2c3_buffer);
+
+        // Wait for all three transactions to finish, or any one of them to fail.
+        match try_join3(i2c1_result, i2c2_result, i2c3_result).await {
+            Ok(_) => {
+                let async_read_total_us = Instant::now().as_micros() - async_read_start_us;
+                info!(
+                    "Async reads completed in {}us: i2c1: {} i2c2: {} i2c3: {}",
+                    async_read_total_us,
+                    a1454_buf_to_i16(&i2c1_buffer),
+                    a1454_buf_to_i16(&i2c2_buffer),
+                    a1454_buf_to_i16(&i2c3_buffer)
+                );
+            }
+            Err(e) => error!("I2C Error during async write-read: {}", e),
+        };
+    }
+}