Merge with Linux 2.3.40.

1 files changed, 531 insertions, 178 deletions

diff --git a/drivers/usb/ov511.c b/drivers/usb/ov511.c
index d0da393f3..c26f96132 100644
--- a/drivers/usb/ov511.c
+++ b/drivers/usb/ov511.c
@@ -11,8 +11,11 @@
  *    DEBUG - Debugging code.
  *    FIXME - Something that is broken or needs improvement.
  *
- * Version History:
- *    Version 1.00 - Initial version
+ * Version: 1.05
+ *
+ * Please see the file: linux/Documentation/usb/ov511.txt 
+ * and the website at:  http://people.delphi.com/mmcclelland/linux/ 
+ * for more info.
  */
 
 /*
@@ -63,13 +66,15 @@
 
 #define OV511_I2C_RETRIES 3
 
-/* Video Size 384 x 288 x 3 bytes for RGB */
-#define MAX_FRAME_SIZE (320 * 240 * 3)
+#define OV7610_AUTO_ADJUST 1
+
+/* Video Size 640 x 480 x 3 bytes for RGB */
+#define MAX_FRAME_SIZE (640 * 480 * 3)
 
 // FIXME - Force CIF to make some apps happy for the moment. Should find a 
 //         better way to do this.
-#define DEFAULT_WIDTH 320
-#define DEFAULT_HEIGHT 240
+#define DEFAULT_WIDTH 640
+#define DEFAULT_HEIGHT 480
 
 char kernel_version[] = UTS_RELEASE;
 
@@ -200,7 +205,9 @@ int ov511_reg_write(struct usb_device *dev, unsigned char reg, unsigned char val
 		USB_TYPE_CLASS | USB_RECIP_DEVICE,
 		0, (__u16)reg, &value, 1, HZ);	
 			
-	PDEBUG("reg write: 0x%02X:0x%02X\n", reg, value);
+#if 0
+	PDEBUG("reg write: 0x%02X:0x%02X", reg, value);
+#endif
 			
 	return rc;
 }
@@ -217,7 +224,9 @@ int ov511_reg_read(struct usb_device *dev, unsigned char reg)
 		USB_DIR_IN | USB_TYPE_CLASS | USB_RECIP_DEVICE,
 		0, (__u16)reg, buffer, 1, HZ);
                                
-	PDEBUG("reg read: 0x%02X:0x%02X\n", reg, buffer[0]);
+#if 0
+	PDEBUG("reg read: 0x%02X:0x%02X", reg, buffer[0]);
+#endif
 	
 	if(rc < 0)
 		return rc;
