diff --git a/doc/onboard.md b/doc/onboard.md
index 4a68cca..141de3b 100644
--- a/doc/onboard.md
+++ b/doc/onboard.md
@@ -30,7 +30,7 @@ Maix Go
 
 - MSA300 - Accelerometer (I2C, address 0x26)
 - STM32F103C8 - JTAG & UART, debug M1 without extra Jlink, this bypasses the CH340C on the module (USB to host, serial to K210)
-- OV2640 - Color CMOS UXGA (2.0 MegaPixel) CAMERA C HI (DVP)
+- OV2640 - Color CMOS UXGA (2.0 MegaPixel) CAMERA C HI (DVP, SCCB address 0x60)
 
 ### External
 
diff --git a/rust/k210-shared/src/board.rs b/rust/k210-shared/src/board.rs
index 7fbc030..1406c34 100644
--- a/rust/k210-shared/src/board.rs
+++ b/rust/k210-shared/src/board.rs
@@ -4,3 +4,4 @@ pub mod lcd_colors;
 pub mod lcd_render;
 pub mod msa300;
 pub mod ns2009;
+pub mod ov2640;
diff --git a/rust/k210-shared/src/board/def.rs b/rust/k210-shared/src/board/def.rs
index b470a45..266d5d8 100644
--- a/rust/k210-shared/src/board/def.rs
+++ b/rust/k210-shared/src/board/def.rs
@@ -26,6 +26,9 @@ pub const MSA300_ADDR_BITS: u32 = 7;
 /** I2C clock speed for MSA300 */
 pub const MSA300_CLK: u32 = 500000;
 
+/** DVP SCCB address for OV2640 */
+pub const OV2640_ADDR: u8 = 0x60;
+
 /** I/O pins for FPIOA */
 #[derive(Copy, Clone)]
 #[repr(u32)]
