[PATCH] USB: SN9C10x driver updates and bugfixes

SN9C10x driver updates and bugfixes.

Changes: + new, - removed, * cleanup, @ bugfix:

@ fix poll()
@ Remove bad get_ctrl()'s
* Reduce ioctl stack usage
* Remove final ";" from some macro definitions
* Better support for SN9C103
+ Add sn9c102_write_regs()
+ Add 0x0c45/0x602d to the list of SN9C10x based devices
+ Add support for OV7630 image sensors
+ Provide support for the built-in microphone interface of the SN9C103
+ Documentation updates
+ Add 0x0c45/0x602e to the list of SN9C10x based devices

Signed-off-by: Luca Risolia <luca.risolia@studio.unibo.it>
Signed-off-by: Greg Kroah-Hartman <gregkh@suse.de>

9 files changed, 955 insertions, 756 deletions

diff --git a/drivers/usb/media/sn9c102.h b/drivers/usb/media/sn9c102.h
index e5cea0e2eb5..967c6b6bc0f 100644
--- a/drivers/usb/media/sn9c102.h
+++ b/drivers/usb/media/sn9c102.h
@@ -1,7 +1,7 @@
 /***************************************************************************
  * V4L2 driver for SN9C10x PC Camera Controllers                           *
  *                                                                         *
- * Copyright (C) 2004-2005 by Luca Risolia <luca.risolia@studio.unibo.it>  *
+ * Copyright (C) 2004-2006 by Luca Risolia <luca.risolia@studio.unibo.it>  *
  *                                                                         *
  * This program is free software; you can redistribute it and/or modify    *
  * it under the terms of the GNU General Public License as published by    *
@@ -53,11 +53,11 @@
 /*****************************************************************************/
 
 #define SN9C102_MODULE_NAME     "V4L2 driver for SN9C10x PC Camera Controllers"
-#define SN9C102_MODULE_AUTHOR   "(C) 2004-2005 Luca Risolia"
+#define SN9C102_MODULE_AUTHOR   "(C) 2004-2006 Luca Risolia"
 #define SN9C102_AUTHOR_EMAIL    "<luca.risolia@studio.unibo.it>"
 #define SN9C102_MODULE_LICENSE  "GPL"