@@ -229,7 +238,9 @@ int ov511_i2c_write(struct usb_device *dev, unsigned char reg, unsigned char val
 {
 	int rc, retries;
 
-	PDEBUG("i2c write: 0x%02X:0x%02X\n", reg, value);
+#if 0
+	PDEBUG("i2c write: 0x%02X:0x%02X", reg, value);
+#endif
 	/* Three byte write cycle */
 	for(retries = OV511_I2C_RETRIES;;) {
 		/* Select camera register */
@@ -309,7 +320,9 @@ int ov511_i2c_read(struct usb_device *dev, unsigned char reg)
 	}
 
 	value = ov511_reg_read(dev, OV511_REG_I2C_DATA_PORT);
-	PDEBUG("i2c read: 0x%02X:0x%02X\n", reg, value);
+#if 0
+	PDEBUG("i2c read: 0x%02X:0x%02X", reg, value);
+#endif
 		
 	/* This is needed to make ov511_i2c_write() work */
 	rc = ov511_reg_write(dev, OV511_REG_I2C_CONTROL, 0x05);
@@ -318,27 +331,94 @@ int ov511_i2c_read(struct usb_device *dev, unsigned char reg)
 	return (value);
 }
 
+#if 0
+static void ov511_dump_i2c_range( struct usb_device *dev, int reg1, int regn)
+{
+	int i;
+	int rc;
+	for(i=reg1; i<=regn; i++) {
+	  rc = ov511_i2c_read(dev, i);
+#if 0
+	  PDEBUG("OV7610[0x%X] = 0x%X", i, rc);
+#endif
+	}
+}
+
+static void ov511_dump_i2c_regs( struct usb_device *dev)
+{
+	PDEBUG("I2C REGS");
+	ov511_dump_i2c_range(dev, 0x00, 0x38);
+}
+
+static void ov511_dump_reg_range( struct usb_device *dev, int reg1, int regn)
+{
+	int i;
+	int rc;
+	for(i=reg1; i<=regn; i++) {
+	  rc = ov511_reg_read(dev, i);
+	  PDEBUG("OV511[0x%X] = 0x%X", i, rc);
+	}
+}
+
+static void ov511_dump_regs( struct usb_device *dev)
+{
+	PDEBUG("CAMERA INTERFACE REGS");
+	ov511_dump_reg_range(dev, 0x10, 0x1f);
+	PDEBUG("DRAM INTERFACE REGS");
+	ov511_dump_reg_range(dev, 0x20, 0x23);
+	PDEBUG("ISO FIFO REGS");
+	ov511_dump_reg_range(dev, 0x30, 0x31);
+	PDEBUG("PIO REGS");
+	ov511_dump_reg_range(dev, 0x38, 0x39);
+	ov511_dump_reg_range(dev, 0x3e, 0x3e);
+	PDEBUG("I2C REGS");
+	ov511_dump_reg_range(dev, 0x40, 0x49);
+	PDEBUG("SYSTEM CONTROL REGS");
+	ov511_dump_reg_range(dev, 0x50, 0x53);
+	ov511_dump_reg_range(dev, 0x5e, 0x5f);
+	PDEBUG("OmniCE REGS");
+	ov511_dump_reg_range(dev, 0x70, 0x79);
+	ov511_dump_reg_range(dev, 0x80, 0x9f);
+	ov511_dump_reg_range(dev, 0xa0, 0xbf);
+
+}
+#endif
+
+int ov511_i2c_reset(struct usb_device *dev)
+{
+	int rc;
+
+	PDEBUG("Reset 7610");
+	rc = ov511_i2c_write(dev, 0x12, 0x80);
+	if (rc < 0)
+		err("i2c reset: command failed");
+
+	return rc;
+}
+
 int ov511_reset(struct usb_device *dev, unsigned char reset_type)
 {
 	int rc;
 	
-	PDEBUG("Reset: type=0x%X\n", reset_type);
+	PDEBUG("Reset: type=0x%X", reset_type);
 	rc = ov511_reg_write(dev, OV511_REG_SYSTEM_RESET, reset_type);
 	if (rc < 0)
-		printk(KERN_ERR "ov511: reset: command failed\n");
+		err("reset: command failed");
 
 	rc = ov511_reg_write(dev, OV511_REG_SYSTEM_RESET, 0);
 	if (rc < 0)
-		printk(KERN_ERR "ov511: reset: command failed\n");
+		err("reset: command failed");
 
 	return rc;
 }
 
 int ov511_set_packet_size(struct usb_ov511 *ov511, int size)
 {
-	int alt, multiplier, err;
+	int alt, multiplier, rc;
 		
-	PDEBUG("set packet size: %d\n", size);
+#if 0
+	PDEBUG("set packet size: %d", size);
+#endif
 	
 	switch (size) {
 		case 992:
@@ -374,21 +454,19 @@ int ov511_set_packet_size(struct usb_ov511 *ov511, int size)
 			multiplier = 1; // FIXME - is this correct?
 			break;
 		default:
-			printk(KERN_ERR "ov511_set_packet_size: invalid size (%d)\n",
-			       size);
+			err("Set packet size: invalid size (%d)", size);
 			return -EINVAL;
 	}
 
-	err = ov511_reg_write(ov511->dev, OV511_REG_FIFO_PACKET_SIZE,
-	                          multiplier);
-	if (err < 0) {
-		printk(KERN_ERR "ov511: Set packet size: Set FIFO size ret %d\n",
-		       err);
+	rc = ov511_reg_write(ov511->dev, OV511_REG_FIFO_PACKET_SIZE,
+	                     multiplier);
+	if (rc < 0) {
+		err("Set packet size: Set FIFO size ret %d", rc);
 		return -ENOMEM;
 	}
 	
 	if (usb_set_interface(ov511->dev, ov511->iface, alt) < 0) {
-		printk(KERN_ERR "ov511: Set packet size: set interface error\n");
+		err("Set packet size: set interface error");
 		return -EBUSY;
 	}
 
@@ -399,8 +477,258 @@ int ov511_set_packet_size(struct usb_ov511 *ov511, int size)
 	return 0;
 }
 
-/* How much data is left in the scratch buf? */
-#define scratch_left(x)	(ov511->scratchlen - (int)((char *)x - (char *)ov511->scratch))
+static inline int ov7610_set_picture(struct usb_ov511 *ov511,
+                                     struct video_picture *p)
+{
+	if(ov511_i2c_write(ov511->dev, OV7610_REG_SAT, p->colour >> 8) < 0)
+		return -EIO;
+
+	if(ov511_i2c_write(ov511->dev, OV7610_REG_CNT, p->contrast >> 8) < 0)
+		return -EIO;
+
+	if(ov511_i2c_write(ov511->dev, OV7610_REG_BRT, p->brightness >> 8) < 0)
+		return -EIO;
+
+	return 0;
+}
+
+static inline int ov7610_get_picture(struct usb_ov511 *ov511,
+                                     struct video_picture *p)
+{
+	int ret;
+
+	if((ret = ov511_i2c_read(ov511->dev, OV7610_REG_SAT)) < 0) return -EIO;
+	p->colour = ret << 8;
+
+	if((ret = ov511_i2c_read(ov511->dev, OV7610_REG_CNT)) < 0) return -EIO;
+	p->contrast = ret << 8;
+
+	if((ret = ov511_i2c_read(ov511->dev, OV7610_REG_BRT)) < 0) return -EIO;
+	p->brightness = ret << 8;
+
+	p->hue = 0x8000;
+	p->whiteness = 105 << 8;
+	p->depth = 24;
+	p->palette = VIDEO_PALETTE_RGB24;
+
+	return 0;
+}
+
+static int ov511_mode_init_regs(struct usb_ov511 *ov511,
+				int width, int height, int mode)
+{
+	int rc = 0;
+	struct usb_device *dev = ov511->dev;
+
+#if 0
+	PDEBUG("ov511_mode_init_regs(ov511, %d, %d, %d)", width, height, mode);
+#endif
+	ov511_set_packet_size(ov511, 0);
+
+	/* Set mode consistent registers */
+	ov511_i2c_write(dev, 0x0f, 0x03);
+	ov511_i2c_write(dev, 0x10, 0xff);
+	ov511_i2c_write(dev, 0x13, 0x01);
+	ov511_i2c_write(dev, 0x16, 0x06);
+	ov511_i2c_write(dev, 0x20, 0x1c);
+	ov511_i2c_write(dev, 0x24, 0x2e); /* 10 */
+	ov511_i2c_write(dev, 0x25, 0x7c); /* 8a */
+	ov511_i2c_write(dev, 0x26, 0x70);
+	ov511_i2c_write(dev, 0x28, 0x24); /* 24 */
+	ov511_i2c_write(dev, 0x2b, 0xac);
+	ov511_i2c_write(dev, 0x2c, 0xfe);
+	ov511_i2c_write(dev, 0x2d, 0x93);
+	ov511_i2c_write(dev, 0x34, 0x8b);
+
+	if (width == 640 && height == 480) {
+		ov511_reg_write(dev, 0x12, 0x4f);
+		ov511_reg_write(dev, 0x13, 0x3d);
+		ov511_reg_write(dev, 0x14, 0x00);
+		ov511_reg_write(dev, 0x15, 0x00);
+		ov511_reg_write(dev, 0x18, 0x03);
+
+		ov511_i2c_write(dev, 0x11, 0x01);
+		ov511_i2c_write(dev, 0x12, 0x24);
+		ov511_i2c_write(dev, 0x14, 0x04);
+		ov511_i2c_write(dev, 0x35, 0x9e);
+	} else if (width == 320 && height == 240) {
+		ov511_reg_write(dev, 0x12, 0x27);
+		ov511_reg_write(dev, 0x13, 0x1f);
+		ov511_reg_write(dev, 0x14, 0x00);
+		ov511_reg_write(dev, 0x15, 0x00);
+		ov511_reg_write(dev, 0x18, 0x03);
+
+		ov511_i2c_write(dev, 0x11, 0x00);
+		ov511_i2c_write(dev, 0x12, 0x04);
+		ov511_i2c_write(dev, 0x14, 0x24);
+		ov511_i2c_write(dev, 0x35, 0x1e);
+	} else {
+		err("Unknown mode (%d, %d): %d", width, height, mode);
+		rc = -EINVAL;
+	}
+	ov511_set_packet_size(ov511, 993);
+
+	return rc;
+}
+
+	
+/*************************************************************
+
+Turn a YUV4:2:0 block into an RGB block
+
+*************************************************************/
+#define LIMIT(x) ((((x)>0xffffff)?0xff0000:(((x)<=0xffff)?0:(x)&0xff0000))>>16)
+static inline void ov511_move_420_block(int y00, int y01, int y10, int y11,
+					int u, int v, int w,
+					unsigned char * pOut)
+{
+	int r    = 68911 * v;
+	int g    = -16915 * u + -35101 * v;
+	int b    = 87097 * u;
+	y00 *= 49152;
+	y01 *= 49152;
+	y10 *= 49152;
+	y11 *= 49152;
+	*(pOut+w*3) = LIMIT(r + y10);
+	*pOut++     = LIMIT(r + y00);
+	*(pOut+w*3) = LIMIT(g + y10);
+	*pOut++     = LIMIT(g + y00);
+	*(pOut+w*3) = LIMIT(b + y10);
+	*pOut++     = LIMIT(b + y00);
+	*(pOut+w*3) = LIMIT(r + y11);
+	*pOut++     = LIMIT(r + y01);
+	*(pOut+w*3) = LIMIT(g + y11);
+	*pOut++     = LIMIT(g + y01);
+	*(pOut+w*3) = LIMIT(b + y11);
+	*pOut++     = LIMIT(b + y01);
+}
+
+/***************************************************************
+
+For a 640x480 YUV4:2:0 images, data shows up in 1200 384 byte segments.  The
+first 64 bytes of each segment are V, the next 64 are U.  The V and
+U are arranged as follows:
+
+  0  1 ...  7
+  8  9 ... 15
+       ...   
+ 56 57 ... 63
+
+The next 256 bytes are Y data and represent 4 squares of 8x8 pixels as
+follows:
+
+  0  1 ...  7    64  65 ...  71   ...  192 193 ... 199
+  8  9 ... 15    72  73 ...  79        200 201 ... 207
+       ...              ...                    ...
+ 56 57 ... 63   120 121     127        248 249 ... 255
+
+If OV511_DUMPPIX is defined, _parse_data just dumps the
+incoming segments, verbatim, in order, into the frame.
+When used with vidcat -f ppm -s 640x480 this puts the data
+on the standard output and can be analyzed with the parseppm.c
+utility I wrote.  That's a much faster way for figuring out how
+this data is scrambled.
+
+****************************************************************/ 
+#define HDIV 8
+#define WDIV (256/HDIV)
+
+static void ov511_parse_data(unsigned char * pIn0,
+			     unsigned char * pOut0,
+			     int iWidth,
+			     int iSegment)
+			    
+{
+#ifndef OV511_DUMPPIX
+    int k, l, m;
+    unsigned char * pIn;
+    unsigned char * pOut, * pOut1;
+
+    int iHalf = (iSegment / (iWidth / 32)) & 1;
+    int iY     = iSegment / (iWidth / WDIV);
+    int jY     = iSegment - iY * (iWidth / WDIV);
+    int iOutY  = (iY*HDIV*iWidth + jY*WDIV) * 3;
+    int iUV    = iSegment / (iWidth / WDIV * 2);
+    int jUV    = iSegment - iUV * (iWidth / WDIV * 2);
+    int iOutUV = (iUV*HDIV*2*iWidth + jUV*WDIV/2) * 3;
+
+    /* Just copy the Y's if in the first stripe */
+    if (!iHalf) {
+	pIn = pIn0 + 128;
+	pOut = pOut0 + iOutY;
+	for(k=0; k<4; k++) {
+	    pOut1 = pOut;
+	    for(l=0; l<8; l++) {
+	      for(m=0; m<8; m++) {
+		*pOut1 = *pIn++;
+		pOut1 += 3;
+	      }
+	      pOut1 += (iWidth - 8) * 3;
+	    }
+	    pOut += 8 * 3;
+	}
+    }
+
+    /* Use the first half of VUs to calculate value */
+    pIn = pIn0;
+    pOut = pOut0 + iOutUV;
+    for(l=0; l<4; l++) {
+	for(m=0; m<8; m++) {
+	    int y00 = *(pOut);
+	    int y01 = *(pOut+3);
+	    int y10 = *(pOut+iWidth*3);
+	    int y11 = *(pOut+iWidth*3+3);
+	    int u   = *(pIn+64) - 128;
+	    int v   = *pIn++ - 128;
+	    ov511_move_420_block(y00, y01, y10, y11, u, v, iWidth, pOut);
+	    pOut += 6;
+	}
+	pOut += (iWidth*2 - 16) * 3;
+    }
+
+    /* Just copy the other UV rows */
+    for(l=0; l<4; l++) {
+	for(m=0; m<8; m++) {
+	  *pOut++ = *(pIn + 64);
+	  *pOut = *pIn++;
+	  pOut += 5;
+	}
+	pOut += (iWidth*2 - 16) * 3;
+    }
+
+    /* Calculate values if it's the second half */
+    if (iHalf) {
+	pIn = pIn0 + 128;
+	pOut = pOut0 + iOutY;
+	for(k=0; k<4; k++) {
+	    pOut1 = pOut;
+	    for(l=0; l<4; l++) {
+	      for(m=0; m<4; m++) {
+		int y10 = *(pIn+8);
+		int y00 = *pIn++;
+		int y11 = *(pIn+8);
+		int y01 = *pIn++;
+		int u   = *pOut1 - 128;
+		int v   = *(pOut1+1) - 128;
+		ov511_move_420_block(y00, y01, y10, y11, u, v, iWidth, pOut1);
+		pOut1 += 6;
+	      }
+	      pOut1 += (iWidth*2 - 8) * 3;
+	      pIn += 8;
+	    }
+	    pOut += 8 * 3;
+	}
+    }
+
+#else
+	/* Just dump pix data straight out for debug */
+	int i;
+	pOut0 += iSegment * 384;
+	for(i=0; i<384; i++) {
+	  *pOut0++ = *pIn0++;
+	}
+#endif
+}
 
 static int ov511_move_data(struct usb_ov511 *ov511, urb_t *urb)
 {
@@ -421,13 +749,10 @@ static int ov511_move_data(struct usb_ov511 *ov511, urb_t *urb)
 
 		if (!n) continue;
 
-		aPackNum[i] = n ? cdata[512] : -1;
+		aPackNum[i] = n ? cdata[992] : -1;
 
-		if (st){ 
-			// Macro - must be in braces!
-			PDEBUG("data error: [%d] len=%d, status=%d\n",
-				i, n, st);
-		}
+		if (st)
+			PDEBUG("data error: [%d] len=%d, status=%d", i, n, st);
 
 		frame = &ov511->frame[ov511->curframe];
 		
@@ -436,13 +761,18 @@ static int ov511_move_data(struct usb_ov511 *ov511, urb_t *urb)
 		     cdata[4] | cdata[5] | cdata[6] | cdata[7]) == 0 &&
 		    (cdata[8] & 8) && (cdata[8] & 0x80)) {
 
-		    PDEBUG("Found Frame End!, packnum = %d\n", (int)(cdata[512]));
-		    PDEBUG("Current frame = %d\n", ov511->curframe);
+#if 0
+		    PDEBUG("Found Frame End!, packnum = %d", (int)(cdata[992]));
+		    PDEBUG("Current frame = %d", ov511->curframe);
+#endif
 
 		    if (frame->scanstate == STATE_LINES) {
 		        if (waitqueue_active(&frame->wq)) {
-			  PDEBUG("About to wake up waiting processes\n");
+#if 0
+			  PDEBUG("About to wake up waiting processes");
+#endif
 			  frame->grabstate = FRAME_DONE;
+			  ov511->curframe = -1;
 			  wake_up_interruptible(&frame->wq);
 			}
 		    }
@@ -452,35 +782,52 @@ static int ov511_move_data(struct usb_ov511 *ov511, urb_t *urb)
 		else if ((cdata[0] | cdata[1] | cdata[2] | cdata[3] | 
 			  cdata[4] | cdata[5] | cdata[6] | cdata[7]) == 0 &&
 			 (cdata[8] & 8)) {
-		    PDEBUG("ov511: Found Frame Start!, packnum = %d\n", (int)(cdata[512]));
+#if 0
+			PDEBUG("ov511: Found Frame Start!, packnum = %d", (int)(cdata[992]));
+			PDEBUG("ov511: Frame Header Byte = 0x%x", (int)(cdata[8]));
+#endif
 		    frame->scanstate = STATE_LINES;
-		    frame->curpix = 0;
+			frame->segment = 0;
 		}
 
 		/* Are we in a frame? */
-		else if (frame->scanstate == STATE_LINES) {
-		  unsigned char *f = frame->data + 3 * frame->curpix;
-		  int i;
-		  if (frame->curpix <= 320 * 240 - 256) {
-		    for (i=0; i<256; i++) {
-		      *f++ = *cdata;
-		      *f++ = *cdata;
-		      *f++ = *cdata++;
-		      *f++ = *cdata;
-		      *f++ = *cdata;
-		      *f++ = *cdata++;
-		    }
-		    frame->curpix += 512;
+		if (frame->scanstate == STATE_LINES) {
+			unsigned char * pData;
+			int iPix;
+
+			/* Deal with leftover from last segment, if any */
+			if (frame->segment) {
+			  pData = ov511->scratch;
+			  iPix = - ov511->scratchlen;
+			  memmove(pData + ov511->scratchlen, cdata, iPix+384);
 		  } else {
-		    PDEBUG("Too many pixels!\n");
+			  pData = &cdata[iPix = 9];
 		  }
+
+			/* Parse the segments */
+			while(iPix <= 992 - 384 &&
+			      frame->segment < frame->width * frame->height / 256) {
+			  ov511_parse_data(pData, frame->data,
+					   frame->width,
+					   frame->segment);
+			  frame->segment++;
+			  iPix += 384;
+			  pData = &cdata[iPix];
 		}
 
+			/* Save extra data for next time */
+			if (frame->segment < frame->width * frame->height / 256) {
+			  memmove(ov511->scratch, pData, 992 - iPix);
+			  ov511->scratchlen = 992 - iPix;
+			}
+		}
 	}
 
+#if 0
 	PDEBUG("pn: %d %d %d %d %d %d %d %d %d %d\n",
 	       aPackNum[0], aPackNum[1], aPackNum[2], aPackNum[3], aPackNum[4],
 	       aPackNum[5],aPackNum[6], aPackNum[7], aPackNum[8], aPackNum[9]);
+#endif
 	return totlen;
 }
 
@@ -491,18 +838,6 @@ static void ov511_isoc_irq(struct urb *urb)
 	struct ov511_sbuf *sbuf;
 	int i;
 
-#if 0
-	static int last_status, last_error_count, last_actual_length;
-	if (last_status != urb->status ||
-	    last_error_count != urb->error_count ||
-	    last_actual_length != urb->actual_length) {
-	  PDEBUG("ov511_isoc_irq: %p status %d, errcount = %d, length = %d\n", urb, urb->status, urb->error_count, urb->actual_length);
-	  last_status = urb->status;
-	  last_error_count = urb->error_count;
-	  last_actual_length = urb->actual_length;
-	}
-#endif
-
 	if (!ov511->streaming) {
 		PDEBUG("hmmm... not streaming, but got interrupt\n");
 		return;
@@ -511,17 +846,10 @@ static void ov511_isoc_irq(struct urb *urb)
 	sbuf = &ov511->sbuf[ov511->cursbuf];
 
 	/* Copy the data received into our scratch buffer */
-	len = ov511_move_data(ov511, urb);
-#if 0
-	/* If we don't have a frame we're current working on, complain */
-	if (ov511->scratchlen) {
-		if (ov511->curframe < 0) {
-			// Macro - must be in braces!!
-			PDEBUG("received data, but no frame available\n");
-		} else
-			ov511_parse_data(ov511);
+	if (ov511->curframe >= 0) {
+	  len = ov511_move_data(ov511, urb);
 	}
-#endif
+
 	for (i = 0; i < FRAMES_PER_DESC; i++) {
 		sbuf->urb->iso_frame_desc[i].status = 0;
 		sbuf->urb->iso_frame_desc[i].actual_length = 0;
@@ -544,30 +872,13 @@ static int ov511_init_isoc(struct usb_ov511 *ov511)
 	ov511->cursbuf = 0;
 	ov511->scratchlen = 0;
 
-	ov511_set_packet_size(ov511, 512);
-	
-#define OV511_COLOR_BAR_TEST
-#ifdef OV511_COLOR_BAR_TEST
-	{
-	  int rc;
-	  rc = ov511_i2c_read(ov511->dev, 0x12);
-	  rc = ov511_i2c_write(ov511->dev, 0x12, 0x3f);
-	  rc = ov511_i2c_read(ov511->dev, 0x12);
-	  rc = ov511_i2c_read(ov511->dev, 0x13);
-	  rc = ov511_i2c_write(ov511->dev, 0x14, 0x4);
-	  rc = ov511_i2c_read(ov511->dev, 0x14);
-	  rc = ov511_i2c_write(ov511->dev, 0x28, 0x60);
-	  rc = ov511_i2c_read(ov511->dev, 0x28);
-	  ov511_reg_write(ov511->dev, OV511_REG_CAMERA_DATA_INPUT_SELECT,
-			      0);
-	}
-#endif
+	ov511_set_packet_size(ov511, 993);
 
 	/* We double buffer the Iso lists */
 	urb = usb_alloc_urb(FRAMES_PER_DESC);
 	
 	if (!urb) {
-		printk(KERN_ERR "ov511_init_isoc: usb_alloc_urb ret. NULL\n");
+		err("ov511_init_isoc: usb_alloc_urb ret. NULL");
 		return -ENOMEM;
 	}
 	ov511->sbuf[0].urb = urb;
@@ -586,7 +897,7 @@ static int ov511_init_isoc(struct usb_ov511 *ov511)
 
 	urb = usb_alloc_urb(FRAMES_PER_DESC);
 	if (!urb) {
-		printk(KERN_ERR "ov511_init_isoc: usb_alloc_urb ret. NULL\n");
+		err("ov511_init_isoc: usb_alloc_urb ret. NULL");
 		return -ENOMEM;
 	}
 	ov511->sbuf[1].urb = urb;
@@ -608,12 +919,10 @@ static int ov511_init_isoc(struct usb_ov511 *ov511)
 
 	err = usb_submit_urb(ov511->sbuf[0].urb);
 	if (err)
-		printk(KERN_ERR "ov511_init_isoc: usb_run_isoc(0) ret %d\n",
-			err);
+		err("ov511_init_isoc: usb_run_isoc(0) ret %d", err);
 	err = usb_submit_urb(ov511->sbuf[1].urb);
 	if (err)
-		printk(KERN_ERR "ov511_init_isoc: usb_run_isoc(1) ret %d\n",
-			err);
+		err("ov511_init_isoc: usb_run_isoc(1) ret %d\n", err);
 
 	ov511->streaming = 1;
 
@@ -685,7 +994,7 @@ static int ov511_open(struct video_device *dev, int flags)
 	int err = -EBUSY;
 	struct usb_ov511 *ov511 = (struct usb_ov511 *)dev;
 
-	PDEBUG("ov511_open\n");
+	PDEBUG("ov511_open");
 	
 	down(&ov511->lock);
 	if (ov511->user)
@@ -704,8 +1013,8 @@ static int ov511_open(struct video_device *dev, int flags)
 	ov511->frame[0].data = ov511->fbuf;
 	ov511->frame[1].data = ov511->fbuf + MAX_FRAME_SIZE;
 
-	PDEBUG("frame [0] @ %p\n", ov511->frame[0].data);
-	PDEBUG("frame [1] @ %p\n", ov511->frame[1].data);
+	PDEBUG("frame [0] @ %p", ov511->frame[0].data);
+	PDEBUG("frame [1] @ %p", ov511->frame[1].data);
 
 	ov511->sbuf[0].data = kmalloc(FRAMES_PER_DESC * FRAME_SIZE_PER_DESC, GFP_KERNEL);
 	if (!ov511->sbuf[0].data)
@@ -714,17 +1023,8 @@ static int ov511_open(struct video_device *dev, int flags)
 	if (!ov511->sbuf[1].data)
 		goto open_err_on1;
 		
-	PDEBUG("sbuf[0] @ %p\n", ov511->sbuf[0].data);
-	PDEBUG("sbuf[1] @ %p\n", ov511->sbuf[1].data);
-
-	/* Set default sizes in case IOCTL (VIDIOCMCAPTURE) is not used
-	 * (using read() instead). */
-	ov511->frame[0].width = DEFAULT_WIDTH;
-	ov511->frame[0].height = DEFAULT_HEIGHT;
-	ov511->frame[0].bytes_read = 0;
-	ov511->frame[1].width = DEFAULT_WIDTH;
-	ov511->frame[1].height = DEFAULT_HEIGHT;
-	ov511->frame[1].bytes_read = 0;
+	PDEBUG("sbuf[0] @ %p", ov511->sbuf[0].data);
+	PDEBUG("sbuf[1] @ %p", ov511->sbuf[1].data);
 
 	err = ov511_init_isoc(ov511);
 	if (err)
@@ -755,7 +1055,7 @@ static void ov511_close(struct video_device *dev)
 {
 	struct usb_ov511 *ov511 = (struct usb_ov511 *)dev;
 
-	PDEBUG("ov511_close\n");
+	PDEBUG("ov511_close");
 	
 	down(&ov511->lock);	
 	ov511->user--;
@@ -783,12 +1083,12 @@ static long ov511_write(struct video_device *dev, const char *buf, unsigned long
 }
 
 // FIXME - Needs much work!!!
-static int ov511_ioctl(struct video_device *dev, unsigned int cmd, void *arg)
+static int ov511_ioctl(struct video_device *vdev, unsigned int cmd, void *arg)
 {
-	struct usb_ov511 *ov511 = (struct usb_ov511 *)dev;
-	
-	PDEBUG("IOCtl: 0x%X\n", cmd);
-	
+	struct usb_ov511 *ov511 = (struct usb_ov511 *)vdev;
+#if 0	
+	PDEBUG("IOCtl: 0x%X", cmd);
+#endif	
 	switch (cmd) {
 		case VIDIOCGCAP:
 		{
@@ -844,14 +1144,9 @@ static int ov511_ioctl(struct video_device *dev, unsigned int cmd, void *arg)
 		{
 			struct video_picture p;
 
-			p.colour = 0x8000;	/* Damn British people :) */
-			p.hue = 0x8000;
-			p.brightness = 180 << 8;	/* XXX */
-			p.contrast = 192 << 8;		/* XXX */
-			p.whiteness = 105 << 8;		/* XXX */
-			p.depth = 24;
-			p.palette = VIDEO_PALETTE_RGB24;
-
+			if (ov7610_get_picture(ov511, &p))
+				return -EIO;
+							
 			if (copy_to_user(arg, &p, sizeof(p)))
 				return -EFAULT;
 
@@ -863,6 +1158,9 @@ static int ov511_ioctl(struct video_device *dev, unsigned int cmd, void *arg)
 
 			if (copy_from_user(&p, arg, sizeof(p)))
 				return -EFAULT;
+			
+			if (ov7610_set_picture(ov511, &p))
+				return -EIO;
 
 			return 0;
 		}
@@ -923,9 +1221,11 @@ static int ov511_ioctl(struct video_device *dev, unsigned int cmd, void *arg)
 			if (copy_from_user((void *)&vm, (void *)arg, sizeof(vm)))
 				return -EFAULT;
 
-			PDEBUG("MCAPTURE\n");
-			PDEBUG("frame: %d, size: %dx%d, format: %d\n",
+#if 0
+			PDEBUG("MCAPTURE");
+			PDEBUG("frame: %d, size: %dx%d, format: %d",
 				vm.frame, vm.width, vm.height, vm.format);
+#endif
 
 			if (vm.format != VIDEO_PALETTE_RGB24)
 				return -EINVAL;
@@ -938,8 +1238,15 @@ static int ov511_ioctl(struct video_device *dev, unsigned int cmd, void *arg)
 
 			/* Don't compress if the size changed */
 			if ((ov511->frame[vm.frame].width != vm.width) ||
-			    (ov511->frame[vm.frame].height != vm.height))
+			    (ov511->frame[vm.frame].height != vm.height)) {
 				ov511->compress = 0;
+				ov511_mode_init_regs(ov511,
+						     vm.width, vm.height, 0);
+#if 0
+				PDEBUG("ov511: Setting frame %d to (%d, %d) : %d",
+				       vm.frame, vm.width, vm.height, 0);
+#endif
+			}
 
 			ov511->frame[vm.frame].width = vm.width;
 			ov511->frame[vm.frame].height = vm.height;
@@ -956,8 +1263,9 @@ static int ov511_ioctl(struct video_device *dev, unsigned int cmd, void *arg)
 			if (copy_from_user((void *)&frame, arg, sizeof(int)))
 				return -EFAULT;
 
-			PDEBUG("syncing to frame %d\n", frame);
-			
+#if 0
+			PDEBUG("syncing to frame %d", frame);
+#endif
 			switch (ov511->frame[frame].grabstate) {
 				case FRAME_UNUSED:
 					return -EINVAL;
@@ -1029,7 +1337,7 @@ static long ov511_read(struct video_device *dev, char *buf, unsigned long count,
 	int frmx = -1;
 	volatile struct ov511_frame *frame;
 
-	PDEBUG("ov511_read: %ld bytes, noblock=%d\n", count, noblock);
+	PDEBUG("ov511_read: %ld bytes, noblock=%d", count, noblock);
 
 	if (!dev || !buf)
 		return -EFAULT;
@@ -1060,20 +1368,20 @@ static long ov511_read(struct video_device *dev, char *buf, unsigned long count,
 
 restart:
 	while (frame->grabstate == FRAME_GRABBING) {
-		interruptible_sleep_on(&frame->wq);
+		interruptible_sleep_on(&ov511->frame[frmx].wq);
 		if (signal_pending(current))
 			return -EINTR;
 	}
 
 	if (frame->grabstate == FRAME_ERROR) {
 		frame->bytes_read = 0;
-		printk(KERN_ERR "ov511_read: errored frame %d\n", ov511->curframe);
+		err("ov511_read: errored frame %d", ov511->curframe);
 		if (ov511_new_frame(ov511, frmx))
-			printk(KERN_ERR "ov511_read: ov511_new_frame error\n");
+			err("ov511_read: ov511_new_frame error");
 		goto restart;
 	}
 
-	PDEBUG("ov511_read: frmx=%d, bytes_read=%ld, scanlength=%ld\n", frmx,
+	PDEBUG("ov511_read: frmx=%d, bytes_read=%ld, scanlength=%ld", frmx,
 		frame->bytes_read, frame->scanlength);
 
 	/* copy bytes to user space; we allow for partials reads */
@@ -1084,7 +1392,7 @@ restart:
 		return -EFAULT;
 
 	frame->bytes_read += count;
-	PDEBUG("ov511_read: {copy} count used=%ld, new bytes_read=%ld\n",
+	PDEBUG("ov511_read: {copy} count used=%ld, new bytes_read=%ld",
 		count, frame->bytes_read);
 
 	if (frame->bytes_read >= frame->scanlength) { /* All data has been read */
@@ -1093,7 +1401,7 @@ restart:
 		/* Mark it as available to be used again. */
 		ov511->frame[frmx].grabstate = FRAME_UNUSED;
 		if (ov511_new_frame(ov511, frmx ? 0 : 1))
-			printk(KERN_ERR "ov511_read: ov511_new_frame returned error\n");
+			err("ov511_read: ov511_new_frame returned error");
 	}
 
 	return count;
@@ -1105,7 +1413,7 @@ static int ov511_mmap(struct video_device *dev, const char *adr, unsigned long s
 	unsigned long start = (unsigned long)adr;
 	unsigned long page, pos;
 
-	PDEBUG("mmap: %ld (%lX) bytes\n", size, size);
+	PDEBUG("mmap: %ld (%lX) bytes", size, size);
 
 	if (size > (((2 * MAX_FRAME_SIZE) + PAGE_SIZE - 1) & ~(PAGE_SIZE - 1)))
 		return -EINVAL;
@@ -1145,14 +1453,52 @@ static struct video_device ov511_template = {
 	0
 };
 
+static int ov7610_configure(struct usb_device *dev)
+{
+	if(ov511_reg_write(dev, OV511_REG_I2C_SLAVE_ID_WRITE,
+	                        OV7610_I2C_WRITE_ID) < 0)
+		return -1;
+
+	if(ov511_reg_write(dev, OV511_REG_I2C_SLAVE_ID_READ,
+	                        OV7610_I2C_READ_ID) < 0)
+		return -1;
+	
+	/* Reset the camera chip */
+	if (ov511_i2c_reset(dev) < 0)
+		return -1;
+
+#if 0
+	if(usb_ov511_reg_write(dev, OV511_REG_I2C_CLOCK_PRESCALER,
+	                            OV511_I2C_CLOCK_PRESCALER))
+		return -1;
+#endif
+	
+	if (ov511_reset(dev, OV511_RESET_NOREGS) < 0)
+		return -1;
+	
+	/* Dummy read to sync I2C */
+	if(ov511_i2c_read(dev, 0x00) < 0)
+		return -1;
+	
+
+	if((ov511_i2c_read(dev, OV7610_REG_ID_HIGH) != 0x7F) ||
+	   (ov511_i2c_read(dev, OV7610_REG_ID_LOW) != 0xA2)) {
+		err("Failed to read OV7610 ID. You might not have an OV7610,");
+		err("or it may be not responding. Report this to");
+		err("mmcclelland@delphi.com");
+		return -1;
+	}
+
+	return 0;
+}
+
 static int ov511_configure(struct usb_ov511 *ov511)
 {
 	struct usb_device *dev = ov511->dev;
-	int temprc;   // DEBUG CODE
 
 	/* Set altsetting 0 */
 	if (usb_set_interface(dev, ov511->iface, 0) < 0) {
-		printk(KERN_ERR "ov511: usb_set_interface error\n");
+		err("usb_set_interface error");
 		return -EBUSY;
 	}
 
@@ -1162,7 +1508,7 @@ static int ov511_configure(struct usb_ov511 *ov511)
 	init_waitqueue_head(&ov511->frame[1].wq);
 
 	if (video_register_device(&ov511->vdev, VFL_TYPE_GRABBER) == -1) {
-		printk(KERN_ERR "ov511: video_register_device failed\n");
+		err("video_register_device failed");
 		return -EBUSY;
 	}
 
@@ -1172,7 +1518,7 @@ static int ov511_configure(struct usb_ov511 *ov511)
 
 	/* Initialize system */
 	if (ov511_reg_write(dev, OV511_REG_SYSTEM_INIT, 0x01) < 0) {
-		printk(KERN_ERR "ov511: enable system: command failed\n");
+		err("enable system: command failed");
 		goto error;
 	}
 
@@ -1180,36 +1526,31 @@ static int ov511_configure(struct usb_ov511 *ov511)
 	if (ov511_reset(dev, OV511_RESET_ALL) < 0)
 		goto error;
 
+	if(ov7610_configure(dev) < 0) {
+		err("failed to configure OV7610");
+ 		goto error;	
+	}
+
 	/* Disable compression */
 	if (ov511_reg_write(dev, OV511_OMNICE_ENABLE, 0x00) < 0) {
-		printk(KERN_ERR "ov511: disable compression: command failed\n");
+		err("disable compression: command failed");
 		goto error;
 	}
 
-// FIXME - error checking needed
-	ov511_reg_write(dev, OV511_REG_I2C_SLAVE_ID_WRITE,
-	                     OV7610_I2C_WRITE_ID);
-	ov511_reg_write(dev, OV511_REG_I2C_SLAVE_ID_READ,
-	                     OV7610_I2C_READ_ID);
-
-// DEBUG CODE
-//	usb_ov511_reg_write(dev, OV511_REG_I2C_CLOCK_PRESCALER,
-//						 OV511_I2C_CLOCK_PRESCALER);
-	
-	if (ov511_reset(dev, OV511_RESET_NOREGS) < 0)
-		goto error;
-	
-	/* Dummy read to sync I2C */
-	ov511_i2c_read(dev, 0x1C);
-	
-// DEBUG - TEST CODE FOR CAMERA REG READ
-	temprc = ov511_i2c_read(dev, 0x1C);
-
-     	temprc = ov511_i2c_read(dev, 0x1D);
-// END DEBUG CODE
-     	
 	ov511->compress = 0;
 	
+	/* Set default sizes in case IOCTL (VIDIOCMCAPTURE) is not used
+	 * (using read() instead). */
+	ov511->frame[0].width = DEFAULT_WIDTH;
+	ov511->frame[0].height = DEFAULT_HEIGHT;
+	ov511->frame[0].bytes_read = 0;
+	ov511->frame[1].width = DEFAULT_WIDTH;
+	ov511->frame[1].height = DEFAULT_HEIGHT;
+	ov511->frame[1].bytes_read = 0;
+
+	/* Initialize to DEFAULT_WIDTH, DEFAULT_HEIGHT, YUV4:2:0 */
+	ov511_mode_init_regs(ov511, DEFAULT_WIDTH, DEFAULT_HEIGHT, 0);
+
 	return 0;
 	
 error:
@@ -1228,7 +1569,7 @@ static void* ov511_probe(struct usb_device *dev, unsigned int ifnum)
 	struct usb_ov511 *ov511;
 	int rc;
 
-	PDEBUG("probing for device...\n");
+	PDEBUG("probing for device...");
 
 	/* We don't handle multi-config cameras */
 	if (dev->descriptor.bNumConfigurations != 1)
@@ -1252,7 +1593,7 @@ static void* ov511_probe(struct usb_device *dev, unsigned int ifnum)
 	printk(KERN_INFO "ov511: USB OV511-based camera found\n");
 
 	if ((ov511 = kmalloc(sizeof(*ov511), GFP_KERNEL)) == NULL) {
-		printk(KERN_ERR "ov511: couldn't kmalloc ov511 struct\n");
+		err("couldn't kmalloc ov511 struct");
 		return NULL;
 	}
 
@@ -1263,18 +1604,30 @@ static void* ov511_probe(struct usb_device *dev, unsigned int ifnum)
 
 	rc = ov511_reg_read(dev, OV511_REG_SYSTEM_CUSTOM_ID);
 	if (rc < 0) {
-		printk("ov511: Unable to read camera bridge registers\n");
+		err("Unable to read camera bridge registers");
 		return NULL;
-	} else if (rc == 3) {  /* D-Link DSB-C300 */
+	}
+	
+	switch(ov511->customid = rc) {
+	case 0: /* This also means that no custom ID was set */
+		printk("ov511: Camera is probably a MediaForte MV300\n");
+		break;
+	case 3:
 		printk("ov511: Camera is a D-Link DSB-C300\n");
-		ov511->customid = 3;
-	} else if (rc == 21) { /* Creative Labs WebCam 3 */
+		break;
+	case 21:
 		printk("ov511: Camera is a Creative Labs WebCam 3\n");
-		ov511->customid = 21;		
-	} else {
-		printk("ov511: Specific camera type (%d) not recognized\n", rc);
-		printk("ov511: Please contact mmcclelland@delphi.com to request\n");
-		printk("ov511: support for your camera.\n");
+		break;
+	case 100:
+		printk("ov511: Camera is a Lifeview RoboCam\n");
+		break;
+	case 102:
+		printk("ov511: Camera is a AverMedia InterCam Elite\n");
+		break;
+	default:
+		err("Specific camera type (%d) not recognized", rc);
+		err("Please contact mmcclelland@delphi.com to request");
+		err("support for your camera.");
 		return NULL;
 	}
 
@@ -1284,7 +1637,7 @@ static void* ov511_probe(struct usb_device *dev, unsigned int ifnum)
 		return ov511;
 	}
 	else {
-		printk(KERN_ERR "ov511: Failed to configure camera\n");
+		err("Failed to configure camera");
 		return NULL;
 	}
     	
@@ -1314,7 +1667,7 @@ static struct usb_driver ov511_driver = {
 
 int usb_ov511_init(void)
 {
-	PDEBUG("usb_ov511_init()\n");
+	PDEBUG("usb_ov511_init()");
 	
 	EXPORT_NO_SYMBOLS;
 	
@@ -1336,7 +1689,7 @@ void cleanup_module(void)
 {
 	usb_ov511_cleanup();
 	
-	PDEBUG("Module unloaded\n");
+	PDEBUG("Module unloaded");
 }
 #endif