From 768eea0366fcf2fea2f18c05412de6d0171f03d0 Mon Sep 17 00:00:00 2001 From: Dominic Fischer Date: Sat, 20 Apr 2024 14:55:12 +0100 Subject: [PATCH] Add LCD_CAM Camera driver --- esp-hal/CHANGELOG.md | 2 + esp-hal/src/dma/mod.rs | 2 +- esp-hal/src/lcd_cam/cam.rs | 556 ++++++++++++++++++++++++++++- esp-hal/src/lcd_cam/mod.rs | 2 +- examples/src/bin/lcd_cam_ov2640.rs | 422 ++++++++++++++++++++++ 5 files changed, 980 insertions(+), 4 deletions(-) create mode 100644 examples/src/bin/lcd_cam_ov2640.rs diff --git a/esp-hal/CHANGELOG.md b/esp-hal/CHANGELOG.md index 6cde99893af..8cdbf81d10a 100644 --- a/esp-hal/CHANGELOG.md +++ b/esp-hal/CHANGELOG.md @@ -9,6 +9,8 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 ### Added +- ESP32-S3: Add LCD_CAM Camera driver (#0000) + ### Fixed ### Changed diff --git a/esp-hal/src/dma/mod.rs b/esp-hal/src/dma/mod.rs index 89575cb18d7..986974e912a 100644 --- a/esp-hal/src/dma/mod.rs +++ b/esp-hal/src/dma/mod.rs @@ -1308,7 +1308,7 @@ where MODE: Mode, { pub tx: C::Tx<'d>, - pub(crate) rx: C::Rx<'d>, + pub rx: C::Rx<'d>, phantom: PhantomData, } diff --git a/esp-hal/src/lcd_cam/cam.rs b/esp-hal/src/lcd_cam/cam.rs index 7c2381316e1..f43179a3f1c 100644 --- a/esp-hal/src/lcd_cam/cam.rs +++ b/esp-hal/src/lcd_cam/cam.rs @@ -1,5 +1,557 @@ -use crate::{peripheral::PeripheralRef, peripherals::LCD_CAM}; +use embedded_dma::WriteBuffer; +use fugit::HertzU32; + +use crate::{ + clock::Clocks, + dma::{ + ChannelRx, + ChannelTypes, + DmaError, + DmaPeripheral, + DmaTransfer, + LcdCamPeripheral, + RegisterAccess, + Rx, + RxChannel, + RxPrivate, + }, + gpio::{InputPin, InputSignal, OutputPin, OutputSignal}, + i2s::Error, + lcd_cam::{cam::private::RxPins, private::calculate_clkm, BitOrder}, + peripheral::{Peripheral, PeripheralRef}, + peripherals::LCD_CAM, +}; + +/// Generation of GDMA SUC EOF +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +pub enum EofMode { + /// Generate GDMA SUC EOF by data byte length + ByteLen, + /// Generate GDMA SUC EOF by the vsync signal + VsyncSignal, +} pub struct Cam<'d> { - pub(crate) _lcd_cam: PeripheralRef<'d, LCD_CAM>, + pub(crate) lcd_cam: PeripheralRef<'d, LCD_CAM>, +} + +pub struct Camera<'d, RX, P> { + lcd_cam: PeripheralRef<'d, LCD_CAM>, + rx_channel: RX, + _pins: P, +} + +impl<'d, T, R, P: RxPins> Camera<'d, ChannelRx<'d, T, R>, P> +where + T: RxChannel, + R: ChannelTypes + RegisterAccess, + R::P: LcdCamPeripheral, +{ + pub fn new( + cam: Cam<'d>, + mut channel: ChannelRx<'d, T, R>, + mut pins: P, + frequency: HertzU32, + clocks: &Clocks, + ) -> Self { + let lcd_cam = cam.lcd_cam; + + let (i, divider) = calculate_clkm( + frequency.to_Hz() as _, + &[ + clocks.xtal_clock.to_Hz() as _, + clocks.cpu_clock.to_Hz() as _, + clocks.crypto_pwm_clock.to_Hz() as _, + ], + ); + + lcd_cam.cam_ctrl().write(|w| { + // Force enable the clock for all configuration registers. + unsafe { + w.cam_clk_sel() + .bits((i + 1) as _) + .cam_clkm_div_num() + .bits(divider.div_num as _) + .cam_clkm_div_b() + .bits(divider.div_b as _) + .cam_clkm_div_a() + .bits(divider.div_a as _) + .cam_vsync_filter_thres() + .bits(0) + .cam_vs_eof_en() + .set_bit() + } + }); + lcd_cam + .cam_ctrl1() + .write(|w| w.cam_vh_de_mode_en().clear_bit()); + + lcd_cam + .cam_rgb_yuv() + .write(|w| w.cam_conv_bypass().clear_bit()); + + lcd_cam.cam_ctrl().modify(|_, w| w.cam_update().set_bit()); + + channel.init_channel(); + pins.configure(); + + Self { + lcd_cam, + rx_channel: channel, + _pins: pins, + } + } +} + +impl<'d, RX: Rx, P: RxPins> Camera<'d, RX, P> { + pub fn set_bit_order(&mut self, bit_order: BitOrder) -> &mut Self { + self.lcd_cam + .cam_ctrl() + .modify(|_, w| w.cam_bit_order().bit(bit_order != BitOrder::default())); + self + } + + pub fn with_master_clock(self, mclk: impl Peripheral