-#define SN9C102_MODULE_VERSION  "1:1.24a"
-#define SN9C102_MODULE_VERSION_CODE  KERNEL_VERSION(1, 0, 24)
+#define SN9C102_MODULE_VERSION  "1:1.25"
+#define SN9C102_MODULE_VERSION_CODE  KERNEL_VERSION(1, 0, 25)
 
 enum sn9c102_bridge {
 	BRIDGE_SN9C101 = 0x01,
@@ -102,12 +102,13 @@ enum sn9c102_stream_state {
 	STREAM_ON,
 };
 
+typedef char sn9c103_sof_header_t[18];
 typedef char sn9c102_sof_header_t[12];
 typedef char sn9c102_eof_header_t[4];
 
 struct sn9c102_sysfs_attr {
 	u8 reg, i2c_reg;
-	sn9c102_sof_header_t frame_header;
+	sn9c103_sof_header_t frame_header;
 };
 
 struct sn9c102_module_param {
@@ -140,8 +141,8 @@ struct sn9c102_device {
 	struct v4l2_jpegcompression compression;
 
 	struct sn9c102_sysfs_attr sysfs;
-	sn9c102_sof_header_t sof_header;
-	u16 reg[32];
+	sn9c103_sof_header_t sof_header;
+	u16 reg[63];
 
 	struct sn9c102_module_param module_param;
 
@@ -170,7 +171,7 @@ sn9c102_attach_sensor(struct sn9c102_device* cam,
 #undef KDBG
 #ifdef SN9C102_DEBUG
 #	define DBG(level, fmt, args...)                                       \
-{                                                                             \
+do {                                                                          \
 	if (debug >= (level)) {                                               \
 		if ((level) == 1)                                             \
 			dev_err(&cam->dev, fmt "\n", ## args);                \
@@ -180,9 +181,9 @@ sn9c102_attach_sensor(struct sn9c102_device* cam,
 			dev_info(&cam->dev, "[%s:%d] " fmt "\n",              \
 			         __FUNCTION__, __LINE__ , ## args);           \
 	}                                                                     \
-}
+} while (0)
 #	define KDBG(level, fmt, args...)                                      \
-{                                                                             \
+do {                                                                          \
 	if (debug >= (level)) {                                               \
 		if ((level) == 1 || (level) == 2)                             \
 			pr_info("sn9c102: " fmt "\n", ## args);               \
@@ -190,17 +191,17 @@ sn9c102_attach_sensor(struct sn9c102_device* cam,
 			pr_debug("sn9c102: [%s:%d] " fmt "\n", __FUNCTION__,  \
 			         __LINE__ , ## args);                         \
 	}                                                                     \
-}
+} while (0)
 #else
-#	define KDBG(level, fmt, args...) do {;} while(0);
-#	define DBG(level, fmt, args...) do {;} while(0);
+#	define KDBG(level, fmt, args...) do {;} while(0)
+#	define DBG(level, fmt, args...) do {;} while(0)
 #endif
 
 #undef PDBG
 #define PDBG(fmt, args...)                                                    \
-dev_info(&cam->dev, "[%s:%d] " fmt "\n", __FUNCTION__, __LINE__ , ## args);
+dev_info(&cam->dev, "[%s:%d] " fmt "\n", __FUNCTION__, __LINE__ , ## args)
 
 #undef PDBGG
-#define PDBGG(fmt, args...) do {;} while(0); /* placeholder */
+#define PDBGG(fmt, args...) do {;} while(0) /* placeholder */
 
 #endif /* _SN9C102_H_ */
diff --git a/drivers/usb/media/sn9c102_core.c b/drivers/usb/media/sn9c102_core.c
index 8d1a1c357d5..6090439663e 100644
--- a/drivers/usb/media/sn9c102_core.c
+++ b/drivers/usb/media/sn9c102_core.c
@@ -1,7 +1,7 @@
 /***************************************************************************
  * V4L2 driver for SN9C10x PC Camera Controllers                           *
  *                                                                         *
- * Copyright (C) 2004-2005 by Luca Risolia <luca.risolia@studio.unibo.it>  *
+ * Copyright (C) 2004-2006 by Luca Risolia <luca.risolia@studio.unibo.it>  *
  *                                                                         *
  * This program is free software; you can redistribute it and/or modify    *
  * it under the terms of the GNU General Public License as published by    *
@@ -70,10 +70,10 @@ static short force_munmap[] = {[0 ... SN9C102_MAX_DEVICES-1] =
                                SN9C102_FORCE_MUNMAP};
 module_param_array(force_munmap, bool, NULL, 0444);
 MODULE_PARM_DESC(force_munmap,
-                 "\n<0|1[,...]> Force the application to unmap previously "
-                 "\nmapped buffer memory before calling any VIDIOC_S_CROP or "
-                 "\nVIDIOC_S_FMT ioctl's. Not all the applications support "
-                 "\nthis feature. This parameter is specific for each "
+                 "\n<0|1[,...]> Force the application to unmap previously"
+                 "\nmapped buffer memory before calling any VIDIOC_S_CROP or"
+                 "\nVIDIOC_S_FMT ioctl's. Not all the applications support"
+                 "\nthis feature. This parameter is specific for each"
                  "\ndetected camera."
                  "\n 0 = do not force memory unmapping"
                  "\n 1 = force memory unmapping (save memory)"
@@ -102,6 +102,9 @@ static sn9c102_sof_header_t sn9c102_sof_header[] = {
 	{0xff, 0xff, 0x00, 0xc4, 0xc4, 0x96, 0x01},
 };
 
+static sn9c103_sof_header_t sn9c103_sof_header[] = {
+	{0xff, 0xff, 0x00, 0xc4, 0xc4, 0x96, 0x20},
+};
 
 static sn9c102_eof_header_t sn9c102_eof_header[] = {
 	{0x00, 0x00, 0x00, 0x00},
@@ -202,6 +205,7 @@ static void sn9c102_release_buffers(struct sn9c102_device* cam)
 		       cam->nbuffers * PAGE_ALIGN(cam->frame[0].buf.length));
 		cam->nbuffers = 0;
 	}
+	cam->frame_current = NULL;
 }
 
 
@@ -219,6 +223,19 @@ static void sn9c102_empty_framequeues(struct sn9c102_device* cam)
 }
 
 
+static void sn9c102_requeue_outqueue(struct sn9c102_device* cam)
+{
+	struct sn9c102_frame_t *i;
+
+	list_for_each_entry(i, &cam->outqueue, frame) {
+		i->state = F_QUEUED;
+		list_add(&i->frame, &cam->inqueue);
+	}
+
+	INIT_LIST_HEAD(&cam->outqueue);
+}
+
+
 static void sn9c102_queue_unusedframes(struct sn9c102_device* cam)
 {
 	unsigned long lock_flags;
@@ -235,19 +252,46 @@ static void sn9c102_queue_unusedframes(struct sn9c102_device* cam)
 
 /*****************************************************************************/
 
+int sn9c102_write_regs(struct sn9c102_device* cam, u8* buff, u16 index)
+{
+	struct usb_device* udev = cam->usbdev;
+	int i, res;
+
+	if (index + sizeof(buff) >= ARRAY_SIZE(cam->reg))
+		return -1;
+
+	res = usb_control_msg(udev, usb_sndctrlpipe(udev, 0), 0x08, 0x41,
+	                      index, 0, buff, sizeof(buff),
+	                      SN9C102_CTRL_TIMEOUT*sizeof(buff));
+	if (res < 0) {
+		DBG(3, "Failed to write registers (index 0x%02X, error %d)",
+		    index, res);
+		return -1;
+	}
+
+	for (i = 0; i < sizeof(buff); i++)
+		cam->reg[index+i] = buff[i];
+
+	return 0;
+}
+
+
 int sn9c102_write_reg(struct sn9c102_device* cam, u8 value, u16 index)
 {
 	struct usb_device* udev = cam->usbdev;
 	u8* buff = cam->control_buffer;
 	int res;
 
+	if (index >= ARRAY_SIZE(cam->reg))
+		return -1;
+
 	*buff = value;
 
 	res = usb_control_msg(udev, usb_sndctrlpipe(udev, 0), 0x08, 0x41,
 	                      index, 0, buff, 1, SN9C102_CTRL_TIMEOUT);
 	if (res < 0) {
 		DBG(3, "Failed to write a register (value 0x%02X, index "
-		       "0x%02X, error %d)", value, index, res)
+		       "0x%02X, error %d)", value, index, res);
 		return -1;
 	}
 
@@ -268,7 +312,7 @@ static int sn9c102_read_reg(struct sn9c102_device* cam, u16 index)
 	                      index, 0, buff, 1, SN9C102_CTRL_TIMEOUT);
 	if (res < 0)
 		DBG(3, "Failed to read a register (index 0x%02X, error %d)",
-		    index, res)
+		    index, res);
 
 	return (res >= 0) ? (int)(*buff) : -1;
 }
@@ -276,8 +320,8 @@ static int sn9c102_read_reg(struct sn9c102_device* cam, u16 index)
 
 int sn9c102_pread_reg(struct sn9c102_device* cam, u16 index)
 {
-	if (index > 0x1f)
-		return -EINVAL;
+	if (index >= ARRAY_SIZE(cam->reg))
+		return -1;
 
 	return cam->reg[index];
 }
@@ -367,10 +411,10 @@ sn9c102_i2c_try_raw_read(struct sn9c102_device* cam,
 	err += sn9c102_i2c_detect_read_error(cam, sensor);
 
 	PDBGG("I2C read: address 0x%02X, first read byte: 0x%02X", data1,
-	      data[4])
+	      data[4]);
 
 	if (err) {
-		DBG(3, "I2C read failed for %s image sensor", sensor->name)
+		DBG(3, "I2C read failed for %s image sensor", sensor->name);
 		return -1;
 	}
 
@@ -410,11 +454,11 @@ sn9c102_i2c_try_raw_write(struct sn9c102_device* cam,
 	err += sn9c102_i2c_detect_write_error(cam, sensor);
 
 	if (err)
-		DBG(3, "I2C write failed for %s image sensor", sensor->name)
+		DBG(3, "I2C write failed for %s image sensor", sensor->name);
 
 	PDBGG("I2C raw write: %u bytes, data0 = 0x%02X, data1 = 0x%02X, "
 	      "data2 = 0x%02X, data3 = 0x%02X, data4 = 0x%02X, data5 = 0x%02X",
-	      n, data0, data1, data2, data3, data4, data5)
+	      n, data0, data1, data2, data3, data4, data5);
 
 	return err ? -1 : 0;
 }
@@ -461,13 +505,27 @@ int sn9c102_i2c_write(struct sn9c102_device* cam, u8 address, u8 value)
 static void*
 sn9c102_find_sof_header(struct sn9c102_device* cam, void* mem, size_t len)
 {
-	size_t soflen = sizeof(sn9c102_sof_header_t), i;
-	u8 j, n = sizeof(sn9c102_sof_header) / soflen;
+	size_t soflen = 0, i;
+	u8 j, n = 0;
+
+	switch (cam->bridge) {
+	case BRIDGE_SN9C101:
+	case BRIDGE_SN9C102:
+		soflen = sizeof(sn9c102_sof_header_t);
+		n = sizeof(sn9c102_sof_header) / soflen;
+		break;
+	case BRIDGE_SN9C103:
+		soflen = sizeof(sn9c103_sof_header_t);
+		n = sizeof(sn9c103_sof_header) / soflen;
+	}
 
-	for (i = 0; (len >= soflen) && (i <= len - soflen); i++)
+ 	for (i = 0; (len >= soflen) && (i <= len - soflen); i++)
 		for (j = 0; j < n; j++)
-			/* It's enough to compare 7 bytes */
-			if (!memcmp(mem + i, sn9c102_sof_header[j], 7)) {
+			/* The invariable part of the header is 6 bytes long */
+			if ((cam->bridge != BRIDGE_SN9C103 &&
+			    !memcmp(mem + i, sn9c102_sof_header[j], 6)) ||
+			    (cam->bridge == BRIDGE_SN9C103 &&
+			    !memcmp(mem + i, sn9c103_sof_header[j], 6))) {
 				memcpy(cam->sof_header, mem + i, soflen);
 				/* Skip the header */
 				return mem + i + soflen;
@@ -499,8 +557,7 @@ static void sn9c102_urb_complete(struct urb *urb, struct pt_regs* regs)
 {
 	struct sn9c102_device* cam = urb->context;
 	struct sn9c102_frame_t** f;
-	size_t imagesize;
-	unsigned long lock_flags;
+	size_t imagesize, soflen;
 	u8 i;
 	int err = 0;
 
@@ -513,7 +570,7 @@ static void sn9c102_urb_complete(struct urb *urb, struct pt_regs* regs)
 		cam->stream = STREAM_OFF;
 		if ((*f))
 			(*f)->state = F_QUEUED;
-		DBG(3, "Stream interrupted")
+		DBG(3, "Stream interrupted");
 		wake_up_interruptible(&cam->wait_stream);
 	}
 
@@ -536,6 +593,10 @@ static void sn9c102_urb_complete(struct urb *urb, struct pt_regs* regs)
 	             cam->sensor->pix_format.height *
 	             cam->sensor->pix_format.priv) / 8;
 
+	soflen = (cam->bridge) == BRIDGE_SN9C103 ?
+	                          sizeof(sn9c103_sof_header_t) :
+	                          sizeof(sn9c102_sof_header_t);
+
 	for (i = 0; i < urb->number_of_packets; i++) {
 		unsigned int img, len, status;
 		void *pos, *sof, *eof;
@@ -545,19 +606,12 @@ static void sn9c102_urb_complete(struct urb *urb, struct pt_regs* regs)
 		pos = urb->iso_frame_desc[i].offset + urb->transfer_buffer;
 
 		if (status) {
-			DBG(3, "Error in isochronous frame")
+			DBG(3, "Error in isochronous frame");
 			(*f)->state = F_ERROR;
 			continue;
 		}
 
-		PDBGG("Isochrnous frame: length %u, #%u i", len, i)
-
-		/*
-		   NOTE: It is probably correct to assume that SOF and EOF
-		         headers do not occur between two consecutive packets,
-		         but who knows..Whatever is the truth, this assumption
-		         doesn't introduce bugs.
-		*/
+		PDBGG("Isochrnous frame: length %u, #%u i", len, i);
 
 redo:
 		sof = sn9c102_find_sof_header(cam, pos, len);
@@ -575,10 +629,10 @@ end_of_frame:
 					        imagesize;
 					img = imagesize - (*f)->buf.bytesused;
 					DBG(3, "Expected EOF not found: "
-					       "video frame cut")
+					       "video frame cut");
 					if (eof)
 						DBG(3, "Exceeded limit: +%u "
-						       "bytes", (unsigned)(b))
+						       "bytes", (unsigned)(b));
 				}
 
 				memcpy((*f)->bufmem + (*f)->buf.bytesused, pos,
@@ -595,8 +649,7 @@ end_of_frame:
 					u32 b = (*f)->buf.bytesused;
 					(*f)->state = F_DONE;
 					(*f)->buf.sequence= ++cam->frame_count;
-					spin_lock_irqsave(&cam->queue_lock,
-					                  lock_flags);
+					spin_lock(&cam->queue_lock);
 					list_move_tail(&(*f)->frame,
 					               &cam->outqueue);
 					if (!list_empty(&cam->inqueue))
@@ -606,13 +659,11 @@ end_of_frame:
 						        frame );
 					else
 						(*f) = NULL;
-					spin_unlock_irqrestore(&cam->queue_lock
-					                       , lock_flags);
+					spin_unlock(&cam->queue_lock);
 					memcpy(cam->sysfs.frame_header,
-					       cam->sof_header,
-					       sizeof(sn9c102_sof_header_t));
-					DBG(3, "Video frame captured: "
-					       "%lu bytes", (unsigned long)(b))
+					       cam->sof_header, soflen);
+					DBG(3, "Video frame captured: %lu "
+					       "bytes", (unsigned long)(b));
 
 					if (!(*f))
 						goto resubmit_urb;
@@ -621,18 +672,19 @@ end_of_frame:
 					(*f)->state = F_ERROR;
 					DBG(3, "Not expected EOF after %lu "
 					       "bytes of image data", 
-					  (unsigned long)((*f)->buf.bytesused))
+					    (unsigned long)
+					    ((*f)->buf.bytesused));
 				}
 
 				if (sof) /* (1) */
 					goto start_of_frame;
 
 			} else if (eof) {
-				DBG(3, "EOF without SOF")
+				DBG(3, "EOF without SOF");
 				continue;
 
 			} else {
-				PDBGG("Ignoring pointless isochronous frame")
+				PDBGG("Ignoring pointless isochronous frame");
 				continue;
 			}
 
@@ -642,7 +694,7 @@ start_of_frame:
 			(*f)->buf.bytesused = 0;
 			len -= (sof - pos);
 			pos = sof;
-			DBG(3, "SOF detected: new video frame")
+			DBG(3, "SOF detected: new video frame");
 			if (len)
 				goto redo;
 
@@ -653,12 +705,13 @@ start_of_frame:
 			else {
 				if (cam->sensor->pix_format.pixelformat ==
 				    V4L2_PIX_FMT_SN9C10X) {
-					eof = sof-sizeof(sn9c102_sof_header_t);
+					eof = sof - soflen;
 					goto end_of_frame;
 				} else {
 					DBG(3, "SOF before expected EOF after "
 					       "%lu bytes of image data", 
-					  (unsigned long)((*f)->buf.bytesused))
+					    (unsigned long)
+					    ((*f)->buf.bytesused));
 					goto start_of_frame;
 				}
 			}
@@ -670,7 +723,7 @@ resubmit_urb:
 	err = usb_submit_urb(urb, GFP_ATOMIC);
 	if (err < 0 && err != -EPERM) {
 		cam->state |= DEV_MISCONFIGURED;
-		DBG(1, "usb_submit_urb() failed")
+		DBG(1, "usb_submit_urb() failed");
 	}
 
 	wake_up_interruptible(&cam->wait_frame);
@@ -681,9 +734,13 @@ static int sn9c102_start_transfer(struct sn9c102_device* cam)
 {
 	struct usb_device *udev = cam->usbdev;
 	struct urb* urb;
-	const unsigned int wMaxPacketSize[] = {0, 128, 256, 384, 512,
-	                                       680, 800, 900, 1023};
-	const unsigned int psz = wMaxPacketSize[SN9C102_ALTERNATE_SETTING];
+	const unsigned int sn9c102_wMaxPacketSize[] = {0, 128, 256, 384, 512,
+	                                               680, 800, 900, 1023};
+	const unsigned int sn9c103_wMaxPacketSize[] = {0, 128, 256, 384, 512,
+	                                               680, 800, 900, 1003};
+	const unsigned int psz = (cam->bridge == BRIDGE_SN9C103) ?
+	                    sn9c103_wMaxPacketSize[SN9C102_ALTERNATE_SETTING] :
+	                    sn9c102_wMaxPacketSize[SN9C102_ALTERNATE_SETTING];
 	s8 i, j;
 	int err = 0;
 
@@ -692,7 +749,7 @@ static int sn9c102_start_transfer(struct sn9c102_device* cam)
 		                                  GFP_KERNEL);
 		if (!cam->transfer_buffer[i]) {
 			err = -ENOMEM;
-			DBG(1, "Not enough memory")
+			DBG(1, "Not enough memory");
 			goto free_buffers;
 		}
 	}
@@ -702,7 +759,7 @@ static int sn9c102_start_transfer(struct sn9c102_device* cam)
 		cam->urb[i] = urb;
 		if (!urb) {
 			err = -ENOMEM;
-			DBG(1, "usb_alloc_urb() failed")
+			DBG(1, "usb_alloc_urb() failed");
 			goto free_urbs;
 		}
 		urb->dev = udev;
@@ -725,14 +782,14 @@ static int sn9c102_start_transfer(struct sn9c102_device* cam)
 		err = sn9c102_write_reg(cam, cam->reg[0x01] | 0x04, 0x01);
 		if (err) {
 			err = -EIO;
-			DBG(1, "I/O hardware error")
+			DBG(1, "I/O hardware error");
 			goto free_urbs;
 		}
 	}
 
 	err = usb_set_interface(udev, 0, SN9C102_ALTERNATE_SETTING);
 	if (err) {
-		DBG(1, "usb_set_interface() failed")
+		DBG(1, "usb_set_interface() failed");
 		goto free_urbs;
 	}
 
@@ -743,7 +800,7 @@ static int sn9c102_start_transfer(struct sn9c102_device* cam)
 		if (err) {
 			for (j = i-1; j >= 0; j--)
 				usb_kill_urb(cam->urb[j]);
-			DBG(1, "usb_submit_urb() failed, error %d", err)
+			DBG(1, "usb_submit_urb() failed, error %d", err);
 			goto free_urbs;
 		}
 	}
