Merge #490

490: DCMI r=matoushybl a=matoushybl



Co-authored-by: Matous Hybl <[email protected]>

5 files changed, 866 insertions, 1 deletions

diff --git a/embassy-stm32/src/dcmi.rs b/embassy-stm32/src/dcmi.rs
new file mode 100644
index 000000000..2d2ad72e4
--- /dev/null
+++ b/embassy-stm32/src/dcmi.rs
@@ -0,0 +1,481 @@
+use core::marker::PhantomData;
+use core::task::Poll;
+
+use crate::gpio::sealed::Pin as __GpioPin;
+use crate::gpio::Pin as GpioPin;
+use embassy::interrupt::{Interrupt, InterruptExt};
+use embassy::util::Unborrow;
+use embassy::waitqueue::AtomicWaker;
+use embassy_hal_common::unborrow;
+use futures::future::poll_fn;
+
+/// The level on the VSync pin when the data is not valid on the parallel interface.
+#[derive(Clone, Copy, PartialEq)]
+pub enum VSyncDataInvalidLevel {
+    Low,
+    High,
+}
+
+/// The level on the VSync pin when the data is not valid on the parallel interface.
+#[derive(Clone, Copy, PartialEq)]
+pub enum HSyncDataInvalidLevel {
+    Low,
+    High,
+}
+
+#[derive(Clone, Copy, PartialEq)]
+pub enum PixelClockPolarity {
+    RisingEdge,
+    FallingEdge,
+}
+
+pub struct State {
+    waker: AtomicWaker,
+}
+impl State {
+    const fn new() -> State {
+        State {
+            waker: AtomicWaker::new(),
+        }
+    }
+}
+
+static STATE: State = State::new();
+
+#[derive(Debug, Eq, PartialEq, Copy, Clone)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+#[non_exhaustive]
+pub enum Error {
+    Overrun,
+    PeripheralError,
+}
+
+pub struct Dcmi<'d, T: Instance, Dma: FrameDma> {
+    inner: T,
+    dma: Dma,
+    phantom: PhantomData<&'d mut T>,
+}
+
+impl<'d, T, Dma> Dcmi<'d, T, Dma>
+where
+    T: Instance,
+    Dma: FrameDma,
+{
+    pub fn new(
+        peri: impl Unborrow<Target = T> + 'd,
+        dma: impl Unborrow<Target = Dma> + 'd,
+        vsync_level: VSyncDataInvalidLevel,
+        hsync_level: HSyncDataInvalidLevel,
+        pixclk_polarity: PixelClockPolarity,
+        use_embedded_synchronization: bool,
+        irq: impl Unborrow<Target = T::Interrupt> + 'd,
+        d0: impl Unborrow<Target = impl D0Pin> + 'd,
+        d1: impl Unborrow<Target = impl D1Pin> + 'd,
+        d2: impl Unborrow<Target = impl D2Pin> + 'd,
+        d3: impl Unborrow<Target = impl D3Pin> + 'd,
+        d4: impl Unborrow<Target = impl D4Pin> + 'd,
+        d5: impl Unborrow<Target = impl D5Pin> + 'd,
+        d6: impl Unborrow<Target = impl D6Pin> + 'd,
+        d7: impl Unborrow<Target = impl D7Pin> + 'd,
+        d8: impl Unborrow<Target = impl D8Pin> + 'd,
+        d9: impl Unborrow<Target = impl D9Pin> + 'd,
+        d10: impl Unborrow<Target = impl D10Pin> + 'd,
+        d11: impl Unborrow<Target = impl D11Pin> + 'd,
+        d12: impl Unborrow<Target = impl D12Pin> + 'd,
+        d13: impl Unborrow<Target = impl D13Pin> + 'd,
+        v_sync: impl Unborrow<Target = impl VSyncPin> + 'd,
+        h_sync: impl Unborrow<Target = impl HSyncPin> + 'd,
+        pixclk: impl Unborrow<Target = impl PixClkPin> + 'd,
+    ) -> Self {
+        T::reset();
+        T::enable();
+
+        unborrow!(
+            peri, dma, irq, d0, d1, d2, d3, d4, d5, d6, d7, d8, d9, d10, d11, d12, d13, v_sync,
+            h_sync, pixclk
+        );
+
+        d0.configure();
+        d1.configure();
+        d2.configure();
+        d3.configure();
+        d4.configure();
+        d5.configure();
+        d6.configure();
+        d7.configure();
+        d8.configure();
+        d9.configure();
+        d10.configure();
+        d11.configure();
+        d12.configure();
+        d13.configure();
+
+        v_sync.configure();
+        h_sync.configure();
+        pixclk.configure();
+
+        let edm = match (
+            d8.pin().is_some(),
+            d9.pin().is_some(),
+            d10.pin().is_some(),
+            d11.pin().is_some(),
+            d12.pin().is_some(),
+            d13.pin().is_some(),
+        ) {
+            (true, true, true, true, true, true) => 0b11, // 14 bits
+            (true, true, true, true, false, false) => 0b10, // 12 bits
+            (true, true, false, false, false, false) => 0b01, // 10 bits
+            (false, false, false, false, false, false) => 0b00, // 8 bits
+            _ => {
+                panic!("Invalid pin configuration.");
+            }
+        };
+
+        unsafe {
+            peri.regs().cr().modify(|r| {
+                r.set_cm(true); // disable continuous mode (snapshot mode)
+                r.set_ess(use_embedded_synchronization);
+                r.set_pckpol(pixclk_polarity == PixelClockPolarity::RisingEdge);
+                r.set_vspol(vsync_level == VSyncDataInvalidLevel::High);
+                r.set_hspol(hsync_level == HSyncDataInvalidLevel::High);
+                r.set_fcrc(0x00); // capture every frame
+                r.set_edm(edm); // extended data mode
+            });
+        }
+
+        irq.set_handler(Self::on_interrupt);
+        irq.unpend();
+        irq.enable();
+
+        Self {
+            inner: peri,
+            dma,
+            phantom: PhantomData,
+        }
+    }
+
+    unsafe fn on_interrupt(_: *mut ()) {
+        let ris = crate::pac::DCMI.ris().read();
+        if ris.err_ris() {
+            error!("DCMI IRQ: Error.");
+            crate::pac::DCMI.ier().modify(|ier| ier.set_err_ie(false));
+        }
+        if ris.ovr_ris() {
+            error!("DCMI IRQ: Overrun.");
+            crate::pac::DCMI.ier().modify(|ier| ier.set_ovr_ie(false));
+        }
+        if ris.frame_ris() {
+            info!("DCMI IRQ: Frame captured.");
+            crate::pac::DCMI.ier().modify(|ier| ier.set_frame_ie(false));
+        }
+        STATE.waker.wake();
+    }
+
+    unsafe fn toggle(enable: bool) {
+        crate::pac::DCMI.cr().modify(|r| {
+            r.set_enable(enable);
+            r.set_capture(enable);
+        })
+    }
+
+    fn enable_irqs() {
+        unsafe {
+            crate::pac::DCMI.ier().modify(|r| {
+                r.set_err_ie(true);
+                r.set_ovr_ie(true);
+                r.set_frame_ie(true);
+            });
+        }
+    }
+
+    fn clear_interrupt_flags() {
+        unsafe {
+            crate::pac::DCMI.icr().write(|r| {
+                r.set_ovr_isc(true);
+                r.set_err_isc(true);
+                r.set_frame_isc(true);
+            })
+        }
+    }
+
+    /// This method starts the capture and finishes when both the dma transfer and DCMI finish the frame transfer.
+    /// The implication is that the input buffer size must be exactly the size of the captured frame.
+    pub async fn capture(&mut self, buffer: &mut [u32]) -> Result<(), Error> {
+        let channel = &mut self.dma;
+        let request = channel.request();
+
+        let r = self.inner.regs();
+        let src = r.dr().ptr() as *mut u32;
+        let dma_read = crate::dma::read(channel, request, src, buffer);
+
+        Self::clear_interrupt_flags();
+        Self::enable_irqs();
+
+        unsafe { Self::toggle(true) };
+
+        let result = poll_fn(|cx| {
+            STATE.waker.register(cx.waker());
+
+            let ris = unsafe { crate::pac::DCMI.ris().read() };
+            if ris.err_ris() {
+                unsafe {
+                    crate::pac::DCMI.icr().write(|r| {
+                        r.set_err_isc(true);
+                    })
+                };
+                Poll::Ready(Err(Error::PeripheralError))
+            } else if ris.ovr_ris() {
+                unsafe {
+                    crate::pac::DCMI.icr().write(|r| {
+                        r.set_ovr_isc(true);
+                    })
+                };
+                Poll::Ready(Err(Error::Overrun))
+            } else if ris.frame_ris() {
+                unsafe {
+                    crate::pac::DCMI.icr().write(|r| {
+                        r.set_frame_isc(true);
+                    })
+                };
+                Poll::Ready(Ok(()))
+            } else {
+                Poll::Pending
+            }
+        });
+
+        let (_, result) = futures::future::join(dma_read, result).await;
+
+        unsafe { Self::toggle(false) };
+
+        result
+    }
+}
+
+mod sealed {
+    use super::*;
+    use crate::rcc::RccPeripheral;
+
+    pub trait Instance: RccPeripheral {
+        fn regs(&self) -> crate::pac::dcmi::Dcmi;
+    }
+
+    pub trait FrameDma {
+        fn request(&self) -> crate::dma::Request;
+    }
+
+    macro_rules! pin {
+        ($name:ident) => {
+            pub trait $name: GpioPin {
+                fn configure(&mut self);
+            }
+        };
+    }
+
+    macro_rules! optional_pin {
+        ($name:ident) => {
+            pub trait $name: crate::gpio::OptionalPin {
+                fn configure(&mut self);
+            }
+        };
+    }
+
+    pin!(D0Pin);
+    pin!(D1Pin);
+    pin!(D2Pin);
+    pin!(D3Pin);
+    pin!(D4Pin);
+    pin!(D5Pin);
+    pin!(D6Pin);
+    pin!(D7Pin);
+    optional_pin!(D8Pin);
+    optional_pin!(D9Pin);
+    optional_pin!(D10Pin);
+    optional_pin!(D11Pin);
+    optional_pin!(D12Pin);
+    optional_pin!(D13Pin);
+
+    optional_pin!(HSyncPin);
+    optional_pin!(VSyncPin);
+    pin!(PixClkPin);
+}
+
+pub trait Instance: sealed::Instance + 'static {
+    type Interrupt: Interrupt;
+}
+
+pub trait FrameDma: sealed::FrameDma + crate::dma::Channel {}
+
+macro_rules! pin {
+    ($name:ident) => {
+        pub trait $name: sealed::$name + 'static {}
+    };
+}
+
+pin!(D0Pin);
+pin!(D1Pin);
+pin!(D2Pin);
+pin!(D3Pin);
+pin!(D4Pin);
+pin!(D5Pin);
+pin!(D6Pin);
+pin!(D7Pin);
+pin!(D8Pin);
+pin!(D9Pin);
+pin!(D10Pin);
+pin!(D11Pin);
+pin!(D12Pin);
+pin!(D13Pin);
+
+pin!(HSyncPin);
+pin!(VSyncPin);
+pin!(PixClkPin);
+
+// allow unused as U5 sources do not contain interrupt nor dma data
+#[allow(unused)]
+macro_rules! impl_peripheral {
+    ($inst:ident, $irq:ident) => {
+        impl sealed::Instance for crate::peripherals::$inst {
+            fn regs(&self) -> crate::pac::dcmi::Dcmi {
+                crate::pac::$inst
+            }
+        }
+
+        impl Instance for crate::peripherals::$inst {
+            type Interrupt = crate::interrupt::$irq;
+        }
+    };
+}
+
+crate::pac::interrupts! {
+    ($inst:ident, dcmi, $block:ident, GLOBAL, $irq:ident) => {
+        impl_peripheral!($inst, $irq);
+    };
+}
+
+// allow unused as U5 sources do not contain interrupt nor dma data
+#[allow(unused)]
+macro_rules! impl_dma {
+    ($inst:ident, {dmamux: $dmamux:ident}, $signal:ident, $request:expr) => {
+        impl<T> sealed::$signal for T
+        where
+            T: crate::dma::MuxChannel<Mux = crate::dma::$dmamux>,
+        {
+            fn request(&self) -> crate::dma::Request {
+                $request
+            }
+        }
+
+        impl<T> $signal for T where T: crate::dma::MuxChannel<Mux = crate::dma::$dmamux> {}
+    };
+    ($inst:ident, {channel: $channel:ident}, $signal:ident, $request:expr) => {
+        impl sealed::$signal for crate::peripherals::$channel {
+            fn request(&self) -> crate::dma::Request {
+                $request
+            }
+        }
+
+        impl $signal for crate::peripherals::$channel {}
+    };
+}
+
+crate::pac::peripheral_dma_channels! {
+    ($peri:ident, dcmi, $kind:ident, PSSI, $channel:tt, $request:expr) => {
+        impl_dma!($peri, $channel, FrameDma, $request);
+    };
+    ($peri:ident, dcmi, $kind:ident, DCMI, $channel:tt, $request:expr) => {
+        impl_dma!($peri, $channel, FrameDma, $request);
+    };
+}
+
+macro_rules! impl_pin {
+    ($pin:ident, $signal:ident, $af:expr) => {
+        impl sealed::$signal for crate::peripherals::$pin {
+            fn configure(&mut self) {
+                // NOTE(unsafe) Exclusive access to the registers
+                critical_section::with(|_| unsafe {
+                    self.set_as_af($af, crate::gpio::sealed::AFType::Input);
+                    self.block().ospeedr().modify(|w| {
+                        w.set_ospeedr(
+                            self.pin() as usize,
+                            crate::pac::gpio::vals::Ospeedr::VERYHIGHSPEED,
+                        )
+                    });
+                })
+            }
+        }
+
+        impl $signal for crate::peripherals::$pin {}
+    };
+}
+
+macro_rules! impl_no_pin {
+    ($signal:ident) => {
+        impl sealed::$signal for crate::gpio::NoPin {
+            fn configure(&mut self) {}
+        }
+        impl $signal for crate::gpio::NoPin {}
+    };
+}
+
+impl_no_pin!(D8Pin);
+impl_no_pin!(D9Pin);
+impl_no_pin!(D10Pin);
+impl_no_pin!(D11Pin);
+impl_no_pin!(D12Pin);
+impl_no_pin!(D13Pin);
+impl_no_pin!(HSyncPin);
+impl_no_pin!(VSyncPin);
+
+crate::pac::peripheral_pins!(
+    ($inst:ident, dcmi, DCMI, $pin:ident, D0, $af:expr) => {
+        impl_pin!($pin, D0Pin, $af);
+    };
+    ($inst:ident, dcmi, DCMI, $pin:ident, D1, $af:expr) => {
+        impl_pin!($pin, D1Pin, $af);
+    };
+    ($inst:ident, dcmi, DCMI, $pin:ident, D2, $af:expr) => {
+        impl_pin!($pin, D2Pin, $af);
+    };
+    ($inst:ident, dcmi, DCMI, $pin:ident, D3, $af:expr) => {
+        impl_pin!($pin, D3Pin, $af);
+    };
+    ($inst:ident, dcmi, DCMI, $pin:ident, D4, $af:expr) => {
+        impl_pin!($pin, D4Pin, $af);
+    };
+    ($inst:ident, dcmi, DCMI, $pin:ident, D5, $af:expr) => {
+        impl_pin!($pin, D5Pin, $af);
+    };
+    ($inst:ident, dcmi, DCMI, $pin:ident, D6, $af:expr) => {
+        impl_pin!($pin, D6Pin, $af);
+    };
+    ($inst:ident, dcmi, DCMI, $pin:ident, D7, $af:expr) => {
+        impl_pin!($pin, D7Pin, $af);
+    };
+    ($inst:ident, dcmi, DCMI, $pin:ident, D8, $af:expr) => {
+        impl_pin!($pin, D8Pin, $af);
+    };
+    ($inst:ident, dcmi, DCMI, $pin:ident, D9, $af:expr) => {
+        impl_pin!($pin, D9Pin, $af);
+    };
+    ($inst:ident, dcmi, DCMI, $pin:ident, D10, $af:expr) => {
+        impl_pin!($pin, D10Pin, $af);
+    };
+    ($inst:ident, dcmi, DCMI, $pin:ident, D11, $af:expr) => {
+        impl_pin!($pin, D11Pin, $af);
+    };
+    ($inst:ident, dcmi, DCMI, $pin:ident, D12, $af:expr) => {
+        impl_pin!($pin, D12Pin, $af);
+    };
+    ($inst:ident, dcmi, DCMI, $pin:ident, D13, $af:expr) => {
+        impl_pin!($pin, D13Pin, $af);
+    };
+    ($inst:ident, dcmi, DCMI, $pin:ident, HSYNC, $af:expr) => {
+        impl_pin!($pin, HSyncPin, $af);
+    };
+    ($inst:ident, dcmi, DCMI, $pin:ident, VSYNC, $af:expr) => {
+        impl_pin!($pin, VSyncPin, $af);
+    };
+    ($inst:ident, dcmi, DCMI, $pin:ident, PIXCLK, $af:expr) => {
+        impl_pin!($pin, PixClkPin, $af);
+    };
+);
diff --git a/embassy-stm32/src/lib.rs b/embassy-stm32/src/lib.rs
index 649b25f10..425516a3f 100644
--- a/embassy-stm32/src/lib.rs
+++ b/embassy-stm32/src/lib.rs
@@ -32,6 +32,8 @@ pub mod can;
 pub mod dac;
 #[cfg(dbgmcu)]
 pub mod dbgmcu;