+ 'd) -> Self { + crate::into_ref!(mclk); + mclk.set_to_push_pull_output() + .connect_peripheral_to_output(OutputSignal::CAM_CLK); + self + } + + pub fn with_ctrl_pins( + self, + vsync: impl Peripheral

+ 'd, + h_enable: impl Peripheral

+ 'd, + pclk: impl Peripheral

+ 'd, + ) -> Self { + crate::into_ref!(vsync); + crate::into_ref!(h_enable); + crate::into_ref!(pclk); + + vsync + .set_to_input() + .connect_input_to_peripheral(InputSignal::CAM_V_SYNC); + h_enable + .set_to_input() + .connect_input_to_peripheral(InputSignal::CAM_H_ENABLE); + pclk.set_to_input() + .connect_input_to_peripheral(InputSignal::CAM_PCLK); + + self + } + + // Reset Camera control unit and Async Rx FIFO + fn reset_unit_and_fifo(&self) { + self.lcd_cam + .cam_ctrl1() + .modify(|_, w| w.cam_reset().set_bit()); + self.lcd_cam + .cam_ctrl1() + .modify(|_, w| w.cam_reset().clear_bit()); + self.lcd_cam + .cam_ctrl1() + .modify(|_, w| w.cam_afifo_reset().set_bit()); + self.lcd_cam + .cam_ctrl1() + .modify(|_, w| w.cam_afifo_reset().clear_bit()); + } + + // Start the Camera unit to listen for incoming DVP stream. + fn start_unit(&self) { + self.lcd_cam + .cam_ctrl1() + .modify(|_, w| w.cam_start().set_bit()); + } + + pub fn read_dma<'t, RXBUF>( + &'t mut self, + buf: &'t mut RXBUF, + ) -> Result, DmaError> + where + RXBUF: WriteBuffer, + { + let (ptr, len) = unsafe { buf.write_buffer() }; + + self.reset_unit_and_fifo(); + + // Start DMA to receive incoming transfer. + self.rx_channel + .prepare_transfer_without_start(false, DmaPeripheral::LcdCam, ptr, len)?; + self.rx_channel.start_transfer()?; + + self.start_unit(); + + Ok(Transfer { instance: self }) + } + + pub fn read_dma_circular<'t, RXBUF>( + &'t mut self, + buf: &'t mut RXBUF, + ) -> Result, DmaError> + where + RXBUF: WriteBuffer, + { + let (ptr, len) = unsafe { buf.write_buffer() }; + + self.reset_unit_and_fifo(); + + // Start DMA to receive incoming transfer. + self.rx_channel + .prepare_transfer_without_start(true, DmaPeripheral::LcdCam, ptr, len)?; + self.rx_channel.start_transfer()?; + + self.start_unit(); + + Ok(Transfer { instance: self }) + } +} + +/// An in-progress transfer +#[must_use] +pub struct Transfer<'t, 'd, RX: Rx, P> { + instance: &'t mut Camera<'d, RX, P>, +} + +impl<'t, 'd, RX: Rx, P> Transfer<'t, 'd, RX, P> { + /// Amount of bytes which can be popped + pub fn available(&mut self) -> usize { + self.instance.rx_channel.available() + } + + pub fn pop(&mut self, data: &mut [u8]) -> Result { + Ok(self.instance.rx_channel.pop(data)?) + } + + /// Wait for the DMA transfer to complete. + /// Length of the received data is returned + #[allow(clippy::type_complexity)] + pub fn wait_receive(self, dst: &mut [u8]) -> Result { + // Wait for DMA transfer to finish. + while !self.is_done() {} + + let len = self + .instance + .rx_channel + .drain_buffer(dst) + .map_err(|e| (e, 0))?; + + if (&self.instance.rx_channel).has_error() { + Err((DmaError::DescriptorError, len)) + } else { + Ok(len) + } + } +} + +impl<'t, 'd, RX: Rx, P> DmaTransfer for Transfer<'t, 'd, RX, P> { + fn wait(self) -> Result<(), DmaError> { + // Wait for DMA transfer to finish. + while !self.is_done() {} + + let ch = &self.instance.rx_channel; + if ch.has_error() { + Err(DmaError::DescriptorError) + } else { + Ok(()) + } + } + + fn is_done(&self) -> bool { + let ch = &self.instance.rx_channel; + // Wait for IN_SUC_EOF (i.e. VSYNC) + if ch.is_done() { + true + } + // Or for IN_DSCR_EMPTY (i.e. No more buffer space) + else if ch.has_dscr_empty_error() { + true + } + // Or for IN_DSCR_ERR (i.e. bad descriptor) + else if ch.has_error() { + true + } else { + false + } + } +} + +impl<'t, 'd, RX: Rx, P> Drop for Transfer<'t, 'd, RX, P> { + fn drop(&mut self) { + self.instance + .lcd_cam + .cam_ctrl1() + .modify(|_, w| w.cam_start().clear_bit()); + // TODO: Stop DMA?? self.instance.rx_channel.stop_transfer(); + } +} + +pub struct RxEightBits<'d, P0, P1, P2, P3, P4, P5, P6, P7> { + pin_0: PeripheralRef<'d, P0>, + pin_1: PeripheralRef<'d, P1>, + pin_2: PeripheralRef<'d, P2>, + pin_3: PeripheralRef<'d, P3>, + pin_4: PeripheralRef<'d, P4>, + pin_5: PeripheralRef<'d, P5>, + pin_6: PeripheralRef<'d, P6>, + pin_7: PeripheralRef<'d, P7>, +} + +impl<'d, P0, P1, P2, P3, P4, P5, P6, P7> RxEightBits<'d, P0, P1, P2, P3, P4, P5, P6, P7> +where + P0: InputPin, + P1: InputPin, + P2: InputPin, + P3: InputPin, + P4: InputPin, + P5: InputPin, + P6: InputPin, + P7: InputPin, +{ + pub fn new( + pin_0: impl Peripheral

+ 'd, + pin_1: impl Peripheral

+ 'd, + pin_2: impl Peripheral

+ 'd, + pin_3: impl Peripheral

+ 'd, + pin_4: impl Peripheral

+ 'd, + pin_5: impl Peripheral

+ 'd, + pin_6: impl Peripheral

+ 'd, + pin_7: impl Peripheral

+ 'd, + ) -> Self { + crate::into_ref!(pin_0); + crate::into_ref!(pin_1); + crate::into_ref!(pin_2); + crate::into_ref!(pin_3); + crate::into_ref!(pin_4); + crate::into_ref!(pin_5); + crate::into_ref!(pin_6); + crate::into_ref!(pin_7); + + Self { + pin_0, + pin_1, + pin_2, + pin_3, + pin_4, + pin_5, + pin_6, + pin_7, + } + } +} + +impl<'d, P0, P1, P2, P3, P4, P5, P6, P7> RxPins for RxEightBits<'d, P0, P1, P2, P3, P4, P5, P6, P7> +where + P0: InputPin, + P1: InputPin, + P2: InputPin, + P3: InputPin, + P4: InputPin, + P5: InputPin, + P6: InputPin, + P7: InputPin, +{ + type Word = u8; + + fn configure(&mut self) { + self.pin_0 + .set_to_input() + .connect_input_to_peripheral(InputSignal::CAM_DATA_0); + self.pin_1 + .set_to_input() + .connect_input_to_peripheral(InputSignal::CAM_DATA_1); + self.pin_2 + .set_to_input() + .connect_input_to_peripheral(InputSignal::CAM_DATA_2); + self.pin_3 + .set_to_input() + .connect_input_to_peripheral(InputSignal::CAM_DATA_3); + self.pin_4 + .set_to_input() + .connect_input_to_peripheral(InputSignal::CAM_DATA_4); + self.pin_5 + .set_to_input() + .connect_input_to_peripheral(InputSignal::CAM_DATA_5); + self.pin_6 + .set_to_input() + .connect_input_to_peripheral(InputSignal::CAM_DATA_6); + self.pin_7 + .set_to_input() + .connect_input_to_peripheral(InputSignal::CAM_DATA_7); + } +} + +pub struct RxSixteenBits<'d, P0, P1, P2, P3, P4, P5, P6, P7, P8, P9, P10, P11, P12, P13, P14, P15> { + pin_0: PeripheralRef<'d, P0>, + pin_1: PeripheralRef<'d, P1>, + pin_2: PeripheralRef<'d, P2>, + pin_3: PeripheralRef<'d, P3>, + pin_4: PeripheralRef<'d, P4>, + pin_5: PeripheralRef<'d, P5>, + pin_6: PeripheralRef<'d, P6>, + pin_7: PeripheralRef<'d, P7>, + pin_8: PeripheralRef<'d, P8>, + pin_9: PeripheralRef<'d, P9>, + pin_10: PeripheralRef<'d, P10>, + pin_11: PeripheralRef<'d, P11>, + pin_12: PeripheralRef<'d, P12>, + pin_13: PeripheralRef<'d, P13>, + pin_14: PeripheralRef<'d, P14>, + pin_15: PeripheralRef<'d, P15>, +} + +impl<'d, P0, P1, P2, P3, P4, P5, P6, P7, P8, P9, P10, P11, P12, P13, P14, P15> + RxSixteenBits<'d, P0, P1, P2, P3, P4, P5, P6, P7, P8, P9, P10, P11, P12, P13, P14, P15> +where + P0: InputPin, + P1: InputPin, + P2: InputPin, + P3: InputPin, + P4: InputPin, + P5: InputPin, + P6: InputPin, + P7: InputPin, + P8: InputPin, + P9: InputPin, + P10: InputPin, + P11: InputPin, + P12: InputPin, + P13: InputPin, + P14: InputPin, + P15: InputPin, +{ + pub fn new( + pin_0: impl Peripheral

+ 'd, + pin_1: impl Peripheral

+ 'd, + pin_2: impl Peripheral

+ 'd, + pin_3: impl Peripheral

+ 'd, + pin_4: impl Peripheral

+ 'd, + pin_5: impl Peripheral

+ 'd, + pin_6: impl Peripheral

+ 'd, + pin_7: impl Peripheral

+ 'd, + pin_8: impl Peripheral

+ 'd, + pin_9: impl Peripheral

+ 'd, + pin_10: impl Peripheral

+ 'd, + pin_11: impl Peripheral

+ 'd, + pin_12: impl Peripheral

+ 'd, + pin_13: impl Peripheral

+ 'd, + pin_14: impl Peripheral

+ 'd, + pin_15: impl Peripheral

+ 'd, + ) -> Self { + crate::into_ref!(pin_0); + crate::into_ref!(pin_1); + crate::into_ref!(pin_2); + crate::into_ref!(pin_3); + crate::into_ref!(pin_4); + crate::into_ref!(pin_5); + crate::into_ref!(pin_6); + crate::into_ref!(pin_7); + crate::into_ref!(pin_8); + crate::into_ref!(pin_9); + crate::into_ref!(pin_10); + crate::into_ref!(pin_11); + crate::into_ref!(pin_12); + crate::into_ref!(pin_13); + crate::into_ref!(pin_14); + crate::into_ref!(pin_15); + + Self { + pin_0, + pin_1, + pin_2, + pin_3, + pin_4, + pin_5, + pin_6, + pin_7, + pin_8, + pin_9, + pin_10, + pin_11, + pin_12, + pin_13, + pin_14, + pin_15, + } + } +} + +impl<'d, P0, P1, P2, P3, P4, P5, P6, P7, P8, P9, P10, P11, P12, P13, P14, P15> RxPins + for RxSixteenBits<'d, P0, P1, P2, P3, P4, P5, P6, P7, P8, P9, P10, P11, P12, P13, P14, P15> +where + P0: InputPin, + P1: InputPin, + P2: InputPin, + P3: InputPin, + P4: InputPin, + P5: InputPin, + P6: InputPin, + P7: InputPin, + P8: InputPin, + P9: InputPin, + P10: InputPin, + P11: InputPin, + P12: InputPin, + P13: InputPin, + P14: InputPin, + P15: InputPin, +{ + type Word = u16; + fn configure(&mut self) { + self.pin_0 + .set_to_input() + .connect_input_to_peripheral(InputSignal::CAM_DATA_0); + self.pin_1 + .set_to_input() + .connect_input_to_peripheral(InputSignal::CAM_DATA_1); + self.pin_2 + .set_to_input() + .connect_input_to_peripheral(InputSignal::CAM_DATA_2); + self.pin_3 + .set_to_input() + .connect_input_to_peripheral(InputSignal::CAM_DATA_3); + self.pin_4 + .set_to_input() + .connect_input_to_peripheral(InputSignal::CAM_DATA_4); + self.pin_5 + .set_to_input() + .connect_input_to_peripheral(InputSignal::CAM_DATA_5); + self.pin_6 + .set_to_input() + .connect_input_to_peripheral(InputSignal::CAM_DATA_6); + self.pin_7 + .set_to_input() + .connect_input_to_peripheral(InputSignal::CAM_DATA_7); + self.pin_8 + .set_to_input() + .connect_input_to_peripheral(InputSignal::CAM_DATA_8); + self.pin_9 + .set_to_input() + .connect_input_to_peripheral(InputSignal::CAM_DATA_9); + self.pin_10 + .set_to_input() + .connect_input_to_peripheral(InputSignal::CAM_DATA_10); + self.pin_11 + .set_to_input() + .connect_input_to_peripheral(InputSignal::CAM_DATA_11); + self.pin_12 + .set_to_input() + .connect_input_to_peripheral(InputSignal::CAM_DATA_12); + self.pin_13 + .set_to_input() + .connect_input_to_peripheral(InputSignal::CAM_DATA_13); + self.pin_14 + .set_to_input() + .connect_input_to_peripheral(InputSignal::CAM_DATA_14); + self.pin_15 + .set_to_input() + .connect_input_to_peripheral(InputSignal::CAM_DATA_15); + } +} + +mod private { + pub trait RxPins { + type Word: Copy; + fn configure(&mut self); + } } diff --git a/esp-hal/src/lcd_cam/mod.rs b/esp-hal/src/lcd_cam/mod.rs index 9afcf996be0..527df73370b 100644 --- a/esp-hal/src/lcd_cam/mod.rs +++ b/esp-hal/src/lcd_cam/mod.rs @@ -31,7 +31,7 @@ impl<'d> LcdCam<'d> { lcd_cam: unsafe { lcd_cam.clone_unchecked() }, }, cam: Cam { - _lcd_cam: unsafe { lcd_cam.clone_unchecked() }, + lcd_cam: unsafe { lcd_cam.clone_unchecked() }, }, } } diff --git a/examples/src/bin/lcd_cam_ov2640.rs b/examples/src/bin/lcd_cam_ov2640.rs new file mode 100644 index 00000000000..158b37fe469 --- /dev/null +++ b/examples/src/bin/lcd_cam_ov2640.rs @@ -0,0 +1,422 @@ +//! Drives the camera on a Freenove ESP32-S3 WROOM +//! +//! This example reads a JPEG from an OV2640 and writes it to the console as hex. +//! +//! Pins used: +//! SIOD GPIO4 +//! SIOC GPIO5 +//! XCLK GPIO15 +//! VSYNC GPIO6 +//! HREF GPIO7 +//! PCLK GPIO13 +//! D0 GPIO11 +//! D1 GPIO9 +//! D2 GPIO8 +//! D3 GPIO10 +//! D4 GPIO12 +//! D5 GPIO18 +//! D6 GPIO17 +//! D7 GPIO16 + +//% CHIPS: esp32s3 + +#![no_std] +#![no_main] + +use esp_backtrace as _; +use esp_hal::{ + clock::ClockControl, + delay::Delay, + dma::{Dma, DmaPriority}, + dma_buffers, + gpio::IO, + i2c, + i2c::I2C, + lcd_cam::{ + cam::{Camera, RxEightBits}, + LcdCam, + }, + peripherals::Peripherals, + prelude::*, + Blocking, +}; +use esp_println::println; + +#[entry] +fn main() -> ! { + let peripherals = Peripherals::take(); + let system = peripherals.SYSTEM.split(); + let clocks = ClockControl::boot_defaults(system.clock_control).freeze(); + + let io = IO::new(peripherals.GPIO, peripherals.IO_MUX); + + let dma = Dma::new(peripherals.DMA); + let channel = dma.channel0; + + let (_, mut tx_descriptors, rx_buffer, mut rx_descriptors) = dma_buffers!(0, 32678); + + let channel = channel.configure( + false, + &mut tx_descriptors, + &mut rx_descriptors, + DmaPriority::Priority0, + ); + + let cam_siod = io.pins.gpio4; + let cam_sioc = io.pins.gpio5; + let cam_xclk = io.pins.gpio15; + let cam_vsync = io.pins.gpio6; + let cam_href = io.pins.gpio7; + let cam_pclk = io.pins.gpio13; + let cam_data_pins = RxEightBits::new( + io.pins.gpio11, + io.pins.gpio9, + io.pins.gpio8, + io.pins.gpio10, + io.pins.gpio12, + io.pins.gpio18, + io.pins.gpio17, + io.pins.gpio16, + ); + + let lcd_cam = LcdCam::new(peripherals.LCD_CAM); + let mut camera = Camera::new(lcd_cam.cam, channel.rx, cam_data_pins, 20u32.MHz(), &clocks) + .with_master_clock(cam_xclk) + .with_ctrl_pins(cam_vsync, cam_href, cam_pclk); + + let delay = Delay::new(&clocks); + + let mut buffer = rx_buffer; + buffer.fill(0u8); + + delay.delay_millis(500u32); + + let i2c = I2C::new( + peripherals.I2C0, + cam_siod, + cam_sioc, + 100u32.kHz(), + &clocks, + None, + ); + + let mut sccb = Sccb::new(i2c); + + // Checking camera address + sccb.probe(OV2640_ADDRESS).unwrap(); + println!("Probe successful!"); + + sccb.write(OV2640_ADDRESS, 0xFF, 0x01).unwrap(); // bank sensor + let pid = sccb.read(OV2640_ADDRESS, 0x0A).unwrap(); + println!("Found PID of {:#02X}, and was expecting 0x26", pid); + + // Start waiting for camera before initialising it to prevent missing the first few bytes. + // This can be improved with a VSYNC interrupt but would complicate this example. + let transfer = camera.read_dma(&mut buffer).unwrap(); + + for (reg, value) in FIRST_BLOCK { + sccb.write(OV2640_ADDRESS, *reg, *value).unwrap(); + } + delay.delay_millis(10u32); + for (reg, value) in SECOND_BLOCK { + sccb.write(OV2640_ADDRESS, *reg, *value).unwrap(); + if *reg == 0xDD && *value == 0x7F { + delay.delay_millis(10u32); + } + } + + transfer.wait().unwrap(); + + // Note: JPEGs starts with "FF, D8, FF, E0" and end with "FF, D9" + + let index_of_end = buffer.windows(2).position(|c| c[0] == 0xFF && c[1] == 0xD9); + let index_of_end = if let Some(idx) = index_of_end { + idx + 2 + } else { + println!("Failed to find JPEG terminator"); + buffer.len() + }; + + println!("Frame data (parse with `xxd -r -p .txt image.jpg`):"); + println!("{:02X?}", &buffer[..index_of_end]); + + loop {} +} + +pub const OV2640_ADDRESS: u8 = 0x30; + +pub struct Sccb<'d, T> { + i2c: I2C<'d, T, Blocking>, +} + +impl<'d, T> Sccb<'d, T> +where + T: i2c::Instance, +{ + pub fn new(i2c: I2C<'d, T, Blocking>) -> Self { + Self { i2c } + } + + pub fn probe(&mut self, address: u8) -> Result<(), i2c::Error> { + self.i2c.write(address, &[]) + } + + pub fn read(&mut self, address: u8, reg: u8) -> Result { + self.i2c.write(address, &[reg])?; + + let mut bytes = [0u8; 1]; + self.i2c.read(address, &mut bytes)?; + Ok(bytes[0]) + } + + pub fn write(&mut self, address: u8, reg: u8, data: u8) -> Result<(), i2c::Error> { + self.i2c.write(address, &[reg, data]) + } +} + +const FIRST_BLOCK: &[(u8, u8)] = &[(0xFF, 0x01), (0x12, 0x80)]; + +const SECOND_BLOCK: &[(u8, u8)] = &[ + (0xFF, 0x00), + (0x2C, 0xFF), + (0x2E, 0xDF), + (0xFF, 0x01), + (0x3C, 0x32), + (0x11, 0x01), + (0x09, 0x02), + (0x04, 0x28), + (0x13, 0xE5), + (0x14, 0x48), + (0x2C, 0x0C), + (0x33, 0x78), + (0x3A, 0x33), + (0x3B, 0xFB), + (0x3E, 0x00), + (0x43, 0x11), + (0x16, 0x10), + (0x39, 0x92), + (0x35, 0xDA), + (0x22, 0x1A), + (0x37, 0xC3), + (0x23, 0x00), + (0x34, 0xC0), + (0x06, 0x88), + (0x07, 0xC0), + (0x0D, 0x87), + (0x0E, 0x41), + (0x4C, 0x00), + (0x4A, 0x81), + (0x21, 0x99), + (0x24, 0x40), + (0x25, 0x38), + (0x26, 0x82), + (0x5C, 0x00), + (0x63, 0x00), + (0x61, 0x70), + (0x62, 0x80), + (0x7C, 0x05), + (0x20, 0x80), + (0x28, 0x30), + (0x6C, 0x00), + (0x6D, 0x80), + (0x6E, 0x00), + (0x70, 0x02), + (0x71, 0x94), + (0x73, 0xC1), + (0x3D, 0x34), + (0x5A, 0x57), + (0x4F, 0xBB), + (0x50, 0x9C), + (0x12, 0x20), + (0x17, 0x11), + (0x18, 0x43), + (0x19, 0x00), + (0x1A, 0x25), + (0x32, 0x89), + (0x37, 0xC0), + (0x4F, 0xCA), + (0x50, 0xA8), + (0x6D, 0x00), + (0x3D, 0x38), + (0xFF, 0x00), + (0xE5, 0x7F), + (0xF9, 0xC0), + (0x41, 0x24), + (0xE0, 0x14), + (0x76, 0xFF), + (0x33, 0xA0), + (0x42, 0x20), + (0x43, 0x18), + (0x4C, 0x00), + (0x87, 0x50), + (0x88, 0x3F), + (0xD7, 0x03), + (0xD9, 0x10), + (0xD3, 0x82), + (0xC8, 0x08), + (0xC9, 0x80), + (0x7C, 0x00), + (0x7D, 0x00), + (0x7C, 0x03), + (0x7D, 0x48), + (0x7D, 0x48), + (0x7C, 0x08), + (0x7D, 0x20), + (0x7D, 0x10), + (0x7D, 0x0E), + (0x90, 0x00), + (0x91, 0x0E), + (0x91, 0x1A), + (0x91, 0x31), + (0x91, 0x5A), + (0x91, 0x69), + (0x91, 0x75), + (0x91, 0x7E), + (0x91, 0x88), + (0x91, 0x8F), + (0x91, 0x96), + (0x91, 0xA3), + (0x91, 0xAF), + (0x91, 0xC4), + (0x91, 0xD7), + (0x91, 0xE8), + (0x91, 0x20), + (0x92, 0x00), + (0x93, 0x06), + (0x93, 0xE3), + (0x93, 0x05), + (0x93, 0x05), + (0x93, 0x00), + (0x93, 0x04), + (0x93, 0x00), + (0x93, 0x00), + (0x93, 0x00), + (0x93, 0x00), + (0x93, 0x00), + (0x93, 0x00), + (0x93, 0x00), + (0x96, 0x00), + (0x97, 0x08), + (0x97, 0x19), + (0x97, 0x02), + (0x97, 0x0C), + (0x97, 0x24), + (0x97, 0x30), + (0x97, 0x28), + (0x97, 0x26), + (0x97, 0x02), + (0x97, 0x98), + (0x97, 0x80), + (0x97, 0x00), + (0x97, 0x00), + (0xA4, 0x00), + (0xA8, 0x00), + (0xC5, 0x11), + (0xC6, 0x51), + (0xBF, 0x80), + (0xC7, 0x10), + (0xB6, 0x66), + (0xB8, 0xA5), + (0xB7, 0x64), + (0xB9, 0x7C), + (0xB3, 0xAF), + (0xB4, 0x97), + (0xB5, 0xFF), + (0xB0, 0xC5), + (0xB1, 0x94), + (0xB2, 0x0F), + (0xC4, 0x5C), + (0xC3, 0xFD), + (0x7F, 0x00), + (0xE5, 0x1F), + (0xE1, 0x67), + (0xDD, 0x7F), + (0xDA, 0x00), + (0xE0, 0x00), + (0x05, 0x00), + (0x05, 0x01), + (0xFF, 0x01), + (0x12, 0x40), + (0x03, 0x0A), + (0x32, 0x09), + (0x17, 0x11), + (0x18, 0x43), + (0x19, 0x00), + (0x1A, 0x4B), + (0x37, 0xC0), + (0x4F, 0xCA), + (0x50, 0xA8), + (0x5A, 0x23), + (0x6D, 0x00), + (0x3D, 0x38), + (0x39, 0x92), + (0x35, 0xDA), + (0x22, 0x1A), + (0x37, 0xC3), + (0x23, 0x00), + (0x34, 0xC0), + (0x06, 0x88), + (0x07, 0xC0), + (0x0D, 0x87), + (0x0E, 0x41), + (0x42, 0x03), + (0x4C, 0x00), + (0xFF, 0x00), + (0xE0, 0x04), + (0xC0, 0x64), + (0xC1, 0x4B), + (0x8C, 0x00), + (0x51, 0xC8), + (0x52, 0x96), + (0x53, 0x00), + (0x54, 0x00), + (0x55, 0x00), + (0x57, 0x00), + (0x86, 0x3D), + (0x50, 0x80), + (0x51, 0xC8), + (0x52, 0x96), + (0x53, 0x00), + (0x54, 0x00), + (0x55, 0x00), + (0x57, 0x00), + (0x5A, 0xA0), + (0x5B, 0x78), + (0x5C, 0x00), + (0xFF, 0x01), + (0x11, 0x00), + (0xFF, 0x00), + (0xD3, 0x10), + (0x05, 0x00), + (0xE0, 0x14), + (0xDA, 0x12), + (0xD7, 0x03), + (0xE1, 0x77), + (0xE5, 0x1F), + (0xD9, 0x10), + (0xDF, 0x80), + (0x33, 0x80), + (0x3C, 0x10), + (0xEB, 0x30), + (0xDD, 0x7F), + (0xE0, 0x00), + (0xE0, 0x14), + (0xDA, 0x12), + (0xD7, 0x03), + (0xE1, 0x77), + (0xE5, 0x1F), + (0xD9, 0x10), + (0xDF, 0x80), + (0x33, 0x80), + (0x3C, 0x10), + (0xEB, 0x30), + (0xDD, 0x7F), + (0xE0, 0x00), + (0xFF, 0x01), + (0x14, 0x08), + (0xFF, 0x00), + (0x87, 0x50), + (0x87, 0x10), + (0xC3, 0xFD), + (0x44, 0x0C), +];