@@ -779,7 +836,7 @@ static int sn9c102_stop_transfer(struct sn9c102_device* cam)
 
 	err = usb_set_interface(udev, 0, 0); /* 0 Mb/s */
 	if (err)
-		DBG(3, "usb_set_interface() failed")
+		DBG(3, "usb_set_interface() failed");
 
 	return err;
 }
@@ -799,7 +856,7 @@ static int sn9c102_stream_interrupt(struct sn9c102_device* cam)
 	else if (err) {
 		cam->state |= DEV_MISCONFIGURED;
 		DBG(1, "The camera is misconfigured. To use it, close and "
-		       "open /dev/video%d again.", cam->v4ldev->minor)
+		       "open /dev/video%d again.", cam->v4ldev->minor);
 		return err;
 	}
 
@@ -885,8 +942,8 @@ sn9c102_store_reg(struct class_device* cd, const char* buf, size_t len)
 
 	cam->sysfs.reg = index;
 
-	DBG(2, "Moved SN9C10X register index to 0x%02X", cam->sysfs.reg)
-	DBG(3, "Written bytes: %zd", count)
+	DBG(2, "Moved SN9C10X register index to 0x%02X", cam->sysfs.reg);
+	DBG(3, "Written bytes: %zd", count);
 
 	up(&sn9c102_sysfs_lock);
 