diff --git a/rust/k210-shared/src/board/ov2640.rs b/rust/k210-shared/src/board/ov2640.rs
new file mode 100644
index 0000000..e6e625c
--- /dev/null
+++ b/rust/k210-shared/src/board/ov2640.rs
@@ -0,0 +1,217 @@
+use crate::soc::dvp::DVP;
+use crate::board::def::OV2640_ADDR;
+
+/** Initial configuration for OV2640 chip */
+static CONFIG: &[(u8, u8)] = &[
+    (0xff, 0x01), // Bank: Sensor Address
+    (0x12, 0x80), // COM7 SRST: Initiates system reset
+
+    (0xff, 0x00), // Bank: DSP Address
+    (0x2c, 0xff), // ??
+    (0x2e, 0xdf), // ??
+
+    (0xff, 0x01), // Bank: Sensor Address
+    (0x3c, 0x32), // ??
+    (0x11, 0x00),
+    (0x09, 0x02),
+    (0x04, 0x88),
+    (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),
+    (0x36, 0x1a),
+    (0x06, 0x88),
+    (0x07, 0xc0),
+    (0x0d, 0x87),
+    (0x0e, 0x41),
+    (0x4c, 0x00),
+    (0x48, 0x00),
+    (0x5b, 0x00),
+    (0x42, 0x03),
+    (0x4a, 0x81),
+    (0x21, 0x99),
+    (0x24, 0x40),
+    (0x25, 0x38),
+    (0x26, 0x82),
+    (0x5c, 0x00),
+    (0x63, 0x00),
+    (0x46, 0x22),
+    (0x0c, 0x3c),
+    (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),
+    (0x12, 0x40),
+    (0x17, 0x11),
+    (0x18, 0x43),
+    (0x19, 0x00),
+    (0x1a, 0x4b),
+    (0x32, 0x09),
+    (0x37, 0xc0),
+    (0x4f, 0xca),
+    (0x50, 0xa8),
+    (0x5a, 0x23),
+    (0x6d, 0x00),
+    (0x3d, 0x38),
+
+    (0xff, 0x00), // Bank: DSP Address
+    (0xe5, 0x7f),
+    (0xf9, 0xc0),
+    (0x41, 0x24),
+    (0xe0, 0x14),
+    (0x76, 0xff),
+    (0x33, 0xa0),
+    (0x42, 0x20),
+    (0x43, 0x18),
+    (0x4c, 0x00),
+    (0x87, 0xd5),
+    (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),
+    (0xc3, 0xed),
+    (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),
+    (0xc0, 0x64),
+    (0xc1, 0x4b),
+    (0x8c, 0x00),
+    (0x86, 0x3d),
+    (0x50, 0x00),
+    (0x51, 0xc8),
+    (0x52, 0x96),
+    (0x53, 0x00),
+    (0x54, 0x00),
+    (0x55, 0x00),
+    (0x5a, 0xc8),
+    (0x5b, 0x96),
+    (0x5c, 0x00),
+    (0xd3, 0x02),
+    (0xc3, 0xed),
+    (0x7f, 0x00),
+    (0xda, 0x08),
+    (0xe5, 0x1f),
+    (0xe1, 0x67),
+    (0xe0, 0x00),
+    (0xdd, 0x7f),
+    (0x05, 0x00),
+
+    (0xff, 0x00), // Bank: DSP Address
+    (0xe0, 0x04),
+    (0x5a, 0x50),
+    (0x5b, 0x3c),
+    (0x5c, 0x00),
+    (0xe0, 0x00),
+];
+
+pub fn ov2640_init(dvp: &DVP) {
+    let (_manuf_id, _device_id) = read_id(dvp);
+    // TODO: do something with the IDs
+    // printf("manuf_id:0x%04x,device_id:0x%04x\n", v_manuf_id, v_device_id);
+    for &(register, value) in CONFIG {
+        dvp.sccb_send_data(OV2640_ADDR, register.into(), value.into());
+    }
+}
+
+/** Return (manuf_id, device_id) */
+pub fn read_id(dvp: &DVP) -> (u16, u16) {
+    // 0xFF RA_DLMT - Register Bank Select: Sensor address
+    dvp.sccb_send_data(OV2640_ADDR, 0xFF, 0x01);
+    // 0x1C MIDH - Manufacturer ID Byte – High (Read only = 0x7F)
+    // 0x1D MIDL - Manufacturer ID Byte – Low (Read only = 0xA2)
+    let manuf_id = ((dvp.sccb_receive_data(OV2640_ADDR, 0x1C) as u16) << 8) | (dvp.sccb_receive_data(OV2640_ADDR, 0x1D) as u16);
+    // 0x0A PIDH - Product ID Number MSB (Read only)
+    // 0x0B PIDL - Product ID Number LSB (Read only)
+    let device_id = ((dvp.sccb_receive_data(OV2640_ADDR, 0x0A) as u16) << 8) | (dvp.sccb_receive_data(OV2640_ADDR, 0x0B) as u16);
+    (manuf_id, device_id)
+}
diff --git a/rust/k210-shared/src/soc.rs b/rust/k210-shared/src/soc.rs
index 504496b..963e07d 100644
--- a/rust/k210-shared/src/soc.rs
+++ b/rust/k210-shared/src/soc.rs
@@ -1,3 +1,4 @@
+pub mod dvp;
 pub mod fpioa;
 pub mod gpio;
 pub mod gpiohs;