+#[cfg(dcmi)]
+pub mod dcmi;
 #[cfg(all(eth, feature = "net"))]
 pub mod eth;
 #[cfg(exti)]
diff --git a/embassy-stm32/src/rcc/h7/mod.rs b/embassy-stm32/src/rcc/h7/mod.rs
index dc458a8a3..be7f440a1 100644
--- a/embassy-stm32/src/rcc/h7/mod.rs
+++ b/embassy-stm32/src/rcc/h7/mod.rs
@@ -73,6 +73,7 @@ pub struct Config {
     pub pll2: PllConfig,
     pub pll3: PllConfig,
     pub enable_dma1: bool,
+    pub enable_dma2: bool,
 }
 
 pub struct Rcc<'d> {
@@ -334,6 +335,10 @@ impl<'d> Rcc<'d> {
                 RCC.ahb1enr().modify(|w| w.set_dma1en(true));
             }
 
+            if self.config.enable_dma2 {
+                RCC.ahb1enr().modify(|w| w.set_dma2en(true));
+            }
+
             CoreClocks {
                 hclk: Hertz(rcc_hclk),
                 pclk1: Hertz(rcc_pclk1),
diff --git a/examples/stm32h7/Cargo.toml b/examples/stm32h7/Cargo.toml
index dc31c6b52..393e779e4 100644
--- a/examples/stm32h7/Cargo.toml
+++ b/examples/stm32h7/Cargo.toml
@@ -30,4 +30,42 @@ micromath = "2.0.0"
 git = "https://github.com/smoltcp-rs/smoltcp"
 rev = "3644b94b82d9433313c75281fdc78942c2450bdf"
 default-features = false
-features = ["defmt"]
\ No newline at end of file
+features = ["defmt"]
+
+# cargo build/run
+[profile.dev]
+codegen-units = 1
+debug = 2
+debug-assertions = true # <-
+incremental = false
+opt-level = 3 # <-
+overflow-checks = true # <-
+
+# cargo test
+[profile.test]
+codegen-units = 1
+debug = 2
+debug-assertions = true # <-
+incremental = false
+opt-level = 3 # <-
+overflow-checks = true # <-
+
+# cargo build/run --release
+[profile.release]
+codegen-units = 1
+debug = 2
+debug-assertions = false # <-
+incremental = false
+lto = 'fat'
+opt-level = 3 # <-
+overflow-checks = false # <-
+
+# cargo test --release
+[profile.bench]
+codegen-units = 1
+debug = 2
+debug-assertions = false # <-
+incremental = false
+lto = 'fat'
+opt-level = 3 # <-
+overflow-checks = false # <-
\ No newline at end of file
diff --git a/examples/stm32h7/src/bin/camera.rs b/examples/stm32h7/src/bin/camera.rs
new file mode 100644
index 000000000..2fa742b83
--- /dev/null
+++ b/examples/stm32h7/src/bin/camera.rs
@@ -0,0 +1,339 @@
+#![no_std]
+#![no_main]
+#![feature(type_alias_impl_trait)]
+
+use embassy::executor::Spawner;
+use embassy::time::{Duration, Timer};
+use embassy_stm32::dcmi::*;
+use embassy_stm32::gpio::{Level, NoPin, Output, Speed};
+use embassy_stm32::i2c::I2c;
+use embassy_stm32::interrupt;
+use embassy_stm32::rcc::{Mco, Mco1Source, McoClock};
+use embassy_stm32::time::U32Ext;
+use embassy_stm32::Peripherals;
+use embedded_hal::digital::v2::OutputPin;
+
+use defmt_rtt as _; // global logger
+use panic_probe as _;
+
+use core::sync::atomic::{AtomicUsize, Ordering};
+use embassy_stm32::Config;
+
+defmt::timestamp! {"{=u64}", {
+        static COUNT: AtomicUsize = AtomicUsize::new(0);
+        // NOTE(no-CAS) `timestamps` runs with interrupts disabled
+        let n = COUNT.load(Ordering::Relaxed);
+        COUNT.store(n + 1, Ordering::Relaxed);
+        n as u64
+    }
+}
+
+#[allow(unused)]
+pub fn config() -> Config {
+    let mut config = Config::default();
+    config.rcc.sys_ck = Some(400.mhz().into());
+    config.rcc.hclk = Some(400.mhz().into());
+    config.rcc.pll1.q_ck = Some(100.mhz().into());
+    config.rcc.enable_dma1 = true;
+    config.rcc.enable_dma2 = true;
+    config.rcc.pclk1 = Some(100.mhz().into());
+    config.rcc.pclk2 = Some(100.mhz().into());
+    config.rcc.pclk3 = Some(100.mhz().into());
+    config.rcc.pclk4 = Some(100.mhz().into());
+    config
+}
+
+use ov7725::*;
+
+const WIDTH: usize = 100;
+const HEIGHT: usize = 100;
+
+static mut FRAME: [u32; WIDTH * HEIGHT / 2] = [0u32; WIDTH * HEIGHT / 2];
+
+#[embassy::main(config = "config()")]
+async fn main(_spawner: Spawner, p: Peripherals) {
+    defmt::info!("Hello World!");
+    let mco = Mco::new(p.MCO1, p.PA8, Mco1Source::Hsi, McoClock::Divided(3));
+
+    let mut led = Output::new(p.PE3, Level::High, Speed::Low);
+    let i2c_irq = interrupt::take!(I2C1_EV);
+    let cam_i2c = I2c::new(
+        p.I2C1,
+        p.PB8,
+        p.PB9,
+        i2c_irq,
+        p.DMA1_CH1,
+        p.DMA1_CH2,
+        100u32.khz(),
+    );
+
+    let mut camera = Ov7725::new(cam_i2c, mco);
+
+    defmt::unwrap!(camera.init().await);
+
+    let manufacturer_id = defmt::unwrap!(camera.read_manufacturer_id().await);
+    let camera_id = defmt::unwrap!(camera.read_product_id().await);
+
+    defmt::info!(
+        "manufacturer: 0x{:x}, pid: 0x{:x}",
+        manufacturer_id,
+        camera_id
+    );
+
+    let dcmi_irq = interrupt::take!(DCMI);
+    let mut dcmi = Dcmi::new(
+        p.DCMI,
+        p.DMA1_CH0,
+        VSyncDataInvalidLevel::High,
+        HSyncDataInvalidLevel::Low,
+        PixelClockPolarity::RisingEdge,
+        false,
+        dcmi_irq,
+        p.PC6,
+        p.PC7,
+        p.PE0,
+        p.PE1,
+        p.PE4,
+        p.PD3,
+        p.PE5,
+        p.PE6,
+        NoPin,
+        NoPin,
+        NoPin,
+        NoPin,
+        NoPin,
+        NoPin,
+        p.PB7,
+        p.PA4,
+        p.PA6,
+    );
+
+    defmt::info!("attempting capture");
+    defmt::unwrap!(dcmi.capture(unsafe { &mut FRAME }).await);
+
+    defmt::info!("captured frame: {:x}", unsafe { &FRAME });
+
+    defmt::info!("main loop running");
+    loop {
+        defmt::info!("high");
+        defmt::unwrap!(led.set_high());
+        Timer::after(Duration::from_millis(500)).await;
+
+        defmt::info!("low");
+        defmt::unwrap!(led.set_low());
+        Timer::after(Duration::from_millis(500)).await;
+    }
+}
+
+mod ov7725 {
+    use core::marker::PhantomData;
+
+    use defmt::Format;
+    use embassy::time::{Duration, Timer};
+    use embassy_stm32::rcc::{Mco, McoInstance};
+    use embassy_traits::i2c::I2c;
+
+    #[repr(u8)]
+    pub enum RgbFormat {
+        Gbr422 = 0,
+        RGB565 = 1,
+        RGB555 = 2,
+        RGB444 = 3,
+    }
+    pub enum PixelFormat {
+        Yuv,
+        ProcessedRawBayer,
+        Rgb(RgbFormat),
+        RawBayer,
+    }
+
+    impl From<PixelFormat> for u8 {
+        fn from(raw: PixelFormat) -> Self {
+            match raw {
+                PixelFormat::Yuv => 0,
+                PixelFormat::ProcessedRawBayer => 1,
+                PixelFormat::Rgb(mode) => 2 | ((mode as u8) << 2),
+                PixelFormat::RawBayer => 3,
+            }
+        }
+    }
+
+    #[derive(Clone, Copy)]
+    #[repr(u8)]
+    #[allow(unused)]
+    pub enum Register {
+        Gain = 0x00,
+        Blue = 0x01,
+        Red = 0x02,
+        Green = 0x03,
+        BAvg = 0x05,
+        GAvg = 0x06,
+        RAvg = 0x07,
+        Aech = 0x08,
+        Com2 = 0x09,
+        PId = 0x0a,
+        Ver = 0x0b,
+        Com3 = 0x0c,
+        Com4 = 0x0d,
+        Com5 = 0x0e,
+        Com6 = 0x0f,
+        Aec = 0x10,
+        ClkRc = 0x11,
+        Com7 = 0x12,
+        Com8 = 0x13,
+        Com9 = 0x14,
+        Com10 = 0x15,
+        Reg16 = 0x16,
+        HStart = 0x17,
+        HSize = 0x18,
+        VStart = 0x19,
+        VSize = 0x1a,
+        PShift = 0x1b,
+        MidH = 0x1c,
+        MidL = 0x1d,
+        Laec = 0x1f,
+        Com11 = 0x20,
+        BdBase = 0x22,
+        BdMStep = 0x23,
+        Aew = 0x24,
+        Aeb = 0x25,
+        Vpt = 0x26,
+        Reg28 = 0x28,
+        HOutSize = 0x29,
+        EXHCH = 0x2a,
+        EXHCL = 0x2b,
+        VOutSize = 0x2c,
+        Advfl = 0x2d,
+        Advfh = 0x2e,
+        Yave = 0x2f,
+        LumHTh = 0x30,
+        LumLTh = 0x31,
+        HRef = 0x32,
+        DspCtrl4 = 0x67,
+        DspAuto = 0xac,
+    }
+
+    const CAM_ADDR: u8 = 0x21;
+
+    #[derive(Format)]
+    pub enum Error<I2cError: Format> {
+        I2c(I2cError),
+    }
+
+    pub struct Ov7725<'d, Bus: I2c> {
+        phantom: PhantomData<&'d ()>,
+        bus: Bus,
+    }
+
+    impl<'d, Bus> Ov7725<'d, Bus>
+    where
+        Bus: I2c,
+        Bus::Error: Format,
+    {
+        pub fn new<T>(bus: Bus, _mco: Mco<T>) -> Self
+        where
+            T: McoInstance,
+        {
+            Self {
+                phantom: PhantomData,
+                bus,
+            }
+        }
+
+        pub async fn init(&mut self) -> Result<(), Error<Bus::Error>> {
+            Timer::after(Duration::from_millis(500)).await;
+            self.reset_regs().await?;
+            Timer::after(Duration::from_millis(500)).await;
+            self.set_pixformat().await?;
+            self.set_resolution().await?;
+            Ok(())
+        }
+
+        pub async fn read_manufacturer_id(&mut self) -> Result<u16, Error<Bus::Error>> {
+            Ok(u16::from_le_bytes([
+                self.read(Register::MidL).await?,
+                self.read(Register::MidH).await?,
+            ]))
+        }
+
+        pub async fn read_product_id(&mut self) -> Result<u16, Error<Bus::Error>> {
+            Ok(u16::from_le_bytes([
+                self.read(Register::Ver).await?,
+                self.read(Register::PId).await?,
+            ]))
+        }
+
+        async fn reset_regs(&mut self) -> Result<(), Error<Bus::Error>> {
+            self.write(Register::Com7, 0x80).await
+        }
+
+        async fn set_pixformat(&mut self) -> Result<(), Error<Bus::Error>> {
+            self.write(Register::DspCtrl4, 0).await?;
+            let mut com7 = self.read(Register::Com7).await?;
+            com7 |= u8::from(PixelFormat::Rgb(RgbFormat::RGB565));
+            self.write(Register::Com7, com7).await?;
+            Ok(())
+        }
+
+        async fn set_resolution(&mut self) -> Result<(), Error<Bus::Error>> {
+            let horizontal: u16 = super::WIDTH as u16;
+            let vertical: u16 = super::HEIGHT as u16;
+
+            let h_high = (horizontal >> 2) as u8;
+            let v_high = (vertical >> 1) as u8;
+            let h_low = (horizontal & 0x03) as u8;
+            let v_low = (vertical & 0x01) as u8;
+
+            self.write(Register::HOutSize, h_high).await?;
+            self.write(Register::VOutSize, v_high).await?;
+            self.write(Register::EXHCH, h_low | (v_low << 2)).await?;
+
+            self.write(Register::Com3, 0xd1).await?;
+
+            let com3 = self.read(Register::Com3).await?;
+            let vflip = com3 & 0x80 > 0;
+
+            self.modify(Register::HRef, |reg| {
+                reg & 0xbf | if vflip { 0x40 } else { 0x40 }
+            })
+            .await?;
+
+            if horizontal <= 320 || vertical <= 240 {
+                self.write(Register::HStart, 0x3f).await?;
+                self.write(Register::HSize, 0x50).await?;
+                self.write(Register::VStart, 0x02).await?; // TODO vflip is subtracted in the original code
+                self.write(Register::VSize, 0x78).await?;
+            } else {
+                defmt::panic!("VGA resolutions not yet supported.");
+            }
+
+            Ok(())
+        }
+
+        async fn read(&mut self, register: Register) -> Result<u8, Error<Bus::Error>> {
+            let mut buffer = [0u8; 1];
+            self.bus
+                .write_read(CAM_ADDR, &[register as u8], &mut buffer[..1])
+                .await
+                .map_err(Error::I2c)?;
+            Ok(buffer[0])
+        }
+
+        async fn write(&mut self, register: Register, value: u8) -> Result<(), Error<Bus::Error>> {
+            self.bus
+                .write(CAM_ADDR, &[register as u8, value])
+                .await
+                .map_err(Error::I2c)
+        }
+
+        async fn modify<F: FnOnce(u8) -> u8>(
+            &mut self,
+            register: Register,
+            f: F,
+        ) -> Result<(), Error<Bus::Error>> {
+            let value = self.read(register).await?;
+            let value = f(value);
+            self.write(register, value).await
+        }
+    }
+}