@@ -916,7 +973,7 @@ static ssize_t sn9c102_show_val(struct class_device* cd, char* buf)
 
 	count = sprintf(buf, "%d\n", val);
 
-	DBG(3, "Read bytes: %zd", count)
+	DBG(3, "Read bytes: %zd", count);
 
 	up(&sn9c102_sysfs_lock);
 
@@ -954,8 +1011,8 @@ sn9c102_store_val(struct class_device* cd, const char* buf, size_t len)
 	}
 
 	DBG(2, "Written SN9C10X reg. 0x%02X, val. 0x%02X",
-	    cam->sysfs.reg, value)
-	DBG(3, "Written bytes: %zd", count)
+	    cam->sysfs.reg, value);
+	DBG(3, "Written bytes: %zd", count);
 
 	up(&sn9c102_sysfs_lock);
 
@@ -979,7 +1036,7 @@ static ssize_t sn9c102_show_i2c_reg(struct class_device* cd, char* buf)
 
 	count = sprintf(buf, "%u\n", cam->sysfs.i2c_reg);
 
-	DBG(3, "Read bytes: %zd", count)
+	DBG(3, "Read bytes: %zd", count);
 
 	up(&sn9c102_sysfs_lock);
 
@@ -1011,8 +1068,8 @@ sn9c102_store_i2c_reg(struct class_device* cd, const char* buf, size_t len)
 
 	cam->sysfs.i2c_reg = index;
 