diff --git a/rust/k210-shared/src/soc/dvp.rs b/rust/k210-shared/src/soc/dvp.rs
new file mode 100644
index 0000000..76b4f15
--- /dev/null
+++ b/rust/k210-shared/src/soc/dvp.rs
@@ -0,0 +1,343 @@
+use core::cmp;
+use k210_hal::pac;
+use pac::dvp;
+
+use crate::soc::sleep::usleep;
+use crate::soc::sysctl;
+
+pub trait DVPExt: Sized {
+    /// Constrains DVP peripheral
+    fn constrain(self) -> DVP;
+}
+
+impl DVPExt for pac::DVP {
+    fn constrain(self) -> DVP { DVP { dvp: self, sccb_addr_len: sccb_addr_len::W8 } }
+}
+
+/** Output mode (RGB565 for display, or for planar for AI) */
+#[derive(Copy, Clone)]
+pub enum output_mode {
+    AI,
+    DISPLAY,
+}
+
+/** SCCB register address width */
+#[derive(Copy, Clone)]
+pub enum sccb_addr_len {
+    W8,
+    W16,
+}
+
+/** Interrupt */
+#[derive(Copy, Clone)]
+pub enum interrupt {
+    frame_start,
+    frame_finish,
+}
+
+pub struct DVP {
+    dvp: pac::DVP,
+    sccb_addr_len: sccb_addr_len,
+}
+
+pub type image_format = dvp::dvp_cfg::FORMATW;
+
+impl DVP {
+    /** Set SCCB clock to a safe and deterministic value (as low as possible) */
+    fn sccb_clk_init(&self) {
+        unsafe {
+            self.dvp.sccb_cfg.modify(|_,w|
+                w.scl_lcnt().bits(255)
+                 .scl_hcnt().bits(255)
+            );
+        }
+    }
+
+    /** Set SCCB clock rate */
+    pub fn sccb_set_clk_rate(&self, clk_rate: u32) -> u32 {
+        let v_sccb_freq = sysctl::clock_get_freq(sysctl::clock::APB1);
+        let v_period_clk_cnt = v_sccb_freq / clk_rate / 2; // TODO: round() i.s.o. truncate?
+
+        if v_period_clk_cnt > 255 {
+            return 0;
+        }
+        unsafe {
+            self.dvp.sccb_cfg.modify(|_,w|
+                w.scl_lcnt().bits(v_period_clk_cnt as u8)
+                 .scl_hcnt().bits(v_period_clk_cnt as u8)
+            );
+        }
+        // confused: why does this use clock::DVP but the period_clk_cnt uses clock::APB1?
+        return sysctl::clock_get_freq(sysctl::clock::DVP) / (v_period_clk_cnt * 2);
+    }
+
+    /** Perform, and wait for a SCCB transfer (read or write) */
+    fn sccb_start_transfer(&self) {
+        while self.dvp.sts.read().sccb_en().bit() {
+            // IDLE
+        }
+        self.dvp.sts.write(|w| w.sccb_en().set_bit()
+                                .sccb_en_we().set_bit());
+        while self.dvp.sts.read().sccb_en().bit() {
+            // IDLE
+        }
+    }
+
+    /** Set a register value through SCCB */
+    pub fn sccb_send_data(&self, dev_addr: u8, reg_addr: u16, reg_data: u8) {
+        use dvp::sccb_cfg::BYTE_NUMW::*;
+        unsafe {
+            match self.sccb_addr_len {
+                sccb_addr_len::W8 => {
+                    self.dvp.sccb_cfg.modify(|_,w| w.byte_num().variant(NUM3));
+                    self.dvp.sccb_ctl.write(|w| w.device_address().bits(dev_addr | 1)
+                                             .reg_address().bits(reg_addr as u8)
+                                             .wdata_byte0().bits(reg_data));
+                },
+                sccb_addr_len::W16 => {
+                    self.dvp.sccb_cfg.modify(|_,w| w.byte_num().variant(NUM4));
+                    self.dvp.sccb_ctl.write(|w| w.device_address().bits(dev_addr | 1)
+                                             .reg_address().bits((reg_addr >> 8) as u8)
+                                             .wdata_byte0().bits((reg_addr & 0xff) as u8)
+                                             .wdata_byte1().bits(reg_data));
+                },
+            }
+        }
+        self.sccb_start_transfer();
+    }
+
+    /** Receive register value through SCCB */
+    pub fn sccb_receive_data(&self, dev_addr: u8, reg_addr: u16) -> u8 {
+        // Write read request
+        use dvp::sccb_cfg::BYTE_NUMW::*;
+        unsafe {
+            match self.sccb_addr_len {
+                sccb_addr_len::W8 => {
+                    self.dvp.sccb_cfg.modify(|_,w| w.byte_num().variant(NUM2));
+                    self.dvp.sccb_ctl.write(|w| w.device_address().bits(dev_addr | 1)
+                                             .reg_address().bits(reg_addr as u8));
+                },
+                sccb_addr_len::W16 => {
+                    self.dvp.sccb_cfg.modify(|_,w| w.byte_num().variant(NUM3));
+                    self.dvp.sccb_ctl.write(|w| w.device_address().bits(dev_addr | 1)
+                                             .reg_address().bits((reg_addr >> 8) as u8)
+                                             .wdata_byte0().bits((reg_addr & 0xff) as u8));
+                },
+            }
+        }
+        self.sccb_start_transfer();
+        // Start read transfer
+        unsafe { self.dvp.sccb_ctl.write(|w| w.device_address().bits(dev_addr)); }
+        self.sccb_start_transfer();
+        self.dvp.sccb_cfg.read().rdata().bits()
+    }
+
+    /** Reset DVP-connected device */
+    fn reset(&self) {
+        /* First power down */
+        self.dvp.cmos_cfg.modify(|_,w| w.power_down().set_bit());
+        usleep(200_000);
+        self.dvp.cmos_cfg.modify(|_,w| w.power_down().clear_bit());
+        usleep(200_000);
+
+        /* Second reset */
+        self.dvp.cmos_cfg.modify(|_,w| w.reset().clear_bit());
+        usleep(200_000);
+        self.dvp.cmos_cfg.modify(|_,w| w.reset().set_bit());
+        usleep(200_000);
+    }
+
+    /** Initialize DVP peripheral */
+    pub fn init(&mut self, sccb_addr_len: sccb_addr_len) {
+        self.sccb_addr_len = sccb_addr_len;
+        sysctl::clock_enable(sysctl::clock::DVP);
+        sysctl::reset(sysctl::reset::DVP);
+        // Set XCLK to hardcoded divider (3+1)*2=8
+        unsafe {
+            self.dvp.cmos_cfg.modify(|_,w| w.clk_div().bits(3)
+                                            .clk_enable().set_bit());
+        }
+        self.sccb_clk_init();
+        self.reset();
+    }
+
+    /** Set XCLK clock rate */
+    pub fn set_xclk_rate(&self, xclk_rate: u32) -> u32 {
+        // Taken directly from SDK: it's strange that this clock is relative to APB1 not DVP clock
+        let v_apb1_clk = sysctl::clock_get_freq(sysctl::clock::APB1);
+        let v_period = if v_apb1_clk > xclk_rate * 2 {
+            cmp::min((v_apb1_clk / (xclk_rate * 2)) - 1, 255) // TODO round instead of trunc?
+        } else {
+            0
+        };
+        unsafe {
+            self.dvp.cmos_cfg.modify(|_,w| w.clk_div().bits(v_period as u8)
+                                            .clk_enable().set_bit());
+        }
+        self.reset();
+        v_apb1_clk / ((v_period + 1) * 2)
+    }
+
+    /** Set input image format */
+    pub fn set_image_format(&self, format: image_format) {
+        self.dvp.dvp_cfg.modify(|_,w| w.format().variant(format));
+    }
+
+    // Not sure it's consistent interface to have two separate functions here, but just going with
+    // the SDK for now... might even want to merge these into set_image_size, or force configuring
+    // the entire peripheral (register size, burst mode, image size) at once
+    /** Enable burst mode */
+    pub fn enable_burst(&self) {
+        self.dvp.dvp_cfg.modify(|_,w| w.burst_size_4beats().set_bit());
+        self.dvp.axi.modify(|_,w| w.gm_mlen().variant(dvp::axi::GM_MLENW::BYTE4));
+    }
+
+    /** Disable burst mode */
+    pub fn disable_burst(&self) {
+        self.dvp.dvp_cfg.modify(|_,w| w.burst_size_4beats().clear_bit());
+        self.dvp.axi.modify(|_,w| w.gm_mlen().variant(dvp::axi::GM_MLENW::BYTE1));
+    }
+
+    /** Set image size. If burst mode is enabled the maximum configurable size is
+     * 8160x1023, without burst mode it is 2040x1023.
+     */
+    pub fn set_image_size(&self, width: u16, height: u16) {
+        // Note: this uses state written in enable/disable_burst, so that needs to be configured before this
+        let burst_num = if self.dvp.dvp_cfg.read().burst_size_4beats().bit() {
+            width / 8 / 4
+        } else {
+            width / 8 / 1
+        };
+        assert!(burst_num < 256);
+        assert!(height < 1024);
+        unsafe {
+            self.dvp.dvp_cfg.modify(|_,w| w.href_burst_num().bits(burst_num as u8)
+                                           .line_num().bits(height));
+        }
+    }
+
+    /** Set address for planar RGB output */
+    pub fn set_ai_addr(&self, r_addr: *mut u8, g_addr: *mut u8, b_addr: *mut u8) {
+        // Makes use of the fact that
+        // a) physical memory is the same as virtual memory on the K210
+        // b) memory wraps around every 2^32
+        unsafe {
+            self.dvp.r_addr.write(|w| w.bits(((r_addr as usize) & 0xffffffff) as u32));
+            self.dvp.g_addr.write(|w| w.bits(((g_addr as usize) & 0xffffffff) as u32));
+            self.dvp.b_addr.write(|w| w.bits(((b_addr as usize) & 0xffffffff) as u32));
+        }
+    }
+
+    /** Set address for 16-bit R5G6B5 output */
+    pub fn set_display_addr(&self, addr: *mut u16) {
+        unsafe {
+            self.dvp.rgb_addr.write(|w| w.bits(((addr as usize) & 0xffffffff) as u32));
+        }
+    }
+
+    /** Start a frame */
+    pub fn start_frame(&self) {
+        while !self.dvp.sts.read().frame_start().bit() {
+            // IDLE
+        }
+        self.dvp.sts.write(|w| w.frame_start().set_bit()
+                                .frame_start_we().set_bit());
+    }
+
+    /** Start conversion of frame */
+    pub fn start_convert(&self) {
+        self.dvp.sts.write(|w| w.dvp_en().set_bit()
+                                .dvp_en_we().set_bit());
+    }
+
+    /** Finish conversion of frame */
+    pub fn finish_convert(&self) {
+        while !self.dvp.sts.read().frame_finish().bit() {
+            // IDLE
+        }
+        self.dvp.sts.write(|w| w.frame_finish().set_bit()
+                                .frame_finish_we().set_bit());
+    }
+
+    /** Wait for an entire frame to complete */
+    pub fn get_image(&self) {
+        while !self.dvp.sts.read().frame_start().bit() {
+            // IDLE
+        }
+        self.dvp.sts.write(|w| w.frame_start().set_bit()
+                                .frame_start_we().set_bit());
+        while !self.dvp.sts.read().frame_start().bit() {
+            // IDLE
+        }
+        self.dvp.sts.write(|w| w.frame_finish().set_bit()
+                                .frame_finish_we().set_bit()
+                                .frame_start().set_bit()
+                                .frame_start_we().set_bit()
+                                .dvp_en().set_bit()
+                                .dvp_en_we().set_bit());
+        while !self.dvp.sts.read().frame_finish().bit() {
+            // IDLE
+        }
+    }
+
+    /** Configure interrupt */
+    pub fn config_interrupt(&self, interrupt: interrupt, enable: bool) {
+        match interrupt {
+            interrupt::frame_start => {
+                self.dvp.dvp_cfg.modify(|_,w| w.start_int_enable().bit(enable));
+            }
+            interrupt::frame_finish => {
+                self.dvp.dvp_cfg.modify(|_,w| w.finish_int_enable().bit(enable));
+            }
+        }
+    }
+
+    /** Get status of an interrupt */
+    pub fn get_interrupt(&self, interrupt: interrupt) -> bool {
+        let sts = self.dvp.sts.read();
+        match interrupt {
+            interrupt::frame_start => { sts.frame_start().bit() }
+            interrupt::frame_finish => { sts.frame_finish().bit() }
+        }
+    }
+
+    /** Clear an interrupt */
+    pub fn clear_interrupt(&self, interrupt: interrupt) {
+        match interrupt {
+            interrupt::frame_start => {
+                self.dvp.sts.modify(|_,w| w.frame_start().set_bit()
+                                           .frame_start_we().set_bit());
+            }
+            interrupt::frame_finish => {
+                self.dvp.sts.modify(|_,w| w.frame_finish().set_bit()
+                                           .frame_finish_we().set_bit());
+            }
+        }
+    }
+
+    /** Enable automatic frame mode */
+    pub fn enable_auto(&self) {
+        self.dvp.dvp_cfg.modify(|_,w| w.auto_enable().set_bit());
+    }
+
+    /** Disable automatic frame mode */
+    pub fn disable_auto(&self) {
+        self.dvp.dvp_cfg.modify(|_,w| w.auto_enable().clear_bit());
+    }
+
+    // The following function could be merged into setting the address?
+    // set_display_addr / set_ai_addr with an Option maybe?
+    /** Enable/disable an output */
+    pub fn set_output_enable(&self, index: output_mode, enable: bool) {
+        match index {
+            output_mode::AI => {
+                self.dvp.dvp_cfg.modify(|_,w| w.ai_output_enable().bit(enable));
+            }
+            output_mode::DISPLAY => {
+                self.dvp.dvp_cfg.modify(|_,w| w.display_output_enable().bit(enable));
+            }
+        }
+    }
+
+}
+