moved other drivers to drv/

19 files changed, 2744 insertions, 0 deletions

diff --git a/fw/fe310/eos/dev/drv/apds9151.c b/fw/fe310/eos/dev/drv/apds9151.c
new file mode 100644
index 0000000..a96b70b
--- /dev/null
+++ b/fw/fe310/eos/dev/drv/apds9151.c
@@ -0,0 +1,119 @@
+#include <stdlib.h>
+#include <stdint.h>
+
+#include "eos.h"
+#include "soc/i2c.h"
+
+#include "apds9151.h"
+
+#define APDS9151_ADDR                   0x52
+
+static int reg_read8(uint8_t reg, uint8_t *data) {
+    int rv;
+
+    rv = eos_i2c_read8(APDS9151_ADDR, reg, data, 1);
+    return rv;
+}
+
+static int reg_write8(uint8_t reg, uint8_t data) {
+    int rv;
+
+    rv = eos_i2c_write8(APDS9151_ADDR, reg, &data, 1);
+    return rv;
+}
+
+static int reg_read16(uint8_t reg, uint16_t *data) {
+    int rv;
+    uint8_t b[2];
+
+    rv = eos_i2c_read8(APDS9151_ADDR, reg, b, 2);
+    if (rv) return rv;
+
+    *data = \
+        ((uint16_t)b[0]) | \
+        ((uint16_t)b[1] << 8);
+    return EOS_OK;
+}
+
+static int reg_write16(uint8_t reg, uint16_t data) {
+    int rv;
+    uint8_t b[2];
+
+    b[0] = data;
+    b[1] = data >> 8;
+    rv = eos_i2c_write8(APDS9151_ADDR, reg, b, 2);
+    if (rv) return rv;
+
+    return EOS_OK;
+}
+
+static int reg_read32(uint8_t reg, uint32_t *data) {
+    int rv;
+    uint8_t b[3];
+
+    rv = eos_i2c_read8(APDS9151_ADDR, reg, b, 3);
+    if (rv) return rv;
+
+    *data = \
+        ((uint32_t)b[0]) | \
+        ((uint32_t)b[1] << 8) | \
+        ((uint32_t)b[2] << 16);
+    return EOS_OK;
+}
+
+static int reg_write32(uint8_t reg, uint32_t data) {
+    int rv;
+    uint8_t b[3];
+
+    b[0] = data;
+    b[1] = data >> 8;
+    b[2] = data >> 16;
+    rv = eos_i2c_write8(APDS9151_ADDR, reg, b, 3);
+    if (rv) return rv;
+
+    return EOS_OK;
+}
+
+int apds9151_reg_read(uint8_t reg, void *data) {
+    int rv = EOS_OK;
+
+    switch (reg & (~APDS9151_REG_MASK)) {
+        case APDS9151_REG_SIZE_8: {
+            rv = reg_read8(reg & APDS9151_REG_MASK, (uint8_t *)data);
+            break;
+        }
+        case APDS9151_REG_SIZE_16: {
+            rv = reg_read16(reg & APDS9151_REG_MASK, (uint16_t *)data);
+            break;
+        }
+        case APDS9151_REG_SIZE_32: {
+            rv = reg_read32(reg & APDS9151_REG_MASK, (uint32_t *)data);
+            break;
+        }
+        default: return EOS_ERR;
+    }
+
+    return rv;
+}
+
+int apds9151_reg_write(uint8_t reg, uint32_t data) {
+    int rv = EOS_OK;
+
+    switch (reg & (~APDS9151_REG_MASK)) {
+        case APDS9151_REG_SIZE_8: {
+            rv = reg_write8(reg & APDS9151_REG_MASK, (uint8_t)data);
+            break;
+        }
+        case APDS9151_REG_SIZE_16: {
+            rv = reg_write16(reg & APDS9151_REG_MASK, (uint16_t)data);
+            break;
+        }
+        case APDS9151_REG_SIZE_32: {
+            rv = reg_write32(reg & APDS9151_REG_MASK, (uint32_t)data);
+            break;
+        }
+        default: return EOS_ERR;
+    }
+
+    return rv;
+}
diff --git a/fw/fe310/eos/dev/drv/apds9151.h b/fw/fe310/eos/dev/drv/apds9151.h
new file mode 100644
index 0000000..af0be4c
--- /dev/null
+++ b/fw/fe310/eos/dev/drv/apds9151.h
@@ -0,0 +1,32 @@
+#include <stdint.h>
+
+#define APDS9151_REG_SIZE_8             0x00
+#define APDS9151_REG_SIZE_16            0x40
+#define APDS9151_REG_SIZE_32            0x80
+
+#define APDS9151_REG_MAIN_CTRL          (0x00 | APDS9151_REG_SIZE_8)
+#define APDS9151_REG_PS_LED             (0x01 | APDS9151_REG_SIZE_8)
+#define APDS9151_REG_PS_PULSES          (0x02 | APDS9151_REG_SIZE_8)
+#define APDS9151_REG_PS_MEAS_RATE       (0x03 | APDS9151_REG_SIZE_8)
+#define APDS9151_REG_LS_MEAS_RATE       (0x04 | APDS9151_REG_SIZE_8)
+#define APDS9151_REG_LS_GAIN            (0x05 | APDS9151_REG_SIZE_8)
+#define APDS9151_REG_PART_ID            (0x06 | APDS9151_REG_SIZE_8)
+#define APDS9151_REG_MAIN_STATUS        (0x07 | APDS9151_REG_SIZE_8)
+#define APDS9151_REG_PS_DATA            (0x08 | APDS9151_REG_SIZE_16)
+#define APDS9151_REG_LS_DATA_IR         (0x0A | APDS9151_REG_SIZE_32)
+#define APDS9151_REG_LS_DATA_GREEN      (0x0D | APDS9151_REG_SIZE_32)
+#define APDS9151_REG_LS_DATA_BLUE       (0x10 | APDS9151_REG_SIZE_32)
+#define APDS9151_REG_LS_DATA_RED        (0x13 | APDS9151_REG_SIZE_32)
+#define APDS9151_REG_INT_CFG            (0x19 | APDS9151_REG_SIZE_8)
+#define APDS9151_REG_INT_PST            (0x1A | APDS9151_REG_SIZE_8)
+#define APDS9151_REG_PS_THRES_UP        (0x1B | APDS9151_REG_SIZE_16)
+#define APDS9151_REG_PS_THRES_LOW       (0x1D | APDS9151_REG_SIZE_16)
+#define APDS9151_REG_PS_CAN             (0x1F | APDS9151_REG_SIZE_16)
+#define APDS9151_REG_LS_THRES_UP        (0x21 | APDS9151_REG_SIZE_32)
+#define APDS9151_REG_LS_THRES_LOW       (0x24 | APDS9151_REG_SIZE_32)
+#define APDS9151_REG_LS_THRES_VAR       (0x27 | APDS9151_REG_SIZE_8)
+
+#define APDS9151_REG_MASK               0x3F
+
+int apds9151_reg_read(uint8_t reg, void *data);
+int apds9151_reg_write(uint8_t reg, uint32_t data);
\ No newline at end of file
diff --git a/fw/fe310/eos/dev/drv/bq25895.c b/fw/fe310/eos/dev/drv/bq25895.c
new file mode 100644
index 0000000..6007598
--- /dev/null
+++ b/fw/fe310/eos/dev/drv/bq25895.c
@@ -0,0 +1,21 @@
+#include <stdlib.h>
+
+#include "eos.h"
+#include "soc/i2c.h"
+
+#include "bq25895.h"
+
+int bq25895_reg_read(uint8_t reg, uint8_t *data) {
+    return eos_i2c_read8(BQ25895_ADDR, reg, data, 1);
+}
+
+int bq25895_reg_write(uint8_t reg, uint8_t data) {
+    return eos_i2c_write8(BQ25895_ADDR, reg, &data, 1);
+}
+
+int bq25895_read_fault(uint8_t *fault) {
+    int rv;
+
+    rv = bq25895_reg_read(0x0c, fault);
+    return rv;
+}
diff --git a/fw/fe310/eos/dev/drv/bq25895.h b/fw/fe310/eos/dev/drv/bq25895.h
new file mode 100644
index 0000000..3fa8cfc
--- /dev/null
+++ b/fw/fe310/eos/dev/drv/bq25895.h
@@ -0,0 +1,8 @@
+#include <stdint.h>
+
+#define BQ25895_ADDR        0x6A
+
+int bq25895_init(uint8_t wakeup_cause);
+int bq25895_reg_read(uint8_t reg, uint8_t *data);
+int bq25895_reg_write(uint8_t reg, uint8_t data);
+int bq25895_read_fault(uint8_t *fault);
\ No newline at end of file
diff --git a/fw/fe310/eos/dev/drv/drv2605.h b/fw/fe310/eos/dev/drv/drv2605.h
new file mode 100644
index 0000000..fe90a9b
--- /dev/null
+++ b/fw/fe310/eos/dev/drv/drv2605.h
@@ -0,0 +1,3 @@
+#include <stdint.h>
+
+#define DRV2605_ADDR        0x5A
diff --git a/fw/fe310/eos/dev/drv/flash.c b/fw/fe310/eos/dev/drv/flash.c
new file mode 100644
index 0000000..4f017b3
--- /dev/null
+++ b/fw/fe310/eos/dev/drv/flash.c
@@ -0,0 +1,162 @@
+#include <stdio.h>
+#include <stdlib.h>
+
+#include "encoding.h"
+#include "platform.h"
+
+#include "eos.h"
+#include "soc/timer.h"
+
+#include "flash.h"
+
+#define IDLE_TICKS  10
+
+__attribute__ ((section (".itim.flash")))
+static void send(uint8_t data) {
+    while (SPI0_REG(SPI_REG_TXFIFO) & SPI_TXFIFO_FULL);
+    SPI0_REG(SPI_REG_TXFIFO) = data;
+}
+
+__attribute__ ((section (".itim.flash")))
+static uint8_t xfer(uint8_t data) {
+    volatile uint32_t x = 0;
+
+    send(data);
+    while ((x = SPI0_REG(SPI_REG_RXFIFO)) & SPI_RXFIFO_EMPTY);
+    return x;
+}
+
+void eos_flash_init(void) {
+    SPI0_REG(SPI_REG_FMT) |= SPI_FMT_DIR(SPI_DIR_TX);
+    SPI0_REG(SPI_REG_TXCTRL) = SPI_TXWM(1);
+    eos_flash_norm();
+}
+
+__attribute__ ((section (".itim.flash")))
+void eos_flash_norm(void) {
+    volatile uint64_t *mtime = (uint64_t *) (CLINT_CTRL_ADDR + CLINT_MTIME);
+    uint32_t mtime0;
+
+    clear_csr(mstatus, MSTATUS_MIE);
+    SPI0_REG(SPI_REG_FCTRL) = 0;
+
+    SPI0_REG(SPI_REG_SCKDIV) = 3;
+    if (SPI0_REG(SPI_REG_FMT) & SPI_FMT_PROTO(SPI_PROTO_Q)) {
+        send(EOS_FLASH_QPIDI);
+        while (!(SPI0_REG(SPI_REG_IP) & SPI_IP_TXWM));
+    }
+
+    SPI0_REG(SPI_REG_FMT) = \
+        SPI_FMT_PROTO(SPI_PROTO_S)      |
+        SPI_FMT_ENDIAN(SPI_ENDIAN_MSB)  |
+        SPI_FMT_DIR(SPI_DIR_TX)         |
+        SPI_FMT_LEN(8);
+
+    SPI0_REG(SPI_REG_FFMT) =
+        SPI_INSN_CMD_EN         |
+        SPI_INSN_ADDR_LEN(3)    |
+	    SPI_INSN_PAD_CNT(0)     |
+	    SPI_INSN_CMD_PROTO(SPI_PROTO_S)     |
+	    SPI_INSN_ADDR_PROTO(SPI_PROTO_S)    |
+	    SPI_INSN_DATA_PROTO(SPI_PROTO_S)    |
+	    SPI_INSN_CMD_CODE(EOS_FLASH_NORD)   |
+	    SPI_INSN_PAD_CODE(0x00);
+
+    mtime0 = *mtime;
+    while ((*mtime - mtime0) < IDLE_TICKS);
+
+    SPI0_REG(SPI_REG_FCTRL) = SPI_FCTRL_EN;
+    set_csr(mstatus, MSTATUS_MIE);
+}
+
+__attribute__ ((section (".itim.flash")))
+void eos_flash_fast(void) {
+    volatile uint64_t *mtime = (uint64_t *) (CLINT_CTRL_ADDR + CLINT_MTIME);
+    uint32_t mtime0;
+
+    clear_csr(mstatus, MSTATUS_MIE);
+    SPI0_REG(SPI_REG_FCTRL) = 0;
+
+    SPI0_REG(SPI_REG_SCKDIV) = 2;
+    if (!(SPI0_REG(SPI_REG_FMT) & SPI_FMT_PROTO(SPI_PROTO_Q))) {
+        send(EOS_FLASH_QPIEN);
+        while (!(SPI0_REG(SPI_REG_IP) & SPI_IP_TXWM));
+    }
+
+    SPI0_REG(SPI_REG_FMT) = \
+        SPI_FMT_PROTO(SPI_PROTO_Q)      |
+        SPI_FMT_ENDIAN(SPI_ENDIAN_MSB)  |
+        SPI_FMT_DIR(SPI_DIR_TX)         |
+        SPI_FMT_LEN(8);
+
+    SPI0_REG(SPI_REG_FFMT) =
+        SPI_INSN_CMD_EN         |
+        SPI_INSN_ADDR_LEN(3)    |
+	    SPI_INSN_PAD_CNT(6)     |
+	    SPI_INSN_CMD_PROTO(SPI_PROTO_Q)     |
+	    SPI_INSN_ADDR_PROTO(SPI_PROTO_Q)    |
+	    SPI_INSN_DATA_PROTO(SPI_PROTO_Q)    |
+	    SPI_INSN_CMD_CODE(EOS_FLASH_FRD)    |
+	    SPI_INSN_PAD_CODE(0x00);
+
+    mtime0 = *mtime;
+    while ((*mtime - mtime0) < IDLE_TICKS);
+
+    SPI0_REG(SPI_REG_FCTRL) = SPI_FCTRL_EN;
+    set_csr(mstatus, MSTATUS_MIE);
+}
+
+__attribute__ ((section (".itim.flash")))
+void eos_flash_wip(void) {
+    uint8_t status;
+
+    do {
+        SPI0_REG(SPI_REG_CSMODE) = SPI_CSMODE_HOLD;
+        xfer(EOS_FLASH_RDSR);
+        status = xfer(0);
+        SPI0_REG(SPI_REG_CSMODE) = SPI_CSMODE_AUTO;
+    } while (status & EOS_FLASH_WIP);
+}
+
+__attribute__ ((section (".itim.flash")))
+void eos_flash_wren(void) {
+    uint8_t status;
+
+    xfer(EOS_FLASH_WREN);
+#if 0
+    do {
+        SPI0_REG(SPI_REG_CSMODE) = SPI_CSMODE_HOLD;
+        xfer(EOS_FLASH_RDSR);
+        status = xfer(0);
+        SPI0_REG(SPI_REG_CSMODE) = SPI_CSMODE_AUTO;
+    } while (!(status & EOS_FLASH_WEL));
+#endif
+}
+
+__attribute__ ((section (".itim.flash")))
+void eos_flash_ser(uint32_t addr) {
+    SPI0_REG(SPI_REG_CSMODE) = SPI_CSMODE_HOLD;
+    xfer(EOS_FLASH_SER);
+    xfer(addr >> 16);
+    xfer(addr >> 8);
+    xfer(addr);
+    SPI0_REG(SPI_REG_CSMODE) = SPI_CSMODE_AUTO;
+}
+
+__attribute__ ((section (".itim.flash")))
+void eos_flash_pp(uint32_t addr, uint8_t *buf) {
+    int i;
+
+    SPI0_REG(SPI_REG_FMT) |= SPI_FMT_DIR(SPI_DIR_TX);
+    SPI0_REG(SPI_REG_CSMODE) = SPI_CSMODE_HOLD;
+    send(EOS_FLASH_PP);
+    send(addr >> 16);
+    send(addr >> 8);
+    send(addr);
+    for (i=0; i<256; i++) {
+        send(buf[i]);
+    }
+    while (!(SPI0_REG(SPI_REG_IP) & SPI_IP_TXWM));
+    SPI0_REG(SPI_REG_CSMODE) = SPI_CSMODE_AUTO;
+    SPI0_REG(SPI_REG_FMT) &= ~SPI_FMT_DIR(SPI_DIR_TX);
+}
diff --git a/fw/fe310/eos/dev/drv/flash.h b/fw/fe310/eos/dev/drv/flash.h
new file mode 100644
index 0000000..6f792cb
--- /dev/null
+++ b/fw/fe310/eos/dev/drv/flash.h
@@ -0,0 +1,25 @@
+#include <stdint.h>
+
+#define EOS_FLASH_RDSR      0x05
+
+#define EOS_FLASH_NORD      0x03
+#define EOS_FLASH_FRD       0x0b
+
+#define EOS_FLASH_WREN      0x06
+#define EOS_FLASH_SER       0x20
+#define EOS_FLASH_PP        0x02
+
+#define EOS_FLASH_QPIEN     0x35
+#define EOS_FLASH_QPIDI     0xF5
+
+#define EOS_FLASH_WIP       0x01
+#define EOS_FLASH_WEL       0x02
+
+void eos_flash_init(void);
+void eos_flash_norm(void);
+void eos_flash_fast(void);
+
+void eos_flash_wip(void);
+void eos_flash_wren(void);
+void eos_flash_ser(uint32_t addr);
+void eos_flash_pp(uint32_t addr, uint8_t *buf);
\ No newline at end of file
diff --git a/fw/fe310/eos/dev/drv/fxl6408.c b/fw/fe310/eos/dev/drv/fxl6408.c
new file mode 100644
index 0000000..2647598
--- /dev/null
+++ b/fw/fe310/eos/dev/drv/fxl6408.c
@@ -0,0 +1,59 @@
+#include <stdlib.h>
+#include <stdint.h>
+
+#include "eos.h"
+#include "soc/i2c.h"
+
+#include "fxl6408.h"
+
+#define FXL6408_ADDR0             0x44
+#define FXL6408_ADDR1             0x43
+
+int fxl6408_reg_read(uint8_t chip_id, uint8_t reg, uint8_t *data) {
+    uint8_t addr = chip_id ? FXL6408_ADDR1 : FXL6408_ADDR0;
+    int rv;
+
+    rv = eos_i2c_read8(addr, reg, data, 1);
+    return rv;
+}
+
+int fxl6408_reg_write(uint8_t chip_id, uint8_t reg, uint8_t data) {
+    uint8_t addr = chip_id ? FXL6408_ADDR1 : FXL6408_ADDR0;
+    int rv;
+
+    rv = eos_i2c_write8(addr, reg, &data, 1);
+    return rv;
+}
+
+int fxl6408_get_pin(uint8_t chip_id, uint8_t reg, uint8_t pin, uint8_t *val) {
+    uint8_t addr = chip_id ? FXL6408_ADDR1 : FXL6408_ADDR0;
+    int rv;
+
+    if (reg == FXL6408_REG_ID_CTRL) return EOS_ERR;
+
+    rv = eos_i2c_read8(addr, reg, val, 1);
+    if (rv) return rv;
+
+    *val = *val >> pin;
+    *val &= 0x01;
+
+    return EOS_OK;
+}
+
+int fxl6408_set_pin(uint8_t chip_id, uint8_t reg, uint8_t pin, uint8_t val) {
+    uint8_t addr = chip_id ? FXL6408_ADDR1 : FXL6408_ADDR0;
+    uint8_t data;
+    int rv;
+
+    if ((reg == FXL6408_REG_ID_CTRL) || (reg == FXL6408_REG_I_STATE)) return EOS_ERR;
+
+    rv = eos_i2c_read8(addr, reg, &data, 1);
+    if (rv) return rv;
+
+    val &= 0x01;
+    data &= ~(1 << pin);
+    if (val) data |= (1 << pin);
+
+    rv = eos_i2c_write8(addr, reg, &data, 1);
+    return rv;
+}
diff --git a/fw/fe310/eos/dev/drv/fxl6408.h b/fw/fe310/eos/dev/drv/fxl6408.h
new file mode 100644
index 0000000..c8e1507
--- /dev/null
+++ b/fw/fe310/eos/dev/drv/fxl6408.h
@@ -0,0 +1,17 @@
+#include <stdint.h>
+
+#define FXL6408_REG_ID_CTRL     0x01
+#define FXL6408_REG_IO_DIR      0x03
+#define FXL6408_REG_O_STATE     0x05
+#define FXL6408_REG_O_HIZ       0x07
+#define FXL6408_REG_I_DEFAULT   0x09
+#define FXL6408_REG_PULL_ENA    0x0B
+#define FXL6408_REG_PULL_DIR    0x0D
+#define FXL6408_REG_I_STATE     0x0F
+#define FXL6408_REG_INT_MASK    0x11
+#define FXL6408_REG_INT_STATE   0x13
+
+int fxl6408_reg_read(uint8_t chip_id, uint8_t reg, uint8_t *data);
+int fxl6408_reg_write(uint8_t chip_id, uint8_t reg, uint8_t data);
+int fxl6408_get_pin(uint8_t chip_id, uint8_t reg, uint8_t pin, uint8_t *val);
+int fxl6408_set_pin(uint8_t chip_id, uint8_t reg, uint8_t pin, uint8_t val);
\ No newline at end of file
diff --git a/fw/fe310/eos/dev/drv/gt911.c b/fw/fe310/eos/dev/drv/gt911.c
new file mode 100644
index 0000000..22b82c6
--- /dev/null
+++ b/fw/fe310/eos/dev/drv/gt911.c
@@ -0,0 +1,225 @@
+#include <stdlib.h>
+#include <stdint.h>
+#include <string.h>
+#include <stdio.h>
+
+#include "encoding.h"
+#include "platform.h"
+#include "board.h"
+
+#include "eos.h"
+
+#include "soc/i2c.h"
+#include "soc/timer.h"
+
+#include "gt911.h"
+
+#define CMD_SLEEP       0x05
+
+#define REG_CMD         0x8040
+#define REG_CMD2        0x8046
+
+#define REG_STATUS      0x814E
+#define REG_POINTS      0x814F
+
+#define REG_CFG         0x8047
+#define REG_CHKSUM      0x80FF
+
+#define REG_MOD_SW1     0x804D
+#define REG_REF_RATE    0x8056
+#define REG_X_THR       0x8057
+#define REG_Y_THR       0x8058
+
+#define REG_PROD_ID     0x8140
+#define REG_FW_VER      0x8144
+
+static int g911_command(uint8_t command) {
+    int rv;
+
+    if (command > 0x07) {
+        rv = eos_i2c_write16(GT911_ADDR, REG_CMD2, &command, 1);
+        if (rv) return rv;
+    }
+
+    rv = eos_i2c_write16(GT911_ADDR, REG_CMD, &command, 1);
+    if (rv) return rv;
+
+    return EOS_OK;
+}
+
+static uint8_t gt911_chksum(uint8_t *buf,  uint8_t len) {
+    int i;
+    uint8_t csum = 0;
+
+    for(i=0; i<len; i++) {
+        csum += buf[i];
+    }
+
+    // csum %= 256;
+    csum = (~csum) + 1;
+
+    return csum;
+}
+
+static int gt911_chip_id(char *buf) {
+    int rv;
+
+    rv = eos_i2c_read16(GT911_ADDR, REG_PROD_ID, buf, 4);
+    return rv;
+}
+
+static int gt911_fw_ver(char *buf) {
+    int rv;
+
+    rv = eos_i2c_read16(GT911_ADDR, REG_FW_VER, buf, 2);
+    return rv;
+}
+
+void eos_gt911_reset(void) {
+    /* INT and RST output and low */
+    GPIO_REG(GPIO_OUTPUT_VAL)   &= ~((1 << CTP_PIN_INT) | (1 << CTP_PIN_RST));
+    GPIO_REG(GPIO_OUTPUT_EN)    |=  ((1 << CTP_PIN_INT) | (1 << CTP_PIN_RST));
+
+    /* T2: > 10ms */
+    eos_sleep(12);
+
+    /* high: 0x28/0x29 (0x14 7bit), low: 0xBA/0xBB (0x5D 7bit) */
+    if (GT911_ADDR == 0x14) {
+        GPIO_REG(GPIO_OUTPUT_VAL) |= (1 << CTP_PIN_INT);
+    }
+
+    /* T3: > 100us */
+    eos_sleep(1);
+    GPIO_REG(GPIO_OUTPUT_EN)    &= ~(1 << CTP_PIN_RST);
+
+    /* T4: > 5ms */
+    eos_sleep(6);
+    GPIO_REG(GPIO_OUTPUT_VAL)   &= ~(1 << CTP_PIN_INT);
+    /* end select I2C slave addr */
+
+    /* T5: > 50ms */
+    eos_sleep(51);
+
+    /* set INT as input */
+    GPIO_REG(GPIO_OUTPUT_EN)    &= ~(1 << CTP_PIN_INT);
+}
+
+int eos_gt911_sleep(void) {
+    int rv;
+
+    GPIO_REG(GPIO_OUTPUT_VAL)   &= ~(1 << CTP_PIN_INT);
+    GPIO_REG(GPIO_OUTPUT_EN)    |=  (1 << CTP_PIN_INT);
+
+    rv = g911_command(CMD_SLEEP);
+    if (rv) return rv;
+
+    return EOS_OK;
+}
+
+void eos_gt911_wake(void) {
+    GPIO_REG(GPIO_OUTPUT_VAL)   |=  (1 << CTP_PIN_INT);
+    GPIO_REG(GPIO_OUTPUT_EN)    |=  (1 << CTP_PIN_INT);
+
+    eos_sleep(5);
+
+    GPIO_REG(GPIO_OUTPUT_EN)    &= ~(1 << CTP_PIN_INT);
+    GPIO_REG(GPIO_OUTPUT_VAL)   &= ~(1 << CTP_PIN_INT);
+}
+
+int eos_gt911_cfg_read(uint8_t *cfg_buf) {
+    int rv;
+
+    rv = eos_i2c_read16(GT911_ADDR, REG_CFG, cfg_buf, GT911_SIZE_CFG);
+    return rv;
+}
+
+int eos_gt911_cfg_write(uint8_t *cfg_buf) {
+    int rv;
+
+    cfg_buf[GT911_SIZE_CFG - 2] = gt911_chksum(cfg_buf, GT911_SIZE_CFG - 2);
+    cfg_buf[GT911_SIZE_CFG - 1] = 1;
+
+    rv = eos_i2c_write16(GT911_ADDR, REG_CFG, cfg_buf, GT911_SIZE_CFG);
+    return rv;
+}
+
+int eos_gt911_cfg_print(void) {
+    int i, rv;
+    uint8_t cfg_buf[GT911_SIZE_CFG];
+
+    rv = eos_gt911_cfg_read(cfg_buf);
+    if (rv) return rv;
+
+    printf("GT911 CFG:\n");
+    for (i=0; i<GT911_SIZE_CFG-2; i++) {
+        printf("%.2X", cfg_buf[i]);
+        if (i % 8 == 7) {
+            printf("\n");
+        } else {
+            printf(" ");
+        }
+    }
+
+    rv = gt911_fw_ver(cfg_buf);
+    if (rv) return rv;
+
+    printf("GT911 FW VER:%.2X%.2X\n", cfg_buf[1], cfg_buf[0]);
+
+    return EOS_OK;
+}
+
+void eos_gt911_set_reg(uint8_t *cfg_buf, uint16_t reg, uint8_t val) {
+    cfg_buf[reg - REG_CFG] = val;
+}
+
+uint8_t eos_gt911_get_reg(uint8_t *cfg_buf, uint16_t reg) {
+    return cfg_buf[reg - REG_CFG];
+}
+
+int eos_gt911_configure(void) {
+    int rv;
+    uint8_t cfg_buf[GT911_SIZE_CFG];
+    uint8_t reg;
+
+    rv = eos_gt911_cfg_read(cfg_buf);
+    if (rv) return rv;
+
+    /* XY coordinate output threshold: 1 */
+    eos_gt911_set_reg(cfg_buf, REG_X_THR, 1);
+    eos_gt911_set_reg(cfg_buf, REG_Y_THR, 1);
+
+    /* INT triggering mechanism: falling edge */
+    reg = eos_gt911_get_reg(cfg_buf, REG_MOD_SW1);
+    reg &= 0xFC;
+    reg |= 1;
+    eos_gt911_set_reg(cfg_buf, REG_MOD_SW1, reg);
+
+    /* Coordinates report rate: 5 ms */
+    reg = eos_gt911_get_reg(cfg_buf, REG_REF_RATE);
+    reg &= 0xF0;
+    eos_gt911_set_reg(cfg_buf, REG_REF_RATE, reg);
+
+    rv = eos_gt911_cfg_write(cfg_buf);
+    return rv;
+}
+
+int eos_gt911_set_status(uint8_t status) {
+    int rv;
+
+    rv = eos_i2c_write16(GT911_ADDR, REG_STATUS, &status, 1);
+    return rv;
+}
+
+int eos_gt911_get_status(uint8_t *status) {
+    int rv;
+
+    rv = eos_i2c_read16(GT911_ADDR, REG_STATUS, status, 1);
+    return rv;
+}
+
+int eos_gt911_get_points(int num_points, uint8_t *points) {
+    int rv;
+
+    rv = eos_i2c_read16(GT911_ADDR, REG_POINTS, points, GT911_SIZE_PBUF * num_points);
+    return rv;
+}
diff --git a/fw/fe310/eos/dev/drv/gt911.h b/fw/fe310/eos/dev/drv/gt911.h
new file mode 100644
index 0000000..46f8132
--- /dev/null
+++ b/fw/fe310/eos/dev/drv/gt911.h
@@ -0,0 +1,24 @@
+#include <stdint.h>
+
+#define GT911_ADDR          0x5d
+
+#define GT911_SIZE_CFG      186
+
+#define GT911_SIZE_PBUF     8
+#define GT911_MAX_POINTS    5
+
+void eos_gt911_reset(void);
+int eos_gt911_sleep(void);
+void eos_gt911_wake(void);
+
+int eos_gt911_cfg_read(uint8_t *cfg_buf);
+int eos_gt911_cfg_write(uint8_t *cfg_buf);
+int eos_gt911_cfg_print(void);
+
+void eos_gt911_set_reg(uint8_t *cfg_buf, uint16_t reg, uint8_t val);
+uint8_t eos_gt911_get_reg(uint8_t *cfg_buf, uint16_t reg);
+int eos_gt911_configure(void);
+
+int eos_gt911_set_status(uint8_t status);
+int eos_gt911_get_status(uint8_t *status);
+int eos_gt911_get_points(int num_points, uint8_t *points);
\ No newline at end of file
diff --git a/fw/fe310/eos/dev/drv/ili9806e.c b/fw/fe310/eos/dev/drv/ili9806e.c
new file mode 100644
index 0000000..ff7a8cb
--- /dev/null
+++ b/fw/fe310/eos/dev/drv/ili9806e.c
@@ -0,0 +1,436 @@
+#include <stdlib.h>
+#include <stdint.h>
+#include <string.h>
+
+#include "eos.h"
+
+#include "soc/spi.h"
+#include "soc/spi9bit.h"
+#include "soc/timer.h"
+
+#include "ili9806e.h"
+
+#ifdef EOS_DEBUG
+#include <stdio.h>
+#endif
+
+int ili9806e_init(void) {
+    int rv;
+    uint8_t chip_id[3];
+
+    eos_spi_cs_set();
+
+    /* LCD Setting */
+    eos_spi9bit_write(0, 0xFF);     // change to Page 1 CMD
+    eos_spi9bit_write(1, 0xFF);
+    eos_spi9bit_write(1, 0x98);
+    eos_spi9bit_write(1, 0x06);
+    eos_spi9bit_write(1, 0x04);
+    eos_spi9bit_write(1, 0x01);
+
+    // eos_spi9bit_write(0, 0x08);  // Output SDA
+    // eos_spi9bit_write(1, 0x10);
+
+    eos_spi9bit_write(0, 0xFE);     // enable read
+    eos_spi9bit_write(1, 0x81);
+
+    eos_spi9bit_write(0, 0x00);     // RDID4
+    eos_spi9bit_read(&chip_id[0]);
+
+    eos_spi9bit_write(0, 0x01);
+    eos_spi9bit_read(&chip_id[1]);
+
+    eos_spi9bit_write(0, 0x02);
+    eos_spi9bit_read(&chip_id[2]);
+
+#ifdef EOS_DEBUG
+    printf("LCD CHIP ID: %.2x%.2x%.2x\n", chip_id[0], chip_id[1], chip_id[2]);
+#endif
+
+    eos_spi9bit_write(0, 0xFE);     // disable read
+    eos_spi9bit_write(1, 0x00);
+
+    if (memcmp(chip_id, "\x98\x06\x04", sizeof(chip_id))) {
+        eos_spi_cs_clear();
+        return EOS_ERR_NOTFOUND;
+    }
+
+    eos_spi9bit_write(0, 0x20);     // set DE/VSYNC mode
+    eos_spi9bit_write(1, 0x00);
+
+    eos_spi9bit_write(0, 0x21);     // DE = 1 Active
+    eos_spi9bit_write(1, 0x01);
+
+    eos_spi9bit_write(0, 0x30);     // resolution setting 480 X 854
+    eos_spi9bit_write(1, 0x01);
+
+    eos_spi9bit_write(0, 0x31);     // inversion setting 2-dot
+    eos_spi9bit_write(1, 0x00);
+
+    eos_spi9bit_write(0, 0x40);     // BT AVDD,AVDD
+    eos_spi9bit_write(1, 0x16);
+
+    eos_spi9bit_write(0, 0x41);
+    eos_spi9bit_write(1, 0x33);     // 22
+
+    eos_spi9bit_write(0, 0x42);
+    eos_spi9bit_write(1, 0x03);     // VGL=DDVDH+VCIP-DDVDL, VGH=2DDVDL-VCIP
+
+    eos_spi9bit_write(0, 0x43);
+    eos_spi9bit_write(1, 0x09);     // set VGH clamp level
+
+    eos_spi9bit_write(0, 0x44);
+    eos_spi9bit_write(1, 0x06);     // set VGL clamp level
+
+    eos_spi9bit_write(0, 0x50);     // VREG1
+    eos_spi9bit_write(1, 0x88);
+
+    eos_spi9bit_write(0, 0x51);     // VREG2
+    eos_spi9bit_write(1, 0x88);
+
+    eos_spi9bit_write(0, 0x52);     // flicker MSB
+    eos_spi9bit_write(1, 0x00);
+
+    eos_spi9bit_write(0, 0x53);     // flicker LSB
+    eos_spi9bit_write(1, 0x49);     // VCOM
+
+    eos_spi9bit_write(0, 0x55);     // flicker
+    eos_spi9bit_write(1, 0x49);
+
+    eos_spi9bit_write(0, 0x60);
+    eos_spi9bit_write(1, 0x07);
+
+    eos_spi9bit_write(0, 0x61);
+    eos_spi9bit_write(1, 0x00);
+
+    eos_spi9bit_write(0, 0x62);
+    eos_spi9bit_write(1, 0x07);
+
+    eos_spi9bit_write(0, 0x63);
+    eos_spi9bit_write(1, 0x00);
+
+    /* Gamma Setting */
+    eos_spi9bit_write(0, 0xA0);     // positive Gamma
+    eos_spi9bit_write(1, 0x00);
+
+    eos_spi9bit_write(0, 0xA1);
+    eos_spi9bit_write(1, 0x09);
+
+    eos_spi9bit_write(0, 0xA2);
+    eos_spi9bit_write(1, 0x11);
+
+    eos_spi9bit_write(0, 0xA3);
+    eos_spi9bit_write(1, 0x0B);
+
+    eos_spi9bit_write(0, 0xA4);
+    eos_spi9bit_write(1, 0x05);
+
+    eos_spi9bit_write(0, 0xA5);
+    eos_spi9bit_write(1, 0x08);
+
+    eos_spi9bit_write(0, 0xA6);
+    eos_spi9bit_write(1, 0x06);
+
+    eos_spi9bit_write(0, 0xA7);
+    eos_spi9bit_write(1, 0x04);
+
+    eos_spi9bit_write(0, 0xA8);
+    eos_spi9bit_write(1, 0x09);
+
+    eos_spi9bit_write(0, 0xA9);
+    eos_spi9bit_write(1, 0x0C);
+
+    eos_spi9bit_write(0, 0xAA);
+    eos_spi9bit_write(1, 0x15);
+
+    eos_spi9bit_write(0, 0xAB);
+    eos_spi9bit_write(1, 0x08);
+
+    eos_spi9bit_write(0, 0xAC);
+    eos_spi9bit_write(1, 0x0F);
+
+    eos_spi9bit_write(0, 0xAD);
+    eos_spi9bit_write(1, 0x12);
+
+    eos_spi9bit_write(0, 0xAE);
+    eos_spi9bit_write(1, 0x09);
+
+    eos_spi9bit_write(0, 0xAF);
+    eos_spi9bit_write(1, 0x00);
+
+    eos_spi9bit_write(0, 0xC0);     // negative Gamma
+    eos_spi9bit_write(1, 0x00);
+
+    eos_spi9bit_write(0, 0xC1);
+    eos_spi9bit_write(1, 0x09);
+
+    eos_spi9bit_write(0, 0xC2);
+    eos_spi9bit_write(1, 0x10);
+
+    eos_spi9bit_write(0, 0xC3);
+    eos_spi9bit_write(1, 0x0C);
+
+    eos_spi9bit_write(0, 0xC4);
+    eos_spi9bit_write(1, 0x05);
+
+    eos_spi9bit_write(0, 0xC5);
+    eos_spi9bit_write(1, 0x08);
+
+    eos_spi9bit_write(0, 0xC6);
+    eos_spi9bit_write(1, 0x06);
+
+    eos_spi9bit_write(0, 0xC7);
+    eos_spi9bit_write(1, 0x04);
+
+    eos_spi9bit_write(0, 0xC8);
+    eos_spi9bit_write(1, 0x08);
+
+    eos_spi9bit_write(0, 0xC9);
+    eos_spi9bit_write(1, 0x0C);
+
+    eos_spi9bit_write(0, 0xCA);
+    eos_spi9bit_write(1, 0x14);
+
+    eos_spi9bit_write(0, 0xCB);
+    eos_spi9bit_write(1, 0x08);
+
+    eos_spi9bit_write(0, 0xCC);
+    eos_spi9bit_write(1, 0x0F);
+
+    eos_spi9bit_write(0, 0xCD);
+    eos_spi9bit_write(1, 0x11);
+
+    eos_spi9bit_write(0, 0xCE);
+    eos_spi9bit_write(1, 0x09);
+
+    eos_spi9bit_write(0, 0xCF);
+    eos_spi9bit_write(1, 0x00);
+
+    eos_spi9bit_write(0, 0xFF);     // change to Page 6 CMD for GIP timing
+    eos_spi9bit_write(1, 0xFF);
+    eos_spi9bit_write(1, 0x98);
+    eos_spi9bit_write(1, 0x06);
+    eos_spi9bit_write(1, 0x04);
+    eos_spi9bit_write(1, 0x06);
+
+    eos_spi9bit_write(0, 0x00);
+    eos_spi9bit_write(1, 0x20);
+
+    eos_spi9bit_write(0, 0x01);
+    eos_spi9bit_write(1, 0x0A);
+
+    eos_spi9bit_write(0, 0x02);
+    eos_spi9bit_write(1, 0x00);
+
+    eos_spi9bit_write(0, 0x03);
+    eos_spi9bit_write(1, 0x00);
+
+    eos_spi9bit_write(0, 0x04);
+    eos_spi9bit_write(1, 0x01);
+
+    eos_spi9bit_write(0, 0x05);
+    eos_spi9bit_write(1, 0x01);
+
+    eos_spi9bit_write(0, 0x06);
+    eos_spi9bit_write(1, 0x98);
+
+    eos_spi9bit_write(0, 0x07);
+    eos_spi9bit_write(1, 0x06);
+
+    eos_spi9bit_write(0, 0x08);
+    eos_spi9bit_write(1, 0x01);
+
+    eos_spi9bit_write(0, 0x09);
+    eos_spi9bit_write(1, 0x80);
+
+    eos_spi9bit_write(0, 0x0A);
+    eos_spi9bit_write(1, 0x00);
+
+    eos_spi9bit_write(0, 0x0B);
+    eos_spi9bit_write(1, 0x00);
+
+    eos_spi9bit_write(0, 0x0C);
+    eos_spi9bit_write(1, 0x01);
+
+    eos_spi9bit_write(0, 0x0D);
+    eos_spi9bit_write(1, 0x01);
+
+    eos_spi9bit_write(0, 0x0E);
+    eos_spi9bit_write(1, 0x05);
+
+    eos_spi9bit_write(0, 0x0F);
+    eos_spi9bit_write(1, 0x00);
+
+    eos_spi9bit_write(0, 0x10);
+    eos_spi9bit_write(1, 0xF0);
+
+    eos_spi9bit_write(0, 0x11);
+    eos_spi9bit_write(1, 0xF4);
+
+    eos_spi9bit_write(0, 0x12);
+    eos_spi9bit_write(1, 0x01);
+
+    eos_spi9bit_write(0, 0x13);
+    eos_spi9bit_write(1, 0x00);
+
+    eos_spi9bit_write(0, 0x14);
+    eos_spi9bit_write(1, 0x00);
+
+    eos_spi9bit_write(0, 0x15);
+    eos_spi9bit_write(1, 0xC0);
+
+    eos_spi9bit_write(0, 0x16);
+    eos_spi9bit_write(1, 0x08);
+
+    eos_spi9bit_write(0, 0x17);
+    eos_spi9bit_write(1, 0x00);
+
+    eos_spi9bit_write(0, 0x18);
+    eos_spi9bit_write(1, 0x00);
+
+    eos_spi9bit_write(0, 0x19);
+    eos_spi9bit_write(1, 0x00);
+
+    eos_spi9bit_write(0, 0x1A);
+    eos_spi9bit_write(1, 0x00);
+
+    eos_spi9bit_write(0, 0x1B);
+    eos_spi9bit_write(1, 0x00);
+
+    eos_spi9bit_write(0, 0x1C);
+    eos_spi9bit_write(1, 0x00);
+
+    eos_spi9bit_write(0, 0x1D);
+    eos_spi9bit_write(1, 0x00);
+
+    eos_spi9bit_write(0, 0x20);
+    eos_spi9bit_write(1, 0x01);
+
+    eos_spi9bit_write(0, 0x21);
+    eos_spi9bit_write(1, 0x23);
+
+    eos_spi9bit_write(0, 0x22);
+    eos_spi9bit_write(1, 0x45);
+
+    eos_spi9bit_write(0, 0x23);
+    eos_spi9bit_write(1, 0x67);
+
+    eos_spi9bit_write(0, 0x24);
+    eos_spi9bit_write(1, 0x01);
+
+    eos_spi9bit_write(0, 0x25);
+    eos_spi9bit_write(1, 0x23);
+
+    eos_spi9bit_write(0, 0x26);
+    eos_spi9bit_write(1, 0x45);
+
+    eos_spi9bit_write(0, 0x27);
+    eos_spi9bit_write(1, 0x67);
+
+    eos_spi9bit_write(0, 0x30);
+    eos_spi9bit_write(1, 0x11);
+
+    eos_spi9bit_write(0, 0x31);
+    eos_spi9bit_write(1, 0x11);
+
+    eos_spi9bit_write(0, 0x32);
+    eos_spi9bit_write(1, 0x00);
+
+    eos_spi9bit_write(0, 0x33);
+    eos_spi9bit_write(1, 0xEE);
+
+    eos_spi9bit_write(0, 0x34);
+    eos_spi9bit_write(1, 0xFF);
+
+    eos_spi9bit_write(0, 0x35);
+    eos_spi9bit_write(1, 0xBB);
+
+    eos_spi9bit_write(0, 0x36);
+    eos_spi9bit_write(1, 0xAA);
+
+    eos_spi9bit_write(0, 0x37);
+    eos_spi9bit_write(1, 0xDD);
+
+    eos_spi9bit_write(0, 0x38);
+    eos_spi9bit_write(1, 0xCC);
+
+    eos_spi9bit_write(0, 0x39);
+    eos_spi9bit_write(1, 0x66);
+
+    eos_spi9bit_write(0, 0x3A);
+    eos_spi9bit_write(1, 0x77);
+
+    eos_spi9bit_write(0, 0x3B);
+    eos_spi9bit_write(1, 0x22);
+
+    eos_spi9bit_write(0, 0x3C);
+    eos_spi9bit_write(1, 0x22);
+
+    eos_spi9bit_write(0, 0x3D);
+    eos_spi9bit_write(1, 0x22);
+
+    eos_spi9bit_write(0, 0x3E);
+    eos_spi9bit_write(1, 0x22);
+
+    eos_spi9bit_write(0, 0x3F);
+    eos_spi9bit_write(1, 0x22);
+
+    eos_spi9bit_write(0, 0x40);
+    eos_spi9bit_write(1, 0x22);
+
+    eos_spi9bit_write(0, 0xFF);     // change to Page 7 CMD for GIP timing
+    eos_spi9bit_write(1, 0xFF);
+    eos_spi9bit_write(1, 0x98);
+    eos_spi9bit_write(1, 0x06);
+    eos_spi9bit_write(1, 0x04);
+    eos_spi9bit_write(1, 0x07);
+
+    eos_spi9bit_write(0, 0x17);
+    eos_spi9bit_write(1, 0x22);
+
+    eos_spi9bit_write(0, 0x02);
+    eos_spi9bit_write(1, 0x77);
+
+    eos_spi9bit_write(0, 0x26);
+    eos_spi9bit_write(1, 0xB2);
+
+    eos_spi9bit_write(0, 0xFF);     // change to Page 0 CMD for normal command
+    eos_spi9bit_write(1, 0xFF);
+    eos_spi9bit_write(1, 0x98);
+    eos_spi9bit_write(1, 0x06);
+    eos_spi9bit_write(1, 0x04);
+    eos_spi9bit_write(1, 0x00);
+
+    eos_spi9bit_write(0, 0x3A);
+    eos_spi9bit_write(1, 0x70);     // 24BIT
+
+    eos_spi9bit_write(0, 0x11);
+    eos_sleep(120);
+    eos_spi9bit_write(0, 0x29);
+    eos_sleep(25);
+
+    eos_spi_cs_clear();
+
+    return EOS_OK;
+}
+
+void ili9806e_sleep(void) {
+    eos_spi_cs_set();
+
+    eos_spi9bit_write(0, 0x28);
+    eos_sleep(10);
+    eos_spi9bit_write(0, 0x10);
+
+    eos_spi_cs_clear();
+
+}
+
+void ili9806e_wake(void) {
+    eos_spi_cs_set();
+
+    eos_spi9bit_write(0, 0x11);
+    eos_sleep(120);
+    eos_spi9bit_write(0, 0x29);
+
+    eos_spi_cs_clear();
+}
diff --git a/fw/fe310/eos/dev/drv/ili9806e.h b/fw/fe310/eos/dev/drv/ili9806e.h
new file mode 100644
index 0000000..b697eee
--- /dev/null
+++ b/fw/fe310/eos/dev/drv/ili9806e.h
@@ -0,0 +1,3 @@
+int ili9806e_init(void);
+void ili9806e_sleep(void);
+void ili9806e_wake(void);
diff --git a/fw/fe310/eos/dev/drv/lsm9ds1.h b/fw/fe310/eos/dev/drv/lsm9ds1.h
new file mode 100644
index 0000000..92220e7
--- /dev/null
+++ b/fw/fe310/eos/dev/drv/lsm9ds1.h
@@ -0,0 +1,4 @@
+#include <stdint.h>
+
+#define LSM9DS1_ADDR_AG     0x1E
+#define LSM9DS1_ADDR_M      0x6B
diff --git a/fw/fe310/eos/dev/drv/ov2640.c b/fw/fe310/eos/dev/drv/ov2640.c
new file mode 100644
index 0000000..b801e20
--- /dev/null
+++ b/fw/fe310/eos/dev/drv/ov2640.c
@@ -0,0 +1,913 @@
+#include <stdlib.h>
+#include <stdint.h>
+#include <math.h>
+
+#include "eos.h"
+#include "soc/timer.h"
+#include "soc/i2c.h"
+
+#include "ov2640_regs.h"
+#include "ov2640.h"
+
+#define XCLK_FREQ       24000000
+//#define XCLK_FREQ       12000000
+
+#define CIF_WIDTH       (400)
+#define CIF_HEIGHT      (296)
+
+#define SVGA_WIDTH      (800)
+#define SVGA_HEIGHT     (600)
+
+#define UXGA_WIDTH      (1600)
+#define UXGA_HEIGHT     (1200)
+
+#define IM_LOG2_2(x)    (((x) &                0x2ULL) ? ( 2                        ) :             1) // NO ({ ... }) !
+#define IM_LOG2_4(x)    (((x) &                0xCULL) ? ( 2 +  IM_LOG2_2((x) >>  2)) :  IM_LOG2_2(x)) // NO ({ ... }) !
+#define IM_LOG2_8(x)    (((x) &               0xF0ULL) ? ( 4 +  IM_LOG2_4((x) >>  4)) :  IM_LOG2_4(x)) // NO ({ ... }) !
+#define IM_LOG2_16(x)   (((x) &             0xFF00ULL) ? ( 8 +  IM_LOG2_8((x) >>  8)) :  IM_LOG2_8(x)) // NO ({ ... }) !
+#define IM_LOG2_32(x)   (((x) &         0xFFFF0000ULL) ? (16 + IM_LOG2_16((x) >> 16)) : IM_LOG2_16(x)) // NO ({ ... }) !
+#define IM_LOG2(x)      (((x) & 0xFFFFFFFF00000000ULL) ? (32 + IM_LOG2_32((x) >> 32)) : IM_LOG2_32(x)) // NO ({ ... }) !
+
+#define IM_MAX(a,b)     ({ __typeof__ (a) _a = (a); __typeof__ (b) _b = (b); _a > _b ? _a : _b; })
+#define IM_MIN(a,b)     ({ __typeof__ (a) _a = (a); __typeof__ (b) _b = (b); _a < _b ? _a : _b; })
+#define IM_DIV(a,b)     ({ __typeof__ (a) _a = (a); __typeof__ (b) _b = (b); _b ? (_a / _b) : 0; })
+#define IM_MOD(a,b)     ({ __typeof__ (a) _a = (a); __typeof__ (b) _b = (b); _b ? (_a % _b) : 0; })
+
+static const int resolution_arr[][2] = {
+    {0,    0   },
+    // C/SIF Resolutions
+    {88,   72  },    /* QQCIF     */
+    {176,  144 },    /* QCIF      */
+    {352,  288 },    /* CIF       */
+    {88,   60  },    /* QQSIF     */
+    {176,  120 },    /* QSIF      */
+    {352,  240 },    /* SIF       */
+    // VGA Resolutions
+    {40,   30  },    /* QQQQVGA   */
+    {80,   60  },    /* QQQVGA    */
+    {160,  120 },    /* QQVGA     */
+    {320,  240 },    /* QVGA      */
+    {640,  480 },    /* VGA       */
+    {60,   40  },    /* HQQQVGA   */
+    {120,  80  },    /* HQQVGA    */
+    {240,  160 },    /* HQVGA     */
+    // FFT Resolutions
+    {64,   32  },    /* 64x32     */
+    {64,   64  },    /* 64x64     */
+    {128,  64  },    /* 128x64    */
+    {128,  128 },    /* 128x128   */
+    {320,  320 },    /* 128x128   */
+    // Other
+    {128,  160 },    /* LCD       */
+    {128,  160 },    /* QQVGA2    */
+    {720,  480 },    /* WVGA      */
+    {752,  480 },    /* WVGA2     */
+    {800,  600 },    /* SVGA      */
+    {1024, 768 },    /* XGA       */
+    {1280, 1024},    /* SXGA      */
+    {1600, 1200},    /* UXGA      */
+    {1280, 720 },    /* HD        */
+    {1920, 1080},    /* FHD       */
+    {2560, 1440},    /* QHD       */
+    {2048, 1536},    /* QXGA      */
+    {2560, 1600},    /* WQXGA     */
+    {2592, 1944},    /* WQXGA2    */
+};
+
+static const uint8_t default_regs[][2] = {
+
+// From Linux Driver.
+
+    {BANK_SEL,      BANK_SEL_DSP},
+    {0x2c,          0xff},
+    {0x2e,          0xdf},
+    {BANK_SEL,      BANK_SEL_SENSOR},
+    {0x3c,          0x32},
+//  {CLKRC,         CLKRC_DOUBLE | 0x02},
+    {CLKRC,         0x01},
+    {COM2,          COM2_OUT_DRIVE_3x},
+    {REG04,         REG04_SET(REG04_HFLIP_IMG | REG04_VFLIP_IMG | REG04_VREF_EN | REG04_HREF_EN)},
+    {COM8,          COM8_SET(COM8_BNDF_EN | COM8_AGC_EN | COM8_AEC_EN)},
+    {COM9,          COM9_AGC_SET(COM9_AGC_GAIN_8x)},
+    {0x2c,          0x0c},
+    {0x33,          0x78},
+    {0x3a,          0x33},
+    {0x3b,          0xfb},
+    {0x3e,          0x00},
+    {0x43,          0x11},
+    {0x16,          0x10},
+    {0x39,          0x02},
+    {0x35,          0x88},
+    {0x22,          0x0a},
+    {0x37,          0x40},
+    {0x23,          0x00},
+    {ARCOM2,        0xa0},
+    {0x06,          0x02},
+    {0x06,          0x88},
+    {0x07,          0xc0},
+    {0x0d,          0xb7},
+    {0x0e,          0x01},
+    {0x4c,          0x00},
+    {0x4a,          0x81},
+    {0x21,          0x99},
+    {AEW,           0x40},
+    {AEB,           0x38},
+    {VV,            VV_AGC_TH_SET(0x08, 0x02)},
+    {0x5c,          0x00},
+    {0x63,          0x00},
+    {FLL,           0x22},
+    {COM3,          COM3_BAND_SET(COM3_BAND_AUTO)},
+    {REG5D,         0x55},
+    {REG5E,         0x7d},
+    {REG5F,         0x7d},
+    {REG60,         0x55},
+    {HISTO_LOW,     0x70},
+    {HISTO_HIGH,    0x80},
+    {0x7c,          0x05},
+    {0x20,          0x80},
+    {0x28,          0x30},
+    {0x6c,          0x00},
+    {0x6d,          0x80},
+    {0x6e,          0x00},
+    {0x70,          0x02},
+    {0x71,          0x94},
+    {0x73,          0xc1},
+    {0x3d,          0x34},
+    {COM7,          COM7_RES_UXGA | COM7_ZOOM_EN},
+    {0x5a,          0x57},
+    {COM25,         0x00},
+    {BD50,          0xbb},
+    {BD60,          0x9c},
+    {BANK_SEL,      BANK_SEL_DSP},
+    {0xe5,          0x7f},
+    {MC_BIST,       MC_BIST_RESET | MC_BIST_BOOT_ROM_SEL},
+    {0x41,          0x24},
+    {RESET,         RESET_JPEG | RESET_DVP},
+    {0x76,          0xff},
+    {0x33,          0xa0},
+    {0x42,          0x20},
+    {0x43,          0x18},
+    {0x4c,          0x00},
+    {CTRL3,         CTRL3_BPC_EN | CTRL3_WPC_EN | 0x10},
+    {0x88,          0x3f},
+    {0xd7,          0x03},
+    {0xd9,          0x10},
+    {R_DVP_SP,      R_DVP_SP_AUTO_MODE | 0x2},
+    {0xc8,          0x08},
+    {0xc9,          0x80},
+    {BPADDR,        0x00},
+    {BPDATA,        0x00},
+    {BPADDR,        0x03},
+    {BPDATA,        0x48},
+    {BPDATA,        0x48},
+    {BPADDR,        0x08},
+    {BPDATA,        0x20},
+    {BPDATA,        0x10},
+    {BPDATA,        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,          0x03},
+    {0x93,          0x03},
+    {0x93,          0x00},
+    {0x93,          0x02},
+    {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},  /* simple AWB */
+    {0xb6,          0x66},
+    {0xb8,          0xA5},
+    {0xb7,          0x64},
+    {0xb9,          0x7C},
+    {0xb3,          0xaf},
+    {0xb4,          0x97},
+    {0xb5,          0xFF},
+    {0xb0,          0xC5},
+    {0xb1,          0x94},
+    {0xb2,          0x0f},
+    {0xc4,          0x5c},
+    {0xa6,          0x00},
+    {0xa7,          0x20},
+    {0xa7,          0xd8},
+    {0xa7,          0x1b},
+    {0xa7,          0x31},
+    {0xa7,          0x00},
+    {0xa7,          0x18},
+    {0xa7,          0x20},
+    {0xa7,          0xd8},
+    {0xa7,          0x19},
+    {0xa7,          0x31},
+    {0xa7,          0x00},
+    {0xa7,          0x18},
+    {0xa7,          0x20},
+    {0xa7,          0xd8},
+    {0xa7,          0x19},
+    {0xa7,          0x31},
+    {0xa7,          0x00},
+    {0xa7,          0x18},
+    {0x7f,          0x00},
+    {0xe5,          0x1f},
+    {0xe1,          0x77},
+    {0xdd,          0x7f},
+    {CTRL0,         CTRL0_YUV422 | CTRL0_YUV_EN | CTRL0_RGB_EN},
+
+// OpenMV Custom.
+
+    {BANK_SEL,      BANK_SEL_SENSOR},
+    {0x0f,          0x4b},
+    {COM1,          0x8f},
+
+// End.
+
+    {0xff,          0xff},
+};
+
+// Looks really bad.
+//static const uint8_t cif_regs[][2] = {
+//    {BANK_SEL,  BANK_SEL_SENSOR},
+//    {COM7,      COM7_RES_CIF},
+//    {COM1,      0x06 | 0x80},
+//    {HSTART,    0x11},
+//    {HSTOP,     0x43},
+//    {VSTART,    0x01}, // 0x01 fixes issue with garbage pixels in the image...
+//    {VSTOP,     0x97},
+//    {REG32,     0x09},
+//    {BANK_SEL,  BANK_SEL_DSP},
+//    {RESET,     RESET_DVP},
+//    {SIZEL,     SIZEL_HSIZE8_11_SET(CIF_WIDTH) | SIZEL_HSIZE8_SET(CIF_WIDTH) | SIZEL_VSIZE8_SET(CIF_HEIGHT)},
+//    {HSIZE8,    HSIZE8_SET(CIF_WIDTH)},
+//    {VSIZE8,    VSIZE8_SET(CIF_HEIGHT)},
+//    {CTRL2,     CTRL2_DCW_EN | CTRL2_SDE_EN | CTRL2_UV_AVG_EN | CTRL2_CMX_EN | CTRL2_UV_ADJ_EN},
+//    {0,         0},
+//};
+
+static const uint8_t svga_regs[][2] = {
+    {BANK_SEL,  BANK_SEL_SENSOR},
+    {COM7,      COM7_RES_SVGA},
+    {COM1,      0x0A | 0x80},
+    {HSTART,    0x11},
+    {HSTOP,     0x43},
+    {VSTART,    0x01}, // 0x01 fixes issue with garbage pixels in the image...
+    {VSTOP,     0x97},
+    {REG32,     0x09},
+    {BANK_SEL,  BANK_SEL_DSP},
+    {RESET,     RESET_DVP},
+    {SIZEL,     SIZEL_HSIZE8_11_SET(SVGA_WIDTH) | SIZEL_HSIZE8_SET(SVGA_WIDTH) | SIZEL_VSIZE8_SET(SVGA_HEIGHT)},
+    {HSIZE8,    HSIZE8_SET(SVGA_WIDTH)},
+    {VSIZE8,    VSIZE8_SET(SVGA_HEIGHT)},
+    {CTRL2,     CTRL2_DCW_EN | CTRL2_SDE_EN | CTRL2_UV_AVG_EN | CTRL2_CMX_EN | CTRL2_UV_ADJ_EN},
+    {0xff,      0xff},
+};
+
+static const uint8_t uxga_regs[][2] = {
+    {BANK_SEL,  BANK_SEL_SENSOR},
+    {COM7,      COM7_RES_UXGA},
+    {COM1,      0x0F | 0x80},
+    {HSTART,    0x11},
+    {HSTOP,     0x75},
+    {VSTART,    0x01},
+    {VSTOP,     0x97},
+    {REG32,     0x36},
+    {BANK_SEL,  BANK_SEL_DSP},
+    {RESET,     RESET_DVP},
+    {SIZEL,     SIZEL_HSIZE8_11_SET(UXGA_WIDTH) | SIZEL_HSIZE8_SET(UXGA_WIDTH) | SIZEL_VSIZE8_SET(UXGA_HEIGHT)},
+    {HSIZE8,    HSIZE8_SET(UXGA_WIDTH)},
+    {VSIZE8,    VSIZE8_SET(UXGA_HEIGHT)},
+    {CTRL2,     CTRL2_DCW_EN | CTRL2_SDE_EN | CTRL2_UV_AVG_EN | CTRL2_CMX_EN | CTRL2_UV_ADJ_EN},
+    {0xff,      0xff},
+};
+
+static const uint8_t yuv422_regs[][2] = {
+    {BANK_SEL,      BANK_SEL_DSP},
+    {R_BYPASS,      R_BYPASS_DSP_EN},
+    {IMAGE_MODE,    IMAGE_MODE_YUV422},
+    {0xd7,          0x03},
+    {0x33,          0xa0},
+    {0xe5,          0x1f},
+    {0xe1,          0x67},
+    {RESET,         0x00},
+    {R_BYPASS,      R_BYPASS_DSP_EN},
+    {0xff,          0xff},
+};
+
+static const uint8_t rgb565_regs[][2] = {
+    {BANK_SEL,      BANK_SEL_DSP},
+    {R_BYPASS,      R_BYPASS_DSP_EN},
+    {IMAGE_MODE,    IMAGE_MODE_RGB565},
+    {0xd7,          0x03},
+    {RESET,         0x00},
+    {R_BYPASS,      R_BYPASS_DSP_EN},
+    {0xff,          0xff},
+};
+
+static const uint8_t bayer_regs[][2] = {
+    {BANK_SEL,      BANK_SEL_DSP},
+    {R_BYPASS,      R_BYPASS_DSP_EN},
+    {IMAGE_MODE,    IMAGE_MODE_RAW10},
+    {0xd7,          0x03},
+    {RESET,         0x00},
+    {R_BYPASS,      R_BYPASS_DSP_EN},
+    {0xff,          0xff},
+};
+
+static const uint8_t jpeg_regs[][2] = {
+    {BANK_SEL,      BANK_SEL_DSP},
+    {R_BYPASS,      R_BYPASS_DSP_EN},
+    {IMAGE_MODE,    IMAGE_MODE_JPEG_EN},
+    {0xd7,          0x03},
+    {RESET,         0x00},
+    {R_BYPASS,      R_BYPASS_DSP_EN},
+    {0xff,          0xff},
+};
+
+#define NUM_BRIGHTNESS_LEVELS (5)
+static const uint8_t brightness_regs[NUM_BRIGHTNESS_LEVELS + 1][5] = {
+    {BPADDR, BPDATA, BPADDR, BPDATA, BPDATA},
+    {0x00, 0x04, 0x09, 0x00, 0x00}, /* -2 */
+    {0x00, 0x04, 0x09, 0x10, 0x00}, /* -1 */
+    {0x00, 0x04, 0x09, 0x20, 0x00}, /*  0 */
+    {0x00, 0x04, 0x09, 0x30, 0x00}, /* +1 */
+    {0x00, 0x04, 0x09, 0x40, 0x00}, /* +2 */
+};
+
+#define NUM_CONTRAST_LEVELS (5)
+static const uint8_t contrast_regs[NUM_CONTRAST_LEVELS + 1][7] = {
+    {BPADDR, BPDATA, BPADDR, BPDATA, BPDATA, BPDATA, BPDATA},
+    {0x00, 0x04, 0x07, 0x20, 0x18, 0x34, 0x06}, /* -2 */
+    {0x00, 0x04, 0x07, 0x20, 0x1c, 0x2a, 0x06}, /* -1 */
+    {0x00, 0x04, 0x07, 0x20, 0x20, 0x20, 0x06}, /*  0 */
+    {0x00, 0x04, 0x07, 0x20, 0x24, 0x16, 0x06}, /* +1 */
+    {0x00, 0x04, 0x07, 0x20, 0x28, 0x0c, 0x06}, /* +2 */
+};
+
+#define NUM_SATURATION_LEVELS (5)
+static const uint8_t saturation_regs[NUM_SATURATION_LEVELS + 1][5] = {
+    {BPADDR, BPDATA, BPADDR, BPDATA, BPDATA},
+    {0x00, 0x02, 0x03, 0x28, 0x28}, /* -2 */
+    {0x00, 0x02, 0x03, 0x38, 0x38}, /* -1 */
+    {0x00, 0x02, 0x03, 0x48, 0x48}, /*  0 */
+    {0x00, 0x02, 0x03, 0x58, 0x58}, /* +1 */
+    {0x00, 0x02, 0x03, 0x68, 0x68}, /* +2 */
+};
+
+static int reg_read(int8_t reg, uint8_t *data) {
+    return eos_i2c_read8(OV2640_ADDR, reg, data, 1);
+}
+
+static int reg_write(uint8_t reg, uint8_t data) {
+    return eos_i2c_write8(OV2640_ADDR, reg, &data, 1);
+}
+
+static int regarr_write(const uint8_t (*regs)[2]) {
+    int i, rv;
+
+    i = 0;
+    rv = EOS_OK;
+
+    while ((regs[i][0] != 0xff) || (regs[i][1] != 0xff)) {
+        if (!rv) rv = reg_write(regs[i][0], regs[i][1]);
+        i++;
+    }
+
+    return rv;
+}
+
+int ov2640_init(void) {
+    int rv;
+
+    // Reset all registers
+    rv = reg_write(BANK_SEL, BANK_SEL_SENSOR);
+    if (rv) return rv;
+
+    rv = reg_write(COM7, COM7_SRST);
+    if (rv) return rv;
+
+    // Delay 5 ms
+    eos_sleep(5);
+
+    // Write default regsiters
+    rv = regarr_write(default_regs);
+    if (rv) return rv;
+
+    // Delay 300 ms
+    eos_sleep(300);
+
+    return EOS_OK;
+}
+
+int ov2640_sleep(int enable) {
+    uint8_t reg;
+    int rv;
+
+    rv = reg_write(BANK_SEL, BANK_SEL_SENSOR);
+    if (rv) return rv;
+
+    rv = reg_read(COM2, &reg);
+    if (rv) return rv;
+
+    if (enable) {
+        reg |= COM2_STDBY;
+    } else {
+        reg &= ~COM2_STDBY;
+    }
+
+    // Write back register
+    return reg_write(COM2, reg);
+}
+
+int ov2640_set_pixfmt(cam_pixformat_t fmt) {
+    const uint8_t (*regs)[2];
+
+    switch (fmt) {
+        case CAM_PIXFORMAT_RGB565:
+            regs = rgb565_regs;
+            break;
+        case CAM_PIXFORMAT_YUV422:
+        case CAM_PIXFORMAT_GRAYSCALE:
+            regs = yuv422_regs;
+            break;
+        case CAM_PIXFORMAT_BAYER:
+            regs = bayer_regs;
+            break;
+        case CAM_PIXFORMAT_JPEG:
+            regs = jpeg_regs;
+            break;
+        default:
+            return EOS_ERR;
+    }
+
+    return regarr_write(regs);
+}
+
+int ov2640_set_framesize(cam_framesize_t framesize) {
+    const uint8_t (*regs)[2];
+    uint16_t sensor_w = 0;
+    uint16_t sensor_h = 0;
+    uint16_t w = resolution_arr[framesize][0];
+    uint16_t h = resolution_arr[framesize][1];
+    int rv;
+
+    if ((w % 4) || (h % 4) || (w > UXGA_WIDTH) || (h > UXGA_HEIGHT)) { // w/h must be divisble by 4
+        return EOS_ERR;
+    }
+
+    // Looks really bad.
+    /* if ((w <= CIF_WIDTH) && (h <= CIF_HEIGHT)) {
+        regs = cif_regs;
+        sensor_w = CIF_WIDTH;
+        sensor_h = CIF_HEIGHT;
+    } else */ if ((w <= SVGA_WIDTH) && (h <= SVGA_HEIGHT)) {
+        regs = svga_regs;
+        sensor_w = SVGA_WIDTH;
+        sensor_h = SVGA_HEIGHT;
+    } else {
+        regs = uxga_regs;
+        sensor_w = UXGA_WIDTH;
+        sensor_h = UXGA_HEIGHT;
+    }
+
+    // Write setup regsiters
+    rv = regarr_write(regs);
+    if (rv) return rv;
+
+    uint64_t tmp_div = IM_MIN(sensor_w / w, sensor_h / h);
+    uint16_t log_div = IM_MIN(IM_LOG2(tmp_div) - 1, 3);
+    uint16_t div = 1 << log_div;
+    uint16_t w_mul = w * div;
+    uint16_t h_mul = h * div;
+    uint16_t x_off = (sensor_w - w_mul) / 2;
+    uint16_t y_off = (sensor_h - h_mul) / 2;
+
+    rv = EOS_OK;
+    if (!rv) rv = reg_write(CTRLI, CTRLI_LP_DP | CTRLI_V_DIV_SET(log_div) | CTRLI_H_DIV_SET(log_div));
+    if (!rv) rv = reg_write(HSIZE, HSIZE_SET(w_mul));
+    if (!rv) rv = reg_write(VSIZE, VSIZE_SET(h_mul));
+    if (!rv) rv = reg_write(XOFFL, XOFFL_SET(x_off));
+    if (!rv) rv = reg_write(YOFFL, YOFFL_SET(y_off));
+    if (!rv) rv = reg_write(VHYX, VHYX_HSIZE_SET(w_mul) | VHYX_VSIZE_SET(h_mul) | VHYX_XOFF_SET(x_off) | VHYX_YOFF_SET(y_off));
+    if (!rv) rv = reg_write(TEST, TEST_HSIZE_SET(w_mul));
+    if (!rv) rv = reg_write(ZMOW, ZMOW_OUTW_SET(w));
+    if (!rv) rv = reg_write(ZMOH, ZMOH_OUTH_SET(h));
+    if (!rv) rv = reg_write(ZMHH, ZMHH_OUTW_SET(w) | ZMHH_OUTH_SET(h));
+    if (!rv) rv = reg_write(R_DVP_SP, div);
+    if (!rv) rv = reg_write(RESET, 0x00);
+
+    return rv;
+}
+
+int ov2640_set_contrast(int level) {
+    int rv = EOS_OK;
+
+    level += (NUM_CONTRAST_LEVELS / 2) + 1;
+    if (level <= 0 || level > NUM_CONTRAST_LEVELS) {
+        return EOS_ERR;
+    }
+
+    /* Switch to DSP register bank */
+    rv = reg_write(BANK_SEL, BANK_SEL_DSP);
+
+    /* Write contrast registers */
+    for (int i=0; i<sizeof(contrast_regs[0])/sizeof(contrast_regs[0][0]); i++) {
+        if (!rv) rv = reg_write(contrast_regs[0][i], contrast_regs[level][i]);
+    }
+
+    return rv;
+}
+
+int ov2640_set_brightness(int level) {
+    int rv = EOS_OK;
+
+    level += (NUM_BRIGHTNESS_LEVELS / 2) + 1;
+    if (level <= 0 || level > NUM_BRIGHTNESS_LEVELS) {
+        return EOS_ERR;
+    }
+
+    /* Switch to DSP register bank */
+    rv = reg_write(BANK_SEL, BANK_SEL_DSP);
+
+    /* Write brightness registers */
+    for (int i=0; i<sizeof(brightness_regs[0])/sizeof(brightness_regs[0][0]); i++) {
+        if (!rv) rv = reg_write(brightness_regs[0][i], brightness_regs[level][i]);
+    }
+
+    return rv;
+}
+
+int ov2640_set_saturation(int level) {
+    int rv = EOS_OK;
+
+    level += (NUM_SATURATION_LEVELS / 2) + 1;
+    if (level <= 0 || level > NUM_SATURATION_LEVELS) {
+        return EOS_ERR;
+    }
+
+    /* Switch to DSP register bank */
+    rv = reg_write(BANK_SEL, BANK_SEL_DSP);
+
+    /* Write saturation registers */
+    for (int i=0; i<sizeof(saturation_regs[0])/sizeof(saturation_regs[0][0]); i++) {
+        if (!rv) rv = reg_write(saturation_regs[0][i], saturation_regs[level][i]);
+    }
+
+    return rv;
+}
+
+int ov2640_set_gainceiling(cam_gainceiling_t gainceiling) {
+    int rv = EOS_OK;
+
+    /* Switch to SENSOR register bank */
+    rv = reg_write(BANK_SEL, BANK_SEL_SENSOR);
+
+    /* Write gain ceiling register */
+    if (!rv) rv = reg_write(COM9, COM9_AGC_SET(gainceiling));
+
+    return rv;
+}
+
+int ov2640_set_quality(int qs) {
+    int rv = EOS_OK;
+
+    /* Switch to DSP register bank */
+    rv = reg_write(BANK_SEL, BANK_SEL_DSP);
+
+    /* Write QS register */
+    if (!rv) rv = reg_write(QS, qs);
+
+    return rv;
+}
+
+int ov2640_set_colorbar(int enable) {
+    int rv = EOS_OK;
+    uint8_t reg;
+
+    rv = reg_write(BANK_SEL, BANK_SEL_SENSOR);
+    if (rv) return rv;
+
+    rv = reg_read(COM7, &reg);
+    if (rv) return rv;
+
+    if (enable) {
+        reg |= COM7_COLOR_BAR;
+    } else {
+        reg &= ~COM7_COLOR_BAR;
+    }
+
+    return reg_write(COM7, reg);
+}
+
+int ov2640_set_auto_gain(int enable, float gain_db, float gain_db_ceiling) {
+    int rv = EOS_OK;
+    uint8_t reg;
+
+    rv = reg_write(BANK_SEL, BANK_SEL_SENSOR);
+    if (rv) return rv;
+
+    rv = reg_read(COM8, &reg);
+    if (rv) return rv;
+
+    rv = reg_write(COM8, (reg & (~COM8_AGC_EN)) | (enable ? COM8_AGC_EN : 0));
+    if (rv) return rv;
+
+    rv = EOS_OK;
+    if (enable && (!isnanf(gain_db_ceiling)) && (!isinff(gain_db_ceiling))) {
+        float gain_ceiling = IM_MAX(IM_MIN(expf((gain_db_ceiling / 20.0) * logf(10.0)), 128.0), 2.0);
+
+        if (!rv) rv = reg_read(COM9, &reg);
+        if (!rv) rv = reg_write(COM9, (reg & 0x1F) | (((int)ceilf(log2f(gain_ceiling)) - 1) << 5));
+    }
+
+    if (!enable && (!isnanf(gain_db)) && (!isinff(gain_db))) {
+        float gain = IM_MAX(IM_MIN(expf((gain_db / 20.0) * logf(10.0)), 32.0), 1.0);
+
+        int gain_temp = roundf(log2f(IM_MAX(gain / 2.0, 1.0)));
+        int gain_hi = 0xF >> (4 - gain_temp);
+        int gain_lo = IM_MIN(roundf(((gain / (1 << gain_temp)) - 1.0) * 16.0), 15);
+
+        if (!rv) rv = reg_write(GAIN, (gain_hi << 4) | (gain_lo << 0));
+    }
+
+    return rv;
+}
+
+int ov2640_get_gain_db(float *gain_db) {
+    int rv = EOS_OK;
+    uint8_t reg, gain;
+
+    rv = reg_write(BANK_SEL, BANK_SEL_SENSOR);
+    if (rv) return rv;
+
+    rv = reg_read(COM8, &reg);
+    if (rv) return rv;
+
+    // DISABLED
+    // if (reg & COM8_AGC_EN) {
+    //     rv = reg_write(COM8, reg & (~COM8_AGC_EN));
+    //     if (rv) return rv;
+    // }
+    // DISABLED
+
+    rv = reg_read(GAIN, &gain);
+    if (rv) return rv;
+
+    // DISABLED
+    // if (reg & COM8_AGC_EN) {
+    //     rv = reg_write(COM8, reg | COM8_AGC_EN);
+    //     if (rv) return rv;
+    // }
+    // DISABLED
+
+    int hi_gain = 1 << (((gain >> 7) & 1) + ((gain >> 6) & 1) + ((gain >> 5) & 1) + ((gain >> 4) & 1));
+    float lo_gain = 1.0 + (((gain >> 0) & 0xF) / 16.0);
+    *gain_db = 20.0 * (logf(hi_gain * lo_gain) / logf(10.0));
+
+    return EOS_OK;
+}
+
+int ov2640_set_auto_exposure(int enable, int exposure_us) {
+    int rv = EOS_OK;
+    uint8_t reg;
+
+    rv = reg_write(BANK_SEL, BANK_SEL_SENSOR);
+    if (rv) return rv;
+
+    rv = reg_read(COM8, &reg);
+    if (rv) return rv;
+
+    rv = reg_write(COM8, COM8_SET_AEC(reg, (enable != 0)));
+    if (rv) return rv;
+
+    if (!enable && (exposure_us >= 0)) {
+        rv = reg_read(COM7, &reg);
+        if (rv) return rv;
+
+        int t_line = 0;
+
+        if (COM7_GET_RES(reg) == COM7_RES_UXGA) t_line = 1600 + 322;
+        if (COM7_GET_RES(reg) == COM7_RES_SVGA) t_line = 800 + 390;
+        if (COM7_GET_RES(reg) == COM7_RES_CIF) t_line = 400 + 195;
+
+        rv = reg_read(CLKRC, &reg);
+        if (rv) return rv;
+
+        int pll_mult = ((reg & CLKRC_DOUBLE) ? 2 : 1) * 3;
+        int clk_rc = (reg & CLKRC_DIVIDER_MASK) + 2;
+
+        rv = reg_write(BANK_SEL, BANK_SEL_DSP);
+        if (rv) return rv;
+
+        rv = reg_read(IMAGE_MODE, &reg);
+        if (rv) return rv;
+
+        int t_pclk = 0;
+
+        if (IMAGE_MODE_GET_FMT(reg) == IMAGE_MODE_YUV422) t_pclk = 2;
+        if (IMAGE_MODE_GET_FMT(reg) == IMAGE_MODE_RAW10) t_pclk = 1;
+        if (IMAGE_MODE_GET_FMT(reg) == IMAGE_MODE_RGB565) t_pclk = 2;
+
+        int exposure = IM_MAX(IM_MIN(((exposure_us*(((XCLK_FREQ/clk_rc)*pll_mult)/1000000))/t_pclk)/t_line,0xFFFF),0x0000);
+
+        if (!rv) rv = reg_write(BANK_SEL, BANK_SEL_SENSOR);
+
+        if (!rv) rv = reg_read(REG04, &reg);
+        if (!rv) rv = reg_write(REG04, (reg & 0xFC) | ((exposure >> 0) & 0x3));
+
+        if (!rv) rv = reg_read(AEC, &reg);
+        if (!rv) rv = reg_write(AEC, (reg & 0x00) | ((exposure >> 2) & 0xFF));
+
+        if (!rv) rv = reg_read(REG45, &reg);
+        if (!rv) rv = reg_write(REG45, (reg & 0xC0) | ((exposure >> 10) & 0x3F));
+    }
+
+    return rv;
+}
+
+int ov2640_get_exposure_us(int *exposure_us) {
+    int rv = EOS_OK;
+    uint8_t reg, aec_10, aec_92, aec_1510;
+
+    rv = reg_write(BANK_SEL, BANK_SEL_SENSOR);
+    if (rv) return rv;
+
+    rv = reg_read(COM8, &reg);
+    if (rv) return rv;
+
+    // DISABLED
+    // if (reg & COM8_AEC_EN) {
+    //     rv = reg_write(COM8, reg & (~COM8_AEC_EN));
+    //     if (rv) return rv;
+    // }
+    // DISABLED
+
+    rv = reg_read(REG04, &aec_10);
+    if (rv) return rv;
+
+    rv = reg_read(AEC, &aec_92);
+    if (rv) return rv;
+
+    rv = reg_read(REG45, &aec_1510);
+    if (rv) return rv;
+
+    // DISABLED
+    // if (reg & COM8_AEC_EN) {
+    //     rv = reg_write(COM8, reg | COM8_AEC_EN);
+    //     if (rv) return rv;
+    // }
+    // DISABLED
+
+    rv = reg_read(COM7, &reg);
+    if (rv) return rv;
+
+    int t_line = 0;
+
+    if (COM7_GET_RES(reg) == COM7_RES_UXGA) t_line = 1600 + 322;
+    if (COM7_GET_RES(reg) == COM7_RES_SVGA) t_line = 800 + 390;
+    if (COM7_GET_RES(reg) == COM7_RES_CIF) t_line = 400 + 195;
+
+    rv = reg_read(CLKRC, &reg);
+    if (rv) return rv;
+
+    int pll_mult = ((reg & CLKRC_DOUBLE) ? 2 : 1) * 3;
+    int clk_rc = (reg & CLKRC_DIVIDER_MASK) + 2;
+
+    rv = reg_write(BANK_SEL, BANK_SEL_DSP);
+    if (rv) return rv;
+
+    rv = reg_read(IMAGE_MODE, &reg);
+    if (rv) return rv;
+
+    int t_pclk = 0;
+
+    if (IMAGE_MODE_GET_FMT(reg) == IMAGE_MODE_YUV422) t_pclk = 2;
+    if (IMAGE_MODE_GET_FMT(reg) == IMAGE_MODE_RAW10) t_pclk = 1;
+    if (IMAGE_MODE_GET_FMT(reg) == IMAGE_MODE_RGB565) t_pclk = 2;
+
+    uint16_t exposure = ((aec_1510 & 0x3F) << 10) + ((aec_92 & 0xFF) << 2) + ((aec_10 & 0x3) << 0);
+    *exposure_us = (exposure*t_line*t_pclk)/(((XCLK_FREQ/clk_rc)*pll_mult)/1000000);
+
+    return EOS_OK;
+}
+
+int ov2640_set_auto_whitebal(int enable, float r_gain_db, float g_gain_db, float b_gain_db) {
+    int rv = EOS_OK;
+    uint8_t reg;
+
+    rv = reg_write(BANK_SEL, BANK_SEL_DSP);
+    if (rv) return rv;
+
+    rv = reg_read(CTRL1, &reg);
+    if (rv) return rv;
+
+    rv = reg_write(CTRL1, (reg & (~CTRL1_AWB)) | (enable ? CTRL1_AWB : 0));
+    if (rv) return rv;
+
+    if (!enable && (!isnanf(r_gain_db)) && (!isnanf(g_gain_db)) && (!isnanf(b_gain_db))
+                && (!isinff(r_gain_db)) && (!isinff(g_gain_db)) && (!isinff(b_gain_db))) {
+    }
+
+    return rv;
+}
+
+int ov2640_set_hmirror(int enable) {
+    int rv = EOS_OK;
+    uint8_t reg;
+
+    rv = reg_write(BANK_SEL, BANK_SEL_SENSOR);
+    if (rv) return rv;
+
+    rv = reg_read(REG04, &reg);
+    if (rv) return rv;
+
+    if (!enable) { // Already mirrored.
+        reg |= REG04_HFLIP_IMG;
+    } else {
+        reg &= ~REG04_HFLIP_IMG;
+    }
+
+    return reg_write(REG04, reg);
+}
+
+int ov2640_set_vflip(int enable) {
+    int rv = EOS_OK;
+    uint8_t reg;
+
+    rv = reg_write(BANK_SEL, BANK_SEL_SENSOR);
+    if (rv) return rv;
+
+    rv = reg_read(REG04, &reg);
+    if (rv) return rv;
+
+    if (!enable) { // Already flipped.
+        reg |= REG04_VFLIP_IMG | REG04_VREF_EN;
+    } else {
+        reg &= ~(REG04_VFLIP_IMG | REG04_VREF_EN);
+    }
+
+    return reg_write(REG04, reg);
+}
+
+int ov2640_set_effect(cam_sde_t sde) {
+    int rv = EOS_OK;
+
+    switch (sde) {
+        case CAM_SDE_NEGATIVE:
+            if (!rv) rv = reg_write(BANK_SEL, BANK_SEL_DSP);
+            if (!rv) rv = reg_write(BPADDR, 0x00);
+            if (!rv) rv = reg_write(BPDATA, 0x40);
+            if (!rv) rv = reg_write(BPADDR, 0x05);
+            if (!rv) rv = reg_write(BPDATA, 0x80);
+            if (!rv) rv = reg_write(BPDATA, 0x80);
+            break;
+        case CAM_SDE_NORMAL:
+            if (!rv) rv = reg_write(BANK_SEL, BANK_SEL_DSP);
+            if (!rv) rv = reg_write(BPADDR, 0x00);
+            if (!rv) rv = reg_write(BPDATA, 0x00);
+            if (!rv) rv = reg_write(BPADDR, 0x05);
+            if (!rv) rv = reg_write(BPDATA, 0x80);
+            if (!rv) rv = reg_write(BPDATA, 0x80);
+            break;
+        default:
+            return EOS_ERR;
+    }
+
+    return rv;
+}
diff --git a/fw/fe310/eos/dev/drv/ov2640.h b/fw/fe310/eos/dev/drv/ov2640.h
new file mode 100644
index 0000000..af282e0
--- /dev/null
+++ b/fw/fe310/eos/dev/drv/ov2640.h
@@ -0,0 +1,74 @@
+#include <stdint.h>
+
+#define OV2640_ADDR     0x30
+
+typedef enum {
+    CAM_PIXFORMAT_INVALID = 0,
+    CAM_PIXFORMAT_BINARY,    // 1BPP/BINARY
+    CAM_PIXFORMAT_GRAYSCALE, // 1BPP/GRAYSCALE
+    CAM_PIXFORMAT_RGB565,    // 2BPP/RGB565
+    CAM_PIXFORMAT_YUV422,    // 2BPP/YUV422
+    CAM_PIXFORMAT_BAYER,     // 1BPP/RAW
+    CAM_PIXFORMAT_JPEG,      // JPEG/COMPRESSED
+} cam_pixformat_t;
+
+typedef enum {
+    CAM_FRAMESIZE_INVALID = 0,
+    // C/SIF Resolutions
+    CAM_FRAMESIZE_QQCIF,    // 88x72
+    CAM_FRAMESIZE_QCIF,     // 176x144
+    CAM_FRAMESIZE_CIF,      // 352x288
+    CAM_FRAMESIZE_QQSIF,    // 88x60
+    CAM_FRAMESIZE_QSIF,     // 176x120
+    CAM_FRAMESIZE_SIF,      // 352x240
+    // VGA Resolutions
+    CAM_FRAMESIZE_QQQQVGA,  // 40x30
+    CAM_FRAMESIZE_QQQVGA,   // 80x60
+    CAM_FRAMESIZE_QQVGA,    // 160x120
+    CAM_FRAMESIZE_QVGA,     // 320x240
+    CAM_FRAMESIZE_VGA,      // 640x480
+    CAM_FRAMESIZE_HQQQVGA,  // 60x40
+    CAM_FRAMESIZE_HQQVGA,   // 120x80
+    CAM_FRAMESIZE_HQVGA,    // 240x160
+    // FFT Resolutions
+    CAM_FRAMESIZE_64X32,    // 64x32
+    CAM_FRAMESIZE_64X64,    // 64x64
+    CAM_FRAMESIZE_128X64,   // 128x64
+    CAM_FRAMESIZE_128X128,  // 128x128
+    CAM_FRAMESIZE_320X320,  // 320x320
+    // Other
+    CAM_FRAMESIZE_LCD,      // 128x160
+    CAM_FRAMESIZE_QQVGA2,   // 128x160
+    CAM_FRAMESIZE_WVGA,     // 720x480
+    CAM_FRAMESIZE_WVGA2,    // 752x480
+    CAM_FRAMESIZE_SVGA,     // 800x600
+    CAM_FRAMESIZE_XGA,      // 1024x768
+    CAM_FRAMESIZE_SXGA,     // 1280x1024
+    CAM_FRAMESIZE_UXGA,     // 1600x1200
+    CAM_FRAMESIZE_HD,       // 1280x720
+    CAM_FRAMESIZE_FHD,      // 1920x1080
+    CAM_FRAMESIZE_QHD,      // 2560x1440
+    CAM_FRAMESIZE_QXGA,     // 2048x1536
+    CAM_FRAMESIZE_WQXGA,    // 2560x1600
+    CAM_FRAMESIZE_WQXGA2,   // 2592x1944
+} cam_framesize_t;
+
+typedef enum {
+    CAM_GAINCEILING_2X,
+    CAM_GAINCEILING_4X,
+    CAM_GAINCEILING_8X,
+    CAM_GAINCEILING_16X,
+    CAM_GAINCEILING_32X,
+    CAM_GAINCEILING_64X,
+    CAM_GAINCEILING_128X,
+} cam_gainceiling_t;
+
+typedef enum {
+    SDE_NORMAL,
+    SDE_NEGATIVE,
+} cam_sde_t;
+
+int ov2640_init(void);
+int ov2640_sleep(int enable);
+int ov2640_set_pixfmt(cam_pixformat_t fmt);
+int ov2640_set_framesize(cam_framesize_t framesize);
diff --git a/fw/fe310/eos/dev/drv/ov2640_regs.h b/fw/fe310/eos/dev/drv/ov2640_regs.h
new file mode 100644
index 0000000..deb7521
--- /dev/null
+++ b/fw/fe310/eos/dev/drv/ov2640_regs.h
@@ -0,0 +1,245 @@
+/*
+ * This file is part of the OpenMV project.
+ *
+ * Copyright (c) 2013-2021 Ibrahim Abdelkader <iabdalkader@openmv.io>
+ * Copyright (c) 2013-2021 Kwabena W. Agyeman <kwagyeman@openmv.io>
+ *
+ * This work is licensed under the MIT license, see the file LICENSE for details.
+ *
+ * OV2640 register definitions.
+ */
+#ifndef __REG_REGS_H__
+#define __REG_REGS_H__
+
+/* DSP register bank FF=0x00*/
+
+#define QS                      0x44
+#define HSIZE                   0x51
+#define VSIZE                   0x52
+#define XOFFL                   0x53
+#define YOFFL                   0x54
+#define VHYX                    0x55
+#define DPRP                    0x56
+#define TEST                    0x57
+#define ZMOW                    0x5A
+#define ZMOH                    0x5B
+#define ZMHH                    0x5C
+#define BPADDR                  0x7C
+#define BPDATA                  0x7D
+#define SIZEL                   0x8C
+#define HSIZE8                  0xC0
+#define VSIZE8                  0xC1
+#define CTRL1                   0xC3
+#define MS_SP                   0xF0
+#define SS_ID                   0xF7
+#define SS_CTRL                 0xF7
+#define MC_AL                   0xFA
+#define MC_AH                   0xFB
+#define MC_D                    0xFC
+#define P_CMD                   0xFD
+#define P_STATUS                0xFE
+
+#define CTRLI                   0x50
+#define CTRLI_LP_DP             0x80
+#define CTRLI_ROUND             0x40
+
+#define CTRL0                   0xC2
+#define CTRL0_AEC_EN            0x80
+#define CTRL0_AEC_SEL           0x40
+#define CTRL0_STAT_SEL          0x20
+#define CTRL0_VFIRST            0x10
+#define CTRL0_YUV422            0x08
+#define CTRL0_YUV_EN            0x04
+#define CTRL0_RGB_EN            0x02
+#define CTRL0_RAW_EN            0x01
+
+#define CTRL2                   0x86
+#define CTRL2_DCW_EN            0x20
+#define CTRL2_SDE_EN            0x10
+#define CTRL2_UV_ADJ_EN         0x08
+#define CTRL2_UV_AVG_EN         0x04
+#define CTRL2_CMX_EN            0x01
+
+#define CTRL3                   0x87
+#define CTRL3_BPC_EN            0x80
+#define CTRL3_WPC_EN            0x40
+#define R_DVP_SP                0xD3
+#define R_DVP_SP_AUTO_MODE      0x80
+
+#define R_BYPASS                0x05
+#define R_BYPASS_DSP_EN         0x00
+#define R_BYPASS_DSP_BYPAS      0x01
+
+#define IMAGE_MODE              0xDA
+#define IMAGE_MODE_Y8_DVP_EN    0x40
+#define IMAGE_MODE_JPEG_EN      0x10
+#define IMAGE_MODE_YUV422       0x00
+#define IMAGE_MODE_RAW10        0x04
+#define IMAGE_MODE_RGB565       0x09
+#define IMAGE_MODE_HREF_VSYNC   0x02
+#define IMAGE_MODE_LBYTE_FIRST  0x01
+#define IMAGE_MODE_GET_FMT(x)   ((x)&0xC)
+
+#define RESET                   0xE0
+#define RESET_MICROC            0x40
+#define RESET_SCCB              0x20
+#define RESET_JPEG              0x10
+#define RESET_DVP               0x04
+#define RESET_IPU               0x02
+#define RESET_CIF               0x01
+
+#define MC_BIST                 0xF9
+#define MC_BIST_RESET           0x80
+#define MC_BIST_BOOT_ROM_SEL    0x40
+#define MC_BIST_12KB_SEL        0x20
+#define MC_BIST_12KB_MASK       0x30
+#define MC_BIST_512KB_SEL       0x08
+#define MC_BIST_512KB_MASK      0x0C
+#define MC_BIST_BUSY_BIT_R      0x02
+#define MC_BIST_MC_RES_ONE_SH_W 0x02
+#define MC_BIST_LAUNCH          0x01
+
+#define BANK_SEL                0xFF
+#define BANK_SEL_DSP            0x00
+#define BANK_SEL_SENSOR         0x01
+
+/* Sensor register bank FF=0x01*/
+
+#define GAIN                0x00
+#define COM1                0x03
+#define REG_PID             0x0A
+#define REG_VER             0x0B
+#define COM4                0x0D
+#define AEC                 0x10
+
+#define CLKRC               0x11
+#define CLKRC_DOUBLE        0x80
+#define CLKRC_DIVIDER_MASK  0x3F
+
+#define COM10               0x15
+#define HSTART              0x17
+#define HSTOP               0x18
+#define VSTART              0x19
+#define VSTOP               0x1A
+#define MIDH                0x1C
+#define MIDL                0x1D
+#define AEW                 0x24
+#define AEB                 0x25
+#define REG2A               0x2A
+#define FRARL               0x2B
+#define ADDVSL              0x2D
+#define ADDVSH              0x2E
+#define YAVG                0x2F
+#define HSDY                0x30
+#define HEDY                0x31
+#define ARCOM2              0x34
+#define REG45               0x45
+#define FLL                 0x46
+#define FLH                 0x47
+#define COM19               0x48
+#define ZOOMS               0x49
+#define COM22               0x4B
+#define COM25               0x4E
+#define BD50                0x4F
+#define BD60                0x50
+#define REG5D               0x5D
+#define REG5E               0x5E
+#define REG5F               0x5F
+#define REG60               0x60
+#define HISTO_LOW           0x61
+#define HISTO_HIGH          0x62
+
+#define REG04               0x04
+#define REG04_DEFAULT       0x28
+#define REG04_HFLIP_IMG     0x80
+#define REG04_VFLIP_IMG     0x40
+#define REG04_VREF_EN       0x10
+#define REG04_HREF_EN       0x08
+#define REG04_SET(x)        (REG04_DEFAULT|x)
+
+#define REG08               0x08
+#define COM2                0x09
+#define COM2_STDBY          0x10
+#define COM2_OUT_DRIVE_1x   0x00
+#define COM2_OUT_DRIVE_2x   0x01
+#define COM2_OUT_DRIVE_3x   0x02
+#define COM2_OUT_DRIVE_4x   0x03
+
+#define COM3                0x0C
+#define COM3_DEFAULT        0x38
+#define COM3_BAND_50Hz      0x04
+#define COM3_BAND_60Hz      0x00
+#define COM3_BAND_AUTO      0x02
+#define COM3_BAND_SET(x)    (COM3_DEFAULT|x)
+
+#define COM7                0x12
+#define COM7_SRST           0x80
+#define COM7_RES_UXGA       0x00 /* UXGA */
+#define COM7_RES_SVGA       0x40 /* SVGA */
+#define COM7_RES_CIF        0x20 /* CIF  */
+#define COM7_ZOOM_EN        0x04 /* Enable Zoom */
+#define COM7_COLOR_BAR      0x02 /* Enable Color Bar Test */
+#define COM7_GET_RES(x)     ((x)&0x70)
+
+#define COM8                0x13
+#define COM8_DEFAULT        0xC0
+#define COM8_BNDF_EN        0x20 /* Enable Banding filter */
+#define COM8_AGC_EN         0x04 /* AGC Auto/Manual control selection */
+#define COM8_AEC_EN         0x01 /* Auto/Manual Exposure control */
+#define COM8_SET(x)         (COM8_DEFAULT|x)
+#define COM8_SET_AEC(r,x)   (((r)&0xFE)|((x)&1))
+
+#define COM9                0x14 /* AGC gain ceiling */
+#define COM9_DEFAULT        0x08
+#define COM9_AGC_GAIN_2x    0x00 /* AGC:    2x */
+#define COM9_AGC_GAIN_4x    0x01 /* AGC:    4x */
+#define COM9_AGC_GAIN_8x    0x02 /* AGC:    8x */
+#define COM9_AGC_GAIN_16x   0x03 /* AGC:   16x */
+#define COM9_AGC_GAIN_32x   0x04 /* AGC:   32x */
+#define COM9_AGC_GAIN_64x   0x05 /* AGC:   64x */
+#define COM9_AGC_GAIN_128x  0x06 /* AGC:  128x */
+#define COM9_AGC_SET(x)     (COM9_DEFAULT|(x<<5))
+
+#define CTRL1_AWB           0x08 /* Enable AWB */
+
+#define VV                  0x26
+#define VV_AGC_TH_SET(h,l)  ((h<<4)|(l&0x0F))
+
+#define REG32               0x32
+#define REG32_UXGA          0x36
+#define REG32_SVGA          0x09
+#define REG32_CIF           0x00
+
+#define VAL_SET(x, mask, rshift, lshift) ((((x) >> rshift) & mask) << lshift)
+
+#define CTRLI_V_DIV_SET(x)      VAL_SET(x, 0x3, 0, 3)
+#define CTRLI_H_DIV_SET(x)      VAL_SET(x, 0x3, 0, 0)
+
+#define SIZEL_HSIZE8_11_SET(x)  VAL_SET(x, 0x1, 11, 6)
+#define SIZEL_HSIZE8_SET(x)     VAL_SET(x, 0x7, 0, 3)
+#define SIZEL_VSIZE8_SET(x)     VAL_SET(x, 0x7, 0, 0)
+
+#define HSIZE8_SET(x)           VAL_SET(x, 0xFF, 3, 0)
+#define VSIZE8_SET(x)           VAL_SET(x, 0xFF, 3, 0)
+
+#define HSIZE_SET(x)            VAL_SET(x, 0xFF, 2, 0)
+#define VSIZE_SET(x)            VAL_SET(x, 0xFF, 2, 0)
+
+#define XOFFL_SET(x)            VAL_SET(x, 0xFF, 0, 0)
+#define YOFFL_SET(x)            VAL_SET(x, 0xFF, 0, 0)
+
+#define VHYX_VSIZE_SET(x)       VAL_SET(x, 0x1, (8+2), 7)
+#define VHYX_HSIZE_SET(x)       VAL_SET(x, 0x1, (8+2), 3)
+#define VHYX_YOFF_SET(x)        VAL_SET(x, 0x3, 8, 4)
+#define VHYX_XOFF_SET(x)        VAL_SET(x, 0x3, 8, 0)
+
+#define TEST_HSIZE_SET(x)       VAL_SET(x, 0x1, (9+2), 7)
+
+#define ZMOW_OUTW_SET(x)        VAL_SET(x, 0xFF, 2, 0)
+#define ZMOH_OUTH_SET(x)        VAL_SET(x, 0xFF, 2, 0)
+
+#define ZMHH_ZSPEED_SET(x)      VAL_SET(x, 0x0F, 0, 4)
+#define ZMHH_OUTH_SET(x)        VAL_SET(x, 0x1, (8+2), 2)
+#define ZMHH_OUTW_SET(x)        VAL_SET(x, 0x3, (8+2), 0)
+
+#endif //__REG_REGS_H__
diff --git a/fw/fe310/eos/dev/drv/tps61052.c b/fw/fe310/eos/dev/drv/tps61052.c
new file mode 100644
index 0000000..2908a45
--- /dev/null
+++ b/fw/fe310/eos/dev/drv/tps61052.c
@@ -0,0 +1,340 @@
+#include <stdlib.h>
+#include <stdint.h>
+
+#include "eos.h"
+#include "soc/i2c.h"
+
+#include "tps61052.h"
+
+#define TPS61052_ADDR       0x33
+
+static int reg_read(uint8_t reg, uint8_t *data) {
+    int rv;
+
+    rv = eos_i2c_read8(TPS61052_ADDR, reg, data, 1);
+    return rv;
+}
+
+static int reg_write(uint8_t reg, uint8_t data) {
+    int rv;
+
+    rv = eos_i2c_write8(TPS61052_ADDR, reg, &data, 1);
+    return rv;
+}
+
+/* register b00 */
+int tps61052_get_tc(uint8_t *tc) {
+    int rv;
+
+    rv = reg_read(0, tc);
+    if (rv) return rv;
+
+    *tc &= 0x07;
+    return EOS_OK;
+}
+
+int tps61052_set_tc(uint8_t tc) {
+    uint8_t data;
+    int rv;
+
+    rv = reg_read(0, &data);
+    if (rv) return rv;
+
+    data &= 0xF8;
+    tc &= 0x07;
+
+    rv = reg_write(0, data | tc);
+    return rv;
+}
+
+int tps61052_get_dim(uint8_t *dim) {
+    int rv;
+
+    rv = reg_read(0, dim);
+    if (rv) return rv;
+
+    *dim = *dim >> 3;
+    *dim &= 0x01;
+    return EOS_OK;
+}
+
+int tps61052_set_dim(uint8_t dim) {
+    uint8_t data;
+    int rv;
+
+    rv = reg_read(0, &data);
+    if (rv) return rv;
+
+    data &= 0xF7;
+    dim &= 0x01;
+    dim = dim << 3;
+
+    rv = reg_write(0, data | dim);
+    return rv;
+}
+
+int tps61052_get_ov(uint8_t *ov) {
+    int rv;
+
+    rv = reg_read(0, ov);
+    if (rv) return rv;
+
+    *ov = *ov >> 4;
+    *ov &= 0x03;
+    return EOS_OK;
+}
+
+int tps61052_set_ov(uint8_t ov) {
+    uint8_t data;
+    int rv;
+
+    rv = reg_read(0, &data);
+    if (rv) return rv;
+
+    data &= 0xCF;
+    ov &= 0x03;
+    ov = ov << 4;
+
+    rv = reg_write(0, data | ov);
+    return rv;
+}
+
+int tps61052_get_mode0(uint8_t *mode) {
+    int rv;
+
+    rv = reg_read(0, mode);
+    if (rv) return rv;
+
+    *mode = *mode >> 6;
+    *mode &= 0x03;
+    return EOS_OK;
+}
+
+int tps61052_set_mode0(uint8_t mode) {
+    uint8_t data;
+    int rv;
+
+    rv = reg_read(0, &data);
+    if (rv) return rv;
+
+    data &= 0x3F;
+    mode &= 0x03;
+    mode = mode << 6;
+
+    rv = reg_write(0, data | mode);
+    return rv;
+}
+
+/* register b01 */
+int tps61052_get_fc(uint8_t *fc) {
+    int rv;
+
+    rv = reg_read(1, fc);
+    if (rv) return rv;
+
+    *fc &= 0x07;
+    return EOS_OK;
+}
+
+int tps61052_set_fc(uint8_t fc) {
+    uint8_t data;
+    int rv;
+
+    rv = reg_read(1, &data);
+    if (rv) return rv;
+
+    data &= 0xF8;
+    fc &= 0x07;
+
+    fc = reg_write(1, data | fc);
+    return rv;
+}
+
+int tps61052_get_sft(uint8_t *sft) {
+    int rv;
+
+    rv = reg_read(1, sft);
+    if (rv) return rv;
+
+    *sft = *sft >> 3;
+    *sft &= 0x01;
+    return EOS_OK;
+}
+
+int tps61052_set_sft(uint8_t sft) {
+    uint8_t data;
+    int rv;
+
+    rv = reg_read(1, &data);
+    if (rv) return rv;
+
+    data &= 0xF7;
+    sft &= 0x01;
+    sft = sft << 3;
+
+    rv = reg_write(1, data | sft);
+    return rv;
+}
+
+int tps61052_get_stt(uint8_t *stt) {
+    int rv;
+
+    rv = reg_read(1, stt);
+    if (rv) return rv;
+
+    *stt = *stt >> 4;
+    *stt &= 0x01;
+    return EOS_OK;
+}
+
+int tps61052_set_stt(uint8_t stt) {
+    uint8_t data;
+    int rv;
+
+    rv = reg_read(1, &data);
+    if (rv) return rv;
+
+    data &= 0xEF;
+    stt &= 0x01;
+    stt = stt << 4;
+
+    rv = reg_write(1, data | stt);
+    return rv;
+}
+
+int tps61052_get_to(uint8_t *to) {
+    int rv;
+
+    rv = reg_read(1, to);
+    if (rv) return rv;
+
+    *to = *to >> 5;
+    *to &= 0x01;
+    return EOS_OK;
+}
+
+int tps61052_get_mode1(uint8_t *mode) {
+    int rv;
+
+    rv = reg_read(1, mode);
+    if (rv) return rv;
+
+    *mode = *mode >> 6;
+    *mode &= 0x03;
+    return EOS_OK;
+}
+
+int tps61052_set_mode1(uint8_t mode) {
+    uint8_t data;
+    int rv;
+
+    rv = reg_read(1, &data);
+    if (rv) return rv;
+
+    data &= 0x3F;
+    mode &= 0x03;
+    mode = mode << 6;
+
+    rv = reg_write(1, data | mode);
+    return rv;
+}
+
+/* register b10 */
+int tps61052_get_adc(uint8_t *adc) {
+    int rv;
+
+    rv = reg_read(2, adc);
+    if (rv) return rv;
+
+    *adc = *adc >> 3;
+    *adc &= 0x07;
+    return EOS_OK;
+}
+
+int tps61052_get_lf(uint8_t *lf) {
+    int rv;
+
+    rv = reg_read(2, lf);
+    if (rv) return rv;
+
+    *lf = *lf >> 6;
+    *lf &= 0x01;
+    return EOS_OK;
+}
+
+int tps61052_get_ot(uint8_t *ot) {
+    int rv;
+
+    rv = reg_read(2, ot);
+    if (rv) return rv;
+
+    *ot = *ot >> 7;
+    *ot &= 0x01;
+    return EOS_OK;
+}
+
+int tps61052_set_ilim(uint8_t ilim) {
+    uint8_t data;
+    int rv;
+
+    rv = reg_read(2, &data);
+    if (rv) return rv;
+
+    data &= 0x9F;
+    ilim &= 0x03;
+    ilim = ilim << 5;
+
+    rv = reg_write(2, data | ilim);
+    return rv;
+}
+
+/* register b11 */
+int tps61052_get_stim(uint8_t *stim) {
+    int rv;
+
+    rv = reg_read(3, stim);
+    if (rv) return rv;
+
+    *stim &= 0x1F;
+    return EOS_OK;
+}
+
+int tps61052_set_stim(uint8_t stim) {
+    uint8_t data;
+    int rv;
+
+    rv = reg_read(3, &data);
+    if (rv) return rv;
+
+    data &= 0xE0;
+    stim &= 0x1F;
+
+    stim = reg_write(3, data | stim);
+    return rv;
+}
+
+int tps61052_get_dctim(uint8_t *dctim) {
+    int rv;
+
+    rv = reg_read(3, dctim);
+    if (rv) return rv;
+
+    *dctim = *dctim >> 5;
+    *dctim &= 0x07;
+    return EOS_OK;
+}
+
+int tps61052_set_dctim(uint8_t dctim) {
+    uint8_t data;
+    int rv;
+
+    rv = reg_read(3, &data);
+    if (rv) return rv;
+
+    data &= 0x1F;
+    dctim &= 0x07;
+    dctim = dctim << 5;
+
+    rv = reg_write(3, data | dctim);
+    return rv;
+}
diff --git a/fw/fe310/eos/dev/drv/tps61052.h b/fw/fe310/eos/dev/drv/tps61052.h
new file mode 100644
index 0000000..6232676
--- /dev/null
+++ b/fw/fe310/eos/dev/drv/tps61052.h
@@ -0,0 +1,34 @@
+#include <stdint.h>
+
+/* register b00 */
+int tps61052_get_tc(uint8_t *tc);
+int tps61052_set_tc(uint8_t tc);
+int tps61052_get_dim(uint8_t *dim);
+int tps61052_set_dim(uint8_t dim);
+int tps61052_get_ov(uint8_t *ov);
+int tps61052_set_ov(uint8_t ov);
+int tps61052_get_mode0(uint8_t *mode);
+int tps61052_set_mode0(uint8_t mode);
+
+/* register b01 */
+int tps61052_get_fc(uint8_t *fc);
+int tps61052_set_fc(uint8_t fc);
+int tps61052_get_sft(uint8_t *sft);
+int tps61052_set_sft(uint8_t sft);
+int tps61052_get_stt(uint8_t *stt);
+int tps61052_set_stt(uint8_t stt);
+int tps61052_get_to(uint8_t *to);
+int tps61052_get_mode1(uint8_t *mode);
+int tps61052_set_mode1(uint8_t mode);
+
+/* register b10 */
+int tps61052_get_adc(uint8_t *adc);
+int tps61052_get_lf(uint8_t *lf);
+int tps61052_get_ot(uint8_t *ot);
+int tps61052_set_ilim(uint8_t ilim);
+
+/* register b11 */
+int tps61052_get_stim(uint8_t *stim);
+int tps61052_set_stim(uint8_t stim);
+int tps61052_get_dctim(uint8_t *dctim);
+int tps61052_set_dctim(uint8_t dctim);