-	DBG(2, "Moved sensor register index to 0x%02X", cam->sysfs.i2c_reg)
-	DBG(3, "Written bytes: %zd", count)
+	DBG(2, "Moved sensor register index to 0x%02X", cam->sysfs.i2c_reg);
+	DBG(3, "Written bytes: %zd", count);
 
 	up(&sn9c102_sysfs_lock);
 
@@ -1047,7 +1104,7 @@ static ssize_t sn9c102_show_i2c_val(struct class_device* cd, char* buf)
 
 	count = sprintf(buf, "%d\n", val);
 
-	DBG(3, "Read bytes: %zd", count)
+	DBG(3, "Read bytes: %zd", count);
 
 	up(&sn9c102_sysfs_lock);
 
@@ -1090,8 +1147,8 @@ sn9c102_store_i2c_val(struct class_device* cd, const char* buf, size_t len)
 	}
 
 	DBG(2, "Written sensor reg. 0x%02X, val. 0x%02X",
-	    cam->sysfs.i2c_reg, value)
-	DBG(3, "Written bytes: %zd", count)
+	    cam->sysfs.i2c_reg, value);
+	DBG(3, "Written bytes: %zd", count);
 
 	up(&sn9c102_sysfs_lock);
 
@@ -1193,7 +1250,7 @@ static ssize_t sn9c102_show_frame_header(struct class_device* cd, char* buf)
 	count = sizeof(cam->sysfs.frame_header);
 	memcpy(buf, cam->sysfs.frame_header, count);
 
