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 + 'd, + dma: impl Unborrow + 'd, + vsync_level: VSyncDataInvalidLevel, + hsync_level: HSyncDataInvalidLevel, + pixclk_polarity: PixelClockPolarity, + use_embedded_synchronization: bool, + irq: impl Unborrow + 'd, + d0: impl Unborrow + 'd, + d1: impl Unborrow + 'd, + d2: impl Unborrow + 'd, + d3: impl Unborrow + 'd, + d4: impl Unborrow + 'd, + d5: impl Unborrow + 'd, + d6: impl Unborrow + 'd, + d7: impl Unborrow + 'd, + d8: impl Unborrow + 'd, + d9: impl Unborrow + 'd, + d10: impl Unborrow + 'd, + d11: impl Unborrow + 'd, + d12: impl Unborrow + 'd, + d13: impl Unborrow + 'd, + v_sync: impl Unborrow + 'd, + h_sync: impl Unborrow + 'd, + pixclk: impl Unborrow + '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 sealed::$signal for T + where + T: crate::dma::MuxChannel, + { + fn request(&self) -> crate::dma::Request { + $request + } + } + + impl $signal for T where T: crate::dma::MuxChannel {} + }; + ($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 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 { + 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(bus: Bus, _mco: Mco) -> Self + where + T: McoInstance, + { + Self { + phantom: PhantomData, + bus, + } + } + + pub async fn init(&mut self) -> Result<(), 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> { + Ok(u16::from_le_bytes([ + self.read(Register::MidL).await?, + self.read(Register::MidH).await?, + ])) + } + + pub async fn read_product_id(&mut self) -> Result> { + Ok(u16::from_le_bytes([ + self.read(Register::Ver).await?, + self.read(Register::PId).await?, + ])) + } + + async fn reset_regs(&mut self) -> Result<(), Error> { + self.write(Register::Com7, 0x80).await + } + + async fn set_pixformat(&mut self) -> Result<(), 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> { + 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> { + 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> { + self.bus + .write(CAM_ADDR, &[register as u8, value]) + .await + .map_err(Error::I2c) + } + + async fn modify u8>( + &mut self, + register: Register, + f: F, + ) -> Result<(), Error> { + let value = self.read(register).await?; + let value = f(value); + self.write(register, value).await + } + } +}