diff --git a/firmware/include/ov7670.h b/firmware/include/ov7670.h
index b577599..c10eff4 100644
--- a/firmware/include/ov7670.h
+++ b/firmware/include/ov7670.h
@@ -6,77 +6,215 @@
 #include <avr/io.h>
 #include <util/delay.h>
 
+#include "twi.h"
+
+static const uint8_t OV7670_ADDRESS = 0x42;
+
 void ov7670_init(void);
 
-static inline uint8_t ov7670_vsync(void) {
-    return (PINC & (1 << PC0)) != 0;
+struct ov7670_register {
+  uint8_t reg_num;
+  uint8_t value;
+};
+
+static inline uint8_t ov7670_write8(uint8_t reg, uint8_t value) {
+  uint8_t data[2] = {reg, value};
+  return twi_write(OV7670_ADDRESS, data, 2);
 }
 
+static inline uint8_t ov7670_vsync(void) { return (PINC & (1 << PC0)) != 0; }
+
 static inline void ov7670_wait_vsync(void) {
-    while(ov7670_vsync());  // Wait until low
-    while(!ov7670_vsync()); // Wait until high
+  while (ov7670_vsync())
+    ; // Wait until low
+  while (!ov7670_vsync())
+    ; // Wait until high
 }
 
-static inline void ov7670_set_rst(void) {
-    PORTD |= (1 << PD3);
-}
+static inline void ov7670_set_rst(void) { PORTD |= (1 << PD3); }
 
-static inline void ov7670_unset_rst(void) {
-    PORTD &= ~(1 << PD3);
-}
+static inline void ov7670_unset_rst(void) { PORTD &= ~(1 << PD3); }
 
-static inline void ov7670_set_wrst(void) {
-    PORTC |= (1 << PC3);
-}
+static inline void ov7670_set_wrst(void) { PORTC |= (1 << PC3); }
 
-static inline void ov7670_unset_wrst(void) {
-    PORTC &= ~(1 << PC3);
-}
+static inline void ov7670_unset_wrst(void) { PORTC &= ~(1 << PC3); }
 
 static inline void ov7670_toggle_wrst(void) {
-    ov7670_set_wrst();
-    _delay_us(5);
-    ov7670_unset_wrst();
+  ov7670_set_wrst();
+  _delay_us(5);
+  ov7670_unset_wrst();
 }
 
-static inline void ov7670_set_rrst(void) {
-    PORTC |= (1 << PC2);
-}
+static inline void ov7670_set_rrst(void) { PORTC |= (1 << PC2); }
 
-static inline void ov7670_unset_rrst(void) {
-    PORTC &= ~(1 << PC2);
-}
+static inline void ov7670_unset_rrst(void) { PORTC &= ~(1 << PC2); }
 
 static inline void ov7670_toggle_rrst(void) {
-    ov7670_set_rrst();
-    _delay_us(5);
-    ov7670_unset_rrst();
+  ov7670_set_rrst();
+  _delay_us(5);
+  ov7670_unset_rrst();
 }
 
-static inline void ov7670_set_we(void) {
-    PORTC |= (1 << PC1);
-}
+static inline void ov7670_set_we(void) { PORTC |= (1 << PC1); }
 
-static inline void ov7670_unset_we(void) {
-    PORTC &= ~(1 << PC1);
-}
+static inline void ov7670_unset_we(void) { PORTC &= ~(1 << PC1); }
 
-static inline void ov7670_set_rck(void) {
-    PORTB |= (1 << PB5);
-}
+static inline void ov7670_set_rck(void) { PORTB |= (1 << PB5); }
 
-static inline void ov7670_unset_rck(void) {
-    PORTB &= ~(1 << PB5);
-}
+static inline void ov7670_unset_rck(void) { PORTB &= ~(1 << PB5); }
 
 static inline void ov7670_toggle_rck(void) {
-    ov7670_set_rck();
-    _delay_us(5);
-    ov7670_unset_rck();
+  ov7670_set_rck();
+  _delay_us(5);
+  ov7670_unset_rck();
 }
 
 static inline uint8_t ov7670_read_bits(void) {
-    return (PINB & 0x0F) | (PIND & 0xF0);
+  return (PINB & 0x0F) | (PIND & 0xF0);
 }
 
+
+// Register List borrowed from https://github.com/dasaki/arduvision
+/* Registers */
+static const uint8_t REG_GAIN = 0x00;     /* Gain lower 8 bits (rest in vref) */
+static const uint8_t REG_BLUE = 0x01;     /* blue gain */
+static const uint8_t REG_RED = 0x02;      /* red gain */
+static const uint8_t REG_VREF = 0x03;     /* Pieces of GAIN, VSTART, VSTOP */
+static const uint8_t REG_COM1 = 0x04;     /* Control 1 */
+static const uint8_t COM1_CCIR656 = 0x40; /* CCIR656 enable */
+
+static const uint8_t REG_BAVE = 0x05;     /* U/B Average level */
+static const uint8_t REG_GbAVE = 0x06;    /* Y/Gb Average level */
+static const uint8_t REG_AECHH = 0x07;    /* AEC MS 5 bits */
+static const uint8_t REG_RAVE = 0x08;     /* V/R Average level */
+static const uint8_t REG_COM2 = 0x09;     /* Control 2 */
+static const uint8_t COM2_SSLEEP = 0x10;  /* Soft sleep mode */
+static const uint8_t REG_PID = 0x0a;      /* Product ID MSB */
+static const uint8_t REG_VER = 0x0b;      /* Product ID LSB */
+static const uint8_t REG_COM3 = 0x0c;     /* Control 3 */
+static const uint8_t COM3_SWAP = 0x40;    /* Byte swap */
+static const uint8_t COM3_SCALEEN = 0x08; /* Enable scaling */
+static const uint8_t COM3_DCWEN = 0x04;   /* Enable downsamp/crop/window */
+static const uint8_t REG_COM4 = 0x0d;     /* Control 4 */
+static const uint8_t REG_COM5 = 0x0e;     /* All "reserved" */
+static const uint8_t REG_COM6 = 0x0f;     /* Control 6 */
+static const uint8_t REG_AECH = 0x10;     /* More bits of AEC value */
+static const uint8_t REG_CLKRC = 0x11;    /* Clocl control */
+static const uint8_t CLK_EXT = 0x40;      /* Use external clock directly */
+static const uint8_t CLK_SCALE = 0x3f;    /* Mask for internal clock scale */
+static const uint8_t REG_COM7 = 0x12; /* Control 7 */ // REG mean address.
+static const uint8_t COM7_RESET = 0x80;               /* Register reset */
+static const uint8_t COM7_FMT_MASK = 0x38;
+static const uint8_t COM7_FMT_VGA = 0x00;
+static const uint8_t COM7_FMT_CIF = 0x20;   /* CIF format */
+static const uint8_t COM7_FMT_QVGA = 0x10;  /* QVGA format */
+static const uint8_t COM7_FMT_QCIF = 0x08;  /* QCIF format */
+static const uint8_t COM7_RGB = 0x04;       /* bits 0 and 2 - RGB format */
+static const uint8_t COM7_YUV = 0x00;       /* YUV */
+static const uint8_t COM7_BAYER = 0x01;     /* Bayer format */
+static const uint8_t COM7_PBAYER = 0x05;    /* "Processed bayer" */
+static const uint8_t REG_COM8 = 0x13;       /* Control 8 */
+static const uint8_t COM8_FASTAEC = 0x80;   /* Enable fast AGC/AEC */
+static const uint8_t COM8_AECSTEP = 0x40;   /* Unlimited AEC step size */
+static const uint8_t COM8_BFILT = 0x20;     /* Band filter enable */
+static const uint8_t COM8_AGC = 0x04;       /* Auto gain enable */
+static const uint8_t COM8_AWB = 0x02;       /* White balance enable */
+static const uint8_t COM8_AEC = 0x01;       /* Auto exposure enable */
+static const uint8_t REG_COM9 = 0x14;       /* Control 9- gain ceiling */
+static const uint8_t REG_COM10 = 0x15;      /* Control 10 */
+static const uint8_t COM10_HSYNC = 0x40;    /* HSYNC instead of HREF */
+static const uint8_t COM10_PCLK_HB = 0x20;  /* Suppress PCLK on horiz blank */
+static const uint8_t COM10_HREF_REV = 0x08; /* Reverse HREF */
+static const uint8_t COM10_VS_LEAD = 0x04;  /* VSYNC on clock leading edge */
+static const uint8_t COM10_VS_NEG = 0x02;   /* VSYNC negative */
+static const uint8_t COM10_HS_NEG = 0x01;   /* HSYNC negative */
+static const uint8_t REG_HSTART = 0x17;     /* Horiz start high bits */
+static const uint8_t REG_HSTOP = 0x18;      /* Horiz stop high bits */
+static const uint8_t REG_VSTART = 0x19;     /* Vert start high bits */
+static const uint8_t REG_VSTOP = 0x1a;      /* Vert stop high bits */
+static const uint8_t REG_PSHFT = 0x1b;      /* Pixel delay after HREF */
+static const uint8_t REG_MIDH = 0x1c;       /* Manuf. ID high */
+static const uint8_t REG_MIDL = 0x1d;       /* Manuf. ID low */
+static const uint8_t REG_MVFP = 0x1e;       /* Mirror / vflip */
+static const uint8_t MVFP_MIRROR = 0x20;    /* Mirror image */
+static const uint8_t MVFP_FLIP = 0x10;      /* Vertical flip */
+
+static const uint8_t REG_AEW = 0x24;      /* AGC upper limit */
+static const uint8_t REG_AEB = 0x25;      /* AGC lower limit */
+static const uint8_t REG_VPT = 0x26;      /* AGC/AEC fast mode op region */
+static const uint8_t REG_HSYST = 0x30;    /* HSYNC rising edge delay */
+static const uint8_t REG_HSYEN = 0x31;    /* HSYNC falling edge delay */
+static const uint8_t REG_HREF = 0x32;     /* HREF pieces */
+static const uint8_t REG_TSLB = 0x3a;     /* lots of stuff */
+static const uint8_t TSLB_YLAST = 0x04;   /* UYVY or VYUY - see com13 */
+static const uint8_t REG_COM11 = 0x3b;    /* Control 11 */
+static const uint8_t COM11_NIGHT = 0x80;  /* NIght mode enable */
+static const uint8_t COM11_NMFR = 0x60;   /* Two bit NM frame rate */
+static const uint8_t COM11_HZAUTO = 0x10; /* Auto detect 50/60 Hz */
+static const uint8_t COM11_50HZ = 0x08;   /* Manual 50Hz select */
+static const uint8_t COM11_EXP = 0x02;
+static const uint8_t REG_COM12 = 0x3c;     /* Control 12 */
+static const uint8_t COM12_HREF = 0x80;    /* HREF always */
+static const uint8_t REG_COM13 = 0x3d;     /* Control 13 */
+static const uint8_t COM13_GAMMA = 0x80;   /* Gamma enable */
+static const uint8_t COM13_UVSAT = 0x40;   /* UV saturation auto adjustment */
+static const uint8_t COM13_UVSWAP = 0x01;  /* V before U - w/TSLB */
+static const uint8_t REG_COM14 = 0x3e;     /* Control 14 */
+static const uint8_t COM14_DCWEN = 0x10;   /* DCW/PCLK-scale enable */
+static const uint8_t REG_EDGE = 0x3f;      /* Edge enhancement factor */
+static const uint8_t REG_COM15 = 0x40;     /* Control 15 */
+static const uint8_t COM15_R10F0 = 0x00;   /* Data range 10 to F0 */
+static const uint8_t COM15_R01FE = 0x80;   /*      01 to FE */
+static const uint8_t COM15_R00FF = 0xc0;   /*      00 to FF */
+static const uint8_t COM15_RGB565 = 0x10;  /* RGB565 output */
+static const uint8_t COM15_RGB555 = 0x30;  /* RGB555 output */
+static const uint8_t REG_COM16 = 0x41;     /* Control 16 */
+static const uint8_t COM16_AWBGAIN = 0x08; /* AWB gain enable */
+static const uint8_t REG_COM17 = 0x42;     /* Control 17 */
+static const uint8_t COM17_AECWIN = 0xc0;  /* AEC window - must match COM4 */
+static const uint8_t COM17_CBAR = 0x08;    /* DSP Color bar */
+/*
+ * This matrix defines how the colors are generated, must be
+ * tweaked to adjust hue and saturation.
+ *
+ * Order: v-red, v-green, v-blue, u-red, u-green, u-blue
+ * They are nine-bit signed quantities, with the sign bit
+ * stored in0x58.Sign for v-red is bit 0, and up from there.
+ */
+static const uint8_t REG_CMATRIX_BASE = 0x4f;
+static const uint8_t CMATRIX_LEN = 6;
+static const uint8_t REG_CMATRIX_SIGN = 0x58;
+static const uint8_t REG_BRIGHT = 0x55;   /* Brightness */
+static const uint8_t REG_CONTRAS = 0x56;  /* Contrast control */
+static const uint8_t REG_GFIX = 0x69;     /* Fix gain control */
+static const uint8_t REG_REG76 = 0x76;    /* OV's name */
+static const uint8_t R76_BLKPCOR = 0x80;  /* Black pixel correction enable */
+static const uint8_t R76_WHTPCOR = 0x40;  /* White pixel correction enable */
+static const uint8_t REG_RGB444 = 0x8c;   /* RGB 444 control */
+static const uint8_t R444_ENABLE = 0x02;  /* Turn on RGB444, overrides 5x5 */
+static const uint8_t R444_RGBX = 0x01;    /* Empty nibble at end */
+static const uint8_t REG_HAECC1 = 0x9f;   /* Hist AEC/AGC control 1 */
+static const uint8_t REG_HAECC2 = 0xa0;   /* Hist AEC/AGC control 2 */
+static const uint8_t REG_BD50MAX = 0xa5;  /* 50hz banding step limit */
+static const uint8_t REG_HAECC3 = 0xa6;   /* Hist AEC/AGC control 3 */
+static const uint8_t REG_HAECC4 = 0xa7;   /* Hist AEC/AGC control 4 */
+static const uint8_t REG_HAECC5 = 0xa8;   /* Hist AEC/AGC control 5 */
+static const uint8_t REG_HAECC6 = 0xa9;   /* Hist AEC/AGC control 6 */
+static const uint8_t REG_HAECC7 = 0xaa;   /* Hist AEC/AGC control 7 */
+static const uint8_t REG_BD60MAX = 0xab;  /* 60hz banding step limit */
+static const uint8_t MTXS = 0x58;        /* Matrix Coefficient Sign */
+static const uint8_t AWBC7 = 0x59;       /* AWB Control 7 */
+static const uint8_t AWBC8 = 0x5a;       /* AWB Control 8 */
+static const uint8_t AWBC9 = 0x5b;       /* AWB Control 9 */
+static const uint8_t AWBC10 = 0x5c;      /* AWB Control 10 */
+static const uint8_t AWBC11 = 0x5d;      /* AWB Control 11 */
+static const uint8_t AWBC12 = 0x5e;      /* AWB Control 12 */
+static const uint8_t REG_GFI = 0x69;     /* Fix gain control */
+static const uint8_t GGAIN = 0x6a;       /* G Channel AWB Gain */
+static const uint8_t DBLV = 0x6b;
+static const uint8_t AWBCTR3 = 0x6c; /* AWB Control 3 */
+static const uint8_t AWBCTR2 = 0x6d; /* AWB Control 2 */
+static const uint8_t AWBCTR1 = 0x6e; /* AWB Control 1 */
+static const uint8_t AWBCTR0 = 0x6f; /* AWB Control 0 */
+
 #endif
diff --git a/firmware/ov7670.c b/firmware/ov7670.c
index 540a4d2..e3383a7 100644
--- a/firmware/ov7670.c
+++ b/firmware/ov7670.c
@@ -1,5 +1,133 @@
+#include <avr/pgmspace.h>
+
 #include "ov7670.h"
 
+// Initialiezer Lists borrowed from https://github.com/dasaki/arduvision
+const struct ov7670_register yuv422_ov7670[] PROGMEM = {
+  { REG_COM7, 0x0 },  /* Selects YUV mode */
+  { REG_RGB444, 0 },  /* No RGB444 please */
+  { REG_COM1, 0 },
+  { REG_COM15, COM15_R00FF },
+  { REG_COM9, 0x6A }, /* 128x gain ceiling; 0x8 is reserved bit */
+  { 0x4f, 0x80 },   /* "matrix coefficient 1" */
+  { 0x50, 0x80 },   /* "matrix coefficient 2" */
+  { 0x51, 0 },    /* vb */
+  { 0x52, 0x22 },   /* "matrix coefficient 4" */
+  { 0x53, 0x5e },   /* "matrix coefficient 5" */
+  { 0x54, 0x80 },   /* "matrix coefficient 6" */
+  { REG_COM13, COM13_UVSAT },
+  { 0xff, 0xff },   /* END MARKER */
+};
+
+//from the linux driver
+const struct ov7670_register ov7670_default_regs[] PROGMEM = {
+  { REG_COM7, COM7_RESET },
+  { REG_TSLB, 0x04 }, /* OV */
+  { REG_COM7, 0 },  /* VGA */
+  /*
+  * Set the hardware window.  These values from OV don't entirely
+  * make sense - hstop is less than hstart.  But they work...
+  */
+  { REG_HSTART, 0x13 }, { REG_HSTOP, 0x01 },
+  { REG_HREF, 0xb6 }, { REG_VSTART, 0x02 },
+  { REG_VSTOP, 0x7a }, { REG_VREF, 0x0a },
+
+  { REG_COM3, 0 }, { REG_COM14, 0 },
+  /* Mystery scaling numbers */
+  { 0x70, 0x3a }, { 0x71, 0x35 },
+  { 0x72, 0x11 }, { 0x73, 0xf0 },
+  { 0xa2,/* 0x02 changed to 1*/1 }, { REG_COM10, 0x0 },
+  /* Gamma curve values */
+  { 0x7a, 0x20 }, { 0x7b, 0x10 },
+  { 0x7c, 0x1e }, { 0x7d, 0x35 },
+  { 0x7e, 0x5a }, { 0x7f, 0x69 },
+  { 0x80, 0x76 }, { 0x81, 0x80 },
+  { 0x82, 0x88 }, { 0x83, 0x8f },
+  { 0x84, 0x96 }, { 0x85, 0xa3 },
+  { 0x86, 0xaf }, { 0x87, 0xc4 },
+  { 0x88, 0xd7 }, { 0x89, 0xe8 },
+  /* AGC and AEC parameters.  Note we start by disabling those features,
+  then turn them only after tweaking the values. */
+  { REG_COM8, COM8_FASTAEC | COM8_AECSTEP },
+  { REG_GAIN, 0 }, { REG_AECH, 0 },
+  { REG_COM4, 0x40 }, /* magic reserved bit */
+  { REG_COM9, 0x18 }, /* 4x gain + magic rsvd bit */
+  { REG_BD50MAX, 0x05 }, { REG_BD60MAX, 0x07 },
+  { REG_AEW, 0x95 }, { REG_AEB, 0x33 },
+  { REG_VPT, 0xe3 }, { REG_HAECC1, 0x78 },
+  { REG_HAECC2, 0x68 }, { 0xa1, 0x03 }, /* magic */
+  { REG_HAECC3, 0xd8 }, { REG_HAECC4, 0xd8 },
+  { REG_HAECC5, 0xf0 }, { REG_HAECC6, 0x90 },
+  { REG_HAECC7, 0x94 },
+  { REG_COM8, COM8_FASTAEC | COM8_AECSTEP | COM8_AGC | COM8_AEC },
+  { 0x30, 0 }, { 0x31, 0 },//disable some delays
+  /* Almost all of these are magic "reserved" values.  */
+  { REG_COM5, 0x61 }, { REG_COM6, 0x4b },
+  { 0x16, 0x02 }, { REG_MVFP, 0x07 },
+  { 0x21, 0x02 }, { 0x22, 0x91 },
+  { 0x29, 0x07 }, { 0x33, 0x0b },
+  { 0x35, 0x0b }, { 0x37, 0x1d },
+  { 0x38, 0x71 }, { 0x39, 0x2a },
+  { REG_COM12, 0x78 }, { 0x4d, 0x40 },
+  { 0x4e, 0x20 }, { REG_GFIX, 0 },
+  /*{0x6b, 0x4a},*/{ 0x74, 0x10 },
+  { 0x8d, 0x4f }, { 0x8e, 0 },
+  { 0x8f, 0 }, { 0x90, 0 },
+  { 0x91, 0 }, { 0x96, 0 },
+  { 0x9a, 0 }, { 0xb0, 0x84 },
+  { 0xb1, 0x0c }, { 0xb2, 0x0e },
+  { 0xb3, 0x82 }, { 0xb8, 0x0a },
+
+  /* More reserved magic, some of which tweaks white balance */
+  { 0x43, 0x0a }, { 0x44, 0xf0 },
+  { 0x45, 0x34 }, { 0x46, 0x58 },
+  { 0x47, 0x28 }, { 0x48, 0x3a },
+  { 0x59, 0x88 }, { 0x5a, 0x88 },
+  { 0x5b, 0x44 }, { 0x5c, 0x67 },
+  { 0x5d, 0x49 }, { 0x5e, 0x0e },
+  { 0x6c, 0x0a }, { 0x6d, 0x55 },
+  { 0x6e, 0x11 }, { 0x6f, 0x9e }, /* it was 0x9F "9e for advance AWB" */
+  { 0x6a, 0x40 }, { REG_BLUE, 0x40 },
+  { REG_RED, 0x60 },
+  { REG_COM8, COM8_FASTAEC | COM8_AECSTEP | COM8_AGC | COM8_AEC | COM8_AWB },
+
+  /* Matrix coefficients */
+  { 0x4f, 0x80 }, { 0x50, 0x80 },
+  { 0x51, 0 },    { 0x52, 0x22 },
+  { 0x53, 0x5e }, { 0x54, 0x80 },
+  { 0x58, 0x9e },
+
+  { REG_COM16, COM16_AWBGAIN }, { REG_EDGE, 0 },
+  { 0x75, 0x05 }, { REG_REG76, 0xe1 },
+  { 0x4c, 0 },     { 0x77, 0x01 },
+  { REG_COM13, /*0xc3*/0x48 }, { 0x4b, 0x09 },
+  { 0xc9, 0x60 },   /*{REG_COM16, 0x38},*/
+  { 0x56, 0x40 },
+
+  { 0x34, 0x11 }, { REG_COM11, COM11_EXP | COM11_HZAUTO },
+  { 0xa4, 0x82/*Was 0x88*/ }, { 0x96, 0 },
+  { 0x97, 0x30 }, { 0x98, 0x20 },
+  { 0x99, 0x30 }, { 0x9a, 0x84 },
+  { 0x9b, 0x29 }, { 0x9c, 0x03 },
+  { 0x9d, 0x4c }, { 0x9e, 0x3f },
+  { 0x78, 0x04 },
+
+  /* Extra-weird stuff.  Some sort of multiplexor register */
+  { 0x79, 0x01 }, { 0xc8, 0xf0 },
+  { 0x79, 0x0f }, { 0xc8, 0x00 },
+  { 0x79, 0x10 }, { 0xc8, 0x7e },
+  { 0x79, 0x0a }, { 0xc8, 0x80 },
+  { 0x79, 0x0b }, { 0xc8, 0x01 },
+  { 0x79, 0x0c }, { 0xc8, 0x0f },
+  { 0x79, 0x0d }, { 0xc8, 0x20 },
+  { 0x79, 0x09 }, { 0xc8, 0x80 },
+  { 0x79, 0x02 }, { 0xc8, 0xc0 },
+  { 0x79, 0x03 }, { 0xc8, 0x40 },
+  { 0x79, 0x05 }, { 0xc8, 0x30 },
+  { 0x79, 0x26 },
+  { 0xff, 0xff }, /* END MARKER */
+};
+
 
 void ov7670_init(void) {
     // WRST, RRST, WE