-	DBG(3, "Frame header, read bytes: %zd", count)
+	DBG(3, "Frame header, read bytes: %zd", count);
 
 	return count;
 } 
@@ -1227,7 +1284,7 @@ static void sn9c102_create_sysfs(struct sn9c102_device* cam)
 		video_device_create_file(v4ldev, &class_device_attr_blue);
 		video_device_create_file(v4ldev, &class_device_attr_red);
 	}
-	if (cam->sensor->sysfs_ops) {
+	if (cam->sensor && cam->sensor->sysfs_ops) {
 		video_device_create_file(v4ldev, &class_device_attr_i2c_reg);
 		video_device_create_file(v4ldev, &class_device_attr_i2c_val);
 	}
@@ -1281,7 +1338,7 @@ static int sn9c102_set_scale(struct sn9c102_device* cam, u8 scale)
 	if (err)
 		return -EIO;
 
-	PDBGG("Scaling factor: %u", scale)
+	PDBGG("Scaling factor: %u", scale);
 
 	return 0;
 }
@@ -1304,7 +1361,7 @@ static int sn9c102_set_crop(struct sn9c102_device* cam, struct v4l2_rect* rect)
 		return -EIO;
 
 	PDBGG("h_start, v_start, h_size, v_size, ho_size, vo_size "
-	      "%u %u %u %u", h_start, v_start, h_size, v_size)
+	      "%u %u %u %u", h_start, v_start, h_size, v_size);
 
 	return 0;
 }
@@ -1336,7 +1393,7 @@ static int sn9c102_init(struct sn9c102_device* cam)
 	if (s->init) {
 		err = s->init(cam);
 		if (err) {
-			DBG(3, "Sensor initialization failed")
+			DBG(3, "Sensor initialization failed");
 			return err;
 		}
 	}
@@ -1353,13 +1410,13 @@ static int sn9c102_init(struct sn9c102_device* cam)
 
 	if (s->pix_format.pixelformat == V4L2_PIX_FMT_SN9C10X)
 		DBG(3, "Compressed video format is active, quality %d",
-		    cam->compression.quality)
+		    cam->compression.quality);
 	else
-		DBG(3, "Uncompressed video format is active")
+		DBG(3, "Uncompressed video format is active");
 
 	if (s->set_crop)
 		if ((err = s->set_crop(cam, rect))) {
-			DBG(3, "set_crop() failed")
+			DBG(3, "set_crop() failed");
 			return err;
 		}
 
@@ -1372,11 +1429,11 @@ static int sn9c102_init(struct sn9c102_device* cam)
 				err = s->set_ctrl(cam, &ctrl);
 				if (err) {
 					DBG(3, "Set %s control failed",
-					    s->qctrl[i].name)
+					    s->qctrl[i].name);
 					return err;
 				}
 				DBG(3, "Image sensor supports '%s' control",
-				    s->qctrl[i].name)
+				    s->qctrl[i].name);
 			}
 	}
 
@@ -1392,7 +1449,7 @@ static int sn9c102_init(struct sn9c102_device* cam)
 		cam->state |= DEV_INITIALIZED;
 	}
 
-	DBG(2, "Initialization succeeded")
+	DBG(2, "Initialization succeeded");
 	return 0;
 }
 
@@ -1401,7 +1458,7 @@ static void sn9c102_release_resources(struct sn9c102_device* cam)
 {
 	down(&sn9c102_sysfs_lock);
 
-	DBG(2, "V4L2 device /dev/video%d deregistered", cam->v4ldev->minor)
+	DBG(2, "V4L2 device /dev/video%d deregistered", cam->v4ldev->minor);
 	video_set_drvdata(cam->v4ldev, NULL);
 	video_unregister_device(cam->v4ldev);
 
@@ -1432,7 +1489,7 @@ static int sn9c102_open(struct inode* inode, struct file* filp)
 	}
 
 	if (cam->users) {
-		DBG(2, "Device /dev/video%d is busy...", cam->v4ldev->minor)
+		DBG(2, "Device /dev/video%d is busy...", cam->v4ldev->minor);
 		if ((filp->f_flags & O_NONBLOCK) ||
 		    (filp->f_flags & O_NDELAY)) {
 			err = -EWOULDBLOCK;
@@ -1458,7 +1515,7 @@ static int sn9c102_open(struct inode* inode, struct file* filp)
 		err = sn9c102_init(cam);
 		if (err) {
 			DBG(1, "Initialization failed again. "
-			       "I will retry on next open().")
+			       "I will retry on next open().");
 			goto out;
 		}
 		cam->state &= ~DEV_MISCONFIGURED;
@@ -1475,7 +1532,7 @@ static int sn9c102_open(struct inode* inode, struct file* filp)
 	cam->frame_count = 0;
 	sn9c102_empty_framequeues(cam);
 
-	DBG(3, "Video device /dev/video%d is open", cam->v4ldev->minor)
+	DBG(3, "Video device /dev/video%d is open", cam->v4ldev->minor);
 
 out:
 	up(&cam->dev_sem);
@@ -1504,7 +1561,7 @@ static int sn9c102_release(struct inode* inode, struct file* filp)
 	cam->users--;
 	wake_up_interruptible_nr(&cam->open, 1);
 
-	DBG(3, "Video device /dev/video%d closed", cam->v4ldev->minor)
+	DBG(3, "Video device /dev/video%d closed", cam->v4ldev->minor);
 
 	up(&cam->dev_sem);
 
@@ -1524,32 +1581,38 @@ sn9c102_read(struct file* filp, char __user * buf, size_t count, loff_t* f_pos)
 		return -ERESTARTSYS;
 
 	if (cam->state & DEV_DISCONNECTED) {
-		DBG(1, "Device not present")
+		DBG(1, "Device not present");
 		up(&cam->fileop_sem);
 		return -ENODEV;
 	}
 
 	if (cam->state & DEV_MISCONFIGURED) {
-		DBG(1, "The camera is misconfigured. Close and open it again.")
+		DBG(1, "The camera is misconfigured. Close and open it "
+		       "again.");
 		up(&cam->fileop_sem);
 		return -EIO;
 	}
 
 	if (cam->io == IO_MMAP) {
 		DBG(3, "Close and open the device again to choose "
-		       "the read method")
+		       "the read method");
 		up(&cam->fileop_sem);
 		return -EINVAL;
 	}
 
 	if (cam->io == IO_NONE) {
 		if (!sn9c102_request_buffers(cam,cam->nreadbuffers, IO_READ)) {
-			DBG(1, "read() failed, not enough memory")
+			DBG(1, "read() failed, not enough memory");
 			up(&cam->fileop_sem);
 			return -ENOMEM;
 		}
 		cam->io = IO_READ;
 		cam->stream = STREAM_ON;
+	}
+
+	if (list_empty(&cam->inqueue)) {
+		if (!list_empty(&cam->outqueue))
+			sn9c102_empty_framequeues(cam);
 		sn9c102_queue_unusedframes(cam);
 	}
 
@@ -1584,6 +1647,16 @@ sn9c102_read(struct file* filp, char __user * buf, size_t count, loff_t* f_pos)
 
 	f = list_entry(cam->outqueue.prev, struct sn9c102_frame_t, frame);
 
+	if (count > f->buf.bytesused)
+		count = f->buf.bytesused;
+
+	if (copy_to_user(buf, f->bufmem, count)) {
+		err = -EFAULT;
+		goto exit;
+	}
+	*f_pos += count;
+
+exit:
 	spin_lock_irqsave(&cam->queue_lock, lock_flags);
 	list_for_each_entry(i, &cam->outqueue, frame)
 		i->state = F_UNUSED;
@@ -1592,16 +1665,8 @@ sn9c102_read(struct file* filp, char __user * buf, size_t count, loff_t* f_pos)
 
 	sn9c102_queue_unusedframes(cam);
 
-	if (count > f->buf.bytesused)
-		count = f->buf.bytesused;
-
-	if (copy_to_user(buf, f->bufmem, count)) {
-		up(&cam->fileop_sem);
-		return -EFAULT;
-	}
-	*f_pos += count;
-
-	PDBGG("Frame #%lu, bytes read: %zu", (unsigned long)f->buf.index,count)
+	PDBGG("Frame #%lu, bytes read: %zu",
+	      (unsigned long)f->buf.index, count);
 
 	up(&cam->fileop_sem);
 
@@ -1612,33 +1677,42 @@ sn9c102_read(struct file* filp, char __user * buf, size_t count, loff_t* f_pos)
 static unsigned int sn9c102_poll(struct file *filp, poll_table *wait)
 {
 	struct sn9c102_device* cam = video_get_drvdata(video_devdata(filp));
+	struct sn9c102_frame_t* f;
+	unsigned long lock_flags;
 	unsigned int mask = 0;
 
 	if (down_interruptible(&cam->fileop_sem))
 		return POLLERR;
 
 	if (cam->state & DEV_DISCONNECTED) {
-		DBG(1, "Device not present")
+		DBG(1, "Device not present");
 		goto error;
 	}
 
 	if (cam->state & DEV_MISCONFIGURED) {
-		DBG(1, "The camera is misconfigured. Close and open it again.")
+		DBG(1, "The camera is misconfigured. Close and open it "
+		       "again.");
 		goto error;
 	}
 
 	if (cam->io == IO_NONE) {
 		if (!sn9c102_request_buffers(cam, cam->nreadbuffers,
 		                             IO_READ)) {
-			DBG(1, "poll() failed, not enough memory")
+			DBG(1, "poll() failed, not enough memory");
 			goto error;
 		}
 		cam->io = IO_READ;
 		cam->stream = STREAM_ON;
 	}
 
-	if (cam->io == IO_READ)
+	if (cam->io == IO_READ) {
+		spin_lock_irqsave(&cam->queue_lock, lock_flags);
+		list_for_each_entry(f, &cam->outqueue, frame)
+			f->state = F_UNUSED;
+		INIT_LIST_HEAD(&cam->outqueue);
+		spin_unlock_irqrestore(&cam->queue_lock, lock_flags);
 		sn9c102_queue_unusedframes(cam);
+	}
 
 	poll_wait(filp, &cam->wait_frame, wait);
 
@@ -1689,13 +1763,14 @@ static int sn9c102_mmap(struct file* filp, struct vm_area_struct *vma)
 		return -ERESTARTSYS;
 
 	if (cam->state & DEV_DISCONNECTED) {
-		DBG(1, "Device not present")
+		DBG(1, "Device not present");
 		up(&cam->fileop_sem);
 		return -ENODEV;
 	}
 
 	if (cam->state & DEV_MISCONFIGURED) {
-		DBG(1, "The camera is misconfigured. Close and open it again.")
+		DBG(1, "The camera is misconfigured. Close and open it "
+		       "again.");
 		up(&cam->fileop_sem);
 		return -EIO;
 	}
@@ -1742,738 +1817,860 @@ static int sn9c102_mmap(struct file* filp, struct vm_area_struct *vma)
 	return 0;
 }
 
+/*****************************************************************************/
 
-static int sn9c102_ioctl_v4l2(struct inode* inode, struct file* filp,
-                              unsigned int cmd, void __user * arg)
+static int
+sn9c102_vidioc_querycap(struct sn9c102_device* cam, void __user * arg)
 {
-	struct sn9c102_device* cam = video_get_drvdata(video_devdata(filp));
+	struct v4l2_capability cap = {
+		.driver = "sn9c102",
+		.version = SN9C102_MODULE_VERSION_CODE,
+		.capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_READWRITE |
+		                V4L2_CAP_STREAMING,
+	};
+
+	strlcpy(cap.card, cam->v4ldev->name, sizeof(cap.card));
+	if (usb_make_path(cam->usbdev, cap.bus_info, sizeof(cap.bus_info)) < 0)
+		strlcpy(cap.bus_info, cam->dev.bus_id, sizeof(cap.bus_info));
+
+	if (copy_to_user(arg, &cap, sizeof(cap)))
+		return -EFAULT;
 
-	switch (cmd) {
+	return 0;
+}
 
-	case VIDIOC_QUERYCAP:
-	{
-		struct v4l2_capability cap = {
-			.driver = "sn9c102",
-			.version = SN9C102_MODULE_VERSION_CODE,
-			.capabilities = V4L2_CAP_VIDEO_CAPTURE | 
-			                V4L2_CAP_READWRITE |
-			                V4L2_CAP_STREAMING,
-		};
-
-		strlcpy(cap.card, cam->v4ldev->name, sizeof(cap.card));
-		if (usb_make_path(cam->usbdev, cap.bus_info,
-		    sizeof(cap.bus_info)) < 0)
-			strlcpy(cap.bus_info, cam->dev.bus_id,
-			        sizeof(cap.bus_info));
-
-		if (copy_to_user(arg, &cap, sizeof(cap)))
-			return -EFAULT;
 
-		return 0;
-	}
+static int
+sn9c102_vidioc_enuminput(struct sn9c102_device* cam, void __user * arg)
+{
+	struct v4l2_input i;
 
-	case VIDIOC_ENUMINPUT:
-	{
-		struct v4l2_input i;
+	if (copy_from_user(&i, arg, sizeof(i)))
+		return -EFAULT;
 
-		if (copy_from_user(&i, arg, sizeof(i)))
-			return -EFAULT;
+	if (i.index)
+		return -EINVAL;
 
-		if (i.index)
-			return -EINVAL;
+	memset(&i, 0, sizeof(i));
+	strcpy(i.name, "USB");
 
-		memset(&i, 0, sizeof(i));
-		strcpy(i.name, "USB");
+	if (copy_to_user(arg, &i, sizeof(i)))
+		return -EFAULT;
 
-		if (copy_to_user(arg, &i, sizeof(i)))
-			return -EFAULT;
+	return 0;
+}
 
-		return 0;
-	}
 
-	case VIDIOC_G_INPUT:
-	case VIDIOC_S_INPUT:
-	{
-		int index;
+static int
+sn9c102_vidioc_gs_input(struct sn9c102_device* cam, void __user * arg)
+{
+	int index;
 
-		if (copy_from_user(&index, arg, sizeof(index)))
-			return -EFAULT;
+	if (copy_from_user(&index, arg, sizeof(index)))
+		return -EFAULT;
 
-		if (index != 0)
-			return -EINVAL;
+	if (index != 0)
+		return -EINVAL;
 
-		return 0;
-	}
+	return 0;
+}
 
-	case VIDIOC_QUERYCTRL:
-	{
-		struct sn9c102_sensor* s = cam->sensor;
-		struct v4l2_queryctrl qc;
-		u8 i;
 
-		if (copy_from_user(&qc, arg, sizeof(qc)))
-			return -EFAULT;
+static int
+sn9c102_vidioc_query_ctrl(struct sn9c102_device* cam, void __user * arg)
+{
+	struct sn9c102_sensor* s = cam->sensor;
+	struct v4l2_queryctrl qc;
+	u8 i;
+