merge: http://linuxtv.org/hg/~pinchartl/uvcvideo/

From: Mauro Carvalho Chehab <mchehab@redhat.com>

Priority: normal

Signed-off-by: Mauro Carvalho Chehab <mchehab@redhat.com>

70 files changed, 7318 insertions, 2185 deletions

diff --git a/linux/drivers/media/dvb/dvb-usb/usb-urb.c b/linux/drivers/media/dvb/dvb-usb/usb-urb.c
index eb3aaaf7a..5becc5a20 100644
--- a/linux/drivers/media/dvb/dvb-usb/usb-urb.c
+++ b/linux/drivers/media/dvb/dvb-usb/usb-urb.c
@@ -160,7 +160,8 @@ static int usb_bulk_urb_init(struct usb_data_stream *stream)
 				stream->props.u.bulk.buffersize,
 				usb_urb_complete, stream);
 
-		stream->urb_list[i]->transfer_flags = 0;
+		stream->urb_list[i]->transfer_flags = URB_NO_TRANSFER_DMA_MAP;
+		stream->urb_list[i]->transfer_dma = stream->dma_addr[i];
 		stream->urbs_initialized++;
 	}
 	return 0;
diff --git a/linux/drivers/media/radio/dsbr100.c b/linux/drivers/media/radio/dsbr100.c
index d1865b164..9c8e96f81 100644
--- a/linux/drivers/media/radio/dsbr100.c
+++ b/linux/drivers/media/radio/dsbr100.c
@@ -1,5 +1,5 @@
-/* A driver for the D-Link DSB-R100 USB radio.  The R100 plugs
- into both the USB and an analog audio input, so this thing
+/* A driver for the D-Link DSB-R100 USB radio and Gemtek USB Radio 21.
+ The device plugs into both the USB and an analog audio input, so this thing
  only deals with initialisation and frequency setting, the
  audio data has to be handled by a sound driver.
 
@@ -33,6 +33,10 @@
 
  History:
 
+ Version 0.44:
+	Add suspend/resume functions, fix unplug of device,
+	a lot of cleanups and fixes by Alexey Klimov <klimov.linux@gmail.com>
+
  Version 0.43:
 	Oliver Neukum: avoided DMA coherency issue
 
@@ -94,8 +98,8 @@
  */
 #include <linux/version.h>	/* for KERNEL_VERSION MACRO	*/
 
-#define DRIVER_VERSION "v0.43"
-#define RADIO_VERSION KERNEL_VERSION(0, 4, 3)
+#define DRIVER_VERSION "v0.44"
+#define RADIO_VERSION KERNEL_VERSION(0, 4, 4)
 
 static struct v4l2_queryctrl radio_qctrl[] = {
 	{
@@ -146,6 +150,7 @@ devices, that would be 76 and 91.  */
 #define FREQ_MAX 108.0
 #define FREQ_MUL 16000
 
+#define videodev_to_radio(d) container_of(d, struct dsbr100_device, videodev)
 
 static int usb_dsbr100_probe(struct usb_interface *intf,
 			     const struct usb_device_id *id);
@@ -162,8 +167,9 @@ module_param(radio_nr, int, 0);
 /* Data for one (physical) device */
 struct dsbr100_device {
 	struct usb_device *usbdev;
-	struct video_device *videodev;
+	struct video_device videodev;
 	u8 *transfer_buffer;
+	struct mutex lock;	/* buffer locking */
 	int curfreq;
 	int stereo;
 	int users;
@@ -171,7 +177,6 @@ struct dsbr100_device {
 	int muted;
 };
 
-
 static struct usb_device_id usb_dsbr100_device_table [] = {
 	{ USB_DEVICE(DSB100_VENDOR, DSB100_PRODUCT) },
 	{ }						/* Terminating entry */
@@ -198,95 +203,190 @@ static struct usb_driver usb_dsbr100_driver = {
 /* switch on radio */
 static int dsbr100_start(struct dsbr100_device *radio)
 {
-	if (usb_control_msg(radio->usbdev, usb_rcvctrlpipe(radio->usbdev, 0),
-			USB_REQ_GET_STATUS,
-			USB_TYPE_VENDOR | USB_RECIP_DEVICE | USB_DIR_IN,
-			0x00, 0xC7, radio->transfer_buffer, 8, 300) < 0 ||
-	usb_control_msg(radio->usbdev, usb_rcvctrlpipe(radio->usbdev, 0),
-			DSB100_ONOFF,
-			USB_TYPE_VENDOR | USB_RECIP_DEVICE | USB_DIR_IN,
-			0x01, 0x00, radio->transfer_buffer, 8, 300) < 0)
-		return -1;
-	radio->muted=0;
+	int retval;
+	int request;
+
+	mutex_lock(&radio->lock);
+
+	retval = usb_control_msg(radio->usbdev,
+		usb_rcvctrlpipe(radio->usbdev, 0),
+		USB_REQ_GET_STATUS,
+		USB_TYPE_VENDOR | USB_RECIP_DEVICE | USB_DIR_IN,
+		0x00, 0xC7, radio->transfer_buffer, 8, 300);
+
+	if (retval < 0) {
+		request = USB_REQ_GET_STATUS;
+		goto usb_control_msg_failed;
+	}
+
+	retval = usb_control_msg(radio->usbdev,
+		usb_rcvctrlpipe(radio->usbdev, 0),
+		DSB100_ONOFF,
+		USB_TYPE_VENDOR | USB_RECIP_DEVICE | USB_DIR_IN,
+		0x01, 0x00, radio->transfer_buffer, 8, 300);
+
+	if (retval < 0) {
+		request = DSB100_ONOFF;
+		goto usb_control_msg_failed;
+	}
+
+	radio->muted = 0;
+	mutex_unlock(&radio->lock);
 	return (radio->transfer_buffer)[0];
-}
 
+usb_control_msg_failed:
+	mutex_unlock(&radio->lock);
+	dev_err(&radio->usbdev->dev,
+		"%s - usb_control_msg returned %i, request %i\n",
+			__func__, retval, request);
+	return retval;
+
+}
 
 /* switch off radio */
 static int dsbr100_stop(struct dsbr100_device *radio)
 {
-	if (usb_control_msg(radio->usbdev, usb_rcvctrlpipe(radio->usbdev, 0),
-			USB_REQ_GET_STATUS,
-			USB_TYPE_VENDOR | USB_RECIP_DEVICE | USB_DIR_IN,
-			0x16, 0x1C, radio->transfer_buffer, 8, 300) < 0 ||
-	usb_control_msg(radio->usbdev, usb_rcvctrlpipe(radio->usbdev, 0),
-			DSB100_ONOFF,
-			USB_TYPE_VENDOR | USB_RECIP_DEVICE | USB_DIR_IN,
-			0x00, 0x00, radio->transfer_buffer, 8, 300) < 0)
-		return -1;
-	radio->muted=1;
+	int retval;
+	int request;
+
+	mutex_lock(&radio->lock);
+
+	retval = usb_control_msg(radio->usbdev,
+		usb_rcvctrlpipe(radio->usbdev, 0),
+		USB_REQ_GET_STATUS,
+		USB_TYPE_VENDOR | USB_RECIP_DEVICE | USB_DIR_IN,
+		0x16, 0x1C, radio->transfer_buffer, 8, 300);
+
+	if (retval < 0) {
+		request = USB_REQ_GET_STATUS;
+		goto usb_control_msg_failed;
+	}
+
+	retval = usb_control_msg(radio->usbdev,
+		usb_rcvctrlpipe(radio->usbdev, 0),
+		DSB100_ONOFF,
+		USB_TYPE_VENDOR | USB_RECIP_DEVICE | USB_DIR_IN,
+		0x00, 0x00, radio->transfer_buffer, 8, 300);
+
+	if (retval < 0) {
+		request = DSB100_ONOFF;
+		goto usb_control_msg_failed;
+	}
+
+	radio->muted = 1;
+	mutex_unlock(&radio->lock);
 	return (radio->transfer_buffer)[0];
+
+usb_control_msg_failed:
+	mutex_unlock(&radio->lock);
+	dev_err(&radio->usbdev->dev,
+		"%s - usb_control_msg returned %i, request %i\n",
+			__func__, retval, request);
+	return retval;
+
 }
 
 /* set a frequency, freq is defined by v4l's TUNER_LOW, i.e. 1/16th kHz */
 static int dsbr100_setfreq(struct dsbr100_device *radio, int freq)
 {
+	int retval;
+	int request;
+
 	freq = (freq / 16 * 80) / 1000 + 856;
-	if (usb_control_msg(radio->usbdev, usb_rcvctrlpipe(radio->usbdev, 0),
-			DSB100_TUNE,
-			USB_TYPE_VENDOR | USB_RECIP_DEVICE | USB_DIR_IN,
-			(freq >> 8) & 0x00ff, freq & 0xff,
-			radio->transfer_buffer, 8, 300) < 0 ||
-	   usb_control_msg(radio->usbdev, usb_rcvctrlpipe(radio->usbdev, 0),
-			USB_REQ_GET_STATUS,
-			USB_TYPE_VENDOR | USB_RECIP_DEVICE | USB_DIR_IN,
-			0x96, 0xB7, radio->transfer_buffer, 8, 300) < 0 ||
-	usb_control_msg(radio->usbdev, usb_rcvctrlpipe(radio->usbdev, 0),
-			USB_REQ_GET_STATUS,
-			USB_TYPE_VENDOR | USB_RECIP_DEVICE |  USB_DIR_IN,
-			0x00, 0x24, radio->transfer_buffer, 8, 300) < 0) {
-		radio->stereo = -1;
-		return -1;
+	mutex_lock(&radio->lock);
+
+	retval = usb_control_msg(radio->usbdev,
+		usb_rcvctrlpipe(radio->usbdev, 0),
+		DSB100_TUNE,
+		USB_TYPE_VENDOR | USB_RECIP_DEVICE | USB_DIR_IN,
+		(freq >> 8) & 0x00ff, freq & 0xff,
+		radio->transfer_buffer, 8, 300);
+
+	if (retval < 0) {
+		request = DSB100_TUNE;
+		goto usb_control_msg_failed;
 	}
+
+	retval = usb_control_msg(radio->usbdev,
+		usb_rcvctrlpipe(radio->usbdev, 0),
+		USB_REQ_GET_STATUS,
+		USB_TYPE_VENDOR | USB_RECIP_DEVICE | USB_DIR_IN,
+		0x96, 0xB7, radio->transfer_buffer, 8, 300);
+
+	if (retval < 0) {
+		request = USB_REQ_GET_STATUS;
+		goto usb_control_msg_failed;
+	}
+
+	retval = usb_control_msg(radio->usbdev,
+		usb_rcvctrlpipe(radio->usbdev, 0),
+		USB_REQ_GET_STATUS,
+		USB_TYPE_VENDOR | USB_RECIP_DEVICE |  USB_DIR_IN,
+		0x00, 0x24, radio->transfer_buffer, 8, 300);
+
+	if (retval < 0) {
+		request = USB_REQ_GET_STATUS;
+		goto usb_control_msg_failed;
+	}
+
 	radio->stereo = !((radio->transfer_buffer)[0] & 0x01);
+	mutex_unlock(&radio->lock);
 	return (radio->transfer_buffer)[0];
+
+usb_control_msg_failed:
+	radio->stereo = -1;
+	mutex_unlock(&radio->lock);
+	dev_err(&radio->usbdev->dev,
+		"%s - usb_control_msg returned %i, request %i\n",
+			__func__, retval, request);
+	return retval;
 }
 
 /* return the device status.  This is, in effect, just whether it
 sees a stereo signal or not.  Pity. */
 static void dsbr100_getstat(struct dsbr100_device *radio)
 {
-	if (usb_control_msg(radio->usbdev, usb_rcvctrlpipe(radio->usbdev, 0),
+	int retval;
+
+	mutex_lock(&radio->lock);
+
+	retval = usb_control_msg(radio->usbdev,
+		usb_rcvctrlpipe(radio->usbdev, 0),
 		USB_REQ_GET_STATUS,
 		USB_TYPE_VENDOR | USB_RECIP_DEVICE | USB_DIR_IN,
-		0x00 , 0x24, radio->transfer_buffer, 8, 300) < 0)
+		0x00 , 0x24, radio->transfer_buffer, 8, 300);
+
+	if (retval < 0) {
 		radio->stereo = -1;
-	else
+		dev_err(&radio->usbdev->dev,
+			"%s - usb_control_msg returned %i, request %i\n",
+				__func__, retval, USB_REQ_GET_STATUS);
+	} else {
 		radio->stereo = !(radio->transfer_buffer[0] & 0x01);
-}
+	}
 
+	mutex_unlock(&radio->lock);
+}
 
 /* USB subsystem interface begins here */
 
-/* handle unplugging of the device, release data structures
-if nothing keeps us from doing it.  If something is still
-keeping us busy, the release callback of v4l will take care
-of releasing it. */
+/*
+ * Handle unplugging of the device.
+ * We call video_unregister_device in any case.
+ * The last function called in this procedure is
+ * usb_dsbr100_video_device_release
+ */
 static void usb_dsbr100_disconnect(struct usb_interface *intf)
 {
 	struct dsbr100_device *radio = usb_get_intfdata(intf);
 
 	usb_set_intfdata (intf, NULL);
-	if (radio) {
-		video_unregister_device(radio->videodev);
-		radio->videodev = NULL;
-		if (radio->users) {
-			kfree(radio->transfer_buffer);
-			kfree(radio);
-		} else {
-			radio->removed = 1;
-		}
-	}
+
+	mutex_lock(&radio->lock);
+	radio->removed = 1;
+	mutex_unlock(&radio->lock);
+
+	video_unregister_device(&radio->videodev);
 }
 
 
@@ -306,6 +406,10 @@ static int vidioc_g_tuner(struct file *file, void *priv,
 {
 	struct dsbr100_device *radio = video_drvdata(file);
 
+	/* safety check */
+	if (radio->removed)
+		return -EIO;
+
 	if (v->index > 0)
 		return -EINVAL;
 
@@ -327,6 +431,12 @@ static int vidioc_g_tuner(struct file *file, void *priv,
 static int vidioc_s_tuner(struct file *file, void *priv,
 				struct v4l2_tuner *v)
 {
+	struct dsbr100_device *radio = video_drvdata(file);
+
+	/* safety check */
+	if (radio->removed)
+		return -EIO;
+
 	if (v->index > 0)
 		return -EINVAL;
 
@@ -337,9 +447,15 @@ static int vidioc_s_frequency(struct file *file, void *priv,
 				struct v4l2_frequency *f)
 {
 	struct dsbr100_device *radio = video_drvdata(file);
+	int retval;
+
+	/* safety check */
+	if (radio->removed)
+		return -EIO;
 
 	radio->curfreq = f->frequency;
-	if (dsbr100_setfreq(radio, radio->curfreq) == -1)
+	retval = dsbr100_setfreq(radio, radio->curfreq);
+	if (retval < 0)
 		dev_warn(&radio->usbdev->dev, "Set frequency failed\n");
 	return 0;
 }
@@ -349,6 +465,10 @@ static int vidioc_g_frequency(struct file *file, void *priv,
 {
 	struct dsbr100_device *radio = video_drvdata(file);
 
+	/* safety check */
+	if (radio->removed)
+		return -EIO;
+
 	f->type = V4L2_TUNER_RADIO;
 	f->frequency = radio->curfreq;
 	return 0;
@@ -373,6 +493,10 @@ static int vidioc_g_ctrl(struct file *file, void *priv,
 {
 	struct dsbr100_device *radio = video_drvdata(file);
 
+	/* safety check */
+	if (radio->removed)
+		return -EIO;
+
 	switch (ctrl->id) {
 	case V4L2_CID_AUDIO_MUTE:
 		ctrl->value = radio->muted;
@@ -385,17 +509,24 @@ static int vidioc_s_ctrl(struct file *file, void *priv,
 				struct v4l2_control *ctrl)
 {
 	struct dsbr100_device *radio = video_drvdata(file);
+	int retval;
+
+	/* safety check */
+	if (radio->removed)
+		return -EIO;
 
 	switch (ctrl->id) {
 	case V4L2_CID_AUDIO_MUTE:
 		if (ctrl->value) {
-			if (dsbr100_stop(radio) == -1) {
+			retval = dsbr100_stop(radio);
+			if (retval < 0) {
 				dev_warn(&radio->usbdev->dev,
 					 "Radio did not respond properly\n");
 				return -EBUSY;
 			}
 		} else {
-			if (dsbr100_start(radio) == -1) {
+			retval = dsbr100_start(radio);
+			if (retval < 0) {
 				dev_warn(&radio->usbdev->dev,
 					 "Radio did not respond properly\n");
 				return -EBUSY;
@@ -447,7 +578,8 @@ static int usb_dsbr100_open(struct inode *inode, struct file *file)
 	radio->users = 1;
 	radio->muted = 1;
 
-	if (dsbr100_start(radio) < 0) {
+	retval = dsbr100_start(radio);
+	if (retval < 0) {
 		dev_warn(&radio->usbdev->dev,
 			 "Radio did not start up properly\n");
 		radio->users = 0;
@@ -456,9 +588,9 @@ static int usb_dsbr100_open(struct inode *inode, struct file *file)
 	}
 
 	retval = dsbr100_setfreq(radio, radio->curfreq);
-
-	if (retval == -1)
-		printk(KERN_WARNING KBUILD_MODNAME ": Set frequency failed\n");
+	if (retval < 0)
+		dev_warn(&radio->usbdev->dev,
+			"set frequency failed\n");
 
 	unlock_kernel();
 	return 0;
@@ -467,13 +599,19 @@ static int usb_dsbr100_open(struct inode *inode, struct file *file)
 static int usb_dsbr100_close(struct inode *inode, struct file *file)
 {
 	struct dsbr100_device *radio = video_drvdata(file);
+	int retval;
 
 	if (!radio)
 		return -ENODEV;
+
 	radio->users = 0;
-	if (radio->removed) {
-		kfree(radio->transfer_buffer);
-		kfree(radio);
+	if (!radio->removed) {
+		retval = dsbr100_stop(radio);
+		if (retval < 0) {
+			dev_warn(&radio->usbdev->dev,
+				"dsbr100_stop failed\n");
+		}
+
 	}
 	return 0;
 }
@@ -485,7 +623,7 @@ static int usb_dsbr100_suspend(struct usb_interface *intf, pm_message_t message)
 	int retval;
 
 	retval = dsbr100_stop(radio);
-	if (retval == -1)
+	if (retval < 0)
 		dev_warn(&intf->dev, "dsbr100_stop failed\n");
 
 	dev_info(&intf->dev, "going into suspend..\n");
@@ -500,7 +638,7 @@ static int usb_dsbr100_resume(struct usb_interface *intf)
 	int retval;
 
 	retval = dsbr100_start(radio);
-	if (retval == -1)
+	if (retval < 0)
 		dev_warn(&intf->dev, "dsbr100_start failed\n");
 
 	dev_info(&intf->dev, "coming out of suspend..\n");
@@ -508,6 +646,15 @@ static int usb_dsbr100_resume(struct usb_interface *intf)
 	return 0;
 }
 
+/* free data structures */
+static void usb_dsbr100_video_device_release(struct video_device *videodev)
+{
+	struct dsbr100_device *radio = videodev_to_radio(videodev);
+
+	kfree(radio->transfer_buffer);
+	kfree(radio);
+}
+
 /* File system interface */
 static const struct file_operations usb_dsbr100_fops = {
 	.owner		= THIS_MODULE,
@@ -536,19 +683,19 @@ static const struct v4l2_ioctl_ops usb_dsbr100_ioctl_ops = {
 };
 
 /* V4L2 interface */
-static struct video_device dsbr100_videodev_template = {
+static struct video_device dsbr100_videodev_data = {
 	.name		= "D-Link DSB-R 100",
 	.fops		= &usb_dsbr100_fops,
 	.ioctl_ops 	= &usb_dsbr100_ioctl_ops,
-	.release	= video_device_release,
+	.release	= usb_dsbr100_video_device_release,
 };
 
-/* check if the device is present and register with v4l and
-usb if it is */
+/* check if the device is present and register with v4l and usb if it is */
 static int usb_dsbr100_probe(struct usb_interface *intf,
 				const struct usb_device_id *id)
 {
 	struct dsbr100_device *radio;
+	int retval;
 
 	radio = kmalloc(sizeof(struct dsbr100_device), GFP_KERNEL);
 
@@ -561,23 +708,18 @@ static int usb_dsbr100_probe(struct usb_interface *intf,
 		kfree(radio);
 		return -ENOMEM;
 	}
-	radio->videodev = video_device_alloc();
 
-	if (!(radio->videodev)) {
-		kfree(radio->transfer_buffer);
-		kfree(radio);
-		return -ENOMEM;
-	}
-	memcpy(radio->videodev, &dsbr100_videodev_template,
-		sizeof(dsbr100_videodev_template));
+	mutex_init(&radio->lock);
+	radio->videodev = dsbr100_videodev_data;
+
 	radio->removed = 0;
 	radio->users = 0;
 	radio->usbdev = interface_to_usbdev(intf);
 	radio->curfreq = FREQ_MIN * FREQ_MUL;
-	video_set_drvdata(radio->videodev, radio);
-	if (video_register_device(radio->videodev, VFL_TYPE_RADIO, radio_nr) < 0) {
-		dev_warn(&intf->dev, "Could not register video device\n");
-		video_device_release(radio->videodev);
+	video_set_drvdata(&radio->videodev, radio);
+	retval = video_register_device(&radio->videodev, VFL_TYPE_RADIO, radio_nr);
+	if (retval < 0) {
+		dev_err(&intf->dev, "couldn't register video device\n");
 		kfree(radio->transfer_buffer);
 		kfree(radio);
 		return -EIO;
diff --git a/linux/drivers/media/radio/radio-mr800.c b/linux/drivers/media/radio/radio-mr800.c
index dbce57c26..7683af18d 100644
--- a/linux/drivers/media/radio/radio-mr800.c
+++ b/linux/drivers/media/radio/radio-mr800.c
@@ -142,7 +142,6 @@ struct amradio_device {
 
 	unsigned char *buffer;
 	struct mutex lock;	/* buffer locking */
-	struct mutex disconnect_lock;
 	int curfreq;
 	int stereo;
 	int users;
@@ -170,7 +169,7 @@ static struct usb_driver usb_amradio_driver = {
 	.reset_resume		= usb_amradio_resume,
 #endif
 	.id_table		= usb_amradio_device_table,
-	.supports_autosuspend	= 1,
+	.supports_autosuspend	= 0,
 };
 
 /* switch on radio. Send 8 bytes to device. */
@@ -305,16 +304,12 @@ static void usb_amradio_disconnect(struct usb_interface *intf)
 {
 	struct amradio_device *radio = usb_get_intfdata(intf);
 
-	mutex_lock(&radio->disconnect_lock);
+	mutex_lock(&radio->lock);
 	radio->removed = 1;
-	usb_set_intfdata(intf, NULL);
+	mutex_unlock(&radio->lock);
 
-	if (radio->users == 0) {
-		video_unregister_device(radio->videodev);
-		kfree(radio->buffer);
-		kfree(radio);
-	}
-	mutex_unlock(&radio->disconnect_lock);
+	usb_set_intfdata(intf, NULL);
+	video_unregister_device(radio->videodev);
 }
 
 /* vidioc_querycap - query device capabilities */
@@ -532,7 +527,7 @@ static int usb_amradio_open(struct inode *inode, struct file *file)
 	return 0;
 }
 
-/*close device - free driver structures */
+/*close device */
 static int usb_amradio_close(struct inode *inode, struct file *file)
 {
 	struct amradio_device *radio = video_get_drvdata(video_devdata(file));
@@ -541,21 +536,15 @@ static int usb_amradio_close(struct inode *inode, struct file *file)
 	if (!radio)
 		return -ENODEV;
 
-	mutex_lock(&radio->disconnect_lock);
 	radio->users = 0;
-	if (radio->removed) {
-		video_unregister_device(radio->videodev);
-		kfree(radio->buffer);
-		kfree(radio);
 
-	} else {
+	if (!radio->removed) {
 		retval = amradio_stop(radio);
 		if (retval < 0)
 			amradio_dev_warn(&radio->videodev->dev,
 				"amradio_stop failed\n");
 	}
 
-	mutex_unlock(&radio->disconnect_lock);
 	return 0;
 }
 
@@ -612,12 +601,24 @@ static const struct v4l2_ioctl_ops usb_amradio_ioctl_ops = {
 	.vidioc_s_input     = vidioc_s_input,
 };
 
+static void usb_amradio_device_release(struct video_device *videodev)
+{
+	struct amradio_device *radio = video_get_drvdata(videodev);
+
+	/* we call v4l to free radio->videodev */
+	video_device_release(videodev);
+
+	/* free rest memory */
+	kfree(radio->buffer);
+	kfree(radio);
+}
+
 /* V4L2 interface */
 static struct video_device amradio_videodev_template = {
 	.name		= "AverMedia MR 800 USB FM Radio",
 	.fops		= &usb_amradio_fops,
 	.ioctl_ops 	= &usb_amradio_ioctl_ops,
-	.release	= video_device_release,
+	.release	= usb_amradio_device_release,
 };
 
 /* check if the device is present and register with v4l and
@@ -655,7 +656,6 @@ static int usb_amradio_probe(struct usb_interface *intf,
 	radio->usbdev = interface_to_usbdev(intf);
 	radio->curfreq = 95.16 * FREQ_MUL;
 
-	mutex_init(&radio->disconnect_lock);
 	mutex_init(&radio->lock);
 
 	video_set_drvdata(radio->videodev, radio);
diff --git a/linux/drivers/media/video/Kconfig b/linux/drivers/media/video/Kconfig
index cc224744b..631112e28 100644
--- a/linux/drivers/media/video/Kconfig
+++ b/linux/drivers/media/video/Kconfig
@@ -736,10 +736,16 @@ config MT9M001_PCA9536_SWITCH
 	  extender to switch between 8 and 10 bit datawidth modes
 
 config SOC_CAMERA_MT9M111
-	tristate "mt9m111 support"
+	tristate "mt9m111 and mt9m112 support"
 	depends on SOC_CAMERA && I2C
 	help
-	  This driver supports MT9M111 cameras from Micron
+	  This driver supports MT9M111 and MT9M112 cameras from Micron
+
+config SOC_CAMERA_MT9T031
+	tristate "mt9t031 support"
+	depends on SOC_CAMERA && I2C
+	help
+	  This driver supports MT9T031 cameras from Micron.
 
 config SOC_CAMERA_MT9V022
 	tristate "mt9v022 support"
@@ -755,6 +761,12 @@ config MT9V022_PCA9536_SWITCH
 	  Select this if your MT9V022 camera uses a PCA9536 I2C GPIO
 	  extender to switch between 8 and 10 bit datawidth modes
 
+config SOC_CAMERA_TW9910
+	tristate "tw9910 support"
+	depends on SOC_CAMERA && I2C
+	help
+	  This is a tw9910 video driver
+
 config SOC_CAMERA_PLATFORM
 	tristate "platform camera support"
 	depends on SOC_CAMERA
diff --git a/linux/drivers/media/video/Makefile b/linux/drivers/media/video/Makefile
index 84a2be0cb..1611c33b1 100644
--- a/linux/drivers/media/video/Makefile
+++ b/linux/drivers/media/video/Makefile
@@ -137,9 +137,11 @@ obj-$(CONFIG_VIDEO_OMAP2)		+= omap2cam.o
 obj-$(CONFIG_SOC_CAMERA)	+= soc_camera.o
 obj-$(CONFIG_SOC_CAMERA_MT9M001)	+= mt9m001.o
 obj-$(CONFIG_SOC_CAMERA_MT9M111)	+= mt9m111.o
+obj-$(CONFIG_SOC_CAMERA_MT9T031)	+= mt9t031.o
 obj-$(CONFIG_SOC_CAMERA_MT9V022)	+= mt9v022.o
 obj-$(CONFIG_SOC_CAMERA_OV772X)		+= ov772x.o
 obj-$(CONFIG_SOC_CAMERA_PLATFORM)	+= soc_camera_platform.o
+obj-$(CONFIG_SOC_CAMERA_TW9910)		+= tw9910.o
 
 obj-$(CONFIG_VIDEO_AU0828) += au0828/
 
diff --git a/linux/drivers/media/video/em28xx/em28xx-cards.c b/linux/drivers/media/video/em28xx/em28xx-cards.c
index c14fca841..a4ce360b1 100644
--- a/linux/drivers/media/video/em28xx/em28xx-cards.c
+++ b/linux/drivers/media/video/em28xx/em28xx-cards.c
@@ -38,6 +38,8 @@
 
 #include "em28xx.h"
 
+#define DRIVER_NAME         "em28xx"
+
 static int tuner = -1;
 module_param(tuner, int, 0444);
 MODULE_PARM_DESC(tuner, "tuner type");
@@ -46,6 +48,13 @@ static unsigned int disable_ir;
 module_param(disable_ir, int, 0444);
 MODULE_PARM_DESC(disable_ir, "disable infrared remote support");
 
+static unsigned int card[]     = {[0 ... (EM28XX_MAXBOARDS - 1)] = UNSET };
+module_param_array(card,  int, NULL, 0444);
+MODULE_PARM_DESC(card,     "card type");
+
+/* Bitmask marking allocated devices from 0 to EM28XX_MAXBOARDS */
+static unsigned long em28xx_devused;
+
 struct em28xx_hash_table {
 	unsigned long hash;
 	unsigned int  model;
@@ -975,8 +984,30 @@ struct em28xx_board em28xx_boards[] = {
 			.amux     = EM28XX_AMUX_LINE_IN,
 		} },
 	},
+	[EM2820_BOARD_PROLINK_PLAYTV_BOX4_USB2] = {
+		.name         = "Pixelview PlayTV Box 4 USB 2.0",
+		.tda9887_conf = TDA9887_PRESENT,
+		.tuner_type   = TUNER_YMEC_TVF_5533MF,
+		.decoder      = EM28XX_SAA711X,
+		.input        = { {
+			.type     = EM28XX_VMUX_TELEVISION,
+			.vmux     = SAA7115_COMPOSITE2,
+			.amux     = EM28XX_AMUX_VIDEO,
+			.aout     = EM28XX_AOUT_MONO | 	/* I2S */
+				    EM28XX_AOUT_MASTER,	/* Line out pin */
+		}, {
+			.type     = EM28XX_VMUX_COMPOSITE1,
+			.vmux     = SAA7115_COMPOSITE0,
+			.amux     = EM28XX_AMUX_LINE_IN,
+		}, {
+			.type     = EM28XX_VMUX_SVIDEO,
+			.vmux     = SAA7115_SVIDEO3,
+			.amux     = EM28XX_AMUX_LINE_IN,
+		} },
+	},
 	[EM2820_BOARD_PROLINK_PLAYTV_USB2] = {
 		.name         = "Pixelview Prolink PlayTV USB 2.0",
+		.has_snapshot_button = 1,
 		.tda9887_conf = TDA9887_PRESENT,
 		.tuner_type   = TUNER_YMEC_TVF_5533MF,
 		.decoder      = EM28XX_SAA711X,
@@ -1410,6 +1441,7 @@ MODULE_DEVICE_TABLE(usb, em28xx_id_table);
 static struct em28xx_hash_table em28xx_eeprom_hash [] = {
 	/* P/N: SA 60002070465 Tuner: TVF7533-MF */
 	{0x6ce05a8f, EM2820_BOARD_PROLINK_PLAYTV_USB2, TUNER_YMEC_TVF_5533MF},
+	{0x72cc5a8b, EM2820_BOARD_PROLINK_PLAYTV_BOX4_USB2, TUNER_YMEC_TVF_5533MF},
 	{0x966a0441, EM2880_BOARD_KWORLD_DVB_310U, TUNER_XC2028},
 };
 
@@ -1870,3 +1902,432 @@ void em28xx_card_setup(struct em28xx *dev)
 	em28xx_ir_init(dev);
 }
 
+
+#if defined(CONFIG_MODULES) && defined(MODULE)
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 20)
+static void request_module_async(void *ptr)
+{
+	struct em28xx *dev = (struct em28xx *)ptr;
+#else
+static void request_module_async(struct work_struct *work)
+{
+	struct em28xx *dev = container_of(work,
+			     struct em28xx, request_module_wk);
+#endif
+
+	if (dev->has_audio_class)
+		request_module("snd-usb-audio");
+	else if (dev->has_alsa_audio)
+		request_module("em28xx-alsa");
+
+	if (dev->board.has_dvb)
+		request_module("em28xx-dvb");
+}
+
+static void request_modules(struct em28xx *dev)
+{
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 20)
+	INIT_WORK(&dev->request_module_wk, request_module_async,
+		  (void *)dev);
+#else
+	INIT_WORK(&dev->request_module_wk, request_module_async);
+#endif
+	schedule_work(&dev->request_module_wk);
+}
+#else
+#define request_modules(dev)
+#endif /* CONFIG_MODULES */
+
+/*
+ * em28xx_realease_resources()
+ * unregisters the v4l2,i2c and usb devices
+ * called when the device gets disconected or at module unload
+*/
+void em28xx_release_resources(struct em28xx *dev)
+{
+	if (dev->sbutton_input_dev)
+		em28xx_deregister_snapshot_button(dev);
+
+	if (dev->ir)
+		em28xx_ir_fini(dev);
+
+	/*FIXME: I2C IR should be disconnected */
+
+	em28xx_release_analog_resources(dev);
+
+	em28xx_remove_from_devlist(dev);
+
+	em28xx_i2c_unregister(dev);
+	usb_put_dev(dev->udev);
+
+	/* Mark device as unused */
+	em28xx_devused &= ~(1 << dev->devno);
+};
+
+/*
+ * em28xx_init_dev()
+ * allocates and inits the device structs, registers i2c bus and v4l device
+ */
+int em28xx_init_dev(struct em28xx **devhandle, struct usb_device *udev,
+			   int minor)
+{
+	struct em28xx *dev = *devhandle;
+	int retval = -ENOMEM;
+	int errCode;
+
+	dev->udev = udev;
+	mutex_init(&dev->ctrl_urb_lock);
+	spin_lock_init(&dev->slock);
+	init_waitqueue_head(&dev->open);
+	init_waitqueue_head(&dev->wait_frame);
+	init_waitqueue_head(&dev->wait_stream);
+
+	dev->em28xx_write_regs = em28xx_write_regs;
+	dev->em28xx_read_reg = em28xx_read_reg;
+	dev->em28xx_read_reg_req_len = em28xx_read_reg_req_len;
+	dev->em28xx_write_regs_req = em28xx_write_regs_req;
+	dev->em28xx_read_reg_req = em28xx_read_reg_req;
+	dev->board.is_em2800 = em28xx_boards[dev->model].is_em2800;
+
+	em28xx_pre_card_setup(dev);
+
+	if (!dev->board.is_em2800) {
+		/* Sets I2C speed to 100 KHz */
+		retval = em28xx_write_reg(dev, EM28XX_R06_I2C_CLK, 0x40);
+		if (retval < 0) {
+			em28xx_errdev("%s: em28xx_write_regs_req failed!"
+				      " retval [%d]\n",
+				      __func__, retval);
+			return retval;
+		}
+	}
+
+	/* register i2c bus */
+	errCode = em28xx_i2c_register(dev);
+	if (errCode < 0) {
+		em28xx_errdev("%s: em28xx_i2c_register - errCode [%d]!\n",
+			__func__, errCode);
+		return errCode;
+	}
+
+	/* Do board specific init and eeprom reading */
+	em28xx_card_setup(dev);
+
+	/* Configure audio */
+	errCode = em28xx_audio_setup(dev);
+	if (errCode < 0) {
+		em28xx_errdev("%s: Error while setting audio - errCode [%d]!\n",
+			__func__, errCode);
+	}
+
+	/* wake i2c devices */
+	em28xx_wake_i2c(dev);
+
+	/* init video dma queues */
+	INIT_LIST_HEAD(&dev->vidq.active);
+	INIT_LIST_HEAD(&dev->vidq.queued);
+
+#if 0
+	video_set_drvdata(dev->vbi_dev, dev);
+#endif
+
+	if (dev->board.has_msp34xx) {
+		/* Send a reset to other chips via gpio */
+		errCode = em28xx_write_reg(dev, EM28XX_R08_GPIO, 0xf7);
+		if (errCode < 0) {
+			em28xx_errdev("%s: em28xx_write_regs_req - "
+				      "msp34xx(1) failed! errCode [%d]\n",
+				      __func__, errCode);
+			return errCode;
+		}
+		msleep(3);
+
+		errCode = em28xx_write_reg(dev, EM28XX_R08_GPIO, 0xff);
+		if (errCode < 0) {
+			em28xx_errdev("%s: em28xx_write_regs_req - "
+				      "msp34xx(2) failed! errCode [%d]\n",
+				      __func__, errCode);
+			return errCode;
+		}
+		msleep(3);
+	}
+
+	em28xx_add_into_devlist(dev);
+
+	retval = em28xx_register_analog_devices(dev);
+	if (retval < 0) {
+		em28xx_release_resources(dev);
+		goto fail_reg_devices;
+	}
+
+	em28xx_init_extension(dev);
+
+	/* Save some power by putting tuner to sleep */
+	em28xx_i2c_call_clients(dev, TUNER_SET_STANDBY, NULL);
+
+	return 0;
+
+fail_reg_devices:
+	return retval;
+}
+
+/*
+ * em28xx_usb_probe()
+ * checks for supported devices
+ */
+static int em28xx_usb_probe(struct usb_interface *interface,
+			    const struct usb_device_id *id)
+{
+	const struct usb_endpoint_descriptor *endpoint;
+	struct usb_device *udev;
+	struct usb_interface *uif;
+	struct em28xx *dev = NULL;
+	int retval = -ENODEV;
+	int i, nr, ifnum, isoc_pipe;
+	char *speed;
+	char descr[255] = "";
+
+	udev = usb_get_dev(interface_to_usbdev(interface));
+	ifnum = interface->altsetting[0].desc.bInterfaceNumber;
+
+	/* Check to see next free device and mark as used */
+	nr = find_first_zero_bit(&em28xx_devused, EM28XX_MAXBOARDS);
+	em28xx_devused |= 1<<nr;
+
+	/* Don't register audio interfaces */
+	if (interface->altsetting[0].desc.bInterfaceClass == USB_CLASS_AUDIO) {
+		em28xx_err(DRIVER_NAME " audio device (%04x:%04x): "
+			"interface %i, class %i\n",
+			le16_to_cpu(udev->descriptor.idVendor),
+			le16_to_cpu(udev->descriptor.idProduct),
+			ifnum,
+			interface->altsetting[0].desc.bInterfaceClass);
+
+		em28xx_devused &= ~(1<<nr);
+		return -ENODEV;
+	}
+
+	endpoint = &interface->cur_altsetting->endpoint[0].desc;
+
+	/* check if the device has the iso in endpoint at the correct place */
+	if ((endpoint->bmAttributes & USB_ENDPOINT_XFERTYPE_MASK) ==
+	    USB_ENDPOINT_XFER_ISOC &&
+	    (interface->altsetting[1].endpoint[0].desc.wMaxPacketSize == 940)) {
+		/* It's a newer em2874/em2875 device */
+		isoc_pipe = 0;
+	} else {
+		int check_interface = 1;
+		isoc_pipe = 1;
+		endpoint = &interface->cur_altsetting->endpoint[1].desc;
+		if ((endpoint->bmAttributes & USB_ENDPOINT_XFERTYPE_MASK) !=
+		    USB_ENDPOINT_XFER_ISOC)
+			check_interface = 0;
+
+		if ((endpoint->bEndpointAddress & USB_ENDPOINT_DIR_MASK) == USB_DIR_OUT)
+			check_interface = 0;
+
+		if (!check_interface) {
+			em28xx_err(DRIVER_NAME " video device (%04x:%04x): "
+				"interface %i, class %i found.\n",
+				le16_to_cpu(udev->descriptor.idVendor),
+				le16_to_cpu(udev->descriptor.idProduct),
+				ifnum,
+				interface->altsetting[0].desc.bInterfaceClass);
+
+			em28xx_err(DRIVER_NAME " This is an anciliary "
+				"interface not used by the driver\n");
+
+			em28xx_devused &= ~(1<<nr);
+			return -ENODEV;
+		}
+	}
+
+	switch (udev->speed) {
+	case USB_SPEED_LOW:
+		speed = "1.5";
+		break;
+	case USB_SPEED_UNKNOWN:
+	case USB_SPEED_FULL:
+		speed = "12";
+		break;
+	case USB_SPEED_HIGH:
+		speed = "480";
+		break;
+	default:
+		speed = "unknown";
+	}
+
+	if (udev->manufacturer)
+		strlcpy(descr, udev->manufacturer, sizeof(descr));
+
+	if (udev->product) {
+		if (*descr)
+			strlcat(descr, " ", sizeof(descr));
+		strlcat(descr, udev->product, sizeof(descr));
+	}
+	if (*descr)
+		strlcat(descr, " ", sizeof(descr));
+
+	printk(DRIVER_NAME ": New device %s@ %s Mbps "
+		"(%04x:%04x, interface %d, class %d)\n",
+		descr,
+		speed,
+		le16_to_cpu(udev->descriptor.idVendor),
+		le16_to_cpu(udev->descriptor.idProduct),
+		ifnum,
+		interface->altsetting->desc.bInterfaceNumber);
+
+	if (nr >= EM28XX_MAXBOARDS) {
+		printk(DRIVER_NAME ": Supports only %i em28xx boards.\n",
+				EM28XX_MAXBOARDS);
+		em28xx_devused &= ~(1<<nr);
+		return -ENOMEM;
+	}
+
+	/* allocate memory for our device state and initialize it */
+	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
+	if (dev == NULL) {
+		em28xx_err(DRIVER_NAME ": out of memory!\n");
+		em28xx_devused &= ~(1<<nr);
+		return -ENOMEM;
+	}
+
+	snprintf(dev->name, 29, "em28xx #%d", nr);
+	dev->devno = nr;
+	dev->model = id->driver_info;
+	dev->alt   = -1;
+
+	/* Checks if audio is provided by some interface */
+	for (i = 0; i < udev->config->desc.bNumInterfaces; i++) {
+		uif = udev->config->interface[i];
+		if (uif->altsetting[0].desc.bInterfaceClass == USB_CLASS_AUDIO) {
+			dev->has_audio_class = 1;
+			break;
+		}
+	}
+
+	/* compute alternate max packet sizes */
+	uif = udev->actconfig->interface[0];
+
+	dev->num_alt = uif->num_altsetting;
+	dev->alt_max_pkt_size = kmalloc(32 * dev->num_alt, GFP_KERNEL);
+
+	if (dev->alt_max_pkt_size == NULL) {
+		em28xx_errdev("out of memory!\n");
+		em28xx_devused &= ~(1<<nr);
+		kfree(dev);
+		return -ENOMEM;
+	}
+
+	for (i = 0; i < dev->num_alt ; i++) {
+		u16 tmp = le16_to_cpu(uif->altsetting[i].endpoint[isoc_pipe].desc.wMaxPacketSize);
+		dev->alt_max_pkt_size[i] =
+		    (tmp & 0x07ff) * (((tmp & 0x1800) >> 11) + 1);
+	}
+
+	if ((card[nr] >= 0) && (card[nr] < em28xx_bcount))
+		dev->model = card[nr];
+
+	/* allocate device struct */
+	mutex_init(&dev->lock);
+	mutex_lock(&dev->lock);
+	retval = em28xx_init_dev(&dev, udev, nr);
+	if (retval) {
+		em28xx_devused &= ~(1<<dev->devno);
+		kfree(dev);
+
+		return retval;
+	}
+
+	/* save our data pointer in this interface device */
+	usb_set_intfdata(interface, dev);
+
+	request_modules(dev);
+
+	/* Should be the last thing to do, to avoid newer udev's to
+	   open the device before fully initializing it
+	 */
+	mutex_unlock(&dev->lock);
+
+	return 0;
+}
+
+/*
+ * em28xx_usb_disconnect()
+ * called when the device gets diconencted
+ * video device will be unregistered on v4l2_close in case it is still open
+ */
+static void em28xx_usb_disconnect(struct usb_interface *interface)
+{
+	struct em28xx *dev;
+
+	dev = usb_get_intfdata(interface);
+	usb_set_intfdata(interface, NULL);
+
+	if (!dev)
+		return;
+
+	em28xx_info("disconnecting %s\n", dev->vdev->name);
+
+	/* wait until all current v4l2 io is finished then deallocate
+	   resources */
+	mutex_lock(&dev->lock);
+
+	wake_up_interruptible_all(&dev->open);
+
+	if (dev->users) {
+		em28xx_warn
+		    ("device /dev/video%d is open! Deregistration and memory "
+		     "deallocation are deferred on close.\n",
+				dev->vdev->num);
+
+		dev->state |= DEV_MISCONFIGURED;
+		em28xx_uninit_isoc(dev);
+		dev->state |= DEV_DISCONNECTED;
+		wake_up_interruptible(&dev->wait_frame);
+		wake_up_interruptible(&dev->wait_stream);
+	} else {
+		dev->state |= DEV_DISCONNECTED;
+		em28xx_release_resources(dev);
+	}
+
+	em28xx_close_extension(dev);
+
+	mutex_unlock(&dev->lock);
+
+	if (!dev->users) {
+		kfree(dev->alt_max_pkt_size);
+		kfree(dev);
+	}
+}
+
+static struct usb_driver em28xx_usb_driver = {
+	.name = "em28xx",
+	.probe = em28xx_usb_probe,
+	.disconnect = em28xx_usb_disconnect,
+	.id_table = em28xx_id_table,
+};
+
+static int __init em28xx_module_init(void)
+{
+	int result;
+
+	/* register this driver with the USB subsystem */
+	result = usb_register(&em28xx_usb_driver);
+	if (result)
+		em28xx_err(DRIVER_NAME
+			   " usb_register failed. Error number %d.\n", result);
+
+	printk(KERN_INFO DRIVER_NAME " driver loaded\n");
+
+	return result;
+}
+
+static void __exit em28xx_module_exit(void)
+{
+	/* deregister this driver with the USB subsystem */
+	usb_deregister(&em28xx_usb_driver);
+}
+
+module_init(em28xx_module_init);
+module_exit(em28xx_module_exit);
diff --git a/linux/drivers/media/video/em28xx/em28xx-core.c b/linux/drivers/media/video/em28xx/em28xx-core.c
index 025a54bb7..1183519eb 100644
--- a/linux/drivers/media/video/em28xx/em28xx-core.c
+++ b/linux/drivers/media/video/em28xx/em28xx-core.c
@@ -26,6 +26,7 @@
 #include <linux/module.h>
 #include <linux/usb.h>
 #include <linux/vmalloc.h>
+#include <media/v4l2-common.h>
 
 #include "em28xx.h"
 
@@ -983,3 +984,145 @@ int em28xx_init_isoc(struct em28xx *dev, int max_packets,
 	return 0;
 }
 EXPORT_SYMBOL_GPL(em28xx_init_isoc);
+
+/*
+ * em28xx_wake_i2c()
+ * configure i2c attached devices
+ */
+void em28xx_wake_i2c(struct em28xx *dev)
+{
+	struct v4l2_routing route;
+	int zero = 0;
+
+	route.input = INPUT(dev->ctl_input)->vmux;
+	route.output = 0;
+	em28xx_i2c_call_clients(dev, VIDIOC_INT_RESET, &zero);
+	em28xx_i2c_call_clients(dev, VIDIOC_INT_S_VIDEO_ROUTING, &route);
+	em28xx_i2c_call_clients(dev, VIDIOC_STREAMON, NULL);
+}
+
+/*
+ * Device control list
+ */
+
+static LIST_HEAD(em28xx_devlist);
+static DEFINE_MUTEX(em28xx_devlist_mutex);
+
+struct em28xx *em28xx_get_device(struct inode *inode,
+				 enum v4l2_buf_type *fh_type,
+				 int *has_radio)
+{
+	struct em28xx *h, *dev = NULL;
+	int minor = iminor(inode);
+
+	*fh_type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	*has_radio = 0;
+
+	mutex_lock(&em28xx_devlist_mutex);
+	list_for_each_entry(h, &em28xx_devlist, devlist) {
+		if (h->vdev->minor == minor)
+			dev = h;
+		if (h->vbi_dev->minor == minor) {
+			dev = h;
+			*fh_type = V4L2_BUF_TYPE_VBI_CAPTURE;
+		}
+		if (h->radio_dev &&
+		    h->radio_dev->minor == minor) {
+			dev = h;
+			*has_radio = 1;
+		}
+	}
+	mutex_unlock(&em28xx_devlist_mutex);
+
+	return dev;
+}
+
+/*
+ * em28xx_realease_resources()
+ * unregisters the v4l2,i2c and usb devices
+ * called when the device gets disconected or at module unload
+*/
+void em28xx_remove_from_devlist(struct em28xx *dev)
+{
+	mutex_lock(&em28xx_devlist_mutex);
+	list_del(&dev->devlist);
+	mutex_unlock(&em28xx_devlist_mutex);
+};
+
+void em28xx_add_into_devlist(struct em28xx *dev)
+{
+	mutex_lock(&em28xx_devlist_mutex);
+	list_add_tail(&dev->devlist, &em28xx_devlist);
+	mutex_unlock(&em28xx_devlist_mutex);
+};
+
+/*
+ * Extension interface
+ */
+
+static LIST_HEAD(em28xx_extension_devlist);
+static DEFINE_MUTEX(em28xx_extension_devlist_lock);
+
+int em28xx_register_extension(struct em28xx_ops *ops)
+{
+	struct em28xx *dev = NULL;
+
+	mutex_lock(&em28xx_devlist_mutex);
+	mutex_lock(&em28xx_extension_devlist_lock);
+	list_add_tail(&ops->next, &em28xx_extension_devlist);
+	list_for_each_entry(dev, &em28xx_devlist, devlist) {
+		if (dev)
+			ops->init(dev);
+	}
+	printk(KERN_INFO "Em28xx: Initialized (%s) extension\n", ops->name);
+	mutex_unlock(&em28xx_extension_devlist_lock);
+	mutex_unlock(&em28xx_devlist_mutex);
+	return 0;
+}
+EXPORT_SYMBOL(em28xx_register_extension);
+
+void em28xx_unregister_extension(struct em28xx_ops *ops)
+{
+	struct em28xx *dev = NULL;
+
+	mutex_lock(&em28xx_devlist_mutex);
+	list_for_each_entry(dev, &em28xx_devlist, devlist) {
+		if (dev)
+			ops->fini(dev);
+	}
+
+	mutex_lock(&em28xx_extension_devlist_lock);
+	printk(KERN_INFO "Em28xx: Removed (%s) extension\n", ops->name);
+	list_del(&ops->next);
+	mutex_unlock(&em28xx_extension_devlist_lock);
+	mutex_unlock(&em28xx_devlist_mutex);
+}
+EXPORT_SYMBOL(em28xx_unregister_extension);
+
+void em28xx_init_extension(struct em28xx *dev)
+{
+	struct em28xx_ops *ops = NULL;
+
+	mutex_lock(&em28xx_extension_devlist_lock);
+	if (!list_empty(&em28xx_extension_devlist)) {
+		list_for_each_entry(ops, &em28xx_extension_devlist, next) {
+			if (ops->init)
+				ops->init(dev);
+		}
+	}
+	mutex_unlock(&em28xx_extension_devlist_lock);
+}
+
+void em28xx_close_extension(struct em28xx *dev)
+{
+	struct em28xx_ops *ops = NULL;
+
+	mutex_lock(&em28xx_extension_devlist_lock);
+	if (!list_empty(&em28xx_extension_devlist)) {
+		list_for_each_entry(ops, &em28xx_extension_devlist, next) {
+			if (ops->fini)
+				ops->fini(dev);
+		}
+	}
+	mutex_unlock(&em28xx_extension_devlist_lock);
+}
diff --git a/linux/drivers/media/video/em28xx/em28xx-video.c b/linux/drivers/media/video/em28xx/em28xx-video.c
index 9a7f57c8b..8be7aadac 100644
--- a/linux/drivers/media/video/em28xx/em28xx-video.c
+++ b/linux/drivers/media/video/em28xx/em28xx-video.c
@@ -48,9 +48,8 @@
 		      "Mauro Carvalho Chehab <mchehab@infradead.org>, " \
 		      "Sascha Sommer <saschasommer@freenet.de>"
 
-#define DRIVER_NAME         "em28xx"
 #define DRIVER_DESC         "Empia em28xx based USB video device driver"
-#define EM28XX_VERSION_CODE  KERNEL_VERSION(0, 1, 0)
+#define EM28XX_VERSION_CODE  KERNEL_VERSION(0, 1, 1)
 
 #define em28xx_videodbg(fmt, arg...) do {\
 	if (video_debug) \
@@ -73,19 +72,13 @@ MODULE_AUTHOR(DRIVER_AUTHOR);
 MODULE_DESCRIPTION(DRIVER_DESC);
 MODULE_LICENSE("GPL");
 
-static LIST_HEAD(em28xx_devlist);
-static DEFINE_MUTEX(em28xx_devlist_mutex);
-
-static unsigned int card[]     = {[0 ... (EM28XX_MAXBOARDS - 1)] = UNSET };
 static unsigned int video_nr[] = {[0 ... (EM28XX_MAXBOARDS - 1)] = UNSET };
 static unsigned int vbi_nr[]   = {[0 ... (EM28XX_MAXBOARDS - 1)] = UNSET };
 static unsigned int radio_nr[] = {[0 ... (EM28XX_MAXBOARDS - 1)] = UNSET };
 
-module_param_array(card,  int, NULL, 0444);
 module_param_array(video_nr, int, NULL, 0444);
 module_param_array(vbi_nr, int, NULL, 0444);
 module_param_array(radio_nr, int, NULL, 0444);
-MODULE_PARM_DESC(card,     "card type");
 MODULE_PARM_DESC(video_nr, "video device numbers");
 MODULE_PARM_DESC(vbi_nr,   "vbi device numbers");
 MODULE_PARM_DESC(radio_nr, "radio device numbers");
@@ -94,9 +87,6 @@ static unsigned int video_debug;
 module_param(video_debug, int, 0644);
 MODULE_PARM_DESC(video_debug, "enable debug messages [video]");
 
-/* Bitmask marking allocated devices from 0 to EM28XX_MAXBOARDS */
-static unsigned long em28xx_devused;
-
 /* supported video standards */
 static struct em28xx_fmt format[] = {
 	{
@@ -131,8 +121,6 @@ static struct v4l2_queryctrl em28xx_qctrl[] = {
 	}
 };
 
-static struct usb_driver em28xx_usb_driver;
-
 /* ------------------------------------------------------------------
 	DMA and thread functions
    ------------------------------------------------------------------*/
@@ -523,58 +511,6 @@ static struct videobuf_queue_ops em28xx_video_qops = {
 
 /*********************  v4l2 interface  **************************************/
 
-/*
- * em28xx_config()
- * inits registers with sane defaults
- */
-static int em28xx_config(struct em28xx *dev)
-{
-	int retval;
-
-	/* Sets I2C speed to 100 KHz */
-	if (!dev->board.is_em2800) {
-		retval = em28xx_write_reg(dev, EM28XX_R06_I2C_CLK, 0x40);
-		if (retval < 0) {
-			em28xx_errdev("%s: em28xx_write_regs_req failed! retval [%d]\n",
-				__func__, retval);
-			return retval;
-		}
-	}
-
-#if 1
-	/* enable vbi capturing */
-
-/*	em28xx_write_reg(dev, EM28XX_R0E_AUDIOSRC, 0xc0); audio register */
-/*	em28xx_write_reg(dev, EM28XX_R0F_XCLK, 0x80); clk register */
-	em28xx_write_reg(dev, EM28XX_R11_VINCTRL, 0x51);
-#endif
-
-	dev->mute = 1;		/* maybe not the right place... */
-	dev->volume = 0x1f;
-
-	em28xx_set_outfmt(dev);
-	em28xx_colorlevels_set_default(dev);
-	em28xx_compression_disable(dev);
-
-	return 0;
-}
-
-/*
- * em28xx_config_i2c()
- * configure i2c attached devices
- */
-static void em28xx_config_i2c(struct em28xx *dev)
-{
-	struct v4l2_routing route;
-	int zero = 0;
-
-	route.input = INPUT(dev->ctl_input)->vmux;
-	route.output = 0;
-	em28xx_i2c_call_clients(dev, VIDIOC_INT_RESET, &zero);
-	em28xx_i2c_call_clients(dev, VIDIOC_INT_S_VIDEO_ROUTING, &route);
-	em28xx_i2c_call_clients(dev, VIDIOC_STREAMON, NULL);
-}
-
 static void video_mux(struct em28xx *dev, int index)
 {
 	struct v4l2_routing route;
@@ -1696,28 +1632,12 @@ static int radio_queryctrl(struct file *file, void *priv,
 static int em28xx_v4l2_open(struct inode *inode, struct file *filp)
 {
 	int minor = iminor(inode);
-	int errCode = 0, radio = 0;
-	struct em28xx *h, *dev = NULL;
+	int errCode = 0, radio;
+	struct em28xx *dev;
+	enum v4l2_buf_type fh_type;
 	struct em28xx_fh *fh;
-	enum v4l2_buf_type fh_type = 0;
 
-	mutex_lock(&em28xx_devlist_mutex);
-	list_for_each_entry(h, &em28xx_devlist, devlist) {
-		if (h->vdev->minor == minor) {
-			dev  = h;
-			fh_type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-		}
-		if (h->vbi_dev->minor == minor) {
-			dev  = h;
-			fh_type = V4L2_BUF_TYPE_VBI_CAPTURE;
-		}
-		if (h->radio_dev &&
-		    h->radio_dev->minor == minor) {
-			radio = 1;
-			dev   = h;
-		}
-	}
-	mutex_unlock(&em28xx_devlist_mutex);
+	dev = em28xx_get_device(inode, &fh_type, &radio);
 
 	if (NULL == dev)
 		return -ENODEV;
@@ -1760,7 +1680,7 @@ static int em28xx_v4l2_open(struct inode *inode, struct file *filp)
 		/* Needed, since GPIO might have disabled power of
 		   some i2c device
 		 */
-		em28xx_config_i2c(dev);
+		em28xx_wake_i2c(dev);
 
 #if 0
 		/* device needs to be initialized before isoc transfer */
@@ -1791,18 +1711,11 @@ static int em28xx_v4l2_open(struct inode *inode, struct file *filp)
  * unregisters the v4l2,i2c and usb devices
  * called when the device gets disconected or at module unload
 */
-static void em28xx_release_resources(struct em28xx *dev)
+void em28xx_release_analog_resources(struct em28xx *dev)
 {
 
 	/*FIXME: I2C IR should be disconnected */
 
-	list_del(&dev->devlist);
-	if (dev->sbutton_input_dev)
-		em28xx_deregister_snapshot_button(dev);
-
-	if (dev->ir)
-		em28xx_ir_fini(dev);
-
 	if (dev->radio_dev) {
 		if (-1 != dev->radio_dev->minor)
 			video_unregister_device(dev->radio_dev);
@@ -1828,11 +1741,6 @@ static void em28xx_release_resources(struct em28xx *dev)
 			video_device_release(dev->vdev);
 		dev->vdev = NULL;
 	}
-	em28xx_i2c_unregister(dev);
-	usb_put_dev(dev->udev);
-
-	/* Mark device as unused */
-	em28xx_devused &= ~(1<<dev->devno);
 }
 
 /*
@@ -2087,44 +1995,6 @@ static struct video_device em28xx_radio_template = {
 /******************************** usb interface ******************************/
 
 
-static LIST_HEAD(em28xx_extension_devlist);
-static DEFINE_MUTEX(em28xx_extension_devlist_lock);
-
-int em28xx_register_extension(struct em28xx_ops *ops)
-{
-	struct em28xx *dev = NULL;
-
-	mutex_lock(&em28xx_devlist_mutex);
-	mutex_lock(&em28xx_extension_devlist_lock);
-	list_add_tail(&ops->next, &em28xx_extension_devlist);
-	list_for_each_entry(dev, &em28xx_devlist, devlist) {
-		if (dev)
-			ops->init(dev);
-	}
-	printk(KERN_INFO "Em28xx: Initialized (%s) extension\n", ops->name);
-	mutex_unlock(&em28xx_extension_devlist_lock);
-	mutex_unlock(&em28xx_devlist_mutex);
-	return 0;
-}
-EXPORT_SYMBOL(em28xx_register_extension);
-
-void em28xx_unregister_extension(struct em28xx_ops *ops)
-{
-	struct em28xx *dev = NULL;
-
-	mutex_lock(&em28xx_devlist_mutex);
-	list_for_each_entry(dev, &em28xx_devlist, devlist) {
-		if (dev)
-			ops->fini(dev);
-	}
-
-	mutex_lock(&em28xx_extension_devlist_lock);
-	printk(KERN_INFO "Em28xx: Removed (%s) extension\n", ops->name);
-	list_del(&ops->next);
-	mutex_unlock(&em28xx_extension_devlist_lock);
-	mutex_unlock(&em28xx_devlist_mutex);
-}
-EXPORT_SYMBOL(em28xx_unregister_extension);
 
 static struct video_device *em28xx_vdev_init(struct em28xx *dev,
 					     const struct video_device *template,
@@ -2147,10 +2017,45 @@ static struct video_device *em28xx_vdev_init(struct em28xx *dev,
 	return vfd;
 }
 
-static int register_analog_devices(struct em28xx *dev)
+int em28xx_register_analog_devices(struct em28xx *dev)
 {
 	int ret;
 
+	printk(KERN_INFO "%s: v4l2 driver version %d.%d.%d\n",
+		dev->name,
+		(EM28XX_VERSION_CODE >> 16) & 0xff,
+		(EM28XX_VERSION_CODE >> 8) & 0xff, EM28XX_VERSION_CODE & 0xff);
+
+	/* Analog specific initialization */
+	dev->format = &format[0];
+	video_mux(dev, 0);
+
+#if 1
+	/* enable vbi capturing */
+
+/*	em28xx_write_reg(dev, EM28XX_R0E_AUDIOSRC, 0xc0); audio register */
+/*	em28xx_write_reg(dev, EM28XX_R0F_XCLK, 0x80); clk register */
+	em28xx_write_reg(dev, EM28XX_R11_VINCTRL, 0x51);
+#endif
+
+	dev->mute = 1;		/* maybe not the right place... */
+	dev->volume = 0x1f;
+
+	em28xx_set_outfmt(dev);
+	em28xx_colorlevels_set_default(dev);
+	em28xx_compression_disable(dev);
+
+	/* set default norm */
+	dev->norm = em28xx_video_template.current_norm;
+	dev->width = norm_maxw(dev);
+	dev->height = norm_maxh(dev);
+	dev->interlaced = EM28XX_INTERLACED_DEFAULT;
+	dev->hscale = 0;
+	dev->vscale = 0;
+
+	/* FIXME: This is a very bad hack! Not all devices have TV on input 2 */
+	dev->ctl_input = 2;
+
 	/* allocate and fill video video_device struct */
 	dev->vdev = em28xx_vdev_init(dev, &em28xx_video_template, "video");
 	if (!dev->vdev) {
@@ -2199,454 +2104,3 @@ static int register_analog_devices(struct em28xx *dev)
 
 	return 0;
 }
-
-
-/*
- * em28xx_init_dev()
- * allocates and inits the device structs, registers i2c bus and v4l device
- */
-static int em28xx_init_dev(struct em28xx **devhandle, struct usb_device *udev,
-			   int minor)
-{
-	struct em28xx_ops *ops = NULL;
-	struct em28xx *dev = *devhandle;
-	int retval = -ENOMEM;
-	int errCode;
-	unsigned int maxh, maxw;
-
-	dev->udev = udev;
-	mutex_init(&dev->ctrl_urb_lock);
-	spin_lock_init(&dev->slock);
-	init_waitqueue_head(&dev->open);
-	init_waitqueue_head(&dev->wait_frame);
-	init_waitqueue_head(&dev->wait_stream);
-
-	dev->em28xx_write_regs = em28xx_write_regs;
-	dev->em28xx_read_reg = em28xx_read_reg;
-	dev->em28xx_read_reg_req_len = em28xx_read_reg_req_len;
-	dev->em28xx_write_regs_req = em28xx_write_regs_req;
-	dev->em28xx_read_reg_req = em28xx_read_reg_req;
-	dev->board.is_em2800 = em28xx_boards[dev->model].is_em2800;
-	dev->format = &format[0];
-
-	em28xx_pre_card_setup(dev);
-
-	errCode = em28xx_config(dev);
-	if (errCode) {
-		em28xx_errdev("error configuring device\n");
-		return -ENOMEM;
-	}
-
-	/* register i2c bus */
-	errCode = em28xx_i2c_register(dev);
-	if (errCode < 0) {
-		em28xx_errdev("%s: em28xx_i2c_register - errCode [%d]!\n",
-			__func__, errCode);
-		return errCode;
-	}
-
-	/* Do board specific init and eeprom reading */
-	em28xx_card_setup(dev);
-
-	/* Configure audio */
-	errCode = em28xx_audio_setup(dev);
-	if (errCode < 0) {
-		em28xx_errdev("%s: Error while setting audio - errCode [%d]!\n",
-			__func__, errCode);
-	}
-
-	/* configure the device */
-	em28xx_config_i2c(dev);
-
-	/* set default norm */
-	dev->norm = em28xx_video_template.current_norm;
-
-	maxw = norm_maxw(dev);
-	maxh = norm_maxh(dev);
-
-	/* set default image size */
-	dev->width = maxw;
-	dev->height = maxh;
-	dev->interlaced = EM28XX_INTERLACED_DEFAULT;
-	dev->hscale = 0;
-	dev->vscale = 0;
-	dev->ctl_input = 2;
-
-	errCode = em28xx_config(dev);
-	if (errCode < 0) {
-		em28xx_errdev("%s: em28xx_config - errCode [%d]!\n",
-			__func__, errCode);
-		return errCode;
-	}
-
-	/* init video dma queues */
-	INIT_LIST_HEAD(&dev->vidq.active);
-	INIT_LIST_HEAD(&dev->vidq.queued);
-
-#if 0
-	video_set_drvdata(dev->vbi_dev, dev);
-#endif
-
-	if (dev->board.has_msp34xx) {
-		/* Send a reset to other chips via gpio */
-		errCode = em28xx_write_reg(dev, EM28XX_R08_GPIO, 0xf7);
-		if (errCode < 0) {
-			em28xx_errdev("%s: em28xx_write_regs_req - msp34xx(1) failed! errCode [%d]\n",
-				__func__, errCode);
-			return errCode;
-		}
-		msleep(3);
-
-		errCode = em28xx_write_reg(dev, EM28XX_R08_GPIO, 0xff);
-		if (errCode < 0) {
-			em28xx_errdev("%s: em28xx_write_regs_req - msp34xx(2) failed! errCode [%d]\n",
-				__func__, errCode);
-			return errCode;
-		}
-		msleep(3);
-	}
-
-	video_mux(dev, 0);
-
-	mutex_lock(&em28xx_devlist_mutex);
-	list_add_tail(&dev->devlist, &em28xx_devlist);
-	retval = register_analog_devices(dev);
-	if (retval < 0) {
-		em28xx_release_resources(dev);
-		mutex_unlock(&em28xx_devlist_mutex);
-		goto fail_reg_devices;
-	}
-
-	mutex_lock(&em28xx_extension_devlist_lock);
-	if (!list_empty(&em28xx_extension_devlist)) {
-		list_for_each_entry(ops, &em28xx_extension_devlist, next) {
-			if (ops->id)
-				ops->init(dev);
-		}
-	}
-	mutex_unlock(&em28xx_extension_devlist_lock);
-	mutex_unlock(&em28xx_devlist_mutex);
-
-	/* Save some power by putting tuner to sleep */
-	em28xx_i2c_call_clients(dev, TUNER_SET_STANDBY, NULL);
-
-	return 0;
-
-fail_reg_devices:
-	return retval;
-}
-
-#if defined(CONFIG_MODULES) && defined(MODULE)
-#if LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 20)
-static void request_module_async(void *ptr)
-{
-	struct em28xx *dev = (struct em28xx *)ptr;
-#else
-static void request_module_async(struct work_struct *work)
-{
-	struct em28xx *dev = container_of(work,
-			     struct em28xx, request_module_wk);
-#endif
-
-	if (dev->has_audio_class)
-		request_module("snd-usb-audio");
-	else if (dev->has_alsa_audio)
-		request_module("em28xx-alsa");
-
-	if (dev->board.has_dvb)
-		request_module("em28xx-dvb");
-}
-
-static void request_modules(struct em28xx *dev)
-{
-#if LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 20)
-	INIT_WORK(&dev->request_module_wk, request_module_async,
-		  (void *)dev);
-#else
-	INIT_WORK(&dev->request_module_wk, request_module_async);
-#endif
-	schedule_work(&dev->request_module_wk);
-}
-#else
-#define request_modules(dev)
-#endif /* CONFIG_MODULES */
-
-/*
- * em28xx_usb_probe()
- * checks for supported devices
- */
-static int em28xx_usb_probe(struct usb_interface *interface,
-			    const struct usb_device_id *id)
-{
-	const struct usb_endpoint_descriptor *endpoint;
-	struct usb_device *udev;
-	struct usb_interface *uif;
-	struct em28xx *dev = NULL;
-	int retval = -ENODEV;
-	int i, nr, ifnum, isoc_pipe;
-	char *speed;
-	char descr[255] = "";
-
-	udev = usb_get_dev(interface_to_usbdev(interface));
-	ifnum = interface->altsetting[0].desc.bInterfaceNumber;
-
-	/* Check to see next free device and mark as used */
-	nr = find_first_zero_bit(&em28xx_devused, EM28XX_MAXBOARDS);
-	em28xx_devused |= 1<<nr;
-
-	/* Don't register audio interfaces */
-	if (interface->altsetting[0].desc.bInterfaceClass == USB_CLASS_AUDIO) {
-		em28xx_err(DRIVER_NAME " audio device (%04x:%04x): "
-			"interface %i, class %i\n",
-			le16_to_cpu(udev->descriptor.idVendor),
-			le16_to_cpu(udev->descriptor.idProduct),
-			ifnum,
-			interface->altsetting[0].desc.bInterfaceClass);
-
-		em28xx_devused &= ~(1<<nr);
-		return -ENODEV;
-	}
-
-	endpoint = &interface->cur_altsetting->endpoint[0].desc;
-
-	/* check if the device has the iso in endpoint at the correct place */
-	if ((endpoint->bmAttributes & USB_ENDPOINT_XFERTYPE_MASK) ==
-	    USB_ENDPOINT_XFER_ISOC &&
-	    (interface->altsetting[1].endpoint[0].desc.wMaxPacketSize == 940))
-	{
-		/* It's a newer em2874/em2875 device */
-		isoc_pipe = 0;
-	} else {
-		int check_interface = 1;
-		isoc_pipe = 1;
-		endpoint = &interface->cur_altsetting->endpoint[1].desc;
-		if ((endpoint->bmAttributes & USB_ENDPOINT_XFERTYPE_MASK) !=
-		    USB_ENDPOINT_XFER_ISOC)
-			check_interface = 0;
-
-		if ((endpoint->bEndpointAddress & USB_ENDPOINT_DIR_MASK) == USB_DIR_OUT)
-			check_interface = 0;
-
-		if (!check_interface) {
-			em28xx_err(DRIVER_NAME " video device (%04x:%04x): "
-				"interface %i, class %i found.\n",
-				le16_to_cpu(udev->descriptor.idVendor),
-				le16_to_cpu(udev->descriptor.idProduct),
-				ifnum,
-				interface->altsetting[0].desc.bInterfaceClass);
-
-			em28xx_err(DRIVER_NAME " This is an anciliary "
-				"interface not used by the driver\n");
-
-			em28xx_devused &= ~(1<<nr);
-			return -ENODEV;
-		}
-	}
-
-	switch (udev->speed) {
-	case USB_SPEED_LOW:
-		speed = "1.5";
-		break;
-	case USB_SPEED_UNKNOWN:
-	case USB_SPEED_FULL:
-		speed = "12";
-		break;
-	case USB_SPEED_HIGH:
-		speed = "480";
-		break;
-	default:
-		speed = "unknown";
-	}
-
-	if (udev->manufacturer)
-		strlcpy(descr, udev->manufacturer, sizeof(descr));
-
-	if (udev->product) {
-		if (*descr)
-			strlcat(descr, " ", sizeof(descr));
-		strlcat(descr, udev->product, sizeof(descr));
-	}
-	if (*descr)
-		strlcat(descr, " ", sizeof(descr));
-
-	printk(DRIVER_NAME ": New device %s@ %s Mbps "
-		"(%04x:%04x, interface %d, class %d)\n",
-		descr,
-		speed,
-		le16_to_cpu(udev->descriptor.idVendor),
-		le16_to_cpu(udev->descriptor.idProduct),
-		ifnum,
-		interface->altsetting->desc.bInterfaceNumber);
-
-	if (nr >= EM28XX_MAXBOARDS) {
-		printk(DRIVER_NAME ": Supports only %i em28xx boards.\n",
-				EM28XX_MAXBOARDS);
-		em28xx_devused &= ~(1<<nr);
-		return -ENOMEM;
-	}
-
-	/* allocate memory for our device state and initialize it */
-	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
-	if (dev == NULL) {
-		em28xx_err(DRIVER_NAME ": out of memory!\n");
-		em28xx_devused &= ~(1<<nr);
-		return -ENOMEM;
-	}
-
-	snprintf(dev->name, 29, "em28xx #%d", nr);
-	dev->devno = nr;
-	dev->model = id->driver_info;
-	dev->alt   = -1;
-
-	/* Checks if audio is provided by some interface */
-	for (i = 0; i < udev->config->desc.bNumInterfaces; i++) {
-		uif = udev->config->interface[i];
-		if (uif->altsetting[0].desc.bInterfaceClass == USB_CLASS_AUDIO) {
-			dev->has_audio_class = 1;
-			break;
-		}
-	}
-
-	/* compute alternate max packet sizes */
-	uif = udev->actconfig->interface[0];
-
-	dev->num_alt = uif->num_altsetting;
-	em28xx_videodbg("Alternate settings: %i\n", dev->num_alt);
-/*	dev->alt_max_pkt_size = kmalloc(sizeof(*dev->alt_max_pkt_size)* */
-	dev->alt_max_pkt_size = kmalloc(32 * dev->num_alt, GFP_KERNEL);
-
-	if (dev->alt_max_pkt_size == NULL) {
-		em28xx_errdev("out of memory!\n");
-		em28xx_devused &= ~(1<<nr);
-		kfree(dev);
-		return -ENOMEM;
-	}
-
-	for (i = 0; i < dev->num_alt ; i++) {
-		u16 tmp = le16_to_cpu(uif->altsetting[i].endpoint[isoc_pipe].desc.
-							wMaxPacketSize);
-		dev->alt_max_pkt_size[i] =
-		    (tmp & 0x07ff) * (((tmp & 0x1800) >> 11) + 1);
-		em28xx_videodbg("Alternate setting %i, max size= %i\n", i,
-						dev->alt_max_pkt_size[i]);
-	}
-
-	if ((card[nr] >= 0) && (card[nr] < em28xx_bcount))
-		dev->model = card[nr];
-
-	/* allocate device struct */
-	mutex_init(&dev->lock);
-	mutex_lock(&dev->lock);
-	retval = em28xx_init_dev(&dev, udev, nr);
-	if (retval) {
-		em28xx_devused &= ~(1<<dev->devno);
-		kfree(dev);
-
-		return retval;
-	}
-
-	/* save our data pointer in this interface device */
-	usb_set_intfdata(interface, dev);
-
-	request_modules(dev);
-
-	/* Should be the last thing to do, to avoid newer udev's to
-	   open the device before fully initializing it
-	 */
-	mutex_unlock(&dev->lock);
-
-	return 0;
-}
-
-/*
- * em28xx_usb_disconnect()
- * called when the device gets diconencted
- * video device will be unregistered on v4l2_close in case it is still open
- */
-static void em28xx_usb_disconnect(struct usb_interface *interface)
-{
-	struct em28xx *dev;
-	struct em28xx_ops *ops = NULL;
-
-	dev = usb_get_intfdata(interface);
-	usb_set_intfdata(interface, NULL);
-
-	if (!dev)
-		return;
-
-	em28xx_info("disconnecting %s\n", dev->vdev->name);
-
-	/* wait until all current v4l2 io is finished then deallocate
-	   resources */
-	mutex_lock(&dev->lock);
-
-	wake_up_interruptible_all(&dev->open);
-
-	if (dev->users) {
-		em28xx_warn
-		    ("device /dev/video%d is open! Deregistration and memory "
-		     "deallocation are deferred on close.\n",
-				dev->vdev->num);
-
-		dev->state |= DEV_MISCONFIGURED;
-		em28xx_uninit_isoc(dev);
-		dev->state |= DEV_DISCONNECTED;
-		wake_up_interruptible(&dev->wait_frame);
-		wake_up_interruptible(&dev->wait_stream);
-	} else {
-		dev->state |= DEV_DISCONNECTED;
-		em28xx_release_resources(dev);
-	}
-	mutex_unlock(&dev->lock);
-
-	mutex_lock(&em28xx_extension_devlist_lock);
-	if (!list_empty(&em28xx_extension_devlist)) {
-		list_for_each_entry(ops, &em28xx_extension_devlist, next) {
-			ops->fini(dev);
-		}
-	}
-	mutex_unlock(&em28xx_extension_devlist_lock);
-
-	if (!dev->users) {
-		kfree(dev->alt_max_pkt_size);
-		kfree(dev);
-	}
-}
-
-static struct usb_driver em28xx_usb_driver = {
-	.name = "em28xx",
-	.probe = em28xx_usb_probe,
-	.disconnect = em28xx_usb_disconnect,
-	.id_table = em28xx_id_table,
-};
-
-static int __init em28xx_module_init(void)
-{
-	int result;
-
-	printk(KERN_INFO DRIVER_NAME " v4l2 driver version %d.%d.%d loaded\n",
-	       (EM28XX_VERSION_CODE >> 16) & 0xff,
-	       (EM28XX_VERSION_CODE >> 8) & 0xff, EM28XX_VERSION_CODE & 0xff);
-#ifdef SNAPSHOT
-	printk(KERN_INFO DRIVER_NAME " snapshot date %04d-%02d-%02d\n",
-	       SNAPSHOT / 10000, (SNAPSHOT / 100) % 100, SNAPSHOT % 100);
-#endif
-
-	/* register this driver with the USB subsystem */
-	result = usb_register(&em28xx_usb_driver);
-	if (result)
-		em28xx_err(DRIVER_NAME
-			   " usb_register failed. Error number %d.\n", result);
-
-	return result;
-}
-
-static void __exit em28xx_module_exit(void)
-{
-	/* deregister this driver with the USB subsystem */
-	usb_deregister(&em28xx_usb_driver);
-}
-
-module_init(em28xx_module_init);
-module_exit(em28xx_module_exit);
-
diff --git a/linux/drivers/media/video/em28xx/em28xx.h b/linux/drivers/media/video/em28xx/em28xx.h
index 84f49934a..93c47f260 100644
--- a/linux/drivers/media/video/em28xx/em28xx.h
+++ b/linux/drivers/media/video/em28xx/em28xx.h
@@ -100,6 +100,7 @@
 #define EM2820_BOARD_COMPRO_VIDEOMATE_FORYOU	  58
 #define EM2874_BOARD_PINNACLE_PCTV_80E		  59
 #define EM2883_BOARD_HAUPPAUGE_WINTV_HVR_850	  60
+#define EM2820_BOARD_PROLINK_PLAYTV_BOX4_USB2	  61
 
 /* Limits minimum and default number of buffers */
 #define EM28XX_MIN_BUF 4
@@ -594,10 +595,20 @@ int em28xx_init_isoc(struct em28xx *dev, int max_packets,
 void em28xx_uninit_isoc(struct em28xx *dev);
 int em28xx_set_mode(struct em28xx *dev, enum em28xx_mode set_mode);
 int em28xx_gpio_set(struct em28xx *dev, struct em28xx_reg_seq *gpio);
-
-/* Provided by em28xx-video.c */
+void em28xx_wake_i2c(struct em28xx *dev);
+void em28xx_remove_from_devlist(struct em28xx *dev);
+void em28xx_add_into_devlist(struct em28xx *dev);
+struct em28xx *em28xx_get_device(struct inode *inode,
+				 enum v4l2_buf_type *fh_type,
+				 int *has_radio);
 int em28xx_register_extension(struct em28xx_ops *dev);
 void em28xx_unregister_extension(struct em28xx_ops *dev);
+void em28xx_init_extension(struct em28xx *dev);
+void em28xx_close_extension(struct em28xx *dev);
+
+/* Provided by em28xx-video.c */
+int em28xx_register_analog_devices(struct em28xx *dev);
+void em28xx_release_analog_resources(struct em28xx *dev);
 
 /* Provided by em28xx-cards.c */
 extern int em2800_variant_detect(struct usb_device *udev, int model);
@@ -608,6 +619,7 @@ extern struct usb_device_id em28xx_id_table[];
 extern const unsigned int em28xx_bcount;
 void em28xx_set_ir(struct em28xx *dev, struct IR_i2c *ir);
 int em28xx_tuner_callback(void *ptr, int component, int command, int arg);
+void em28xx_release_resources(struct em28xx *dev);
 
 /* Provided by em28xx-input.c */
 int em28xx_get_key_terratec(struct IR_i2c *ir, u32 *ir_key, u32 *ir_raw);
diff --git a/linux/drivers/media/video/gspca/Kconfig b/linux/drivers/media/video/gspca/Kconfig
index 770fb699d..ee6a691df 100644
--- a/linux/drivers/media/video/gspca/Kconfig
+++ b/linux/drivers/media/video/gspca/Kconfig
@@ -18,6 +18,7 @@ menuconfig USB_GSPCA
 if USB_GSPCA && VIDEO_V4L2
 
 source "drivers/media/video/gspca/m5602/Kconfig"
+source "drivers/media/video/gspca/stv06xx/Kconfig"
 
 config USB_GSPCA_CONEX
 	tristate "Conexant Camera Driver"
diff --git a/linux/drivers/media/video/gspca/Makefile b/linux/drivers/media/video/gspca/Makefile
index 6c8046e23..bd8d9ee40 100644
--- a/linux/drivers/media/video/gspca/Makefile
+++ b/linux/drivers/media/video/gspca/Makefile
@@ -47,4 +47,4 @@ gspca_vc032x-objs		:= vc032x.o
 gspca_zc3xx-objs		:= zc3xx.o
 
 obj-$(CONFIG_USB_M5602)		+= m5602/
-
+obj-$(CONFIG_USB_STV06XX) 	+= stv06xx/
diff --git a/linux/drivers/media/video/gspca/conex.c b/linux/drivers/media/video/gspca/conex.c
index de28354ea..1753f5bb3 100644
--- a/linux/drivers/media/video/gspca/conex.c
+++ b/linux/drivers/media/video/gspca/conex.c
@@ -93,7 +93,7 @@ static struct ctrl sd_ctrls[] = {
 	},
 };
 
-static struct v4l2_pix_format vga_mode[] = {
+static const struct v4l2_pix_format vga_mode[] = {
 	{176, 144, V4L2_PIX_FMT_JPEG, V4L2_FIELD_NONE,
 		.bytesperline = 176,
 		.sizeimage = 176 * 144 * 3 / 8 + 590,
diff --git a/linux/drivers/media/video/gspca/etoms.c b/linux/drivers/media/video/gspca/etoms.c
index 28869ef26..84b56c303 100644
--- a/linux/drivers/media/video/gspca/etoms.c
+++ b/linux/drivers/media/video/gspca/etoms.c
@@ -112,7 +112,7 @@ static struct ctrl sd_ctrls[] = {
 	 },
 };
 
-static struct v4l2_pix_format vga_mode[] = {
+static const struct v4l2_pix_format vga_mode[] = {
 	{320, 240, V4L2_PIX_FMT_SBGGR8, V4L2_FIELD_NONE,
 		.bytesperline = 320,
 		.sizeimage = 320 * 240,
@@ -125,7 +125,7 @@ static struct v4l2_pix_format vga_mode[] = {
 		.priv = 0}, */
 };
 
-static struct v4l2_pix_format sif_mode[] = {
+static const struct v4l2_pix_format sif_mode[] = {
 	{176, 144, V4L2_PIX_FMT_SBGGR8, V4L2_FIELD_NONE,
 		.bytesperline = 176,
 		.sizeimage = 176 * 144,
diff --git a/linux/drivers/media/video/gspca/finepix.c b/linux/drivers/media/video/gspca/finepix.c
index 03cb94466..afc8b2dd3 100644
--- a/linux/drivers/media/video/gspca/finepix.c
+++ b/linux/drivers/media/video/gspca/finepix.c
@@ -72,7 +72,7 @@ struct usb_fpix {
 }
 
 /* These cameras only support 320x200. */
-static struct v4l2_pix_format fpix_mode[1] = {
+static const struct v4l2_pix_format fpix_mode[1] = {
 	{ 320, 240, V4L2_PIX_FMT_JPEG, V4L2_FIELD_NONE,
 		.bytesperline = 320,
 		.sizeimage = 320 * 240 * 3 / 8 + 590,
diff --git a/linux/drivers/media/video/gspca/gspca.c b/linux/drivers/media/video/gspca/gspca.c
index 71e7afe1b..5adbe7301 100644
--- a/linux/drivers/media/video/gspca/gspca.c
+++ b/linux/drivers/media/video/gspca/gspca.c
@@ -785,8 +785,6 @@ static int vidioc_g_fmt_vid_cap(struct file *file, void *priv,
 	struct gspca_dev *gspca_dev = priv;
 	int mode;
 
-	if (fmt->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-		return -EINVAL;
 	mode = gspca_dev->curr_mode;
 	memcpy(&fmt->fmt.pix, &gspca_dev->cam.cam_mode[mode],
 		sizeof fmt->fmt.pix);
@@ -798,8 +796,6 @@ static int try_fmt_vid_cap(struct gspca_dev *gspca_dev,
 {
 	int w, h, mode, mode2;
 
-	if (fmt->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-		return -EINVAL;
 	w = fmt->fmt.pix.width;
 	h = fmt->fmt.pix.height;
 
@@ -1153,8 +1149,6 @@ static int vidioc_reqbufs(struct file *file, void *priv,
 	struct gspca_dev *gspca_dev = priv;
 	int i, ret = 0;
 
-	if (rb->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-		return -EINVAL;
 	switch (rb->memory) {
 	case GSPCA_MEMORY_READ:			/* (internal call) */
 	case V4L2_MEMORY_MMAP:
@@ -1219,8 +1213,7 @@ static int vidioc_querybuf(struct file *file, void *priv,
 	struct gspca_dev *gspca_dev = priv;
 	struct gspca_frame *frame;
 
-	if (v4l2_buf->type != V4L2_BUF_TYPE_VIDEO_CAPTURE
-	    || v4l2_buf->index < 0
+	if (v4l2_buf->index < 0
 	    || v4l2_buf->index >= gspca_dev->nframes)
 		return -EINVAL;
 
@@ -1243,7 +1236,8 @@ static int vidioc_streamon(struct file *file, void *priv,
 		ret = -ENODEV;
 		goto out;
 	}
-	if (gspca_dev->nframes == 0) {
+	if (gspca_dev->nframes == 0
+	    || !(gspca_dev->frame[0].v4l2_buf.flags & V4L2_BUF_FLAG_QUEUED)) {
 		ret = -EINVAL;
 		goto out;
 	}
@@ -1561,8 +1555,6 @@ static int vidioc_dqbuf(struct file *file, void *priv,
 	int i, ret;
 
 	PDEBUG(D_FRAM, "dqbuf");
-	if (v4l2_buf->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-		return -EINVAL;
 	if (v4l2_buf->memory != gspca_dev->memory)
 		return -EINVAL;
 
@@ -1617,8 +1609,6 @@ static int vidioc_qbuf(struct file *file, void *priv,
 	int i, index, ret;
 
 	PDEBUG(D_FRAM, "qbuf %d", v4l2_buf->index);
-	if (v4l2_buf->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-		return -EINVAL;
 
 	if (mutex_lock_interruptible(&gspca_dev->queue_lock))
 		return -ERESTARTSYS;
@@ -1828,7 +1818,7 @@ static struct file_operations dev_fops = {
 	.release = dev_close,
 	.read = dev_read,
 	.mmap = dev_mmap,
-	.ioctl = video_ioctl2,
+	.unlocked_ioctl = __video_ioctl2,
 #ifdef CONFIG_COMPAT
 	.compat_ioctl = v4l_compat_ioctl32,
 #endif
diff --git a/linux/drivers/media/video/gspca/gspca.h b/linux/drivers/media/video/gspca/gspca.h
index 79cef31a5..c90af9cb1 100644
--- a/linux/drivers/media/video/gspca/gspca.h
+++ b/linux/drivers/media/video/gspca/gspca.h
@@ -56,7 +56,7 @@ extern int gspca_debug;
 /* device information - set at probe time */
 struct cam {
 	int bulk_size;		/* buffer size when image transfer by bulk */
-	struct v4l2_pix_format *cam_mode;	/* size nmodes */
+	const struct v4l2_pix_format *cam_mode;	/* size nmodes */
 	char nmodes;
 	__u8 bulk_nurbs;	/* number of URBs in bulk mode
 				 * - cannot be > MAX_NURBS
diff --git a/linux/drivers/media/video/gspca/m5602/m5602_bridge.h b/linux/drivers/media/video/gspca/m5602/m5602_bridge.h
index c1c7ce524..a3f3b7a0c 100644
--- a/linux/drivers/media/video/gspca/m5602/m5602_bridge.h
+++ b/linux/drivers/media/video/gspca/m5602/m5602_bridge.h
@@ -109,7 +109,6 @@ static const unsigned char sensor_urb_skeleton[] = {
 	0x13, M5602_XB_I2C_CTRL, 0x81, 0x11
 };
 
-/* m5602 device descriptor, currently it just wraps the m5602_camera struct */
 struct sd {
 	struct gspca_dev gspca_dev;
 
@@ -134,4 +133,10 @@ int m5602_read_bridge(
 int m5602_write_bridge(
 	struct sd *sd, u8 address, u8 i2c_data);
 
+int m5602_write_sensor(struct sd *sd, const u8 address,
+		       u8 *i2c_data, const u8 len);
+
+int m5602_read_sensor(struct sd *sd, const u8 address,
+		      u8 *i2c_data, const u8 len);
+
 #endif
diff --git a/linux/drivers/media/video/gspca/m5602/m5602_core.c b/linux/drivers/media/video/gspca/m5602/m5602_core.c
index fd6ce384b..ed906fe31 100644
--- a/linux/drivers/media/video/gspca/m5602/m5602_core.c
+++ b/linux/drivers/media/video/gspca/m5602/m5602_core.c
@@ -24,7 +24,7 @@
 
 /* Kernel module parameters */
 int force_sensor;
-int dump_bridge;
+static int dump_bridge;
 int dump_sensor;
 
 static const __devinitdata struct usb_device_id m5602_table[] = {
@@ -80,6 +80,97 @@ int m5602_write_bridge(struct sd *sd, u8 address, u8 i2c_data)
 	return (err < 0) ? err : 0;
 }
 
+int m5602_read_sensor(struct sd *sd, const u8 address,
+		       u8 *i2c_data, const u8 len)
+{
+	int err, i;
+
+	if (!len || len > sd->sensor->i2c_regW)
+		return -EINVAL;
+
+	do {
+		err = m5602_read_bridge(sd, M5602_XB_I2C_STATUS, i2c_data);
+	} while ((*i2c_data & I2C_BUSY) && !err);
+	if (err < 0)
+		goto out;
+
+	err = m5602_write_bridge(sd, M5602_XB_I2C_DEV_ADDR,
+				 sd->sensor->i2c_slave_id);
+	if (err < 0)
+		goto out;
+
+	err = m5602_write_bridge(sd, M5602_XB_I2C_REG_ADDR, address);
+	if (err < 0)
+		goto out;
+
+	if (sd->sensor->i2c_regW == 1) {
+		err = m5602_write_bridge(sd, M5602_XB_I2C_CTRL, len);
+		if (err < 0)
+			goto out;
+
+		err = m5602_write_bridge(sd, M5602_XB_I2C_CTRL, 0x08);
+		if (err < 0)
+			goto out;
+	} else {
+		err = m5602_write_bridge(sd, M5602_XB_I2C_CTRL, 0x18 + len);
+		if (err < 0)
+			goto out;
+	}
+
+	for (i = 0; (i < len) && !err; i++) {
+		err = m5602_read_bridge(sd, M5602_XB_I2C_DATA, &(i2c_data[i]));
+
+		PDEBUG(D_CONF, "Reading sensor register "
+			       "0x%x containing 0x%x ", address, *i2c_data);
+	}
+out:
+	return err;
+}
+
+int m5602_write_sensor(struct sd *sd, const u8 address,
+			u8 *i2c_data, const u8 len)
+{
+	int err, i;
+	u8 *p;
+	struct usb_device *udev = sd->gspca_dev.dev;
+	__u8 *buf = sd->gspca_dev.usb_buf;
+
+	/* No sensor with a data width larger than 16 bits has yet been seen */
+	if (len > sd->sensor->i2c_regW || !len)
+		return -EINVAL;
+
+	memcpy(buf, sensor_urb_skeleton,
+	       sizeof(sensor_urb_skeleton));
+
+	buf[11] = sd->sensor->i2c_slave_id;
+	buf[15] = address;
+
+	/* Special case larger sensor writes */
+	p = buf + 16;
+
+	/* Copy a four byte write sequence for each byte to be written to */
+	for (i = 0; i < len; i++) {
+		memcpy(p, sensor_urb_skeleton + 16, 4);
+		p[3] = i2c_data[i];
+		p += 4;
+		PDEBUG(D_CONF, "Writing sensor register 0x%x with 0x%x",
+		       address, i2c_data[i]);
+	}
+
+	/* Copy the tailer */
+	memcpy(p, sensor_urb_skeleton + 20, 4);
+
+	/* Set the total length */
+	p[3] = 0x10 + len;
+
+	err = usb_control_msg(udev, usb_sndctrlpipe(udev, 0),
+			      0x04, 0x40, 0x19,
+			      0x0000, buf,
+			      20 + len * 4, M5602_URB_MSG_TIMEOUT);
+
+	return (err < 0) ? err : 0;
+}
+
 /* Dump all the registers of the m5602 bridge,
    unfortunately this breaks the camera until it's power cycled */
 static void m5602_dump_bridge(struct sd *sd)
@@ -150,11 +241,15 @@ static int m5602_start_transfer(struct gspca_dev *gspca_dev)
 
 	/* Send start command to the camera */
 	const u8 buffer[4] = {0x13, 0xf9, 0x0f, 0x01};
+
+	if (sd->sensor->start)
+		sd->sensor->start(sd);
+
 	memcpy(buf, buffer, sizeof(buffer));
 	err = usb_control_msg(gspca_dev->dev,
 			      usb_sndctrlpipe(gspca_dev->dev, 0),
 			      0x04, 0x40, 0x19, 0x0000, buf,
-			      4, M5602_URB_MSG_TIMEOUT);
+			      sizeof(buffer), M5602_URB_MSG_TIMEOUT);
 
 	PDEBUG(D_STREAM, "Transfer started");
 	return (err < 0) ? err : 0;
@@ -284,6 +379,7 @@ static int __init mod_m5602_init(void)
 	PDEBUG(D_PROBE, "registered");
 	return 0;
 }
+
 static void __exit mod_m5602_exit(void)
 {
 	usb_deregister(&sd_driver);
diff --git a/linux/drivers/media/video/gspca/m5602/m5602_mt9m111.c b/linux/drivers/media/video/gspca/m5602/m5602_mt9m111.c
index fb700c2d0..c0e71c331 100644
--- a/linux/drivers/media/video/gspca/m5602/m5602_mt9m111.c
+++ b/linux/drivers/media/video/gspca/m5602/m5602_mt9m111.c
@@ -18,6 +18,8 @@
 
 #include "m5602_mt9m111.h"
 
+static void mt9m111_dump_registers(struct sd *sd);
+
 int mt9m111_probe(struct sd *sd)
 {
 	u8 data[2] = {0x00, 0x00};
@@ -44,12 +46,12 @@ int mt9m111_probe(struct sd *sd)
 		} else {
 			data[0] = preinit_mt9m111[i][2];
 			data[1] = preinit_mt9m111[i][3];
-			mt9m111_write_sensor(sd,
+			m5602_write_sensor(sd,
 				preinit_mt9m111[i][1], data, 2);
 		}
 	}
 
-	if (mt9m111_read_sensor(sd, MT9M111_SC_CHIPVER, data, 2))
+	if (m5602_read_sensor(sd, MT9M111_SC_CHIPVER, data, 2))
 		return -ENODEV;
 
 	if ((data[0] == 0x14) && (data[1] == 0x3a)) {
@@ -72,7 +74,7 @@ int mt9m111_init(struct sd *sd)
 	int i, err = 0;
 
 	/* Init the sensor */
-	for (i = 0; i < ARRAY_SIZE(init_mt9m111); i++) {
+	for (i = 0; i < ARRAY_SIZE(init_mt9m111) && !err; i++) {
 		u8 data[2];
 
 		if (init_mt9m111[i][0] == BRIDGE) {
@@ -82,7 +84,7 @@ int mt9m111_init(struct sd *sd)
 		} else {
 			data[0] = init_mt9m111[i][2];
 			data[1] = init_mt9m111[i][3];
-			err = mt9m111_write_sensor(sd,
+			err = m5602_write_sensor(sd,
 				init_mt9m111[i][1], data, 2);
 		}
 	}
@@ -104,12 +106,12 @@ int mt9m111_get_vflip(struct gspca_dev *gspca_dev, __s32 *val)
 	u8 data[2] = {0x00, 0x00};
 	struct sd *sd = (struct sd *) gspca_dev;
 
-	err = mt9m111_read_sensor(sd, MT9M111_SC_R_MODE_CONTEXT_B,
+	err = m5602_read_sensor(sd, MT9M111_SC_R_MODE_CONTEXT_B,
 				  data, 2);
 	*val = data[0] & MT9M111_RMB_MIRROR_ROWS;
 	PDEBUG(D_V4L2, "Read vertical flip %d", *val);
 
-	return (err < 0) ? err : 0;
+	return err;
 }
 
 int mt9m111_set_vflip(struct gspca_dev *gspca_dev, __s32 val)
@@ -121,19 +123,19 @@ int mt9m111_set_vflip(struct gspca_dev *gspca_dev, __s32 val)
 	PDEBUG(D_V4L2, "Set vertical flip to %d", val);
 
 	/* Set the correct page map */
-	err = mt9m111_write_sensor(sd, MT9M111_PAGE_MAP, data, 2);
+	err = m5602_write_sensor(sd, MT9M111_PAGE_MAP, data, 2);
 	if (err < 0)
 		goto out;
 
-	err = mt9m111_read_sensor(sd, MT9M111_SC_R_MODE_CONTEXT_B, data, 2);
+	err = m5602_read_sensor(sd, MT9M111_SC_R_MODE_CONTEXT_B, data, 2);
 	if (err < 0)
 		goto out;
 
 	data[0] = (data[0] & 0xfe) | val;
-	err = mt9m111_write_sensor(sd, MT9M111_SC_R_MODE_CONTEXT_B,
+	err = m5602_write_sensor(sd, MT9M111_SC_R_MODE_CONTEXT_B,
 				   data, 2);
 out:
-	return (err < 0) ? err : 0;
+	return err;
 }
 
 int mt9m111_get_hflip(struct gspca_dev *gspca_dev, __s32 *val)
@@ -142,12 +144,12 @@ int mt9m111_get_hflip(struct gspca_dev *gspca_dev, __s32 *val)
 	u8 data[2] = {0x00, 0x00};
 	struct sd *sd = (struct sd *) gspca_dev;
 
-	err = mt9m111_read_sensor(sd, MT9M111_SC_R_MODE_CONTEXT_B,
+	err = m5602_read_sensor(sd, MT9M111_SC_R_MODE_CONTEXT_B,
 				  data, 2);
 	*val = data[0] & MT9M111_RMB_MIRROR_COLS;
 	PDEBUG(D_V4L2, "Read horizontal flip %d", *val);
 
-	return (err < 0) ? err : 0;
+	return err;
 }
 
 int mt9m111_set_hflip(struct gspca_dev *gspca_dev, __s32 val)
@@ -159,19 +161,19 @@ int mt9m111_set_hflip(struct gspca_dev *gspca_dev, __s32 val)
 	PDEBUG(D_V4L2, "Set horizontal flip to %d", val);
 
 	/* Set the correct page map */
-	err = mt9m111_write_sensor(sd, MT9M111_PAGE_MAP, data, 2);
+	err = m5602_write_sensor(sd, MT9M111_PAGE_MAP, data, 2);
 	if (err < 0)
 		goto out;
 
-	err = mt9m111_read_sensor(sd, MT9M111_SC_R_MODE_CONTEXT_B, data, 2);
+	err = m5602_read_sensor(sd, MT9M111_SC_R_MODE_CONTEXT_B, data, 2);
 	if (err < 0)
 		goto out;
 
 	data[0] = (data[0] & 0xfd) | ((val << 1) & 0x02);
-	err = mt9m111_write_sensor(sd, MT9M111_SC_R_MODE_CONTEXT_B,
+	err = m5602_write_sensor(sd, MT9M111_SC_R_MODE_CONTEXT_B,
 					data, 2);
 out:
-	return (err < 0) ? err : 0;
+	return err;
 }
 
 int mt9m111_get_gain(struct gspca_dev *gspca_dev, __s32 *val)
@@ -180,7 +182,7 @@ int mt9m111_get_gain(struct gspca_dev *gspca_dev, __s32 *val)
 	u8 data[2] = {0x00, 0x00};
 	struct sd *sd = (struct sd *) gspca_dev;
 
-	err = mt9m111_read_sensor(sd, MT9M111_SC_GLOBAL_GAIN, data, 2);
+	err = m5602_read_sensor(sd, MT9M111_SC_GLOBAL_GAIN, data, 2);
 	tmp = ((data[1] << 8) | data[0]);
 
 	*val = ((tmp & (1 << 10)) * 2) |
@@ -190,7 +192,7 @@ int mt9m111_get_gain(struct gspca_dev *gspca_dev, __s32 *val)
 
 	PDEBUG(D_V4L2, "Read gain %d", *val);
 
-	return (err < 0) ? err : 0;
+	return err;
 }
 
 int mt9m111_set_gain(struct gspca_dev *gspca_dev, __s32 val)
@@ -200,7 +202,7 @@ int mt9m111_set_gain(struct gspca_dev *gspca_dev, __s32 val)
 	struct sd *sd = (struct sd *) gspca_dev;
 
 	/* Set the correct page map */
-	err = mt9m111_write_sensor(sd, MT9M111_PAGE_MAP, data, 2);
+	err = m5602_write_sensor(sd, MT9M111_PAGE_MAP, data, 2);
 	if (err < 0)
 		goto out;
 
@@ -225,90 +227,13 @@ int mt9m111_set_gain(struct gspca_dev *gspca_dev, __s32 val)
 	PDEBUG(D_V4L2, "tmp=%d, data[1]=%d, data[0]=%d", tmp,
 	       data[1], data[0]);
 
-	err = mt9m111_write_sensor(sd, MT9M111_SC_GLOBAL_GAIN,
+	err = m5602_write_sensor(sd, MT9M111_SC_GLOBAL_GAIN,
 				   data, 2);
 out:
-	return (err < 0) ? err : 0;
-}
-
-int mt9m111_read_sensor(struct sd *sd, const u8 address,
-			u8 *i2c_data, const u8 len) {
-	int err, i;
-
-	do {
-		err = m5602_read_bridge(sd, M5602_XB_I2C_STATUS, i2c_data);
-	} while ((*i2c_data & I2C_BUSY) && !err);
-	if (err < 0)
-		goto out;
-
-	err = m5602_write_bridge(sd, M5602_XB_I2C_DEV_ADDR,
-				 sd->sensor->i2c_slave_id);
-	if (err < 0)
-		goto out;
-
-	err = m5602_write_bridge(sd, M5602_XB_I2C_REG_ADDR, address);
-	if (err < 0)
-		goto out;
-
-	err = m5602_write_bridge(sd, M5602_XB_I2C_CTRL, 0x1a);
-	if (err < 0)
-		goto out;
-
-	for (i = 0; i < len && !err; i++) {
-		err = m5602_read_bridge(sd, M5602_XB_I2C_DATA, &(i2c_data[i]));
-
-		PDEBUG(D_CONF, "Reading sensor register "
-		       "0x%x contains 0x%x ", address, *i2c_data);
-	}
-out:
-	return (err < 0) ? err : 0;
-}
-
-int mt9m111_write_sensor(struct sd *sd, const u8 address,
-				u8 *i2c_data, const u8 len)
-{
-	int err, i;
-	u8 *p;
-	struct usb_device *udev = sd->gspca_dev.dev;
-	__u8 *buf = sd->gspca_dev.usb_buf;
-
-	/* No sensor with a data width larger
-	   than 16 bits has yet been seen, nor with 0 :p*/
-	if (len > 2 || !len)
-		return -EINVAL;
-
-	memcpy(buf, sensor_urb_skeleton,
-	       sizeof(sensor_urb_skeleton));
-
-	buf[11] = sd->sensor->i2c_slave_id;
-	buf[15] = address;
-
-	p = buf + 16;
-
-	/* Copy a four byte write sequence for each byte to be written to */
-	for (i = 0; i < len; i++) {
-		memcpy(p, sensor_urb_skeleton + 16, 4);
-		p[3] = i2c_data[i];
-		p += 4;
-		PDEBUG(D_CONF, "Writing sensor register 0x%x with 0x%x",
-		       address, i2c_data[i]);
-	}
-
-	/* Copy the tailer */
-	memcpy(p, sensor_urb_skeleton + 20, 4);
-
-	/* Set the total length */
-	p[3] = 0x10 + len;
-
-	err = usb_control_msg(udev, usb_sndctrlpipe(udev, 0),
-			      0x04, 0x40, 0x19,
-			      0x0000, buf,
-			      20 + len * 4, M5602_URB_MSG_TIMEOUT);
-
-	return (err < 0) ? err : 0;
+	return err;
 }
 
-void mt9m111_dump_registers(struct sd *sd)
+static void mt9m111_dump_registers(struct sd *sd)
 {
 	u8 address, value[2] = {0x00, 0x00};
 
@@ -316,27 +241,27 @@ void mt9m111_dump_registers(struct sd *sd)
 
 	info("Dumping the mt9m111 sensor core registers");
 	value[1] = MT9M111_SENSOR_CORE;
-	mt9m111_write_sensor(sd, MT9M111_PAGE_MAP, value, 2);
+	m5602_write_sensor(sd, MT9M111_PAGE_MAP, value, 2);
 	for (address = 0; address < 0xff; address++) {
-		mt9m111_read_sensor(sd, address, value, 2);
+		m5602_read_sensor(sd, address, value, 2);
 		info("register 0x%x contains 0x%x%x",
 		     address, value[0], value[1]);
 	}
 
 	info("Dumping the mt9m111 color pipeline registers");
 	value[1] = MT9M111_COLORPIPE;
-	mt9m111_write_sensor(sd, MT9M111_PAGE_MAP, value, 2);
+	m5602_write_sensor(sd, MT9M111_PAGE_MAP, value, 2);
 	for (address = 0; address < 0xff; address++) {
-		mt9m111_read_sensor(sd, address, value, 2);
+		m5602_read_sensor(sd, address, value, 2);
 		info("register 0x%x contains 0x%x%x",
 		     address, value[0], value[1]);
 	}
 
 	info("Dumping the mt9m111 camera control registers");
 	value[1] = MT9M111_CAMERA_CONTROL;
-	mt9m111_write_sensor(sd, MT9M111_PAGE_MAP, value, 2);
+	m5602_write_sensor(sd, MT9M111_PAGE_MAP, value, 2);
 	for (address = 0; address < 0xff; address++) {
-		mt9m111_read_sensor(sd, address, value, 2);
+		m5602_read_sensor(sd, address, value, 2);
 		info("register 0x%x contains 0x%x%x",
 		     address, value[0], value[1]);
 	}
diff --git a/linux/drivers/media/video/gspca/m5602/m5602_mt9m111.h b/linux/drivers/media/video/gspca/m5602/m5602_mt9m111.h
index 315209d5a..e795ab7a3 100644
--- a/linux/drivers/media/video/gspca/m5602/m5602_mt9m111.h
+++ b/linux/drivers/media/video/gspca/m5602/m5602_mt9m111.h
@@ -87,14 +87,6 @@ int mt9m111_probe(struct sd *sd);
 int mt9m111_init(struct sd *sd);
 int mt9m111_power_down(struct sd *sd);
 
-int mt9m111_read_sensor(struct sd *sd, const u8 address,
-			u8 *i2c_data, const u8 len);
-
-int mt9m111_write_sensor(struct sd *sd, const u8 address,
-			 u8 *i2c_data, const u8 len);
-
-void mt9m111_dump_registers(struct sd *sd);
-
 int mt9m111_set_vflip(struct gspca_dev *gspca_dev, __s32 val);
 int mt9m111_get_vflip(struct gspca_dev *gspca_dev, __s32 *val);
 int mt9m111_get_hflip(struct gspca_dev *gspca_dev, __s32 *val);
@@ -106,14 +98,12 @@ static struct m5602_sensor mt9m111 = {
 	.name = "MT9M111",
 
 	.i2c_slave_id = 0xba,
+	.i2c_regW = 2,
 
 	.probe = mt9m111_probe,
 	.init = mt9m111_init,
 	.power_down = mt9m111_power_down,
 
-	.read_sensor = mt9m111_read_sensor,
-	.write_sensor = mt9m111_write_sensor,
-
 	.nctrls = 3,
 	.ctrls = {
 	{
@@ -1003,7 +993,7 @@ static const unsigned char init_mt9m111[][4] =
 	{BRIDGE, M5602_XB_SIG_INI, 0x02, 0x00},
 	{BRIDGE, M5602_XB_HSYNC_PARA, 0x00, 0x00},
 	{BRIDGE, M5602_XB_HSYNC_PARA, 0x00, 0x00},
-	{BRIDGE, M5602_XB_HSYNC_PARA, 0x02, 0x00},
+	{BRIDGE, M5602_XB_HSYNC_PARA, 0x02, 0x00}, /* 639*/
 	{BRIDGE, M5602_XB_HSYNC_PARA, 0x7f, 0x00},
 	{BRIDGE, M5602_XB_SIG_INI, 0x00, 0x00},
 	{BRIDGE, M5602_XB_SEN_CLK_DIV, 0x00, 0x00},
diff --git a/linux/drivers/media/video/gspca/m5602/m5602_ov9650.c b/linux/drivers/media/video/gspca/m5602/m5602_ov9650.c
index 07ef2b3db..dde5075ed 100644
--- a/linux/drivers/media/video/gspca/m5602/m5602_ov9650.c
+++ b/linux/drivers/media/video/gspca/m5602/m5602_ov9650.c
@@ -47,86 +47,30 @@ static
 		}
 	},
 	{
+		.ident = "ASUS A6Ja",
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "ASUSTeK Computer Inc."),
+			DMI_MATCH(DMI_PRODUCT_NAME, "A6J")
+		}
+	},
+	{
 		.ident = "ASUS A6Kt",
 		.matches = {
 			DMI_MATCH(DMI_SYS_VENDOR, "ASUSTeK Computer Inc."),
 			DMI_MATCH(DMI_PRODUCT_NAME, "A6Kt")
 		}
 	},
+	{
+		.ident = "Alienware Aurora m9700",
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "alienware"),
+			DMI_MATCH(DMI_PRODUCT_NAME, "Aurora m9700")
+		}
+	},
 	{ }
 };
 
-int ov9650_read_sensor(struct sd *sd, const u8 address,
-		      u8 *i2c_data, const u8 len)
-{
-	int err, i;
-
-	/* The ov9650 registers have a max depth of one byte */
-	if (len > 1 || !len)
-		return -EINVAL;
-
-	do {
-		err = m5602_read_bridge(sd, M5602_XB_I2C_STATUS, i2c_data);
-	} while ((*i2c_data & I2C_BUSY) && !err);
-
-	m5602_write_bridge(sd, M5602_XB_I2C_DEV_ADDR,
-			   ov9650.i2c_slave_id);
-	m5602_write_bridge(sd, M5602_XB_I2C_REG_ADDR, address);
-	m5602_write_bridge(sd, M5602_XB_I2C_CTRL, 0x10 + len);
-	m5602_write_bridge(sd, M5602_XB_I2C_CTRL, 0x08);
-
-	for (i = 0; i < len; i++) {
-		err = m5602_read_bridge(sd, M5602_XB_I2C_DATA, &(i2c_data[i]));
-
-		PDEBUG(D_CONF, "Reading sensor register "
-				"0x%x containing 0x%x ", address, *i2c_data);
-	}
-	return (err < 0) ? err : 0;
-}
-
-int ov9650_write_sensor(struct sd *sd, const u8 address,
-			u8 *i2c_data, const u8 len)
-{
-	int err, i;
-	u8 *p;
-	struct usb_device *udev = sd->gspca_dev.dev;
-	__u8 *buf = sd->gspca_dev.usb_buf;
-
-	/* The ov9650 only supports one byte writes */
-	if (len > 1 || !len)
-		return -EINVAL;
-
-	memcpy(buf, sensor_urb_skeleton,
-	       sizeof(sensor_urb_skeleton));
-
-	buf[11] = sd->sensor->i2c_slave_id;
-	buf[15] = address;
-
-	/* Special case larger sensor writes */
-	p = buf + 16;
-
-	/* Copy a four byte write sequence for each byte to be written to */
-	for (i = 0; i < len; i++) {
-		memcpy(p, sensor_urb_skeleton + 16, 4);
-		p[3] = i2c_data[i];
-		p += 4;
-		PDEBUG(D_CONF, "Writing sensor register 0x%x with 0x%x",
-		       address, i2c_data[i]);
-	}
-
-	/* Copy the tailer */
-	memcpy(p, sensor_urb_skeleton + 20, 4);
-
-	/* Set the total length */
-	p[3] = 0x10 + len;
-
-	err = usb_control_msg(udev, usb_sndctrlpipe(udev, 0),
-			      0x04, 0x40, 0x19,
-			      0x0000, buf,
-			      20 + len * 4, M5602_URB_MSG_TIMEOUT);
-
-	return (err < 0) ? err : 0;
-}
+static void ov9650_dump_registers(struct sd *sd);
 
 int ov9650_probe(struct sd *sd)
 {
@@ -148,16 +92,16 @@ int ov9650_probe(struct sd *sd)
 	for (i = 0; i < ARRAY_SIZE(preinit_ov9650); i++) {
 		u8 data = preinit_ov9650[i][2];
 		if (preinit_ov9650[i][0] == SENSOR)
-			ov9650_write_sensor(sd,
+			m5602_write_sensor(sd,
 					    preinit_ov9650[i][1], &data, 1);
 		else
 			m5602_write_bridge(sd, preinit_ov9650[i][1], data);
 	}
 
-	if (ov9650_read_sensor(sd, OV9650_PID, &prod_id, 1))
+	if (m5602_read_sensor(sd, OV9650_PID, &prod_id, 1))
 		return -ENODEV;
 
-	if (ov9650_read_sensor(sd, OV9650_VER, &ver_id, 1))
+	if (m5602_read_sensor(sd, OV9650_VER, &ver_id, 1))
 		return -ENODEV;
 
 	if ((prod_id == 0x96) && (ver_id == 0x52)) {
@@ -186,34 +130,90 @@ int ov9650_init(struct sd *sd)
 	for (i = 0; i < ARRAY_SIZE(init_ov9650) && !err; i++) {
 		data = init_ov9650[i][2];
 		if (init_ov9650[i][0] == SENSOR)
-			err = ov9650_write_sensor(sd, init_ov9650[i][1],
+			err = m5602_write_sensor(sd, init_ov9650[i][1],
 						  &data, 1);
 		else
 			err = m5602_write_bridge(sd, init_ov9650[i][1], data);
 	}
 
-	if (!err && dmi_check_system(ov9650_flip_dmi_table)) {
+	if (dmi_check_system(ov9650_flip_dmi_table) && !err) {
 		info("vflip quirk active");
 		data = 0x30;
-		err = ov9650_write_sensor(sd, OV9650_MVFP, &data, 1);
+		err = m5602_write_sensor(sd, OV9650_MVFP, &data, 1);
 	}
+	return err;
+}
 
-	return (err < 0) ? err : 0;
+int ov9650_start(struct sd *sd)
+{
+	int i, err = 0;
+	struct cam *cam = &sd->gspca_dev.cam;
+
+	for (i = 0; i < ARRAY_SIZE(res_init_ov9650) && !err; i++) {
+		u8 data = res_init_ov9650[i][1];
+		err = m5602_write_bridge(sd, res_init_ov9650[i][0], data);
+	}
+	if (err < 0)
+		return err;
+
+	switch (cam->cam_mode[sd->gspca_dev.curr_mode].width)
+	{
+	case 640:
+		PDEBUG(D_V4L2, "Configuring camera for VGA mode");
+
+		for (i = 0; i < ARRAY_SIZE(VGA_ov9650) && !err; i++) {
+			u8 data = VGA_ov9650[i][2];
+			if (VGA_ov9650[i][0] == SENSOR)
+				err = m5602_write_sensor(sd,
+					VGA_ov9650[i][1], &data, 1);
+			else
+				err = m5602_write_bridge(sd, VGA_ov9650[i][1], data);
+		}
+		break;
+
+	case 352:
+		PDEBUG(D_V4L2, "Configuring camera for CIF mode");
+
+		for (i = 0; i < ARRAY_SIZE(CIF_ov9650) && !err; i++) {
+			u8 data = CIF_ov9650[i][2];
+			if (CIF_ov9650[i][0] == SENSOR)
+				err = m5602_write_sensor(sd,
+					CIF_ov9650[i][1], &data, 1);
+			else
+				err = m5602_write_bridge(sd, CIF_ov9650[i][1], data);
+		}
+		break;
+
+	case 320:
+		PDEBUG(D_V4L2, "Configuring camera for QVGA mode");
+
+		for (i = 0; i < ARRAY_SIZE(QVGA_ov9650) && !err; i++) {
+			u8 data = QVGA_ov9650[i][2];
+			if (QVGA_ov9650[i][0] == SENSOR)
+				err = m5602_write_sensor(sd,
+					QVGA_ov9650[i][1], &data, 1);
+			else
+				err = m5602_write_bridge(sd, QVGA_ov9650[i][1], data);
+		}
+		break;
+	}
+	return err;
 }
 
 int ov9650_power_down(struct sd *sd)
 {
-	int i;
-	for (i = 0; i < ARRAY_SIZE(power_down_ov9650); i++) {
+	int i, err = 0;
+	for (i = 0; i < ARRAY_SIZE(power_down_ov9650) && !err; i++) {
 		u8 data = power_down_ov9650[i][2];
 		if (power_down_ov9650[i][0] == SENSOR)
-			ov9650_write_sensor(sd,
+			err = m5602_write_sensor(sd,
 					    power_down_ov9650[i][1], &data, 1);
 		else
-			m5602_write_bridge(sd, power_down_ov9650[i][1], data);
+			err = m5602_write_bridge(sd, power_down_ov9650[i][1],
+						 data);
 	}
 
-	return 0;
+	return err;
 }
 
 int ov9650_get_exposure(struct gspca_dev *gspca_dev, __s32 *val)
@@ -222,24 +222,24 @@ int ov9650_get_exposure(struct gspca_dev *gspca_dev, __s32 *val)
 	u8 i2c_data;
 	int err;
 
-	err = ov9650_read_sensor(sd, OV9650_COM1, &i2c_data, 1);
+	err = m5602_read_sensor(sd, OV9650_COM1, &i2c_data, 1);
 	if (err < 0)
 		goto out;
 	*val = i2c_data & 0x03;
 
-	err = ov9650_read_sensor(sd, OV9650_AECH, &i2c_data, 1);
+	err = m5602_read_sensor(sd, OV9650_AECH, &i2c_data, 1);
 	if (err < 0)
 		goto out;
 	*val |= (i2c_data << 2);
 
-	err = ov9650_read_sensor(sd, OV9650_AECHM, &i2c_data, 1);
+	err = m5602_read_sensor(sd, OV9650_AECHM, &i2c_data, 1);
 	if (err < 0)
 		goto out;
 	*val |= (i2c_data & 0x3f) << 10;
 
 	PDEBUG(D_V4L2, "Read exposure %d", *val);
 out:
-	return (err < 0) ? err : 0;
+	return err;
 }
 
 int ov9650_set_exposure(struct gspca_dev *gspca_dev, __s32 val)
@@ -253,24 +253,24 @@ int ov9650_set_exposure(struct gspca_dev *gspca_dev, __s32 val)
 
 	/* The 6 MSBs */
 	i2c_data = (val >> 10) & 0x3f;
-	err = ov9650_write_sensor(sd, OV9650_AECHM,
+	err = m5602_write_sensor(sd, OV9650_AECHM,
 				  &i2c_data, 1);
 	if (err < 0)
 		goto out;
 
 	/* The 8 middle bits */
 	i2c_data = (val >> 2) & 0xff;
-	err = ov9650_write_sensor(sd, OV9650_AECH,
+	err = m5602_write_sensor(sd, OV9650_AECH,
 				  &i2c_data, 1);
 	if (err < 0)
 		goto out;
 
 	/* The 2 LSBs */
 	i2c_data = val & 0x03;
-	err = ov9650_write_sensor(sd, OV9650_COM1, &i2c_data, 1);
+	err = m5602_write_sensor(sd, OV9650_COM1, &i2c_data, 1);
 
 out:
-	return (err < 0) ? err : 0;
+	return err;
 }
 
 int ov9650_get_gain(struct gspca_dev *gspca_dev, __s32 *val)
@@ -279,13 +279,13 @@ int ov9650_get_gain(struct gspca_dev *gspca_dev, __s32 *val)
 	u8 i2c_data;
 	struct sd *sd = (struct sd *) gspca_dev;
 
-	ov9650_read_sensor(sd, OV9650_VREF, &i2c_data, 1);
+	m5602_read_sensor(sd, OV9650_VREF, &i2c_data, 1);
 	*val = (i2c_data & 0x03) << 8;
 
-	err = ov9650_read_sensor(sd, OV9650_GAIN, &i2c_data, 1);
+	err = m5602_read_sensor(sd, OV9650_GAIN, &i2c_data, 1);
 	*val |= i2c_data;
 	PDEBUG(D_V4L2, "Read gain %d", *val);
-	return (err < 0) ? err : 0;
+	return err;
 }
 
 int ov9650_set_gain(struct gspca_dev *gspca_dev, __s32 val)
@@ -297,16 +297,16 @@ int ov9650_set_gain(struct gspca_dev *gspca_dev, __s32 val)
 	/* The 2 MSB */
 	/* Read the OV9650_VREF register first to avoid
 	   corrupting the VREF high and low bits */
-	ov9650_read_sensor(sd, OV9650_VREF, &i2c_data, 1);
+	m5602_read_sensor(sd, OV9650_VREF, &i2c_data, 1);
 	/* Mask away all uninteresting bits */
 	i2c_data = ((val & 0x0300) >> 2) |
 			(i2c_data & 0x3F);
-	err = ov9650_write_sensor(sd, OV9650_VREF, &i2c_data, 1);
+	err = m5602_write_sensor(sd, OV9650_VREF, &i2c_data, 1);
 
 	/* The 8 LSBs */
 	i2c_data = val & 0xff;
-	err = ov9650_write_sensor(sd, OV9650_GAIN, &i2c_data, 1);
-	return (err < 0) ? err : 0;
+	err = m5602_write_sensor(sd, OV9650_GAIN, &i2c_data, 1);
+	return err;
 }
 
 int ov9650_get_red_balance(struct gspca_dev *gspca_dev, __s32 *val)
@@ -315,12 +315,12 @@ int ov9650_get_red_balance(struct gspca_dev *gspca_dev, __s32 *val)
 	u8 i2c_data;
 	struct sd *sd = (struct sd *) gspca_dev;
 
-	err = ov9650_read_sensor(sd, OV9650_RED, &i2c_data, 1);
+	err = m5602_read_sensor(sd, OV9650_RED, &i2c_data, 1);
 	*val = i2c_data;
 
 	PDEBUG(D_V4L2, "Read red gain %d", *val);
 
-	return (err < 0) ? err : 0;
+	return err;
 }
 
 int ov9650_set_red_balance(struct gspca_dev *gspca_dev, __s32 val)
@@ -333,9 +333,9 @@ int ov9650_set_red_balance(struct gspca_dev *gspca_dev, __s32 val)
 			     val & 0xff);
 
 	i2c_data = val & 0xff;
-	err = ov9650_write_sensor(sd, OV9650_RED, &i2c_data, 1);
+	err = m5602_write_sensor(sd, OV9650_RED, &i2c_data, 1);
 
-	return (err < 0) ? err : 0;
+	return err;
 }
 
 int ov9650_get_blue_balance(struct gspca_dev *gspca_dev, __s32 *val)
@@ -344,12 +344,12 @@ int ov9650_get_blue_balance(struct gspca_dev *gspca_dev, __s32 *val)
 	u8 i2c_data;
 	struct sd *sd = (struct sd *) gspca_dev;
 
-	err = ov9650_read_sensor(sd, OV9650_BLUE, &i2c_data, 1);
+	err = m5602_read_sensor(sd, OV9650_BLUE, &i2c_data, 1);
 	*val = i2c_data;
 
 	PDEBUG(D_V4L2, "Read blue gain %d", *val);
 
-	return (err < 0) ? err : 0;
+	return err;
 }
 
 int ov9650_set_blue_balance(struct gspca_dev *gspca_dev, __s32 val)
@@ -362,9 +362,9 @@ int ov9650_set_blue_balance(struct gspca_dev *gspca_dev, __s32 val)
 	       val & 0xff);
 
 	i2c_data = val & 0xff;
-	err = ov9650_write_sensor(sd, OV9650_BLUE, &i2c_data, 1);
+	err = m5602_write_sensor(sd, OV9650_BLUE, &i2c_data, 1);
 
-	return (err < 0) ? err : 0;
+	return err;
 }
 
 int ov9650_get_hflip(struct gspca_dev *gspca_dev, __s32 *val)
@@ -373,14 +373,14 @@ int ov9650_get_hflip(struct gspca_dev *gspca_dev, __s32 *val)
 	u8 i2c_data;
 	struct sd *sd = (struct sd *) gspca_dev;
 
-	err = ov9650_read_sensor(sd, OV9650_MVFP, &i2c_data, 1);
+	err = m5602_read_sensor(sd, OV9650_MVFP, &i2c_data, 1);
 	if (dmi_check_system(ov9650_flip_dmi_table))
 		*val = ((i2c_data & OV9650_HFLIP) >> 5) ? 0 : 1;
 	else
 		*val = (i2c_data & OV9650_HFLIP) >> 5;
 	PDEBUG(D_V4L2, "Read horizontal flip %d", *val);
 
-	return (err < 0) ? err : 0;
+	return err;
 }
 
 int ov9650_set_hflip(struct gspca_dev *gspca_dev, __s32 val)
@@ -390,20 +390,20 @@ int ov9650_set_hflip(struct gspca_dev *gspca_dev, __s32 val)
 	struct sd *sd = (struct sd *) gspca_dev;
 
 	PDEBUG(D_V4L2, "Set horizontal flip to %d", val);
-	err = ov9650_read_sensor(sd, OV9650_MVFP, &i2c_data, 1);
+	err = m5602_read_sensor(sd, OV9650_MVFP, &i2c_data, 1);
 	if (err < 0)
 		goto out;
 
 	if (dmi_check_system(ov9650_flip_dmi_table))
 		i2c_data = ((i2c_data & 0xdf) |
-				(((val ? 0 : 1) & 0x01) << 5));
+			   (((val ? 0 : 1) & 0x01) << 5));
 	else
 		i2c_data = ((i2c_data & 0xdf) |
-				((val & 0x01) << 5));
+			   ((val & 0x01) << 5));
 
-	err = ov9650_write_sensor(sd, OV9650_MVFP, &i2c_data, 1);
+	err = m5602_write_sensor(sd, OV9650_MVFP, &i2c_data, 1);
 out:
-	return (err < 0) ? err : 0;
+	return err;
 }
 
 int ov9650_get_vflip(struct gspca_dev *gspca_dev, __s32 *val)
@@ -412,14 +412,14 @@ int ov9650_get_vflip(struct gspca_dev *gspca_dev, __s32 *val)
 	u8 i2c_data;
 	struct sd *sd = (struct sd *) gspca_dev;
 
-	err = ov9650_read_sensor(sd, OV9650_MVFP, &i2c_data, 1);
+	err = m5602_read_sensor(sd, OV9650_MVFP, &i2c_data, 1);
 	if (dmi_check_system(ov9650_flip_dmi_table))
 		*val = ((i2c_data & 0x10) >> 4) ? 0 : 1;
 	else
 		*val = (i2c_data & 0x10) >> 4;
 	PDEBUG(D_V4L2, "Read vertical flip %d", *val);
 
-	return (err < 0) ? err : 0;
+	return err;
 }
 
 int ov9650_set_vflip(struct gspca_dev *gspca_dev, __s32 val)
@@ -429,7 +429,7 @@ int ov9650_set_vflip(struct gspca_dev *gspca_dev, __s32 val)
 	struct sd *sd = (struct sd *) gspca_dev;
 
 	PDEBUG(D_V4L2, "Set vertical flip to %d", val);
-	err = ov9650_read_sensor(sd, OV9650_MVFP, &i2c_data, 1);
+	err = m5602_read_sensor(sd, OV9650_MVFP, &i2c_data, 1);
 	if (err < 0)
 		goto out;
 
@@ -440,9 +440,9 @@ int ov9650_set_vflip(struct gspca_dev *gspca_dev, __s32 val)
 		i2c_data = ((i2c_data & 0xef) |
 				((val & 0x01) << 4));
 
-	err = ov9650_write_sensor(sd, OV9650_MVFP, &i2c_data, 1);
+	err = m5602_write_sensor(sd, OV9650_MVFP, &i2c_data, 1);
 out:
-	return (err < 0) ? err : 0;
+	return err;
 }
 
 int ov9650_get_brightness(struct gspca_dev *gspca_dev, __s32 *val)
@@ -451,16 +451,16 @@ int ov9650_get_brightness(struct gspca_dev *gspca_dev, __s32 *val)
 	u8 i2c_data;
 	struct sd *sd = (struct sd *) gspca_dev;
 
-	err = ov9650_read_sensor(sd, OV9650_VREF, &i2c_data, 1);
+	err = m5602_read_sensor(sd, OV9650_VREF, &i2c_data, 1);
 	if (err < 0)
 		goto out;
 	*val = (i2c_data & 0x03) << 8;
 
-	err = ov9650_read_sensor(sd, OV9650_GAIN, &i2c_data, 1);
+	err = m5602_read_sensor(sd, OV9650_GAIN, &i2c_data, 1);
 	*val |= i2c_data;
 	PDEBUG(D_V4L2, "Read gain %d", *val);
 out:
-	return (err < 0) ? err : 0;
+	return err;
 }
 
 int ov9650_set_brightness(struct gspca_dev *gspca_dev, __s32 val)
@@ -473,22 +473,22 @@ int ov9650_set_brightness(struct gspca_dev *gspca_dev, __s32 val)
 
 	/* Read the OV9650_VREF register first to avoid
 		corrupting the VREF high and low bits */
-	err = ov9650_read_sensor(sd, OV9650_VREF, &i2c_data, 1);
+	err = m5602_read_sensor(sd, OV9650_VREF, &i2c_data, 1);
 	if (err < 0)
 		goto out;
 
 	/* Mask away all uninteresting bits */
 	i2c_data = ((val & 0x0300) >> 2) | (i2c_data & 0x3F);
-	err = ov9650_write_sensor(sd, OV9650_VREF, &i2c_data, 1);
+	err = m5602_write_sensor(sd, OV9650_VREF, &i2c_data, 1);
 	if (err < 0)
 		goto out;
 
 	/* The 8 LSBs */
 	i2c_data = val & 0xff;
-	err = ov9650_write_sensor(sd, OV9650_GAIN, &i2c_data, 1);
+	err = m5602_write_sensor(sd, OV9650_GAIN, &i2c_data, 1);
 
 out:
-	return (err < 0) ? err : 0;
+	return err;
 }
 
 int ov9650_get_auto_white_balance(struct gspca_dev *gspca_dev, __s32 *val)
@@ -497,11 +497,11 @@ int ov9650_get_auto_white_balance(struct gspca_dev *gspca_dev, __s32 *val)
 	u8 i2c_data;
 	struct sd *sd = (struct sd *) gspca_dev;
 
-	err = ov9650_read_sensor(sd, OV9650_COM8, &i2c_data, 1);
+	err = m5602_read_sensor(sd, OV9650_COM8, &i2c_data, 1);
 	*val = (i2c_data & OV9650_AWB_EN) >> 1;
 	PDEBUG(D_V4L2, "Read auto white balance %d", *val);
 
-	return (err < 0) ? err : 0;
+	return err;
 }
 
 int ov9650_set_auto_white_balance(struct gspca_dev *gspca_dev, __s32 val)
@@ -511,14 +511,14 @@ int ov9650_set_auto_white_balance(struct gspca_dev *gspca_dev, __s32 val)
 	struct sd *sd = (struct sd *) gspca_dev;
 
 	PDEBUG(D_V4L2, "Set auto white balance to %d", val);
-	err = ov9650_read_sensor(sd, OV9650_COM8, &i2c_data, 1);
+	err = m5602_read_sensor(sd, OV9650_COM8, &i2c_data, 1);
 	if (err < 0)
 		goto out;
 
 	i2c_data = ((i2c_data & 0xfd) | ((val & 0x01) << 1));
-	err = ov9650_write_sensor(sd, OV9650_COM8, &i2c_data, 1);
+	err = m5602_write_sensor(sd, OV9650_COM8, &i2c_data, 1);
 out:
-	return (err < 0) ? err : 0;
+	return err;
 }
 
 int ov9650_get_auto_gain(struct gspca_dev *gspca_dev, __s32 *val)
@@ -527,11 +527,11 @@ int ov9650_get_auto_gain(struct gspca_dev *gspca_dev, __s32 *val)
 	u8 i2c_data;
 	struct sd *sd = (struct sd *) gspca_dev;
 
-	err = ov9650_read_sensor(sd, OV9650_COM8, &i2c_data, 1);
+	err = m5602_read_sensor(sd, OV9650_COM8, &i2c_data, 1);
 	*val = (i2c_data & OV9650_AGC_EN) >> 2;
 	PDEBUG(D_V4L2, "Read auto gain control %d", *val);
 
-	return (err < 0) ? err : 0;
+	return err;
 }
 
 int ov9650_set_auto_gain(struct gspca_dev *gspca_dev, __s32 val)
@@ -541,23 +541,23 @@ int ov9650_set_auto_gain(struct gspca_dev *gspca_dev, __s32 val)
 	struct sd *sd = (struct sd *) gspca_dev;
 
 	PDEBUG(D_V4L2, "Set auto gain control to %d", val);
-	err = ov9650_read_sensor(sd, OV9650_COM8, &i2c_data, 1);
+	err = m5602_read_sensor(sd, OV9650_COM8, &i2c_data, 1);
 	if (err < 0)
 		goto out;
 
 	i2c_data = ((i2c_data & 0xfb) | ((val & 0x01) << 2));
-	err = ov9650_write_sensor(sd, OV9650_COM8, &i2c_data, 1);
+	err = m5602_write_sensor(sd, OV9650_COM8, &i2c_data, 1);
 out:
-	return (err < 0) ? err : 0;
+	return err;
 }
 
-void ov9650_dump_registers(struct sd *sd)
+static void ov9650_dump_registers(struct sd *sd)
 {
 	int address;
 	info("Dumping the ov9650 register state");
 	for (address = 0; address < 0xa9; address++) {
 		u8 value;
-		ov9650_read_sensor(sd, address, &value, 1);
+		m5602_read_sensor(sd, address, &value, 1);
 		info("register 0x%x contains 0x%x",
 		     address, value);
 	}
@@ -569,9 +569,9 @@ void ov9650_dump_registers(struct sd *sd)
 		u8 old_value, ctrl_value;
 		u8 test_value[2] = {0xff, 0xff};
 
-		ov9650_read_sensor(sd, address, &old_value, 1);
-		ov9650_write_sensor(sd, address, test_value, 1);
-		ov9650_read_sensor(sd, address, &ctrl_value, 1);
+		m5602_read_sensor(sd, address, &old_value, 1);
+		m5602_write_sensor(sd, address, test_value, 1);
+		m5602_read_sensor(sd, address, &ctrl_value, 1);
 
 		if (ctrl_value == test_value[0])
 			info("register 0x%x is writeable", address);
@@ -579,6 +579,6 @@ void ov9650_dump_registers(struct sd *sd)
 			info("register 0x%x is read only", address);
 
 		/* Restore original value */
-		ov9650_write_sensor(sd, address, &old_value, 1);
+		m5602_write_sensor(sd, address, &old_value, 1);
 	}
 }
diff --git a/linux/drivers/media/video/gspca/m5602/m5602_ov9650.h b/linux/drivers/media/video/gspca/m5602/m5602_ov9650.h
index e0efdb930..f4b33b8e8 100644
--- a/linux/drivers/media/video/gspca/m5602/m5602_ov9650.h
+++ b/linux/drivers/media/video/gspca/m5602/m5602_ov9650.h
@@ -20,7 +20,6 @@
 #define M5602_OV9650_H_
 
 #include <linux/dmi.h>
-
 #include "m5602_sensor.h"
 
 /*****************************************************************************/
@@ -36,6 +35,7 @@
 #define OV9650_PID			0x0a
 #define OV9650_VER			0x0b
 #define OV9650_COM3			0x0c
+#define OV9650_COM4			0x0d
 #define OV9650_COM5			0x0e
 #define OV9650_COM6			0x0f
 #define OV9650_AECH			0x10
@@ -94,6 +94,8 @@
 
 #define OV9650_REGISTER_RESET		(1 << 7)
 #define OV9650_VGA_SELECT		(1 << 6)
+#define OV9650_CIF_SELECT		(1 << 5)
+#define OV9650_QVGA_SELECT		(1 << 4)
 #define OV9650_RGB_SELECT		(1 << 2)
 #define OV9650_RAW_RGB_SELECT		(1 << 0)
 
@@ -108,6 +110,8 @@
 #define OV9650_SYSTEM_CLK_SEL		(1 << 7)
 #define OV9650_SLAM_MODE 		(1 << 4)
 
+#define OV9650_QVGA_VARIOPIXEL		(1 << 7)
+
 #define OV9650_VFLIP			(1 << 4)
 #define OV9650_HFLIP			(1 << 5)
 
@@ -124,15 +128,9 @@ extern int dump_sensor;
 
 int ov9650_probe(struct sd *sd);
 int ov9650_init(struct sd *sd);
+int ov9650_start(struct sd *sd);
 int ov9650_power_down(struct sd *sd);
 
-int ov9650_read_sensor(struct sd *sd, const u8 address,
-		       u8 *i2c_data, const u8 len);
-int ov9650_write_sensor(struct sd *sd, const u8 address,
-		       u8 *i2c_data, const u8 len);
-
-void ov9650_dump_registers(struct sd *sd);
-
 int ov9650_set_exposure(struct gspca_dev *gspca_dev, __s32 val);
 int ov9650_get_exposure(struct gspca_dev *gspca_dev, __s32 *val);
 int ov9650_get_gain(struct gspca_dev *gspca_dev, __s32 *val);
@@ -155,11 +153,11 @@ int ov9650_set_auto_gain(struct gspca_dev *gspca_dev, __s32 val);
 static struct m5602_sensor ov9650 = {
 	.name = "OV9650",
 	.i2c_slave_id = 0x60,
+	.i2c_regW = 1,
 	.probe = ov9650_probe,
 	.init = ov9650_init,
+	.start = ov9650_start,
 	.power_down = ov9650_power_down,
-	.read_sensor = ov9650_read_sensor,
-	.write_sensor = ov9650_write_sensor,
 
 	.nctrls = 8,
 	.ctrls = {
@@ -264,18 +262,38 @@ static struct m5602_sensor ov9650 = {
 	}
 	},
 
-	.nmodes = 1,
+	.nmodes = 3,
 	.modes = {
 	{
-		M5602_DEFAULT_FRAME_WIDTH,
-		M5602_DEFAULT_FRAME_HEIGHT,
+		320,
+		240,
+		V4L2_PIX_FMT_SBGGR8,
+		V4L2_FIELD_NONE,
+		.sizeimage =
+			320 * 240,
+		.bytesperline = 320,
+		.colorspace = V4L2_COLORSPACE_SRGB,
+		.priv = 0
+	}, {
+		352,
+		288,
 		V4L2_PIX_FMT_SBGGR8,
 		V4L2_FIELD_NONE,
 		.sizeimage =
-			M5602_DEFAULT_FRAME_WIDTH * M5602_DEFAULT_FRAME_HEIGHT,
-		.bytesperline = M5602_DEFAULT_FRAME_WIDTH,
+			352 * 288,
+		.bytesperline = 352,
 		.colorspace = V4L2_COLORSPACE_SRGB,
-		.priv = 1
+		.priv = 0
+	}, {
+		640,
+		480,
+		V4L2_PIX_FMT_SBGGR8,
+		V4L2_FIELD_NONE,
+		.sizeimage =
+			640 * 480,
+		.bytesperline = 640,
+		.colorspace = V4L2_COLORSPACE_SRGB,
+		.priv = 0
 	}
 	}
 };
@@ -324,6 +342,7 @@ static const unsigned char init_ov9650[][3] =
 	{BRIDGE, M5602_XB_GPIO_DAT_H, 0x00},
 	{BRIDGE, M5602_XB_GPIO_DAT, 0x00},
 	{BRIDGE, M5602_XB_I2C_CLK_DIV, 0x0a},
+
 	/* Reset chip */
 	{SENSOR, OV9650_COM7, OV9650_REGISTER_RESET},
 	/* Enable double clock */
@@ -331,8 +350,6 @@ static const unsigned char init_ov9650[][3] =
 	/* Do something out of spec with the power */
 	{SENSOR, OV9650_OFON, 0x40},
 
-	/* Set QQVGA */
-	{SENSOR, OV9650_COM1, 0x20},
 	/* Set fast AGC/AEC algorithm with unlimited step size */
 	{SENSOR, OV9650_COM8, OV9650_FAST_AGC_AEC |
 			      OV9650_AEC_UNLIM_STEP_SIZE |
@@ -343,7 +360,7 @@ static const unsigned char init_ov9650[][3] =
 	{SENSOR, OV9650_ACOM38, 0x81},
 	/* Turn off color matrix coefficient double option */
 	{SENSOR, OV9650_COM16, 0x00},
-		/* Enable color matrix for RGB/YUV, Delay Y channel,
+	/* Enable color matrix for RGB/YUV, Delay Y channel,
 	set output Y/UV delay to 1 */
 	{SENSOR, OV9650_COM13, 0x19},
 	/* Enable digital BLC, Set output mode to U Y V Y */
@@ -352,7 +369,7 @@ static const unsigned char init_ov9650[][3] =
 	{SENSOR, OV9650_COM24, 0x00},
 	/* Enable HREF and some out of spec things */
 	{SENSOR, OV9650_COM12, 0x73},
-		/* Set all DBLC offset signs to positive and
+	/* Set all DBLC offset signs to positive and
 	do some out of spec stuff */
 	{SENSOR, OV9650_DBLC1, 0xdf},
 	{SENSOR, OV9650_COM21, 0x06},
@@ -364,7 +381,7 @@ static const unsigned char init_ov9650[][3] =
 	{SENSOR, OV9650_RSVD96, 0x04},
 	/* Enable full range output */
 	{SENSOR, OV9650_COM15, 0x0},
-		/* Enable HREF at optical black, enable ADBLC bias,
+	/* Enable HREF at optical black, enable ADBLC bias,
 	enable ADBLC, reset timings at format change */
 	{SENSOR, OV9650_COM6, 0x4b},
 	/* Subtract 32 from the B channel bias */
@@ -385,7 +402,7 @@ static const unsigned char init_ov9650[][3] =
 	{SENSOR, OV9650_AEB, 0x5c},
 	/* Set the high and low limit nibbles to 3 */
 	{SENSOR, OV9650_VPT, 0xc3},
-		/* Set the Automatic Gain Ceiling (AGC) to 128x,
+	/* Set the Automatic Gain Ceiling (AGC) to 128x,
 	drop VSYNC at frame drop,
 	limit exposure timing,
 	drop frame when the AEC step is larger than the exposure gap */
@@ -394,9 +411,9 @@ static const unsigned char init_ov9650[][3] =
 	and set PWDN to SLVS (slave mode vertical sync) */
 	{SENSOR, OV9650_COM10, 0x42},
 	/* Set horizontal column start high to default value */
-	{SENSOR, OV9650_HSTART, 0x1a},
+	{SENSOR, OV9650_HSTART, 0x1a}, /* 210 */
 	/* Set horizontal column end */
-	{SENSOR, OV9650_HSTOP, 0xbf},
+	{SENSOR, OV9650_HSTOP, 0xbf}, /* 1534 */
 	/* Complementing register to the two writes above */
 	{SENSOR, OV9650_HREF, 0xb2},
 	/* Set vertical row start high bits */
@@ -405,10 +422,6 @@ static const unsigned char init_ov9650[][3] =
 	{SENSOR, OV9650_VSTOP, 0x7e},
 	/* Set complementing vertical frame control */
 	{SENSOR, OV9650_VREF, 0x10},
-	/* Set raw RGB output format with VGA resolution */
-	{SENSOR, OV9650_COM7, OV9650_VGA_SELECT |
-			      OV9650_RGB_SELECT |
-			      OV9650_RAW_RGB_SELECT},
 	{SENSOR, OV9650_ADC, 0x04},
 	{SENSOR, OV9650_HV, 0x40},
 	/* Enable denoise, and white-pixel erase */
@@ -417,31 +430,15 @@ static const unsigned char init_ov9650[][3] =
 	/* Set the high bits of the exposure value */
 	{SENSOR, OV9650_AECH, ((EXPOSURE_DEFAULT & 0xff00) >> 8)},
 
+	/* Enable VARIOPIXEL */
+	{SENSOR, OV9650_COM3, OV9650_VARIOPIXEL},
+	{SENSOR, OV9650_COM4, OV9650_QVGA_VARIOPIXEL},
+
 	/* Set the low bits of the exposure value */
 	{SENSOR, OV9650_COM1, (EXPOSURE_DEFAULT & 0xff)},
 	{SENSOR, OV9650_GAIN, GAIN_DEFAULT},
 	{SENSOR, OV9650_BLUE, BLUE_GAIN_DEFAULT},
 	{SENSOR, OV9650_RED, RED_GAIN_DEFAULT},
-
-	{SENSOR, OV9650_COM3, OV9650_VARIOPIXEL},
-	{SENSOR, OV9650_COM5, OV9650_SYSTEM_CLK_SEL},
-
-	{BRIDGE, M5602_XB_LINE_OF_FRAME_H, 0x82},
-	{BRIDGE, M5602_XB_LINE_OF_FRAME_L, 0x00},
-	{BRIDGE, M5602_XB_PIX_OF_LINE_H, 0x82},
-	{BRIDGE, M5602_XB_PIX_OF_LINE_L, 0x00},
-	{BRIDGE, M5602_XB_SIG_INI, 0x01},
-	{BRIDGE, M5602_XB_VSYNC_PARA, 0x00},
-	/* Moves the view window in a vertical orientation */
-	{BRIDGE, M5602_XB_VSYNC_PARA, 0x09},
-	{BRIDGE, M5602_XB_VSYNC_PARA, 0x00},
-	{BRIDGE, M5602_XB_VSYNC_PARA, 0x01},
-	{BRIDGE, M5602_XB_VSYNC_PARA, 0xe0},
-	{BRIDGE, M5602_XB_VSYNC_PARA, 0x00},
-	{BRIDGE, M5602_XB_HSYNC_PARA, 0x00},
-	{BRIDGE, M5602_XB_HSYNC_PARA, 0x5e},
-	{BRIDGE, M5602_XB_HSYNC_PARA, 0x02},
-	{BRIDGE, M5602_XB_HSYNC_PARA, 0xde}
 };
 
 static const unsigned char power_down_ov9650[][3] =
@@ -461,7 +458,76 @@ static const unsigned char power_down_ov9650[][3] =
 	{BRIDGE, M5602_XB_GPIO_EN_L, 0x06},
 	{BRIDGE, M5602_XB_GPIO_DAT_H, 0x02},
 	{BRIDGE, M5602_XB_SEN_CLK_DIV, 0x04},
-	{BRIDGE, M5602_XB_SEN_CLK_CTRL, 0xb0}
+	{BRIDGE, M5602_XB_SEN_CLK_CTRL, 0xb0},
+};
+
+static const unsigned char res_init_ov9650[][2] =
+{
+	{M5602_XB_LINE_OF_FRAME_H, 0x82},
+	{M5602_XB_LINE_OF_FRAME_L, 0x00},
+	{M5602_XB_PIX_OF_LINE_H, 0x82},
+	{M5602_XB_PIX_OF_LINE_L, 0x00},
+	{M5602_XB_SIG_INI, 0x01}
+};
+
+static const unsigned char VGA_ov9650[][3] =
+{
+	/* Moves the view window in a vertical orientation */
+	{BRIDGE, M5602_XB_VSYNC_PARA, 0x00},
+	{BRIDGE, M5602_XB_VSYNC_PARA, 0x09},
+	{BRIDGE, M5602_XB_VSYNC_PARA, 0x00},
+	{BRIDGE, M5602_XB_VSYNC_PARA, 0x01},
+	{BRIDGE, M5602_XB_VSYNC_PARA, 0xe0}, /* 480 */
+	{BRIDGE, M5602_XB_VSYNC_PARA, 0x00},
+	{BRIDGE, M5602_XB_VSYNC_PARA, 0x00},
+	{BRIDGE, M5602_XB_HSYNC_PARA, 0x00},
+	{BRIDGE, M5602_XB_HSYNC_PARA, 0x62}, /* 98 */
+	{BRIDGE, M5602_XB_HSYNC_PARA, 0x02}, /* 640 + 98 */
+	{BRIDGE, M5602_XB_HSYNC_PARA, 0xe2},
+
+	{SENSOR, OV9650_COM7, OV9650_VGA_SELECT |
+			      OV9650_RGB_SELECT |
+			      OV9650_RAW_RGB_SELECT},
+};
+
+static const unsigned char CIF_ov9650[][3] =
+{
+	/* Moves the view window in a vertical orientation */
+	{BRIDGE, M5602_XB_VSYNC_PARA, 0x00},
+	{BRIDGE, M5602_XB_VSYNC_PARA, 0x09},
+	{BRIDGE, M5602_XB_VSYNC_PARA, 0x00},
+	{BRIDGE, M5602_XB_VSYNC_PARA, 0x01},
+	{BRIDGE, M5602_XB_VSYNC_PARA, 0x20}, /* 288 */
+	{BRIDGE, M5602_XB_VSYNC_PARA, 0x00},
+	{BRIDGE, M5602_XB_VSYNC_PARA, 0x00},
+	{BRIDGE, M5602_XB_HSYNC_PARA, 0x00},
+	{BRIDGE, M5602_XB_HSYNC_PARA, 0x62}, /* 98 */
+	{BRIDGE, M5602_XB_HSYNC_PARA, 0x01}, /* 352 + 98 */
+	{BRIDGE, M5602_XB_HSYNC_PARA, 0xc2},
+
+	{SENSOR, OV9650_COM7, OV9650_CIF_SELECT |
+			      OV9650_RGB_SELECT |
+			      OV9650_RAW_RGB_SELECT},
+};
+
+static const unsigned char QVGA_ov9650[][3] =
+{
+	/* Moves the view window in a vertical orientation */
+	{BRIDGE, M5602_XB_VSYNC_PARA, 0x00},
+	{BRIDGE, M5602_XB_VSYNC_PARA, 0x08},
+	{BRIDGE, M5602_XB_VSYNC_PARA, 0x00},
+	{BRIDGE, M5602_XB_VSYNC_PARA, 0x00},
+	{BRIDGE, M5602_XB_VSYNC_PARA, 0xf0}, /* 240 */
+	{BRIDGE, M5602_XB_VSYNC_PARA, 0x00},
+	{BRIDGE, M5602_XB_VSYNC_PARA, 0x00},
+	{BRIDGE, M5602_XB_HSYNC_PARA, 0x00},
+	{BRIDGE, M5602_XB_HSYNC_PARA, 0x31}, /* 50 */
+	{BRIDGE, M5602_XB_HSYNC_PARA, 0x01}, /* 320 + 50 */
+	{BRIDGE, M5602_XB_HSYNC_PARA, 0x71},
+
+	{SENSOR, OV9650_COM7, OV9650_QVGA_SELECT |
+			      OV9650_RGB_SELECT |
+			      OV9650_RAW_RGB_SELECT},
 };
 
 #endif
diff --git a/linux/drivers/media/video/gspca/m5602/m5602_po1030.c b/linux/drivers/media/video/gspca/m5602/m5602_po1030.c
index d17ac5256..2e7fb9167 100644
--- a/linux/drivers/media/video/gspca/m5602/m5602_po1030.c
+++ b/linux/drivers/media/video/gspca/m5602/m5602_po1030.c
@@ -18,6 +18,8 @@
 
 #include "m5602_po1030.h"
 
+static void po1030_dump_registers(struct sd *sd);
+
 int po1030_probe(struct sd *sd)
 {
 	u8 prod_id = 0, ver_id = 0, i;
@@ -38,16 +40,16 @@ int po1030_probe(struct sd *sd)
 	for (i = 0; i < ARRAY_SIZE(preinit_po1030); i++) {
 		u8 data = preinit_po1030[i][2];
 		if (preinit_po1030[i][0] == SENSOR)
-			po1030_write_sensor(sd,
+			m5602_write_sensor(sd,
 				preinit_po1030[i][1], &data, 1);
 		else
 			m5602_write_bridge(sd, preinit_po1030[i][1], data);
 	}
 
-	if (po1030_read_sensor(sd, 0x3, &prod_id, 1))
+	if (m5602_read_sensor(sd, 0x3, &prod_id, 1))
 		return -ENODEV;
 
-	if (po1030_read_sensor(sd, 0x4, &ver_id, 1))
+	if (m5602_read_sensor(sd, 0x4, &ver_id, 1))
 		return -ENODEV;
 
 	if ((prod_id == 0x02) && (ver_id == 0xef)) {
@@ -64,78 +66,12 @@ sensor_found:
 	return 0;
 }
 
-int po1030_read_sensor(struct sd *sd, const u8 address,
-			u8 *i2c_data, const u8 len)
-{
-	int err, i;
-
-	do {
-		err = m5602_read_bridge(sd, M5602_XB_I2C_STATUS, i2c_data);
-	} while ((*i2c_data & I2C_BUSY) && !err);
-
-	m5602_write_bridge(sd, M5602_XB_I2C_DEV_ADDR,
-			   sd->sensor->i2c_slave_id);
-	m5602_write_bridge(sd, M5602_XB_I2C_REG_ADDR, address);
-	m5602_write_bridge(sd, M5602_XB_I2C_CTRL, 0x10 + len);
-	m5602_write_bridge(sd, M5602_XB_I2C_CTRL, 0x08);
-
-	for (i = 0; i < len; i++) {
-		err = m5602_read_bridge(sd, M5602_XB_I2C_DATA, &(i2c_data[i]));
-
-		PDEBUG(D_CONF, "Reading sensor register "
-				"0x%x containing 0x%x ", address, *i2c_data);
-	}
-	return (err < 0) ? err : 0;
-}
-
-int po1030_write_sensor(struct sd *sd, const u8 address,
-			u8 *i2c_data, const u8 len)
-{
-	int err, i;
-	u8 *p;
-	struct usb_device *udev = sd->gspca_dev.dev;
-	__u8 *buf = sd->gspca_dev.usb_buf;
-
-	/* The po1030 only supports one byte writes */
-	if (len > 1 || !len)
-		return -EINVAL;
-
-	memcpy(buf, sensor_urb_skeleton, sizeof(sensor_urb_skeleton));
-
-	buf[11] = sd->sensor->i2c_slave_id;
-	buf[15] = address;
-
-	p = buf + 16;
-
-	/* Copy a four byte write sequence for each byte to be written to */
-	for (i = 0; i < len; i++) {
-		memcpy(p, sensor_urb_skeleton + 16, 4);
-		p[3] = i2c_data[i];
-		p += 4;
-		PDEBUG(D_CONF, "Writing sensor register 0x%x with 0x%x",
-		       address, i2c_data[i]);
-	}
-
-	/* Copy the footer */
-	memcpy(p, sensor_urb_skeleton + 20, 4);
-
-	/* Set the total length */
-	p[3] = 0x10 + len;
-
-	err = usb_control_msg(udev, usb_sndctrlpipe(udev, 0),
-			      0x04, 0x40, 0x19,
-			      0x0000, buf,
-			      20 + len * 4, M5602_URB_MSG_TIMEOUT);
-
-	return (err < 0) ? err : 0;
-}
-
 int po1030_init(struct sd *sd)
 {
 	int i, err = 0;
 
 	/* Init the sensor */
-	for (i = 0; i < ARRAY_SIZE(init_po1030); i++) {
+	for (i = 0; i < ARRAY_SIZE(init_po1030) && !err; i++) {
 		u8 data[2] = {0x00, 0x00};
 
 		switch (init_po1030[i][0]) {
@@ -147,16 +83,10 @@ int po1030_init(struct sd *sd)
 
 		case SENSOR:
 			data[0] = init_po1030[i][2];
-			err = po1030_write_sensor(sd,
+			err = m5602_write_sensor(sd,
 				init_po1030[i][1], data, 1);
 			break;
 
-		case SENSOR_LONG:
-			data[0] = init_po1030[i][2];
-			data[1] = init_po1030[i][3];
-			err = po1030_write_sensor(sd,
-				init_po1030[i][1], data, 2);
-			break;
 		default:
 			info("Invalid stream command, exiting init");
 			return -EINVAL;
@@ -166,7 +96,7 @@ int po1030_init(struct sd *sd)
 	if (dump_sensor)
 		po1030_dump_registers(sd);
 
-	return (err < 0) ? err : 0;
+	return err;
 }
 
 int po1030_get_exposure(struct gspca_dev *gspca_dev, __s32 *val)
@@ -175,19 +105,19 @@ int po1030_get_exposure(struct gspca_dev *gspca_dev, __s32 *val)
 	u8 i2c_data;
 	int err;
 
-	err = po1030_read_sensor(sd, PO1030_REG_INTEGLINES_H,
+	err = m5602_read_sensor(sd, PO1030_REG_INTEGLINES_H,
 				 &i2c_data, 1);
 	if (err < 0)
 		goto out;
 	*val = (i2c_data << 8);
 
-	err = po1030_read_sensor(sd, PO1030_REG_INTEGLINES_M,
+	err = m5602_read_sensor(sd, PO1030_REG_INTEGLINES_M,
 				 &i2c_data, 1);
 	*val |= i2c_data;
 
 	PDEBUG(D_V4L2, "Exposure read as %d", *val);
 out:
-	return (err < 0) ? err : 0;
+	return err;
 }
 
 int po1030_set_exposure(struct gspca_dev *gspca_dev, __s32 val)
@@ -202,7 +132,7 @@ int po1030_set_exposure(struct gspca_dev *gspca_dev, __s32 val)
 	PDEBUG(D_V4L2, "Set exposure to high byte to 0x%x",
 	       i2c_data);
 
-	err = po1030_write_sensor(sd, PO1030_REG_INTEGLINES_H,
+	err = m5602_write_sensor(sd, PO1030_REG_INTEGLINES_H,
 				  &i2c_data, 1);
 	if (err < 0)
 		goto out;
@@ -210,11 +140,11 @@ int po1030_set_exposure(struct gspca_dev *gspca_dev, __s32 val)
 	i2c_data = (val & 0xff);
 	PDEBUG(D_V4L2, "Set exposure to low byte to 0x%x",
 	       i2c_data);
-	err = po1030_write_sensor(sd, PO1030_REG_INTEGLINES_M,
+	err = m5602_write_sensor(sd, PO1030_REG_INTEGLINES_M,
 				  &i2c_data, 1);
 
 out:
-	return (err < 0) ? err : 0;
+	return err;
 }
 
 int po1030_get_gain(struct gspca_dev *gspca_dev, __s32 *val)
@@ -223,12 +153,12 @@ int po1030_get_gain(struct gspca_dev *gspca_dev, __s32 *val)
 	u8 i2c_data;
 	int err;
 
-	err = po1030_read_sensor(sd, PO1030_REG_GLOBALGAIN,
+	err = m5602_read_sensor(sd, PO1030_REG_GLOBALGAIN,
 				 &i2c_data, 1);
 	*val = i2c_data;
 	PDEBUG(D_V4L2, "Read global gain %d", *val);
 
-	return (err < 0) ? err : 0;
+	return err;
 }
 
 int po1030_get_hflip(struct gspca_dev *gspca_dev, __s32 *val)
@@ -237,14 +167,14 @@ int po1030_get_hflip(struct gspca_dev *gspca_dev, __s32 *val)
 	u8 i2c_data;
 	int err;
 
-	err = po1030_read_sensor(sd, PO1030_REG_CONTROL2,
+	err = m5602_read_sensor(sd, PO1030_REG_CONTROL2,
 				 &i2c_data, 1);
 
 	*val = (i2c_data >> 7) & 0x01 ;
 
 	PDEBUG(D_V4L2, "Read hflip %d", *val);
 
-	return (err < 0) ? err : 0;
+	return err;
 }
 
 int po1030_set_hflip(struct gspca_dev *gspca_dev, __s32 val)
@@ -254,13 +184,17 @@ int po1030_set_hflip(struct gspca_dev *gspca_dev, __s32 val)
 	int err;
 
 	PDEBUG(D_V4L2, "Set hflip %d", val);
+	err = m5602_read_sensor(sd, PO1030_REG_CONTROL2, &i2c_data, 1);
+	if (err < 0)
+		goto out;
 
-	i2c_data = (val & 0x01) << 7;
+	i2c_data = (0x7f & i2c_data) | ((val & 0x01) << 7);
 
-	err = po1030_write_sensor(sd, PO1030_REG_CONTROL2,
-				  &i2c_data, 1);
+	err = m5602_write_sensor(sd, PO1030_REG_CONTROL2,
+				 &i2c_data, 1);
 
-	return (err < 0) ? err : 0;
+out:
+	return err;
 }
 
 int po1030_get_vflip(struct gspca_dev *gspca_dev, __s32 *val)
@@ -269,14 +203,14 @@ int po1030_get_vflip(struct gspca_dev *gspca_dev, __s32 *val)
 	u8 i2c_data;
 	int err;
 
-	err = po1030_read_sensor(sd, PO1030_REG_GLOBALGAIN,
+	err = m5602_read_sensor(sd, PO1030_REG_GLOBALGAIN,
 				 &i2c_data, 1);
 
 	*val = (i2c_data >> 6) & 0x01;
 
 	PDEBUG(D_V4L2, "Read vflip %d", *val);
 
-	return (err < 0) ? err : 0;
+	return err;
 }
 
 int po1030_set_vflip(struct gspca_dev *gspca_dev, __s32 val)
@@ -286,13 +220,17 @@ int po1030_set_vflip(struct gspca_dev *gspca_dev, __s32 val)
 	int err;
 
 	PDEBUG(D_V4L2, "Set vflip %d", val);
+	err = m5602_read_sensor(sd, PO1030_REG_CONTROL2, &i2c_data, 1);
+	if (err < 0)
+		goto out;
 
-	i2c_data = (val & 0x01) << 6;
+	i2c_data = (i2c_data & 0xbf) | ((val & 0x01) << 6);
 
-	err = po1030_write_sensor(sd, PO1030_REG_CONTROL2,
-				  &i2c_data, 1);
+	err = m5602_write_sensor(sd, PO1030_REG_CONTROL2,
+				 &i2c_data, 1);
 
-	return (err < 0) ? err : 0;
+out:
+	return err;
 }
 
 int po1030_set_gain(struct gspca_dev *gspca_dev, __s32 val)
@@ -303,9 +241,9 @@ int po1030_set_gain(struct gspca_dev *gspca_dev, __s32 val)
 
 	i2c_data = val & 0xff;
 	PDEBUG(D_V4L2, "Set global gain to %d", i2c_data);
-	err = po1030_write_sensor(sd, PO1030_REG_GLOBALGAIN,
+	err = m5602_write_sensor(sd, PO1030_REG_GLOBALGAIN,
 				  &i2c_data, 1);
-	return (err < 0) ? err : 0;
+	return err;
 }
 
 int po1030_get_red_balance(struct gspca_dev *gspca_dev, __s32 *val)
@@ -314,11 +252,11 @@ int po1030_get_red_balance(struct gspca_dev *gspca_dev, __s32 *val)
 	u8 i2c_data;
 	int err;
 
-	err = po1030_read_sensor(sd, PO1030_REG_RED_GAIN,
+	err = m5602_read_sensor(sd, PO1030_REG_RED_GAIN,
 				 &i2c_data, 1);
 	*val = i2c_data;
 	PDEBUG(D_V4L2, "Read red gain %d", *val);
-	return (err < 0) ? err : 0;
+	return err;
 }
 
 int po1030_set_red_balance(struct gspca_dev *gspca_dev, __s32 val)
@@ -329,9 +267,9 @@ int po1030_set_red_balance(struct gspca_dev *gspca_dev, __s32 val)
 
 	i2c_data = val & 0xff;
 	PDEBUG(D_V4L2, "Set red gain to %d", i2c_data);
-	err = po1030_write_sensor(sd, PO1030_REG_RED_GAIN,
+	err = m5602_write_sensor(sd, PO1030_REG_RED_GAIN,
 				  &i2c_data, 1);
-	return (err < 0) ? err : 0;
+	return err;
 }
 
 int po1030_get_blue_balance(struct gspca_dev *gspca_dev, __s32 *val)
@@ -340,12 +278,12 @@ int po1030_get_blue_balance(struct gspca_dev *gspca_dev, __s32 *val)
 	u8 i2c_data;
 	int err;
 
-	err = po1030_read_sensor(sd, PO1030_REG_BLUE_GAIN,
+	err = m5602_read_sensor(sd, PO1030_REG_BLUE_GAIN,
 				 &i2c_data, 1);
 	*val = i2c_data;
 	PDEBUG(D_V4L2, "Read blue gain %d", *val);
 
-	return (err < 0) ? err : 0;
+	return err;
 }
 
 int po1030_set_blue_balance(struct gspca_dev *gspca_dev, __s32 val)
@@ -355,10 +293,10 @@ int po1030_set_blue_balance(struct gspca_dev *gspca_dev, __s32 val)
 	int err;
 	i2c_data = val & 0xff;
 	PDEBUG(D_V4L2, "Set blue gain to %d", i2c_data);
-	err = po1030_write_sensor(sd, PO1030_REG_BLUE_GAIN,
+	err = m5602_write_sensor(sd, PO1030_REG_BLUE_GAIN,
 				  &i2c_data, 1);
 
-	return (err < 0) ? err : 0;
+	return err;
 }
 
 int po1030_power_down(struct sd *sd)
@@ -366,14 +304,14 @@ int po1030_power_down(struct sd *sd)
 	return 0;
 }
 
-void po1030_dump_registers(struct sd *sd)
+static void po1030_dump_registers(struct sd *sd)
 {
 	int address;
 	u8 value = 0;
 
 	info("Dumping the po1030 sensor core registers");
 	for (address = 0; address < 0x7f; address++) {
-		po1030_read_sensor(sd, address, &value, 1);
+		m5602_read_sensor(sd, address, &value, 1);
 		info("register 0x%x contains 0x%x",
 		     address, value);
 	}
@@ -385,9 +323,9 @@ void po1030_dump_registers(struct sd *sd)
 		u8 old_value, ctrl_value;
 		u8 test_value[2] = {0xff, 0xff};
 
-		po1030_read_sensor(sd, address, &old_value, 1);
-		po1030_write_sensor(sd, address, test_value, 1);
-		po1030_read_sensor(sd, address, &ctrl_value, 1);
+		m5602_read_sensor(sd, address, &old_value, 1);
+		m5602_write_sensor(sd, address, test_value, 1);
+		m5602_read_sensor(sd, address, &ctrl_value, 1);
 
 		if (ctrl_value == test_value[0])
 			info("register 0x%x is writeable", address);
@@ -395,6 +333,6 @@ void po1030_dump_registers(struct sd *sd)
 			info("register 0x%x is read only", address);
 
 		/* Restore original value */
-		po1030_write_sensor(sd, address, &old_value, 1);
+		m5602_write_sensor(sd, address, &old_value, 1);
 	}
 }
diff --git a/linux/drivers/media/video/gspca/m5602/m5602_po1030.h b/linux/drivers/media/video/gspca/m5602/m5602_po1030.h
index a0b75ff61..def39d5bc 100644
--- a/linux/drivers/media/video/gspca/m5602/m5602_po1030.h
+++ b/linux/drivers/media/video/gspca/m5602/m5602_po1030.h
@@ -10,7 +10,7 @@
  * v4l2 interface modeled after the V4L2 driver
  * for SN9C10x PC Camera Controllers
  *
- * Register defines taken from Pascal Stangs Proxycon Armlib
+ * Register defines taken from Pascal Stangs Procyon Armlib
  *
  * This program is free software; you can redistribute it and/or
  * modify it under the terms of the GNU General Public License as
@@ -128,13 +128,6 @@ int po1030_probe(struct sd *sd);
 int po1030_init(struct sd *sd);
 int po1030_power_down(struct sd *sd);
 
-void po1030_dump_registers(struct sd *sd);
-
-int po1030_read_sensor(struct sd *sd, const u8 address,
-			      u8 *i2c_data, const u8 len);
-int po1030_write_sensor(struct sd *sd, const u8 address,
-			       u8 *i2c_data, const u8 len);
-
 int po1030_get_exposure(struct gspca_dev *gspca_dev, __s32 *val);
 int po1030_set_exposure(struct gspca_dev *gspca_dev, __s32 val);
 int po1030_get_gain(struct gspca_dev *gspca_dev, __s32 *val);
@@ -152,6 +145,7 @@ static struct m5602_sensor po1030 = {
 	.name = "PO1030",
 
 	.i2c_slave_id = 0xdc,
+	.i2c_regW = 1,
 
 	.probe = po1030_probe,
 	.init = po1030_init,
diff --git a/linux/drivers/media/video/gspca/m5602/m5602_s5k4aa.c b/linux/drivers/media/video/gspca/m5602/m5602_s5k4aa.c
index 1f72e7eae..2bd7d4d99 100644
--- a/linux/drivers/media/video/gspca/m5602/m5602_s5k4aa.c
+++ b/linux/drivers/media/video/gspca/m5602/m5602_s5k4aa.c
@@ -52,6 +52,7 @@ static
 	{ }
 };
 
+static void s5k4aa_dump_registers(struct sd *sd);
 
 int s5k4aa_probe(struct sd *sd)
 {
@@ -84,7 +85,7 @@ int s5k4aa_probe(struct sd *sd)
 
 		case SENSOR:
 			data[0] = preinit_s5k4aa[i][2];
-			err = s5k4aa_write_sensor(sd,
+			err = m5602_write_sensor(sd,
 						  preinit_s5k4aa[i][1],
 						  data, 1);
 			break;
@@ -92,7 +93,7 @@ int s5k4aa_probe(struct sd *sd)
 		case SENSOR_LONG:
 			data[0] = preinit_s5k4aa[i][2];
 			data[1] = preinit_s5k4aa[i][3];
-			err = s5k4aa_write_sensor(sd,
+			err = m5602_write_sensor(sd,
 						  preinit_s5k4aa[i][1],
 						  data, 2);
 			break;
@@ -103,13 +104,14 @@ int s5k4aa_probe(struct sd *sd)
 	}
 
 	/* Test some registers, but we don't know their exact meaning yet */
-	if (s5k4aa_read_sensor(sd, 0x00, prod_id, sizeof(prod_id)))
+	if (m5602_read_sensor(sd, 0x00, prod_id, sizeof(prod_id)))
 		return -ENODEV;
 
 	if (memcmp(prod_id, expected_prod_id, sizeof(prod_id)))
 		return -ENODEV;
 	else
 		info("Detected a s5k4aa sensor");
+
 sensor_found:
 	sd->gspca_dev.cam.cam_mode = s5k4aa.modes;
 	sd->gspca_dev.cam.nmodes = s5k4aa.nmodes;
@@ -119,90 +121,6 @@ sensor_found:
 	return 0;
 }
 
-int s5k4aa_read_sensor(struct sd *sd, const u8 address,
-		       u8 *i2c_data, const u8 len)
-{
-	int err, i;
-
-	do {
-		err = m5602_read_bridge(sd, M5602_XB_I2C_STATUS, i2c_data);
-	} while ((*i2c_data & I2C_BUSY) && !err);
-	if (err < 0)
-		goto out;
-
-	err = m5602_write_bridge(sd, M5602_XB_I2C_DEV_ADDR,
-				 sd->sensor->i2c_slave_id);
-	if (err < 0)
-		goto out;
-
-	err = m5602_write_bridge(sd, M5602_XB_I2C_REG_ADDR, address);
-	if (err < 0)
-		goto out;
-
-	err = m5602_write_bridge(sd, M5602_XB_I2C_CTRL, 0x18 + len);
-	if (err < 0)
-		goto out;
-
-	do {
-		err = m5602_read_bridge(sd, M5602_XB_I2C_STATUS, i2c_data);
-	} while ((*i2c_data & I2C_BUSY) && !err);
-	if (err < 0)
-		goto out;
-
-	for (i = 0; (i < len) & !err; i++) {
-		err = m5602_read_bridge(sd, M5602_XB_I2C_DATA, &(i2c_data[i]));
-
-		PDEBUG(D_CONF, "Reading sensor register "
-				  "0x%x containing 0x%x ", address, *i2c_data);
-	}
-out:
-	return (err < 0) ? err : 0;
-}
-
-int s5k4aa_write_sensor(struct sd *sd, const u8 address,
-			u8 *i2c_data, const u8 len)
-{
-	int err, i;
-	u8 *p;
-	struct usb_device *udev = sd->gspca_dev.dev;
-	__u8 *buf = sd->gspca_dev.usb_buf;
-
-	/* No sensor with a data width larger than 16 bits has yet been seen */
-	if (len > 2 || !len)
-		return -EINVAL;
-
-	memcpy(buf, sensor_urb_skeleton,
-	       sizeof(sensor_urb_skeleton));
-
-	buf[11] = sd->sensor->i2c_slave_id;
-	buf[15] = address;
-
-	/* Special case larger sensor writes */
-	p = buf + 16;
-
-	/* Copy a four byte write sequence for each byte to be written to */
-	for (i = 0; i < len; i++) {
-		memcpy(p, sensor_urb_skeleton + 16, 4);
-		p[3] = i2c_data[i];
-		p += 4;
-		PDEBUG(D_CONF, "Writing sensor register 0x%x with 0x%x",
-		       address, i2c_data[i]);
-	}
-
-	/* Copy the tailer */
-	memcpy(p, sensor_urb_skeleton + 20, 4);
-
-	/* Set the total length */
-	p[3] = 0x10 + len;
-
-	err = usb_control_msg(udev, usb_sndctrlpipe(udev, 0),
-			      0x04, 0x40, 0x19,
-			      0x0000, buf,
-			      20 + len * 4, M5602_URB_MSG_TIMEOUT);
-
-	return (err < 0) ? err : 0;
-}
-
 int s5k4aa_init(struct sd *sd)
 {
 	int i, err = 0;
@@ -219,14 +137,14 @@ int s5k4aa_init(struct sd *sd)
 
 		case SENSOR:
 			data[0] = init_s5k4aa[i][2];
-			err = s5k4aa_write_sensor(sd,
+			err = m5602_write_sensor(sd,
 				init_s5k4aa[i][1], data, 1);
 			break;
 
 		case SENSOR_LONG:
 			data[0] = init_s5k4aa[i][2];
 			data[1] = init_s5k4aa[i][3];
-			err = s5k4aa_write_sensor(sd,
+			err = m5602_write_sensor(sd,
 				init_s5k4aa[i][1], data, 2);
 			break;
 		default:
@@ -241,21 +159,21 @@ int s5k4aa_init(struct sd *sd)
 	if (!err && dmi_check_system(s5k4aa_vflip_dmi_table)) {
 		u8 data = 0x02;
 		info("vertical flip quirk active");
-		s5k4aa_write_sensor(sd, S5K4AA_PAGE_MAP, &data, 1);
-		s5k4aa_read_sensor(sd, S5K4AA_READ_MODE, &data, 1);
+		m5602_write_sensor(sd, S5K4AA_PAGE_MAP, &data, 1);
+		m5602_read_sensor(sd, S5K4AA_READ_MODE, &data, 1);
 		data |= S5K4AA_RM_V_FLIP;
 		data &= ~S5K4AA_RM_H_FLIP;
-		s5k4aa_write_sensor(sd, S5K4AA_READ_MODE, &data, 1);
+		m5602_write_sensor(sd, S5K4AA_READ_MODE, &data, 1);
 
 		/* Decrement COLSTART to preserve color order (BGGR) */
-		s5k4aa_read_sensor(sd, S5K4AA_COLSTART_LO, &data, 1);
+		m5602_read_sensor(sd, S5K4AA_COLSTART_LO, &data, 1);
 		data--;
-		s5k4aa_write_sensor(sd, S5K4AA_COLSTART_LO, &data, 1);
+		m5602_write_sensor(sd, S5K4AA_COLSTART_LO, &data, 1);
 
 		/* Increment ROWSTART to preserve color order (BGGR) */
-		s5k4aa_read_sensor(sd, S5K4AA_ROWSTART_LO, &data, 1);
+		m5602_read_sensor(sd, S5K4AA_ROWSTART_LO, &data, 1);
 		data++;
-		s5k4aa_write_sensor(sd, S5K4AA_ROWSTART_LO, &data, 1);
+		m5602_write_sensor(sd, S5K4AA_ROWSTART_LO, &data, 1);
 	}
 
 	return (err < 0) ? err : 0;
@@ -272,20 +190,20 @@ int s5k4aa_get_exposure(struct gspca_dev *gspca_dev, __s32 *val)
 	u8 data = S5K4AA_PAGE_MAP_2;
 	int err;
 
-	err = s5k4aa_write_sensor(sd, S5K4AA_PAGE_MAP, &data, 1);
+	err = m5602_write_sensor(sd, S5K4AA_PAGE_MAP, &data, 1);
 	if (err < 0)
 		goto out;
 
-	err = s5k4aa_read_sensor(sd, S5K4AA_EXPOSURE_HI, &data, 1);
+	err = m5602_read_sensor(sd, S5K4AA_EXPOSURE_HI, &data, 1);
 	if (err < 0)
 		goto out;
 
 	*val = data << 8;
-	err = s5k4aa_read_sensor(sd, S5K4AA_EXPOSURE_LO, &data, 1);
+	err = m5602_read_sensor(sd, S5K4AA_EXPOSURE_LO, &data, 1);
 	*val |= data;
 	PDEBUG(D_V4L2, "Read exposure %d", *val);
 out:
-	return (err < 0) ? err : 0;
+	return err;
 }
 
 int s5k4aa_set_exposure(struct gspca_dev *gspca_dev, __s32 val)
@@ -295,17 +213,17 @@ int s5k4aa_set_exposure(struct gspca_dev *gspca_dev, __s32 val)
 	int err;
 
 	PDEBUG(D_V4L2, "Set exposure to %d", val);
-	err = s5k4aa_write_sensor(sd, S5K4AA_PAGE_MAP, &data, 1);
+	err = m5602_write_sensor(sd, S5K4AA_PAGE_MAP, &data, 1);
 	if (err < 0)
 		goto out;
 	data = (val >> 8) & 0xff;
-	err = s5k4aa_write_sensor(sd, S5K4AA_EXPOSURE_HI, &data, 1);
+	err = m5602_write_sensor(sd, S5K4AA_EXPOSURE_HI, &data, 1);
 	if (err < 0)
 		goto out;
 	data = val & 0xff;
-	err = s5k4aa_write_sensor(sd, S5K4AA_EXPOSURE_LO, &data, 1);
+	err = m5602_write_sensor(sd, S5K4AA_EXPOSURE_LO, &data, 1);
 out:
-	return (err < 0) ? err : 0;
+	return err;
 }
 
 int s5k4aa_get_vflip(struct gspca_dev *gspca_dev, __s32 *val)
@@ -314,16 +232,16 @@ int s5k4aa_get_vflip(struct gspca_dev *gspca_dev, __s32 *val)
 	u8 data = S5K4AA_PAGE_MAP_2;
 	int err;
 
-	err = s5k4aa_write_sensor(sd, S5K4AA_PAGE_MAP, &data, 1);
+	err = m5602_write_sensor(sd, S5K4AA_PAGE_MAP, &data, 1);
 	if (err < 0)
 		goto out;
 
-	err = s5k4aa_read_sensor(sd, S5K4AA_PAGE_MAP, &data, 1);
+	err = m5602_read_sensor(sd, S5K4AA_PAGE_MAP, &data, 1);
 	*val = (data & S5K4AA_RM_V_FLIP) >> 7;
 	PDEBUG(D_V4L2, "Read vertical flip %d", *val);
 
 out:
-	return (err < 0) ? err : 0;
+	return err;
 }
 
 int s5k4aa_set_vflip(struct gspca_dev *gspca_dev, __s32 val)
@@ -333,35 +251,35 @@ int s5k4aa_set_vflip(struct gspca_dev *gspca_dev, __s32 val)
 	int err;
 
 	PDEBUG(D_V4L2, "Set vertical flip to %d", val);
-	err = s5k4aa_write_sensor(sd, S5K4AA_PAGE_MAP, &data, 1);
+	err = m5602_write_sensor(sd, S5K4AA_PAGE_MAP, &data, 1);
 	if (err < 0)
 		goto out;
-	err = s5k4aa_write_sensor(sd, S5K4AA_READ_MODE, &data, 1);
+	err = m5602_write_sensor(sd, S5K4AA_READ_MODE, &data, 1);
 	if (err < 0)
 		goto out;
 	data = ((data & ~S5K4AA_RM_V_FLIP)
 			| ((val & 0x01) << 7));
-	err = s5k4aa_write_sensor(sd, S5K4AA_READ_MODE, &data, 1);
+	err = m5602_write_sensor(sd, S5K4AA_READ_MODE, &data, 1);
 	if (err < 0)
 		goto out;
 
 	if (val) {
-		err = s5k4aa_read_sensor(sd, S5K4AA_ROWSTART_LO, &data, 1);
+		err = m5602_read_sensor(sd, S5K4AA_ROWSTART_LO, &data, 1);
 		if (err < 0)
 			goto out;
 
 		data++;
-		err = s5k4aa_write_sensor(sd, S5K4AA_ROWSTART_LO, &data, 1);
+		err = m5602_write_sensor(sd, S5K4AA_ROWSTART_LO, &data, 1);
 	} else {
-		err = s5k4aa_read_sensor(sd, S5K4AA_ROWSTART_LO, &data, 1);
+		err = m5602_read_sensor(sd, S5K4AA_ROWSTART_LO, &data, 1);
 		if (err < 0)
 			goto out;
 
 		data--;
-		err = s5k4aa_write_sensor(sd, S5K4AA_ROWSTART_LO, &data, 1);
+		err = m5602_write_sensor(sd, S5K4AA_ROWSTART_LO, &data, 1);
 	}
 out:
-	return (err < 0) ? err : 0;
+	return err;
 }
 
 int s5k4aa_get_hflip(struct gspca_dev *gspca_dev, __s32 *val)
@@ -370,15 +288,15 @@ int s5k4aa_get_hflip(struct gspca_dev *gspca_dev, __s32 *val)
 	u8 data = S5K4AA_PAGE_MAP_2;
 	int err;
 
-	err = s5k4aa_write_sensor(sd, S5K4AA_PAGE_MAP, &data, 1);
+	err = m5602_write_sensor(sd, S5K4AA_PAGE_MAP, &data, 1);
 	if (err < 0)
 		goto out;
 
-	err = s5k4aa_read_sensor(sd, S5K4AA_PAGE_MAP, &data, 1);
+	err = m5602_read_sensor(sd, S5K4AA_PAGE_MAP, &data, 1);
 	*val = (data & S5K4AA_RM_H_FLIP) >> 6;
 	PDEBUG(D_V4L2, "Read horizontal flip %d", *val);
 out:
-	return (err < 0) ? err : 0;
+	return err;
 }
 
 int s5k4aa_set_hflip(struct gspca_dev *gspca_dev, __s32 val)
@@ -389,35 +307,35 @@ int s5k4aa_set_hflip(struct gspca_dev *gspca_dev, __s32 val)
 
 	PDEBUG(D_V4L2, "Set horizontal flip to %d",
 	       val);
-	err = s5k4aa_write_sensor(sd, S5K4AA_PAGE_MAP, &data, 1);
+	err = m5602_write_sensor(sd, S5K4AA_PAGE_MAP, &data, 1);
 	if (err < 0)
 		goto out;
-	err = s5k4aa_write_sensor(sd, S5K4AA_READ_MODE, &data, 1);
+	err = m5602_write_sensor(sd, S5K4AA_READ_MODE, &data, 1);
 	if (err < 0)
 		goto out;
 
 	data = ((data & ~S5K4AA_RM_H_FLIP) | ((val & 0x01) << 6));
-	err = s5k4aa_write_sensor(sd, S5K4AA_READ_MODE, &data, 1);
+	err = m5602_write_sensor(sd, S5K4AA_READ_MODE, &data, 1);
 	if (err < 0)
 		goto out;
 
 	if (val) {
-		err = s5k4aa_read_sensor(sd, S5K4AA_COLSTART_LO, &data, 1);
+		err = m5602_read_sensor(sd, S5K4AA_COLSTART_LO, &data, 1);
 		if (err < 0)
 			goto out;
 		data++;
-		err = s5k4aa_write_sensor(sd, S5K4AA_COLSTART_LO, &data, 1);
+		err = m5602_write_sensor(sd, S5K4AA_COLSTART_LO, &data, 1);
 		if (err < 0)
 			goto out;
 	} else {
-		err = s5k4aa_read_sensor(sd, S5K4AA_COLSTART_LO, &data, 1);
+		err = m5602_read_sensor(sd, S5K4AA_COLSTART_LO, &data, 1);
 		if (err < 0)
 			goto out;
 		data--;
-		err = s5k4aa_write_sensor(sd, S5K4AA_COLSTART_LO, &data, 1);
+		err = m5602_write_sensor(sd, S5K4AA_COLSTART_LO, &data, 1);
 	}
 out:
-	return (err < 0) ? err : 0;
+	return err;
 }
 
 int s5k4aa_get_gain(struct gspca_dev *gspca_dev, __s32 *val)
@@ -426,16 +344,16 @@ int s5k4aa_get_gain(struct gspca_dev *gspca_dev, __s32 *val)
 	u8 data = S5K4AA_PAGE_MAP_2;
 	int err;
 
-	err = s5k4aa_write_sensor(sd, S5K4AA_PAGE_MAP, &data, 1);
+	err = m5602_write_sensor(sd, S5K4AA_PAGE_MAP, &data, 1);
 	if (err < 0)
 		goto out;
 
-	err = s5k4aa_read_sensor(sd, S5K4AA_GAIN_2, &data, 1);
+	err = m5602_read_sensor(sd, S5K4AA_GAIN_2, &data, 1);
 	*val = data;
 	PDEBUG(D_V4L2, "Read gain %d", *val);
 
 out:
-	return (err < 0) ? err : 0;
+	return err;
 }
 
 int s5k4aa_set_gain(struct gspca_dev *gspca_dev, __s32 val)
@@ -445,28 +363,28 @@ int s5k4aa_set_gain(struct gspca_dev *gspca_dev, __s32 val)
 	int err;
 
 	PDEBUG(D_V4L2, "Set gain to %d", val);
-	err = s5k4aa_write_sensor(sd, S5K4AA_PAGE_MAP, &data, 1);
+	err = m5602_write_sensor(sd, S5K4AA_PAGE_MAP, &data, 1);
 	if (err < 0)
 		goto out;
 
 	data = val & 0xff;
-	err = s5k4aa_write_sensor(sd, S5K4AA_GAIN_2, &data, 1);
+	err = m5602_write_sensor(sd, S5K4AA_GAIN_2, &data, 1);
 
 out:
-	return (err < 0) ? err : 0;
+	return err;
 }
 
-void s5k4aa_dump_registers(struct sd *sd)
+static void s5k4aa_dump_registers(struct sd *sd)
 {
 	int address;
 	u8 page, old_page;
-	s5k4aa_read_sensor(sd, S5K4AA_PAGE_MAP, &old_page, 1);
+	m5602_read_sensor(sd, S5K4AA_PAGE_MAP, &old_page, 1);
 	for (page = 0; page < 16; page++) {
-		s5k4aa_write_sensor(sd, S5K4AA_PAGE_MAP, &page, 1);
+		m5602_write_sensor(sd, S5K4AA_PAGE_MAP, &page, 1);
 		info("Dumping the s5k4aa register state for page 0x%x", page);
 		for (address = 0; address <= 0xff; address++) {
 			u8 value = 0;
-			s5k4aa_read_sensor(sd, address, &value, 1);
+			m5602_read_sensor(sd, address, &value, 1);
 			info("register 0x%x contains 0x%x",
 			     address, value);
 		}
@@ -474,15 +392,15 @@ void s5k4aa_dump_registers(struct sd *sd)
 	info("s5k4aa register state dump complete");
 
 	for (page = 0; page < 16; page++) {
-		s5k4aa_write_sensor(sd, S5K4AA_PAGE_MAP, &page, 1);
+		m5602_write_sensor(sd, S5K4AA_PAGE_MAP, &page, 1);
 		info("Probing for which registers that are "
 		     "read/write for page 0x%x", page);
 		for (address = 0; address <= 0xff; address++) {
 			u8 old_value, ctrl_value, test_value = 0xff;
 
-			s5k4aa_read_sensor(sd, address, &old_value, 1);
-			s5k4aa_write_sensor(sd, address, &test_value, 1);
-			s5k4aa_read_sensor(sd, address, &ctrl_value, 1);
+			m5602_read_sensor(sd, address, &old_value, 1);
+			m5602_write_sensor(sd, address, &test_value, 1);
+			m5602_read_sensor(sd, address, &ctrl_value, 1);
 
 			if (ctrl_value == test_value)
 				info("register 0x%x is writeable", address);
@@ -490,9 +408,9 @@ void s5k4aa_dump_registers(struct sd *sd)
 				info("register 0x%x is read only", address);
 
 			/* Restore original value */
-			s5k4aa_write_sensor(sd, address, &old_value, 1);
+			m5602_write_sensor(sd, address, &old_value, 1);
 		}
 	}
 	info("Read/write register probing complete");
-	s5k4aa_write_sensor(sd, S5K4AA_PAGE_MAP, &old_page, 1);
+	m5602_write_sensor(sd, S5K4AA_PAGE_MAP, &old_page, 1);
 }
diff --git a/linux/drivers/media/video/gspca/m5602/m5602_s5k4aa.h b/linux/drivers/media/video/gspca/m5602/m5602_s5k4aa.h
index 151c6f530..1f88b0d04 100644
--- a/linux/drivers/media/video/gspca/m5602/m5602_s5k4aa.h
+++ b/linux/drivers/media/video/gspca/m5602/m5602_s5k4aa.h
@@ -41,11 +41,10 @@
 #define S5K4AA_WINDOW_HEIGHT_LO		0x09
 #define S5K4AA_WINDOW_WIDTH_HI		0x0a
 #define S5K4AA_WINDOW_WIDTH_LO		0x0b
-#define S5K4AA_GLOBAL_GAIN__		0x0f /* Only a guess ATM !!! */
-#define S5K4AA_H_BLANK_HI__		0x1d /* Only a guess ATM !!! sync lost
-						if too low, reduces frame rate
-						if too high */
-#define S5K4AA_H_BLANK_LO__		0x1e /* Only a guess ATM !!! */
+#define S5K4AA_GLOBAL_GAIN__		0x0f
+/* sync lost, if too low, reduces frame rate if too high */
+#define S5K4AA_H_BLANK_HI__		0x1d
+#define S5K4AA_H_BLANK_LO__		0x1e
 #define S5K4AA_EXPOSURE_HI		0x17
 #define S5K4AA_EXPOSURE_LO		0x18
 #define S5K4AA_GAIN_1			0x1f /* (digital?) gain : 5 bits */
@@ -68,13 +67,6 @@ int s5k4aa_probe(struct sd *sd);
 int s5k4aa_init(struct sd *sd);
 int s5k4aa_power_down(struct sd *sd);
 
-void s5k4aa_dump_registers(struct sd *sd);
-
-int s5k4aa_read_sensor(struct sd *sd, const u8 address,
-		       u8 *i2c_data, const u8 len);
-int s5k4aa_write_sensor(struct sd *sd, const u8 address,
-			u8 *i2c_data, const u8 len);
-
 int s5k4aa_get_exposure(struct gspca_dev *gspca_dev, __s32 *val);
 int s5k4aa_set_exposure(struct gspca_dev *gspca_dev, __s32 val);
 int s5k4aa_get_vflip(struct gspca_dev *gspca_dev, __s32 *val);
@@ -89,9 +81,8 @@ static struct m5602_sensor s5k4aa = {
 	.probe = s5k4aa_probe,
 	.init = s5k4aa_init,
 	.power_down = s5k4aa_power_down,
-	.read_sensor = s5k4aa_read_sensor,
-	.write_sensor = s5k4aa_write_sensor,
 	.i2c_slave_id = 0x5a,
+	.i2c_regW = 2,
 	.nctrls = 4,
 	.ctrls = {
 	{
diff --git a/linux/drivers/media/video/gspca/m5602/m5602_s5k83a.c b/linux/drivers/media/video/gspca/m5602/m5602_s5k83a.c
index 8988a728e..af3f2dc2c 100644
--- a/linux/drivers/media/video/gspca/m5602/m5602_s5k83a.c
+++ b/linux/drivers/media/video/gspca/m5602/m5602_s5k83a.c
@@ -18,6 +18,8 @@
 
 #include "m5602_s5k83a.h"
 
+static void s5k83a_dump_registers(struct sd *sd);
+
 int s5k83a_probe(struct sd *sd)
 {
 	u8 prod_id = 0, ver_id = 0;
@@ -39,7 +41,7 @@ int s5k83a_probe(struct sd *sd)
 	for (i = 0; i < ARRAY_SIZE(preinit_s5k83a) && !err; i++) {
 		u8 data[2] = {preinit_s5k83a[i][2], preinit_s5k83a[i][3]};
 		if (preinit_s5k83a[i][0] == SENSOR)
-			err = s5k83a_write_sensor(sd, preinit_s5k83a[i][1],
+			err = m5602_write_sensor(sd, preinit_s5k83a[i][1],
 				data, 2);
 		else
 			err = m5602_write_bridge(sd, preinit_s5k83a[i][1],
@@ -49,10 +51,10 @@ int s5k83a_probe(struct sd *sd)
 	/* We don't know what register (if any) that contain the product id
 	 * Just pick the first addresses that seem to produce the same results
 	 * on multiple machines */
-	if (s5k83a_read_sensor(sd, 0x00, &prod_id, 1))
+	if (m5602_read_sensor(sd, 0x00, &prod_id, 1))
 		return -ENODEV;
 
-	if (s5k83a_read_sensor(sd, 0x01, &ver_id, 1))
+	if (m5602_read_sensor(sd, 0x01, &ver_id, 1))
 		return -ENODEV;
 
 	if ((prod_id == 0xff) || (ver_id == 0xff))
@@ -68,91 +70,6 @@ sensor_found:
 	return 0;
 }
 
-int s5k83a_read_sensor(struct sd *sd, const u8 address,
-			      u8 *i2c_data, const u8 len)
-{
-	int err, i;
-
-	do {
-		err = m5602_read_bridge(sd, M5602_XB_I2C_STATUS, i2c_data);
-	} while ((*i2c_data & I2C_BUSY) && !err);
-	if (err < 0)
-		goto out;
-
-	err = m5602_write_bridge(sd, M5602_XB_I2C_DEV_ADDR,
-				 sd->sensor->i2c_slave_id);
-	if (err < 0)
-		goto out;
-
-	err = m5602_write_bridge(sd, M5602_XB_I2C_REG_ADDR, address);
-	if (err < 0)
-		goto out;
-
-	err = m5602_write_bridge(sd, M5602_XB_I2C_CTRL, 0x18 + len);
-	if (err < 0)
-		goto out;
-
-	do {
-		err = m5602_read_bridge(sd, M5602_XB_I2C_STATUS, i2c_data);
-	} while ((*i2c_data & I2C_BUSY) && !err);
-
-	if (err < 0)
-		goto out;
-	for (i = 0; i < len && !len; i++) {
-		err = m5602_read_bridge(sd, M5602_XB_I2C_DATA, &(i2c_data[i]));
-
-		PDEBUG(D_CONF, "Reading sensor register "
-				  "0x%x containing 0x%x ", address, *i2c_data);
-	}
-
-out:
-	return (err < 0) ? err : 0;
-}
-
-int s5k83a_write_sensor(struct sd *sd, const u8 address,
-			       u8 *i2c_data, const u8 len)
-{
-	int err, i;
-	u8 *p;
-	struct usb_device *udev = sd->gspca_dev.dev;
-	__u8 *buf = sd->gspca_dev.usb_buf;
-
-	/* No sensor with a data width larger than 16 bits has yet been seen */
-	if (len > 2 || !len)
-		return -EINVAL;
-
-	memcpy(buf, sensor_urb_skeleton,
-	       sizeof(sensor_urb_skeleton));
-
-	buf[11] = sd->sensor->i2c_slave_id;
-	buf[15] = address;
-
-	/* Special case larger sensor writes */
-	p = buf + 16;
-
-	/* Copy a four byte write sequence for each byte to be written to */
-	for (i = 0; i < len; i++) {
-		memcpy(p, sensor_urb_skeleton + 16, 4);
-		p[3] = i2c_data[i];
-		p += 4;
-		PDEBUG(D_CONF, "Writing sensor register 0x%x with 0x%x",
-		       address, i2c_data[i]);
-	}
-
-	/* Copy the tailer */
-	memcpy(p, sensor_urb_skeleton + 20, 4);
-
-	/* Set the total length */
-	p[3] = 0x10 + len;
-
-	err = usb_control_msg(udev, usb_sndctrlpipe(udev, 0),
-			      0x04, 0x40, 0x19,
-			      0x0000, buf,
-			      20 + len * 4, M5602_URB_MSG_TIMEOUT);
-
-	return (err < 0) ? err : 0;
-}
-
 int s5k83a_init(struct sd *sd)
 {
 	int i, err = 0;
@@ -169,14 +86,14 @@ int s5k83a_init(struct sd *sd)
 
 		case SENSOR:
 			data[0] = init_s5k83a[i][2];
-			err = s5k83a_write_sensor(sd,
+			err = m5602_write_sensor(sd,
 				init_s5k83a[i][1], data, 1);
 			break;
 
 		case SENSOR_LONG:
 			data[0] = init_s5k83a[i][2];
 			data[1] = init_s5k83a[i][3];
-			err = s5k83a_write_sensor(sd,
+			err = m5602_write_sensor(sd,
 				init_s5k83a[i][1], data, 2);
 			break;
 		default:
@@ -200,14 +117,14 @@ void s5k83a_dump_registers(struct sd *sd)
 {
 	int address;
 	u8 page, old_page;
-	s5k83a_read_sensor(sd, S5K83A_PAGE_MAP, &old_page, 1);
+	m5602_read_sensor(sd, S5K83A_PAGE_MAP, &old_page, 1);
 
 	for (page = 0; page < 16; page++) {
-		s5k83a_write_sensor(sd, S5K83A_PAGE_MAP, &page, 1);
+		m5602_write_sensor(sd, S5K83A_PAGE_MAP, &page, 1);
 		info("Dumping the s5k83a register state for page 0x%x", page);
 		for (address = 0; address <= 0xff; address++) {
 			u8 val = 0;
-			s5k83a_read_sensor(sd, address, &val, 1);
+			m5602_read_sensor(sd, address, &val, 1);
 			info("register 0x%x contains 0x%x",
 			     address, val);
 		}
@@ -215,15 +132,15 @@ void s5k83a_dump_registers(struct sd *sd)
 	info("s5k83a register state dump complete");
 
 	for (page = 0; page < 16; page++) {
-		s5k83a_write_sensor(sd, S5K83A_PAGE_MAP, &page, 1);
+		m5602_write_sensor(sd, S5K83A_PAGE_MAP, &page, 1);
 		info("Probing for which registers that are read/write "
 		      "for page 0x%x", page);
 		for (address = 0; address <= 0xff; address++) {
 			u8 old_val, ctrl_val, test_val = 0xff;
 
-			s5k83a_read_sensor(sd, address, &old_val, 1);
-			s5k83a_write_sensor(sd, address, &test_val, 1);
-			s5k83a_read_sensor(sd, address, &ctrl_val, 1);
+			m5602_read_sensor(sd, address, &old_val, 1);
+			m5602_write_sensor(sd, address, &test_val, 1);
+			m5602_read_sensor(sd, address, &ctrl_val, 1);
 
 			if (ctrl_val == test_val)
 				info("register 0x%x is writeable", address);
@@ -231,11 +148,11 @@ void s5k83a_dump_registers(struct sd *sd)
 				info("register 0x%x is read only", address);
 
 			/* Restore original val */
-			s5k83a_write_sensor(sd, address, &old_val, 1);
+			m5602_write_sensor(sd, address, &old_val, 1);
 		}
 	}
 	info("Read/write register probing complete");
-	s5k83a_write_sensor(sd, S5K83A_PAGE_MAP, &old_page, 1);
+	m5602_write_sensor(sd, S5K83A_PAGE_MAP, &old_page, 1);
 }
 
 int s5k83a_get_brightness(struct gspca_dev *gspca_dev, __s32 *val)
@@ -244,11 +161,15 @@ int s5k83a_get_brightness(struct gspca_dev *gspca_dev, __s32 *val)
 	u8 data[2];
 	struct sd *sd = (struct sd *) gspca_dev;
 
-	err = s5k83a_read_sensor(sd, S5K83A_BRIGHTNESS, data, 2);
+	err = m5602_read_sensor(sd, S5K83A_BRIGHTNESS, data, 2);
+	if (err < 0)
+		goto out;
+
 	data[1] = data[1] << 1;
 	*val = data[1];
 
-	return (err < 0) ? err : 0;
+out:
+	return err;
 }
 
 int s5k83a_set_brightness(struct gspca_dev *gspca_dev, __s32 val)
@@ -259,23 +180,24 @@ int s5k83a_set_brightness(struct gspca_dev *gspca_dev, __s32 val)
 
 	data[0] = 0x00;
 	data[1] = 0x20;
-	err = s5k83a_write_sensor(sd, 0x14, data, 2);
+	err = m5602_write_sensor(sd, 0x14, data, 2);
 	if (err < 0)
-		return err;
+		goto out;
 
 	data[0] = 0x01;
 	data[1] = 0x00;
-	err = s5k83a_write_sensor(sd, 0x0d, data, 2);
+	err = m5602_write_sensor(sd, 0x0d, data, 2);
 	if (err < 0)
-		return err;
+		goto out;
 
 	/* FIXME: This is not sane, we need to figure out the composition
 		  of these registers */
 	data[0] = val >> 3; /* brightness, high 5 bits */
 	data[1] = val >> 1; /* brightness, high 7 bits */
-	err = s5k83a_write_sensor(sd, S5K83A_BRIGHTNESS, data, 2);
+	err = m5602_write_sensor(sd, S5K83A_BRIGHTNESS, data, 2);
 
-	return (err < 0) ? err : 0;
+out:
+	return err;
 }
 
 int s5k83a_get_whiteness(struct gspca_dev *gspca_dev, __s32 *val)
@@ -284,10 +206,14 @@ int s5k83a_get_whiteness(struct gspca_dev *gspca_dev, __s32 *val)
 	u8 data;
 	struct sd *sd = (struct sd *) gspca_dev;
 
-	err = s5k83a_read_sensor(sd, S5K83A_WHITENESS, &data, 1);
+	err = m5602_read_sensor(sd, S5K83A_WHITENESS, &data, 1);
+	if (err < 0)
+		goto out;
 
 	*val = data;
-	return (err < 0) ? err : 0;
+
+out:
+	return err;
 }
 
 int s5k83a_set_whiteness(struct gspca_dev *gspca_dev, __s32 val)
@@ -297,9 +223,9 @@ int s5k83a_set_whiteness(struct gspca_dev *gspca_dev, __s32 val)
 	struct sd *sd = (struct sd *) gspca_dev;
 
 	data[0] = val;
-	err = s5k83a_write_sensor(sd, S5K83A_WHITENESS, data, 1);
+	err = m5602_write_sensor(sd, S5K83A_WHITENESS, data, 1);
 
-	return (err < 0) ? err : 0;
+	return err;
 }
 
 int s5k83a_get_gain(struct gspca_dev *gspca_dev, __s32 *val)
@@ -308,7 +234,9 @@ int s5k83a_get_gain(struct gspca_dev *gspca_dev, __s32 *val)
 	u8 data[2];
 	struct sd *sd = (struct sd *) gspca_dev;
 
-	err = s5k83a_read_sensor(sd, S5K83A_GAIN, data, 2);
+	err = m5602_read_sensor(sd, S5K83A_GAIN, data, 2);
+	if (err < 0)
+		goto out;
 
 	data[1] = data[1] & 0x3f;
 	if (data[1] > S5K83A_MAXIMUM_GAIN)
@@ -316,7 +244,8 @@ int s5k83a_get_gain(struct gspca_dev *gspca_dev, __s32 *val)
 
 	*val = data[1];
 
-	return (err < 0) ? err : 0;
+out:
+	return err;
 }
 
 int s5k83a_set_gain(struct gspca_dev *gspca_dev, __s32 val)
@@ -327,9 +256,8 @@ int s5k83a_set_gain(struct gspca_dev *gspca_dev, __s32 val)
 
 	data[0] = 0;
 	data[1] = val;
-	err = s5k83a_write_sensor(sd, S5K83A_GAIN, data, 2);
-
-	return (err < 0) ? err : 0;
+	err = m5602_write_sensor(sd, S5K83A_GAIN, data, 2);
+	return err;
 }
 
 int s5k83a_get_vflip(struct gspca_dev *gspca_dev, __s32 *val)
@@ -339,14 +267,15 @@ int s5k83a_get_vflip(struct gspca_dev *gspca_dev, __s32 *val)
 	struct sd *sd = (struct sd *) gspca_dev;
 
 	data[0] = 0x05;
-	err = s5k83a_write_sensor(sd, S5K83A_PAGE_MAP, data, 1);
+	err = m5602_write_sensor(sd, S5K83A_PAGE_MAP, data, 1);
 	if (err < 0)
-		return err;
+		goto out;
 
-	err = s5k83a_read_sensor(sd, S5K83A_FLIP, data, 1);
+	err = m5602_read_sensor(sd, S5K83A_FLIP, data, 1);
 	*val = (data[0] | 0x40) ? 1 : 0;
 
-	return (err < 0) ? err : 0;
+out:
+	return err;
 }
 
 int s5k83a_set_vflip(struct gspca_dev *gspca_dev, __s32 val)
@@ -356,25 +285,26 @@ int s5k83a_set_vflip(struct gspca_dev *gspca_dev, __s32 val)
 	struct sd *sd = (struct sd *) gspca_dev;
 
 	data[0] = 0x05;
-	err = s5k83a_write_sensor(sd, S5K83A_PAGE_MAP, data, 1);
+	err = m5602_write_sensor(sd, S5K83A_PAGE_MAP, data, 1);
 	if (err < 0)
-		return err;
+		goto out;
 
-	err = s5k83a_read_sensor(sd, S5K83A_FLIP, data, 1);
+	err = m5602_read_sensor(sd, S5K83A_FLIP, data, 1);
 	if (err < 0)
-		return err;
+		goto out;
 
 	/* set or zero six bit, seven is hflip */
 	data[0] = (val) ? (data[0] & 0x80) | 0x40 | S5K83A_FLIP_MASK
 			: (data[0] & 0x80) | S5K83A_FLIP_MASK;
-	err = s5k83a_write_sensor(sd, S5K83A_FLIP, data, 1);
+	err = m5602_write_sensor(sd, S5K83A_FLIP, data, 1);
 	if (err < 0)
-		return err;
+		goto out;
 
 	data[0] = (val) ? 0x0b : 0x0a;
-	err = s5k83a_write_sensor(sd, S5K83A_VFLIP_TUNE, data, 1);
+	err = m5602_write_sensor(sd, S5K83A_VFLIP_TUNE, data, 1);
 
-	return (err < 0) ? err : 0;
+out:
+	return err;
 }
 
 int s5k83a_get_hflip(struct gspca_dev *gspca_dev, __s32 *val)
@@ -384,14 +314,15 @@ int s5k83a_get_hflip(struct gspca_dev *gspca_dev, __s32 *val)
 	struct sd *sd = (struct sd *) gspca_dev;
 
 	data[0] = 0x05;
-	err = s5k83a_write_sensor(sd, S5K83A_PAGE_MAP, data, 1);
+	err = m5602_write_sensor(sd, S5K83A_PAGE_MAP, data, 1);
 	if (err < 0)
-		return err;
+		goto out;
 
-	err = s5k83a_read_sensor(sd, S5K83A_FLIP, data, 1);
+	err = m5602_read_sensor(sd, S5K83A_FLIP, data, 1);
 	*val = (data[0] | 0x80) ? 1 : 0;
 
-	return (err < 0) ? err : 0;
+out:
+	return err;
 }
 
 int s5k83a_set_hflip(struct gspca_dev *gspca_dev, __s32 val)
@@ -401,23 +332,23 @@ int s5k83a_set_hflip(struct gspca_dev *gspca_dev, __s32 val)
 	struct sd *sd = (struct sd *) gspca_dev;
 
 	data[0] = 0x05;
-	err = s5k83a_write_sensor(sd, S5K83A_PAGE_MAP, data, 1);
+	err = m5602_write_sensor(sd, S5K83A_PAGE_MAP, data, 1);
 	if (err < 0)
-		return err;
+		goto out;
 
-	err = s5k83a_read_sensor(sd, S5K83A_FLIP, data, 1);
+	err = m5602_read_sensor(sd, S5K83A_FLIP, data, 1);
 	if (err < 0)
-		return err;
+		goto out;
 
 	/* set or zero seven bit, six is vflip */
 	data[0] = (val) ? (data[0] & 0x40) | 0x80 | S5K83A_FLIP_MASK
 			: (data[0] & 0x40) | S5K83A_FLIP_MASK;
-	err = s5k83a_write_sensor(sd, S5K83A_FLIP, data, 1);
+	err = m5602_write_sensor(sd, S5K83A_FLIP, data, 1);
 	if (err < 0)
-		return err;
+		goto out;
 
 	data[0] = (val) ? 0x0a : 0x0b;
-	err = s5k83a_write_sensor(sd, S5K83A_HFLIP_TUNE, data, 1);
-
-	return (err < 0) ? err : 0;
+	err = m5602_write_sensor(sd, S5K83A_HFLIP_TUNE, data, 1);
+out:
+	return err;
 }
diff --git a/linux/drivers/media/video/gspca/m5602/m5602_s5k83a.h b/linux/drivers/media/video/gspca/m5602/m5602_s5k83a.h
index ee3ee9cfc..05ccb5b57 100644
--- a/linux/drivers/media/video/gspca/m5602/m5602_s5k83a.h
+++ b/linux/drivers/media/video/gspca/m5602/m5602_s5k83a.h
@@ -22,15 +22,15 @@
 #include "m5602_sensor.h"
 
 #define S5K83A_FLIP				0x01
-#define S5K83A_HFLIP_TUNE		0x03
-#define S5K83A_VFLIP_TUNE		0x05
-#define S5K83A_WHITENESS		0x0a
+#define S5K83A_HFLIP_TUNE			0x03
+#define S5K83A_VFLIP_TUNE			0x05
+#define S5K83A_WHITENESS			0x0a
 #define S5K83A_GAIN				0x18
-#define S5K83A_BRIGHTNESS		0x1b
-#define S5K83A_PAGE_MAP			0xec
+#define S5K83A_BRIGHTNESS			0x1b
+#define S5K83A_PAGE_MAP				0xec
 
-#define S5K83A_DEFAULT_BRIGHTNESS	0x71
-#define S5K83A_DEFAULT_WHITENESS	0x7e
+#define S5K83A_DEFAULT_BRIGHTNESS		0x71
+#define S5K83A_DEFAULT_WHITENESS		0x7e
 #define S5K83A_DEFAULT_GAIN			0x00
 #define S5K83A_MAXIMUM_GAIN			0x3c
 #define S5K83A_FLIP_MASK			0x10
@@ -46,13 +46,6 @@ int s5k83a_probe(struct sd *sd);
 int s5k83a_init(struct sd *sd);
 int s5k83a_power_down(struct sd *sd);
 
-void s5k83a_dump_registers(struct sd *sd);
-
-int s5k83a_read_sensor(struct sd *sd, const u8 address,
-		       u8 *i2c_data, const u8 len);
-int s5k83a_write_sensor(struct sd *sd, const u8 address,
-			u8 *i2c_data, const u8 len);
-
 int s5k83a_set_brightness(struct gspca_dev *gspca_dev, __s32 val);
 int s5k83a_get_brightness(struct gspca_dev *gspca_dev, __s32 *val);
 int s5k83a_set_whiteness(struct gspca_dev *gspca_dev, __s32 val);
@@ -64,15 +57,13 @@ int s5k83a_set_vflip(struct gspca_dev *gspca_dev, __s32 val);
 int s5k83a_get_hflip(struct gspca_dev *gspca_dev, __s32 *val);
 int s5k83a_set_hflip(struct gspca_dev *gspca_dev, __s32 val);
 
-
 static struct m5602_sensor s5k83a = {
 	.name = "S5K83A",
 	.probe = s5k83a_probe,
 	.init = s5k83a_init,
 	.power_down = s5k83a_power_down,
-	.read_sensor = s5k83a_read_sensor,
-	.write_sensor = s5k83a_write_sensor,
 	.i2c_slave_id = 0x5a,
+	.i2c_regW = 2,
 	.nctrls = 5,
 	.ctrls = {
 	{
diff --git a/linux/drivers/media/video/gspca/m5602/m5602_sensor.h b/linux/drivers/media/video/gspca/m5602/m5602_sensor.h
index 60c9a48e0..261623f0d 100644
--- a/linux/drivers/media/video/gspca/m5602/m5602_sensor.h
+++ b/linux/drivers/media/video/gspca/m5602/m5602_sensor.h
@@ -49,23 +49,21 @@ struct m5602_sensor {
 	/* What i2c address the sensor is connected to */
 	u8 i2c_slave_id;
 
+	/* Width of each i2c register (in bytes) */
+	u8 i2c_regW;
+
 	/* Probes if the sensor is connected */
 	int (*probe)(struct sd *sd);
 
 	/* Performs a initialization sequence */
 	int (*init)(struct sd *sd);
 
+	/* Executed when the camera starts to send data */
+	int (*start)(struct sd *sd);
+
 	/* Performs a power down sequence */
 	int (*power_down)(struct sd *sd);
 
-	/* Reads a sensor register */
-	int (*read_sensor)(struct sd *sd, const u8 address,
-	      u8 *i2c_data, const u8 len);
-
-	/* Writes to a sensor register */
-	int (*write_sensor)(struct sd *sd, const u8 address,
-	      u8 *i2c_data, const u8 len);
-
 	int nctrls;
 	struct ctrl ctrls[M5602_MAX_CTRLS];
 
diff --git a/linux/drivers/media/video/gspca/mars.c b/linux/drivers/media/video/gspca/mars.c
index 492cdd3b5..3d2090e67 100644
--- a/linux/drivers/media/video/gspca/mars.c
+++ b/linux/drivers/media/video/gspca/mars.c
@@ -39,7 +39,7 @@ struct sd {
 static struct ctrl sd_ctrls[] = {
 };
 
-static struct v4l2_pix_format vga_mode[] = {
+static const struct v4l2_pix_format vga_mode[] = {
 	{320, 240, V4L2_PIX_FMT_JPEG, V4L2_FIELD_NONE,
 		.bytesperline = 320,
 		.sizeimage = 320 * 240 * 3 / 8 + 589,
diff --git a/linux/drivers/media/video/gspca/ov519.c b/linux/drivers/media/video/gspca/ov519.c
index 5fb519da2..5e31a9145 100644
--- a/linux/drivers/media/video/gspca/ov519.c
+++ b/linux/drivers/media/video/gspca/ov519.c
@@ -164,7 +164,7 @@ static struct ctrl sd_ctrls[] = {
 	},
 };
 
-static struct v4l2_pix_format vga_mode[] = {
+static const struct v4l2_pix_format vga_mode[] = {
 	{320, 240, V4L2_PIX_FMT_JPEG, V4L2_FIELD_NONE,
 		.bytesperline = 320,
 		.sizeimage = 320 * 240 * 3 / 8 + 590,
@@ -176,7 +176,7 @@ static struct v4l2_pix_format vga_mode[] = {
 		.colorspace = V4L2_COLORSPACE_JPEG,
 		.priv = 0},
 };
-static struct v4l2_pix_format sif_mode[] = {
+static const struct v4l2_pix_format sif_mode[] = {
 	{176, 144, V4L2_PIX_FMT_JPEG, V4L2_FIELD_NONE,
 		.bytesperline = 176,
 		.sizeimage = 176 * 144 * 3 / 8 + 590,
diff --git a/linux/drivers/media/video/gspca/ov534.c b/linux/drivers/media/video/gspca/ov534.c
index 7332947cb..3bf15e401 100644
--- a/linux/drivers/media/video/gspca/ov534.c
+++ b/linux/drivers/media/video/gspca/ov534.c
@@ -55,7 +55,7 @@ struct sd {
 static struct ctrl sd_ctrls[] = {
 };
 
-static struct v4l2_pix_format vga_mode[] = {
+static const struct v4l2_pix_format vga_mode[] = {
 	{640, 480, V4L2_PIX_FMT_YUYV, V4L2_FIELD_NONE,
 	 .bytesperline = 640 * 2,
 	 .sizeimage = 640 * 480 * 2,
@@ -63,65 +63,66 @@ static struct v4l2_pix_format vga_mode[] = {
 	 .priv = 0},
 };
 
-static void ov534_reg_write(struct usb_device *udev, u16 reg, u8 val)
+static void ov534_reg_write(struct gspca_dev *gspca_dev, u16 reg, u8 val)
 {
-	u8 data = val;
+	struct usb_device *udev = gspca_dev->dev;
 	int ret;
 
 	PDEBUG(D_USBO, "reg=0x%04x, val=0%02x", reg, val);
+	gspca_dev->usb_buf[0] = val;
 	ret = usb_control_msg(udev,
 			      usb_sndctrlpipe(udev, 0),
 			      0x1,
 			      USB_DIR_OUT | USB_TYPE_VENDOR | USB_RECIP_DEVICE,
-			      0x0, reg, &data, 1, CTRL_TIMEOUT);
+			      0x0, reg, gspca_dev->usb_buf, 1, CTRL_TIMEOUT);
 	if (ret < 0)
 		PDEBUG(D_ERR, "write failed");
 }
 
-static u8 ov534_reg_read(struct usb_device *udev, u16 reg)
+static u8 ov534_reg_read(struct gspca_dev *gspca_dev, u16 reg)
 {
-	u8 data;
+	struct usb_device *udev = gspca_dev->dev;
 	int ret;
 
 	ret = usb_control_msg(udev,
 			      usb_rcvctrlpipe(udev, 0),
 			      0x1,
 			      USB_DIR_IN | USB_TYPE_VENDOR | USB_RECIP_DEVICE,
-			      0x0, reg, &data, 1, CTRL_TIMEOUT);
-	PDEBUG(D_USBI, "reg=0x%04x, data=0x%02x", reg, data);
+			      0x0, reg, gspca_dev->usb_buf, 1, CTRL_TIMEOUT);
+	PDEBUG(D_USBI, "reg=0x%04x, data=0x%02x", reg, gspca_dev->usb_buf[0]);
 	if (ret < 0)
 		PDEBUG(D_ERR, "read failed");
-	return data;
+	return gspca_dev->usb_buf[0];
 }
 
 /* Two bits control LED: 0x21 bit 7 and 0x23 bit 7.
  * (direction and output)? */
-static void ov534_set_led(struct usb_device *udev, int status)
+static void ov534_set_led(struct gspca_dev *gspca_dev, int status)
 {
 	u8 data;
 
 	PDEBUG(D_CONF, "led status: %d", status);
 
-	data = ov534_reg_read(udev, 0x21);
+	data = ov534_reg_read(gspca_dev, 0x21);
 	data |= 0x80;
-	ov534_reg_write(udev, 0x21, data);
+	ov534_reg_write(gspca_dev, 0x21, data);
 
-	data = ov534_reg_read(udev, 0x23);
+	data = ov534_reg_read(gspca_dev, 0x23);
 	if (status)
 		data |= 0x80;
 	else
 		data &= ~(0x80);
 
-	ov534_reg_write(udev, 0x23, data);
+	ov534_reg_write(gspca_dev, 0x23, data);
 }
 
-static int sccb_check_status(struct usb_device *udev)
+static int sccb_check_status(struct gspca_dev *gspca_dev)
 {
 	u8 data;
 	int i;
 
 	for (i = 0; i < 5; i++) {
-		data = ov534_reg_read(udev, OV534_REG_STATUS);
+		data = ov534_reg_read(gspca_dev, OV534_REG_STATUS);
 
 		switch (data) {
 		case 0x00:
@@ -131,37 +132,37 @@ static int sccb_check_status(struct usb_device *udev)
 		case 0x03:
 			break;
 		default:
-			PDEBUG(D_ERR, "sccb status 0x%02x, attempt %d/5\n",
+			PDEBUG(D_ERR, "sccb status 0x%02x, attempt %d/5",
 			       data, i + 1);
 		}
 	}
 	return 0;
 }
 
-static void sccb_reg_write(struct usb_device *udev, u16 reg, u8 val)
+static void sccb_reg_write(struct gspca_dev *gspca_dev, u16 reg, u8 val)
 {
 	PDEBUG(D_USBO, "reg: 0x%04x, val: 0x%02x", reg, val);
-	ov534_reg_write(udev, OV534_REG_SUBADDR, reg);
-	ov534_reg_write(udev, OV534_REG_WRITE, val);
-	ov534_reg_write(udev, OV534_REG_OPERATION, OV534_OP_WRITE_3);
+	ov534_reg_write(gspca_dev, OV534_REG_SUBADDR, reg);
+	ov534_reg_write(gspca_dev, OV534_REG_WRITE, val);
+	ov534_reg_write(gspca_dev, OV534_REG_OPERATION, OV534_OP_WRITE_3);
 
-	if (!sccb_check_status(udev))
+	if (!sccb_check_status(gspca_dev))
 		PDEBUG(D_ERR, "sccb_reg_write failed");
 }
 
 #ifdef GSPCA_DEBUG
-static u8 sccb_reg_read(struct usb_device *udev, u16 reg)
+static u8 sccb_reg_read(struct gspca_dev *gspca_dev, u16 reg)
 {
-	ov534_reg_write(udev, OV534_REG_SUBADDR, reg);
-	ov534_reg_write(udev, OV534_REG_OPERATION, OV534_OP_WRITE_2);
-	if (!sccb_check_status(udev))
+	ov534_reg_write(gspca_dev, OV534_REG_SUBADDR, reg);
+	ov534_reg_write(gspca_dev, OV534_REG_OPERATION, OV534_OP_WRITE_2);
+	if (!sccb_check_status(gspca_dev))
 		PDEBUG(D_ERR, "sccb_reg_read failed 1");
 
-	ov534_reg_write(udev, OV534_REG_OPERATION, OV534_OP_READ_2);
-	if (!sccb_check_status(udev))
+	ov534_reg_write(gspca_dev, OV534_REG_OPERATION, OV534_OP_READ_2);
+	if (!sccb_check_status(gspca_dev))
 		PDEBUG(D_ERR, "sccb_reg_read failed 2");
 
-	return ov534_reg_read(udev, OV534_REG_READ);
+	return ov534_reg_read(gspca_dev, OV534_REG_READ);
 }
 #endif
 
@@ -318,26 +319,26 @@ static void ov534_set_frame_rate(struct gspca_dev *gspca_dev)
 
 	switch (fr) {
 	case 50:
-		sccb_reg_write(gspca_dev->dev, 0x11, 0x01);
-		sccb_reg_write(gspca_dev->dev, 0x0d, 0x41);
-		ov534_reg_write(gspca_dev->dev, 0xe5, 0x02);
+		sccb_reg_write(gspca_dev, 0x11, 0x01);
+		sccb_reg_write(gspca_dev, 0x0d, 0x41);
+		ov534_reg_write(gspca_dev, 0xe5, 0x02);
 		break;
 	case 40:
-		sccb_reg_write(gspca_dev->dev, 0x11, 0x02);
-		sccb_reg_write(gspca_dev->dev, 0x0d, 0xc1);
-		ov534_reg_write(gspca_dev->dev, 0xe5, 0x04);
+		sccb_reg_write(gspca_dev, 0x11, 0x02);
+		sccb_reg_write(gspca_dev, 0x0d, 0xc1);
+		ov534_reg_write(gspca_dev, 0xe5, 0x04);
 		break;
 /*	case 30: */
 	default:
 		fr = 30;
-		sccb_reg_write(gspca_dev->dev, 0x11, 0x04);
-		sccb_reg_write(gspca_dev->dev, 0x0d, 0x81);
-		ov534_reg_write(gspca_dev->dev, 0xe5, 0x02);
+		sccb_reg_write(gspca_dev, 0x11, 0x04);
+		sccb_reg_write(gspca_dev, 0x0d, 0x81);
+		ov534_reg_write(gspca_dev, 0xe5, 0x02);
 		break;
 	case 15:
-		sccb_reg_write(gspca_dev->dev, 0x11, 0x03);
-		sccb_reg_write(gspca_dev->dev, 0x0d, 0x41);
-		ov534_reg_write(gspca_dev->dev, 0xe5, 0x04);
+		sccb_reg_write(gspca_dev, 0x11, 0x03);
+		sccb_reg_write(gspca_dev, 0x0d, 0x41);
+		ov534_reg_write(gspca_dev, 0xe5, 0x04);
 		break;
 	}
 
@@ -346,27 +347,28 @@ static void ov534_set_frame_rate(struct gspca_dev *gspca_dev)
 }
 
 /* setup method */
-static void ov534_setup(struct usb_device *udev)
+static void ov534_setup(struct gspca_dev *gspca_dev)
 {
 	int i;
 
 	/* Initialize bridge chip */
 	for (i = 0; i < ARRAY_SIZE(ov534_reg_initdata); i++)
-		ov534_reg_write(udev, ov534_reg_initdata[i][0],
+		ov534_reg_write(gspca_dev, ov534_reg_initdata[i][0],
 				ov534_reg_initdata[i][1]);
 
 	PDEBUG(D_PROBE, "sensor is ov%02x%02x",
-		sccb_reg_read(udev, 0x0a), sccb_reg_read(udev, 0x0b));
+		sccb_reg_read(gspca_dev, 0x0a),
+		sccb_reg_read(gspca_dev, 0x0b));
 
-	ov534_set_led(udev, 1);
+	ov534_set_led(gspca_dev, 1);
 
 	/* Initialize sensor */
 	for (i = 0; i < ARRAY_SIZE(ov772x_reg_initdata); i++)
-		sccb_reg_write(udev, ov772x_reg_initdata[i][0],
+		sccb_reg_write(gspca_dev, ov772x_reg_initdata[i][0],
 			       ov772x_reg_initdata[i][1]);
 
-	ov534_reg_write(udev, 0xe0, 0x09);
-	ov534_set_led(udev, 0);
+	ov534_reg_write(gspca_dev, 0xe0, 0x09);
+	ov534_set_led(gspca_dev, 0);
 }
 
 /* this function is called at probe time */
@@ -390,7 +392,7 @@ static int sd_config(struct gspca_dev *gspca_dev,
 /* this function is called at probe and resume time */
 static int sd_init(struct gspca_dev *gspca_dev)
 {
-	ov534_setup(gspca_dev->dev);
+	ov534_setup(gspca_dev);
 	ov534_set_frame_rate(gspca_dev);
 
 	return 0;
@@ -399,8 +401,8 @@ static int sd_init(struct gspca_dev *gspca_dev)
 static int sd_start(struct gspca_dev *gspca_dev)
 {
 	/* start streaming data */
-	ov534_set_led(gspca_dev->dev, 1);
-	ov534_reg_write(gspca_dev->dev, 0xe0, 0x00);
+	ov534_set_led(gspca_dev, 1);
+	ov534_reg_write(gspca_dev, 0xe0, 0x00);
 
 	return 0;
 }
@@ -408,8 +410,8 @@ static int sd_start(struct gspca_dev *gspca_dev)
 static void sd_stopN(struct gspca_dev *gspca_dev)
 {
 	/* stop streaming data */
-	ov534_reg_write(gspca_dev->dev, 0xe0, 0x09);
-	ov534_set_led(gspca_dev->dev, 0);
+	ov534_reg_write(gspca_dev, 0xe0, 0x09);
+	ov534_set_led(gspca_dev, 0);
 }
 
 /* Values for bmHeaderInfo (Video and Still Image Payload Headers, 2.4.3.3) */
@@ -499,8 +501,8 @@ discard:
 }
 
 /* get stream parameters (framerate) */
-int sd_get_streamparm(struct gspca_dev *gspca_dev,
-		      struct v4l2_streamparm *parm)
+static int sd_get_streamparm(struct gspca_dev *gspca_dev,
+			     struct v4l2_streamparm *parm)
 {
 	struct v4l2_captureparm *cp = &parm->parm.capture;
 	struct v4l2_fract *tpf = &cp->timeperframe;
@@ -517,8 +519,8 @@ int sd_get_streamparm(struct gspca_dev *gspca_dev,
 }
 
 /* set stream parameters (framerate) */
-int sd_set_streamparm(struct gspca_dev *gspca_dev,
-		      struct v4l2_streamparm *parm)
+static int sd_set_streamparm(struct gspca_dev *gspca_dev,
+			     struct v4l2_streamparm *parm)
 {
 	struct v4l2_captureparm *cp = &parm->parm.capture;
 	struct v4l2_fract *tpf = &cp->timeperframe;
diff --git a/linux/drivers/media/video/gspca/pac207.c b/linux/drivers/media/video/gspca/pac207.c
index 39473e6b9..c90ac852b 100644
--- a/linux/drivers/media/video/gspca/pac207.c
+++ b/linux/drivers/media/video/gspca/pac207.c
@@ -1,7 +1,7 @@
 /*
  * Pixart PAC207BCA library
  *
- * Copyright (C) 2008 Hans de Goede <j.w.r.degoede@hhs.nl>
+ * Copyright (C) 2008 Hans de Goede <hdgoede@redhat.com>
  * Copyright (C) 2005 Thomas Kaiser thomas@kaiser-linux.li
  * Copyleft (C) 2005 Michel Xhaard mxhaard@magic.fr
  *
@@ -27,7 +27,7 @@
 
 #include "gspca.h"
 
-MODULE_AUTHOR("Hans de Goede <j.w.r.degoede@hhs.nl>");
+MODULE_AUTHOR("Hans de Goede <hdgoede@redhat.com>");
 MODULE_DESCRIPTION("Pixart PAC207");
 MODULE_LICENSE("GPL");
 
@@ -149,7 +149,7 @@ static struct ctrl sd_ctrls[] = {
 	},
 };
 
-static struct v4l2_pix_format sif_mode[] = {
+static const struct v4l2_pix_format sif_mode[] = {
 	{176, 144, V4L2_PIX_FMT_PAC207, V4L2_FIELD_NONE,
 		.bytesperline = 176,
 		.sizeimage = (176 + 2) * 144,
@@ -529,6 +529,7 @@ static const struct sd_desc sd_desc = {
 static const __devinitdata struct usb_device_id device_table[] = {
 	{USB_DEVICE(0x041e, 0x4028)},
 	{USB_DEVICE(0x093a, 0x2460)},
+	{USB_DEVICE(0x093a, 0x2461)},
 	{USB_DEVICE(0x093a, 0x2463)},
 	{USB_DEVICE(0x093a, 0x2464)},
 	{USB_DEVICE(0x093a, 0x2468)},
diff --git a/linux/drivers/media/video/gspca/pac7311.c b/linux/drivers/media/video/gspca/pac7311.c
index b0a9d0687..83ab0df0e 100644
--- a/linux/drivers/media/video/gspca/pac7311.c
+++ b/linux/drivers/media/video/gspca/pac7311.c
@@ -226,7 +226,7 @@ static struct ctrl sd_ctrls[] = {
 	},
 };
 
-static struct v4l2_pix_format vga_mode[] = {
+static const struct v4l2_pix_format vga_mode[] = {
 	{160, 120, V4L2_PIX_FMT_PJPG, V4L2_FIELD_NONE,
 		.bytesperline = 160,
 		.sizeimage = 160 * 120 * 3 / 8 + 590,
@@ -1078,11 +1078,13 @@ static __devinitdata struct usb_device_id device_table[] = {
 	{USB_DEVICE(0x093a, 0x2608), .driver_info = SENSOR_PAC7311},
 	{USB_DEVICE(0x093a, 0x260e), .driver_info = SENSOR_PAC7311},
 	{USB_DEVICE(0x093a, 0x260f), .driver_info = SENSOR_PAC7311},
+	{USB_DEVICE(0x093a, 0x2620), .driver_info = SENSOR_PAC7302},
 	{USB_DEVICE(0x093a, 0x2621), .driver_info = SENSOR_PAC7302},
 	{USB_DEVICE(0x093a, 0x2622), .driver_info = SENSOR_PAC7302},
 	{USB_DEVICE(0x093a, 0x2624), .driver_info = SENSOR_PAC7302},
 	{USB_DEVICE(0x093a, 0x2626), .driver_info = SENSOR_PAC7302},
 	{USB_DEVICE(0x093a, 0x262a), .driver_info = SENSOR_PAC7302},
+	{USB_DEVICE(0x093a, 0x262c), .driver_info = SENSOR_PAC7302},
 	{}
 };
 MODULE_DEVICE_TABLE(usb, device_table);
diff --git a/linux/drivers/media/video/gspca/sonixb.c b/linux/drivers/media/video/gspca/sonixb.c
index 5ca8ab780..75b44b362 100644
--- a/linux/drivers/media/video/gspca/sonixb.c
+++ b/linux/drivers/media/video/gspca/sonixb.c
@@ -227,7 +227,7 @@ static struct ctrl sd_ctrls[] = {
 	},
 };
 
-static struct v4l2_pix_format vga_mode[] = {
+static const struct v4l2_pix_format vga_mode[] = {
 	{160, 120, V4L2_PIX_FMT_SBGGR8, V4L2_FIELD_NONE,
 		.bytesperline = 160,
 		.sizeimage = 160 * 120,
@@ -249,7 +249,7 @@ static struct v4l2_pix_format vga_mode[] = {
 		.colorspace = V4L2_COLORSPACE_SRGB,
 		.priv = 0},
 };
-static struct v4l2_pix_format sif_mode[] = {
+static const struct v4l2_pix_format sif_mode[] = {
 	{160, 120, V4L2_PIX_FMT_SBGGR8, V4L2_FIELD_NONE,
 		.bytesperline = 160,
 		.sizeimage = 160 * 120,
diff --git a/linux/drivers/media/video/gspca/sonixj.c b/linux/drivers/media/video/gspca/sonixj.c
index ff94cd639..217db0d88 100644
--- a/linux/drivers/media/video/gspca/sonixj.c
+++ b/linux/drivers/media/video/gspca/sonixj.c
@@ -225,7 +225,7 @@ static __u32 ctrl_dis[] = {
 						/* SENSOR_OV7660 6 */
 };
 
-static struct v4l2_pix_format vga_mode[] = {
+static const struct v4l2_pix_format vga_mode[] = {
 	{160, 120, V4L2_PIX_FMT_JPEG, V4L2_FIELD_NONE,
 		.bytesperline = 160,
 		.sizeimage = 160 * 120 * 4 / 8 + 590,
diff --git a/linux/drivers/media/video/gspca/spca500.c b/linux/drivers/media/video/gspca/spca500.c
index 2e1c5e50e..6b4865280 100644
--- a/linux/drivers/media/video/gspca/spca500.c
+++ b/linux/drivers/media/video/gspca/spca500.c
@@ -111,7 +111,7 @@ static struct ctrl sd_ctrls[] = {
 	},
 };
 
-static struct v4l2_pix_format vga_mode[] = {
+static const struct v4l2_pix_format vga_mode[] = {
 	{320, 240, V4L2_PIX_FMT_JPEG, V4L2_FIELD_NONE,
 		.bytesperline = 320,
 		.sizeimage = 320 * 240 * 3 / 8 + 590,
@@ -124,7 +124,7 @@ static struct v4l2_pix_format vga_mode[] = {
 		.priv = 0},
 };
 
-static struct v4l2_pix_format sif_mode[] = {
+static const struct v4l2_pix_format sif_mode[] = {
 	{176, 144, V4L2_PIX_FMT_JPEG, V4L2_FIELD_NONE,
 		.bytesperline = 176,
 		.sizeimage = 176 * 144 * 3 / 8 + 590,
diff --git a/linux/drivers/media/video/gspca/spca501.c b/linux/drivers/media/video/gspca/spca501.c
index 17dab9859..609459321 100644
--- a/linux/drivers/media/video/gspca/spca501.c
+++ b/linux/drivers/media/video/gspca/spca501.c
@@ -132,7 +132,7 @@ static struct ctrl sd_ctrls[] = {
 	},
 };
 
-static struct v4l2_pix_format vga_mode[] = {
+static const struct v4l2_pix_format vga_mode[] = {
 	{160, 120, V4L2_PIX_FMT_SPCA501, V4L2_FIELD_NONE,
 		.bytesperline = 160,
 		.sizeimage = 160 * 120 * 3 / 2,
diff --git a/linux/drivers/media/video/gspca/spca505.c b/linux/drivers/media/video/gspca/spca505.c
index c52598e94..854ed1ff5 100644
--- a/linux/drivers/media/video/gspca/spca505.c
+++ b/linux/drivers/media/video/gspca/spca505.c
@@ -59,7 +59,7 @@ static struct ctrl sd_ctrls[] = {
 	},
 };
 
-static struct v4l2_pix_format vga_mode[] = {
+static const struct v4l2_pix_format vga_mode[] = {
 	{160, 120, V4L2_PIX_FMT_SPCA505, V4L2_FIELD_NONE,
 		.bytesperline = 160,
 		.sizeimage = 160 * 120 * 3 / 2,
diff --git a/linux/drivers/media/video/gspca/spca506.c b/linux/drivers/media/video/gspca/spca506.c
index 645ee9d44..96e2512e0 100644
--- a/linux/drivers/media/video/gspca/spca506.c
+++ b/linux/drivers/media/video/gspca/spca506.c
@@ -110,7 +110,7 @@ static struct ctrl sd_ctrls[] = {
 	},
 };
 
-static struct v4l2_pix_format vga_mode[] = {
+static const struct v4l2_pix_format vga_mode[] = {
 	{160, 120, V4L2_PIX_FMT_SPCA505, V4L2_FIELD_NONE,
 		.bytesperline = 160,
 		.sizeimage = 160 * 120 * 3 / 2,
diff --git a/linux/drivers/media/video/gspca/spca508.c b/linux/drivers/media/video/gspca/spca508.c
index c9360cdb8..698d4ed95 100644
--- a/linux/drivers/media/video/gspca/spca508.c
+++ b/linux/drivers/media/video/gspca/spca508.c
@@ -62,7 +62,7 @@ static struct ctrl sd_ctrls[] = {
 	},
 };
 
-static struct v4l2_pix_format sif_mode[] = {
+static const struct v4l2_pix_format sif_mode[] = {
 	{160, 120, V4L2_PIX_FMT_SPCA508, V4L2_FIELD_NONE,
 		.bytesperline = 160,
 		.sizeimage = 160 * 120 * 3 / 2,
diff --git a/linux/drivers/media/video/gspca/spca561.c b/linux/drivers/media/video/gspca/spca561.c
index 4c0046cc7..499511fa0 100644
--- a/linux/drivers/media/video/gspca/spca561.c
+++ b/linux/drivers/media/video/gspca/spca561.c
@@ -32,22 +32,22 @@ MODULE_LICENSE("GPL");
 struct sd {
 	struct gspca_dev gspca_dev;	/* !! must be the first item */
 
-	__u16 contrast;			/* rev72a only */
-#define CONTRAST_MIN 0x0000
-#define CONTRAST_DEF 0x2000
-#define CONTRAST_MAX 0x3fff
-
 	__u16 exposure;			/* rev12a only */
 #define EXPOSURE_MIN 1
 #define EXPOSURE_DEF 200
 #define EXPOSURE_MAX (4095 - 900) /* see set_exposure */
 
+	__u8 contrast;			/* rev72a only */
+#define CONTRAST_MIN 0x00
+#define CONTRAST_DEF 0x20
+#define CONTRAST_MAX 0x3f
+
 	__u8 brightness;		/* rev72a only */
 #define BRIGHTNESS_MIN 0
-#define BRIGHTNESS_DEF 32
-#define BRIGHTNESS_MAX 63
+#define BRIGHTNESS_DEF 0x20
+#define BRIGHTNESS_MAX 0x3f
 
-	__u8 white;			/* rev12a only */
+	__u8 white;
 #define WHITE_MIN 1
 #define WHITE_DEF 0x40
 #define WHITE_MAX 0x7f
@@ -73,7 +73,7 @@ struct sd {
 #define AG_CNT_START 13
 };
 
-static struct v4l2_pix_format sif_012a_mode[] = {
+static const struct v4l2_pix_format sif_012a_mode[] = {
 	{160, 120, V4L2_PIX_FMT_SGBRG8, V4L2_FIELD_NONE,
 		.bytesperline = 160,
 		.sizeimage = 160 * 120,
@@ -96,7 +96,7 @@ static struct v4l2_pix_format sif_012a_mode[] = {
 		.priv = 0},
 };
 
-static struct v4l2_pix_format sif_072a_mode[] = {
+static const struct v4l2_pix_format sif_072a_mode[] = {
 	{160, 120, V4L2_PIX_FMT_SGBRG8, V4L2_FIELD_NONE,
 		.bytesperline = 160,
 		.sizeimage = 160 * 120,
@@ -146,98 +146,7 @@ static struct v4l2_pix_format sif_072a_mode[] = {
 #define SPCA561_SNAPBIT 0x20
 #define SPCA561_SNAPCTRL 0x40
 
-static void reg_w_val(struct usb_device *dev, __u16 index, __u8 value)
-{
-	int ret;
-
-	ret = usb_control_msg(dev, usb_sndctrlpipe(dev, 0),
-			      0,		/* request */
-			      USB_DIR_OUT | USB_TYPE_VENDOR | USB_RECIP_DEVICE,
-			      value, index, NULL, 0, 500);
-	PDEBUG(D_USBO, "reg write: 0x%02x:0x%02x", index, value);
-	if (ret < 0)
-		PDEBUG(D_ERR, "reg write: error %d", ret);
-}
-
-static void write_vector(struct gspca_dev *gspca_dev,
-			const __u16 data[][2])
-{
-	struct usb_device *dev = gspca_dev->dev;
-	int i;
-
-	i = 0;
-	while (data[i][1] != 0) {
-		reg_w_val(dev, data[i][1], data[i][0]);
-		i++;
-	}
-}
-
-/* read 'len' bytes to gspca_dev->usb_buf */
-static void reg_r(struct gspca_dev *gspca_dev,
-		  __u16 index, __u16 length)
-{
-	usb_control_msg(gspca_dev->dev,
-			usb_rcvctrlpipe(gspca_dev->dev, 0),
-			0,			/* request */
-			USB_DIR_IN | USB_TYPE_VENDOR | USB_RECIP_DEVICE,
-			0,			/* value */
-			index, gspca_dev->usb_buf, length, 500);
-}
-
-static void reg_w_buf(struct gspca_dev *gspca_dev,
-		      __u16 index, const __u8 *buffer, __u16 len)
-{
-	memcpy(gspca_dev->usb_buf, buffer, len);
-	usb_control_msg(gspca_dev->dev,
-			usb_sndctrlpipe(gspca_dev->dev, 0),
-			0,			/* request */
-			USB_DIR_OUT | USB_TYPE_VENDOR | USB_RECIP_DEVICE,
-			0,			/* value */
-			index, gspca_dev->usb_buf, len, 500);
-}
-
-static void i2c_write(struct gspca_dev *gspca_dev, __u16 valeur, __u16 reg)
-{
-	int retry = 60;
-	__u8 DataLow;
-	__u8 DataHight;
-
-	DataLow = valeur;
-	DataHight = valeur >> 8;
-	reg_w_val(gspca_dev->dev, 0x8801, reg);
-	reg_w_val(gspca_dev->dev, 0x8805, DataLow);
-	reg_w_val(gspca_dev->dev, 0x8800, DataHight);
-	while (retry--) {
-		reg_r(gspca_dev, 0x8803, 1);
-		if (!gspca_dev->usb_buf[0])
-			break;
-	}
-}
-
-static int i2c_read(struct gspca_dev *gspca_dev, __u16 reg, __u8 mode)
-{
-	int retry = 60;
-	__u8 value;
-	__u8 vallsb;
-
-	reg_w_val(gspca_dev->dev, 0x8804, 0x92);
-	reg_w_val(gspca_dev->dev, 0x8801, reg);
-	reg_w_val(gspca_dev->dev, 0x8802, (mode | 0x01));
-	do {
-		reg_r(gspca_dev, 0x8803, 1);
-		if (!gspca_dev->usb_buf[0])
-			break;
-	} while (--retry);
-	if (retry == 0)
-		return -1;
-	reg_r(gspca_dev, 0x8800, 1);
-	value = gspca_dev->usb_buf[0];
-	reg_r(gspca_dev, 0x8805, 1);
-	vallsb = gspca_dev->usb_buf[0];
-	return ((int) value << 8) | vallsb;
-}
-
-static const __u16 spca561_init_data[][2] = {
+static const __u16 rev72a_init_data1[][2] = {
 	{0x0000, 0x8114},	/* Software GPIO output data */
 	{0x0001, 0x8114},	/* Software GPIO output data */
 	{0x0000, 0x8112},	/* Some kind of reset */
@@ -247,44 +156,26 @@ static const __u16 spca561_init_data[][2] = {
 	{0x0001, 0x8118},	/* Conf sensor */
 	{0x0092, 0x8804},	/* I know nothing about these */
 	{0x0010, 0x8802},	/* 0x88xx registers, so I won't */
-	/***************/
 	{0x000d, 0x8805},	/* sensor default setting */
-	{0x0001, 0x8801},	/* 1 <- 0x0d */
-	{0x0000, 0x8800},
-	{0x0018, 0x8805},
-	{0x0002, 0x8801},	/* 2 <- 0x18 */
-	{0x0000, 0x8800},
-	{0x0065, 0x8805},
-	{0x0004, 0x8801},	/* 4 <- 0x01 0x65 */
-	{0x0001, 0x8800},
-	{0x0021, 0x8805},
-	{0x0005, 0x8801},	/* 5 <- 0x21 */
-	{0x0000, 0x8800},
-	{0x00aa, 0x8805},
-	{0x0007, 0x8801},	/* 7 <- 0xaa */
-	{0x0000, 0x8800},
-	{0x0004, 0x8805},
-	{0x0020, 0x8801},	/* 0x20 <- 0x15 0x04 */
-	{0x0015, 0x8800},
-	{0x0002, 0x8805},
-	{0x0039, 0x8801},	/* 0x39 <- 0x02 */
-	{0x0000, 0x8800},
-	{0x0010, 0x8805},
-	{0x0035, 0x8801},	/* 0x35 <- 0x10 */
-	{0x0000, 0x8800},
-	{0x0049, 0x8805},
-	{0x0009, 0x8801},	/* 0x09 <- 0x10 0x49 */
-	{0x0010, 0x8800},
-	{0x000b, 0x8805},
-	{0x0028, 0x8801},	/* 0x28 <- 0x0b */
-	{0x0000, 0x8800},
-	{0x000f, 0x8805},
-	{0x003b, 0x8801},	/* 0x3b <- 0x0f */
-	{0x0000, 0x8800},
-	{0x0000, 0x8805},
-	{0x003c, 0x8801},	/* 0x3c <- 0x00 */
-	{0x0000, 0x8800},
-	/***************/
+	{}
+};
+static const __u16 rev72a_init_sensor1[][2] = {
+				/* ms-win values */
+	{0x0001, 0x0018},	/* 0x01 <- 0x0d */
+	{0x0002, 0x0065},	/* 0x02 <- 0x18 */
+	{0x0004, 0x0121},	/* 0x04 <- 0x0165 */
+	{0x0005, 0x00aa},	/* 0x05 <- 0x21 */
+	{0x0007, 0x0004},	/* 0x07 <- 0xaa */
+	{0x0020, 0x1502},	/* 0x20 <- 0x1504 */
+	{0x0039, 0x0010},	/* 0x39 <- 0x02 */
+	{0x0035, 0x0049},	/* 0x35 <- 0x10 */
+	{0x0009, 0x100b},	/* 0x09 <- 0x1049 */
+	{0x0028, 0x000f},	/* 0x28 <- 0x0b */
+	{0x003b, 0x003c},	/* 0x3b <- 0x0f */
+	{0x003c, 0x0000},	/* 0x3c <- 0x00 */
+	{}
+};
+static const __u16 rev72a_init_data2[][2] = {
 	{0x0018, 0x8601},	/* Pixel/line selection for color separation */
 	{0x0000, 0x8602},	/* Optical black level for user setting */
 	{0x0060, 0x8604},	/* Optical black horizontal offset */
@@ -309,10 +200,18 @@ static const __u16 spca561_init_data[][2] = {
 	{0x0004, 0x8612},	/* Gr offset for white balance */
 	{0x0007, 0x8613},	/* B offset for white balance */
 	{0x0000, 0x8614},	/* Gb offset for white balance */
+#if 1
+/* from ms-win */
+	{0x0035, 0x8651},	/* R gain for white balance */
+	{0x0040, 0x8652},	/* Gr gain for white balance */
+	{0x005f, 0x8653},	/* B gain for white balance */
+	{0x0040, 0x8654},	/* Gb gain for white balance */
+#else
 	{0x008c, 0x8651},	/* R gain for white balance */
 	{0x008c, 0x8652},	/* Gr gain for white balance */
 	{0x00b5, 0x8653},	/* B gain for white balance */
 	{0x008c, 0x8654},	/* Gb gain for white balance */
+#endif
 	{0x0002, 0x8502},	/* Maximum average bit rate stuff */
 
 	{0x0011, 0x8802},
@@ -324,29 +223,22 @@ static const __u16 spca561_init_data[][2] = {
 
 	{0x0002, 0x865b},	/* Horizontal offset for valid pixels */
 	{0x0003, 0x865c},	/* Vertical offset for valid lines */
-	/***************//* sensor active */
-	{0x0003, 0x8801},	/* 0x03 <- 0x01 0x21 //289 */
-	{0x0021, 0x8805},
-	{0x0001, 0x8800},
-	{0x0004, 0x8801},	/* 0x04 <- 0x01 0x65 //357 */
-	{0x0065, 0x8805},
-	{0x0001, 0x8800},
-	{0x0005, 0x8801},	/* 0x05 <- 0x2f */
-	{0x002f, 0x8805},
-	{0x0000, 0x8800},
-	{0x0006, 0x8801},	/* 0x06 <- 0 */
-	{0x0000, 0x8805},
-	{0x0000, 0x8800},
-	{0x000a, 0x8801},	/* 0x0a <- 2 */
-	{0x0002, 0x8805},
-	{0x0000, 0x8800},
-	{0x0009, 0x8801},	/* 0x09 <- 0x1061 */
-	{0x0061, 0x8805},
-	{0x0010, 0x8800},
-	{0x0035, 0x8801},	/* 0x35 <-0x14 */
-	{0x0014, 0x8805},
-	{0x0000, 0x8800},
+	{}
+};
+static const __u16 rev72a_init_sensor2[][2] = {
+				/* ms-win values */
+	{0x0003, 0x0121},	/* 0x03 <- 0x01 0x21 //289 */
+	{0x0004, 0x0165},	/* 0x04 <- 0x01 0x65 //357 */
+	{0x0005, 0x002f},	/* 0x05 <- 0x2f */
+	{0x0006, 0x0000},	/* 0x06 <- 0 */
+	{0x000a, 0x0002},	/* 0x0a <- 2 */
+	{0x0009, 0x1061},	/* 0x09 <- 0x1061 */
+	{0x0035, 0x0014},	/* 0x35 <- 0x14 */
+	{}
+};
+static const __u16 rev72a_init_data3[][2] = {
 	{0x0030, 0x8112},	/* ISO and drop packet enable */
+/*fixme: should stop here*/
 	{0x0000, 0x8112},	/* Some kind of reset ???? */
 	{0x0009, 0x8118},	/* Enable sensor and set standby */
 	{0x0000, 0x8114},	/* Software GPIO output data */
@@ -434,21 +326,6 @@ static const __u16 spca561_init_data[][2] = {
 	{}
 };
 
-#if 0
-static void sensor_reset(struct gspca_dev *gspca_dev)
-{
-	reg_w_val(gspca_dev->dev, 0x8631, 0xc8);
-	reg_w_val(gspca_dev->dev, 0x8634, 0xc8);
-	reg_w_val(gspca_dev->dev, 0x8112, 0x00);
-	reg_w_val(gspca_dev->dev, 0x8114, 0x00);
-	reg_w_val(gspca_dev->dev, 0x8118, 0x21);
-	reg_w_val(gspca_dev->dev, 0x8804, 0x92);	/* i2c init */
-	reg_w_val(gspca_dev->dev, 0x8802, 0x14);
-	i2c_write(gspca_dev, 0x0001, 0x0d);
-	i2c_write(gspca_dev, 0x0000, 0x0d);
-}
-#endif
-
 /******************** QC Express etch2 stuff ********************/
 static const __u16 Pb100_1map8300[][2] = {
 	/* reg, value */
@@ -529,22 +406,112 @@ static const __u16 spca561_161rev12A_data2[][2] = {
 	{}
 };
 
-static void sensor_mapwrite(struct gspca_dev *gspca_dev,
-			    const __u16 sensormap[][2])
+static void reg_w_val(struct usb_device *dev, __u16 index, __u8 value)
+{
+	int ret;
+
+	ret = usb_control_msg(dev, usb_sndctrlpipe(dev, 0),
+			      0,		/* request */
+			      USB_DIR_OUT | USB_TYPE_VENDOR | USB_RECIP_DEVICE,
+			      value, index, NULL, 0, 500);
+	PDEBUG(D_USBO, "reg write: 0x%02x:0x%02x", index, value);
+	if (ret < 0)
+		PDEBUG(D_ERR, "reg write: error %d", ret);
+}
+
+static void write_vector(struct gspca_dev *gspca_dev,
+			const __u16 data[][2])
 {
-	int i = 0;
-	__u8 usbval[2];
+	struct usb_device *dev = gspca_dev->dev;
+	int i;
 
-	while (sensormap[i][0]) {
-		usbval[0] = sensormap[i][1];
-		usbval[1] = sensormap[i][1] >> 8;
-		reg_w_buf(gspca_dev, sensormap[i][0], usbval, 2);
+	i = 0;
+	while (data[i][1] != 0) {
+		reg_w_val(dev, data[i][1], data[i][0]);
 		i++;
 	}
 }
+
+/* read 'len' bytes to gspca_dev->usb_buf */
+static void reg_r(struct gspca_dev *gspca_dev,
+		  __u16 index, __u16 length)
+{
+	usb_control_msg(gspca_dev->dev,
+			usb_rcvctrlpipe(gspca_dev->dev, 0),
+			0,			/* request */
+			USB_DIR_IN | USB_TYPE_VENDOR | USB_RECIP_DEVICE,
+			0,			/* value */
+			index, gspca_dev->usb_buf, length, 500);
+}
+
+/* write 'len' bytes from gspca_dev->usb_buf */
+static void reg_w_buf(struct gspca_dev *gspca_dev,
+		      __u16 index, __u16 len)
+{
+	usb_control_msg(gspca_dev->dev,
+			usb_sndctrlpipe(gspca_dev->dev, 0),
+			0,			/* request */
+			USB_DIR_OUT | USB_TYPE_VENDOR | USB_RECIP_DEVICE,
+			0,			/* value */
+			index, gspca_dev->usb_buf, len, 500);
+}
+
+static void i2c_write(struct gspca_dev *gspca_dev, __u16 value, __u16 reg)
+{
+	int retry = 60;
+
+	reg_w_val(gspca_dev->dev, 0x8801, reg);
+	reg_w_val(gspca_dev->dev, 0x8805, value);
+	reg_w_val(gspca_dev->dev, 0x8800, value >> 8);
+	do {
+		reg_r(gspca_dev, 0x8803, 1);
+		if (!gspca_dev->usb_buf[0])
+			return;
+	} while (--retry);
+}
+
+static int i2c_read(struct gspca_dev *gspca_dev, __u16 reg, __u8 mode)
+{
+	int retry = 60;
+	__u8 value;
+
+	reg_w_val(gspca_dev->dev, 0x8804, 0x92);
+	reg_w_val(gspca_dev->dev, 0x8801, reg);
+	reg_w_val(gspca_dev->dev, 0x8802, mode | 0x01);
+	do {
+		reg_r(gspca_dev, 0x8803, 1);
+		if (!gspca_dev->usb_buf[0]) {
+			reg_r(gspca_dev, 0x8800, 1);
+			value = gspca_dev->usb_buf[0];
+			reg_r(gspca_dev, 0x8805, 1);
+			return ((int) value << 8) | gspca_dev->usb_buf[0];
+		}
+	} while (--retry);
+	return -1;
+}
+
+static void sensor_mapwrite(struct gspca_dev *gspca_dev,
+			    const __u16 (*sensormap)[2])
+{
+	while ((*sensormap)[0]) {
+		gspca_dev->usb_buf[0] = (*sensormap)[1];
+		gspca_dev->usb_buf[1] = (*sensormap)[1] >> 8;
+		reg_w_buf(gspca_dev, (*sensormap)[0], 2);
+		sensormap++;
+	}
+}
+
+static void write_sensor_72a(struct gspca_dev *gspca_dev,
+			    const __u16 (*sensor)[2])
+{
+	while ((*sensor)[0]) {
+		i2c_write(gspca_dev, (*sensor)[1], (*sensor)[0]);
+		sensor++;
+	}
+}
+
 static void init_161rev12A(struct gspca_dev *gspca_dev)
 {
-/*	sensor_reset(gspca_dev);	(not in win) */
 	write_vector(gspca_dev, spca561_161rev12A_data1);
 	sensor_mapwrite(gspca_dev, Pb100_1map8300);
 /*fixme: should be in sd_start*/
@@ -612,49 +579,68 @@ static int sd_init_12a(struct gspca_dev *gspca_dev)
 static int sd_init_72a(struct gspca_dev *gspca_dev)
 {
 	PDEBUG(D_STREAM, "Chip revision: 072a");
-	write_vector(gspca_dev, spca561_init_data);
+	write_vector(gspca_dev, rev72a_init_data1);
+	write_sensor_72a(gspca_dev, rev72a_init_sensor1);
+	write_vector(gspca_dev, rev72a_init_data2);
+	write_sensor_72a(gspca_dev, rev72a_init_sensor2);
+	write_vector(gspca_dev, rev72a_init_data3);
 	return 0;
 }
 
-static void setcontrast(struct gspca_dev *gspca_dev)
+/* rev 72a only */
+static void setbrightness(struct gspca_dev *gspca_dev)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
 	struct usb_device *dev = gspca_dev->dev;
-	__u8 lowb;
+	__u8 value;
 
-	switch (sd->chip_revision) {
-	case Rev072A:
-		lowb = sd->contrast >> 8;
-		reg_w_val(dev, 0x8651, lowb);
-		reg_w_val(dev, 0x8652, lowb);
-		reg_w_val(dev, 0x8653, lowb);
-		reg_w_val(dev, 0x8654, lowb);
-		break;
-	default: {
-/*	case Rev012A: { */
-		static const __u8 Reg8391[] =
-			{ 0x92, 0x30, 0x20, 0x00, 0x0c, 0x00, 0x00, 0x00 };
+	value = sd->brightness;
 
-		reg_w_buf(gspca_dev, 0x8391, Reg8391, 8);
-		reg_w_buf(gspca_dev, 0x8390, Reg8391, 8);
-		break;
-	    }
-	}
+	/* offsets for white balance */
+	reg_w_val(dev, 0x8611, value);		/* R */
+	reg_w_val(dev, 0x8612, value);		/* Gr */
+	reg_w_val(dev, 0x8613, value);		/* B */
+	reg_w_val(dev, 0x8614, value);		/* Gb */
 }
 
-/* rev12a only */
 static void setwhite(struct gspca_dev *gspca_dev)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
 	__u16 white;
-	__u8 reg8614, reg8616;
+	__u8 blue, red;
+	__u16 reg;
 
-	white = sd->white;
 	/* try to emulate MS-win as possible */
-	reg8616 = 0x90 - white * 5 / 8;
-	reg_w_val(gspca_dev->dev, 0x8616, reg8616);
-	reg8614 = 0x20 + white * 3 / 8;
-	reg_w_val(gspca_dev->dev, 0x8614, reg8614);
+	white = sd->white;
+	red = 0x20 + white * 3 / 8;
+	blue = 0x90 - white * 5 / 8;
+	if (sd->chip_revision == Rev012A) {
+		reg = 0x8614;
+	} else {
+		reg = 0x8651;
+		red += sd->contrast - 0x20;
+		blue += sd->contrast - 0x20;
+	}
+	reg_w_val(gspca_dev->dev, reg, red);
+	reg_w_val(gspca_dev->dev, reg + 2, blue);
+}
+
+static void setcontrast(struct gspca_dev *gspca_dev)
+{
+	struct sd *sd = (struct sd *) gspca_dev;
+	struct usb_device *dev = gspca_dev->dev;
+	__u8 value;
+
+	if (sd->chip_revision != Rev072A)
+		return;
+	value = sd->contrast + 0x20;
+
+	/* gains for white balance */
+	setwhite(gspca_dev);
+/*	reg_w_val(dev, 0x8651, value);		 * R - done by setwhite */
+	reg_w_val(dev, 0x8652, value);		/* Gr */
+/*	reg_w_val(dev, 0x8653, value);		 * B - done by setwhite */
+	reg_w_val(dev, 0x8654, value);		/* Gb */
 }
 
 /* rev 12a only */
@@ -663,7 +649,6 @@ static void setexposure(struct gspca_dev *gspca_dev)
 	struct sd *sd = (struct sd *) gspca_dev;
 	int expo;
 	int clock_divider;
-	__u8 data[2];
 
 	/* Register 0x8309 controls exposure for the spca561,
 	   the basic exposure setting goes from 1-2047, where 1 is completely
@@ -687,20 +672,19 @@ static void setexposure(struct gspca_dev *gspca_dev)
 		clock_divider = 3;
 	}
 	expo |= clock_divider << 11;
-	data[0] = expo;
-	data[1] = expo >> 8;
-	reg_w_buf(gspca_dev, 0x8309, data, 2);
+	gspca_dev->usb_buf[0] = expo;
+	gspca_dev->usb_buf[1] = expo >> 8;
+	reg_w_buf(gspca_dev, 0x8309, 2);
 }
 
 /* rev 12a only */
 static void setgain(struct gspca_dev *gspca_dev)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
-	__u8 data[2];
 
-	data[0] = sd->gain;
-	data[1] = 0;
-	reg_w_buf(gspca_dev, 0x8335, data, 2);
+	gspca_dev->usb_buf[0] = sd->gain;
+	gspca_dev->usb_buf[1] = 0;
+	reg_w_buf(gspca_dev, 0x8335, 2);
 }
 
 static void setautogain(struct gspca_dev *gspca_dev)
@@ -716,9 +700,9 @@ static void setautogain(struct gspca_dev *gspca_dev)
 static int sd_start_12a(struct gspca_dev *gspca_dev)
 {
 	struct usb_device *dev = gspca_dev->dev;
-	int Clck = 0x8a; /* lower 0x8X values lead to fps > 30 */
-	__u8 Reg8307[] = { 0xaa, 0x00 };
 	int mode;
+	static const __u8 Reg8391[8] =
+		{0x92, 0x30, 0x20, 0x00, 0x0c, 0x00, 0x00, 0x00};
 
 	mode = gspca_dev->cam.cam_mode[(int) gspca_dev->curr_mode].priv;
 	if (mode <= 1) {
@@ -730,14 +714,21 @@ static int sd_start_12a(struct gspca_dev *gspca_dev)
 		 * is sufficient to push raw frames at ~20fps */
 		reg_w_val(dev, 0x8500, mode);
 	}		/* -- qq@kuku.eu.org */
-	reg_w_buf(gspca_dev, 0x8307, Reg8307, 2);
-	reg_w_val(gspca_dev->dev, 0x8700, Clck);
+
+	gspca_dev->usb_buf[0] = 0xaa;
+	gspca_dev->usb_buf[1] = 0x00;
+	reg_w_buf(gspca_dev, 0x8307, 2);
+	/* clock - lower 0x8X values lead to fps > 30 */
+	reg_w_val(gspca_dev->dev, 0x8700, 0x8a);
 					/* 0x8f 0x85 0x27 clock */
 	reg_w_val(gspca_dev->dev, 0x8112, 0x1e | 0x20);
 	reg_w_val(gspca_dev->dev, 0x850b, 0x03);
-	setcontrast(gspca_dev);
+	memcpy(gspca_dev->usb_buf, Reg8391, 8);
+	reg_w_buf(gspca_dev, 0x8391, 8);
+	reg_w_buf(gspca_dev, 0x8390, 8);
 	setwhite(gspca_dev);
 	setautogain(gspca_dev);
+/*	setgain(gspca_dev);		*/
 	setexposure(gspca_dev);
 	return 0;
 }
@@ -764,6 +755,9 @@ static int sd_start_72a(struct gspca_dev *gspca_dev)
 	reg_w_val(dev, 0x8500, mode);	/* mode */
 	reg_w_val(dev, 0x8700, Clck);	/* 0x27 clock */
 	reg_w_val(dev, 0x8112, 0x10 | 0x20);
+	setcontrast(gspca_dev);
+/*	setbrightness(gspca_dev);	 * fixme: bad values */
+	setwhite(gspca_dev);
 	setautogain(gspca_dev);
 	return 0;
 }
@@ -805,7 +799,6 @@ static void do_autogain(struct gspca_dev *gspca_dev)
 	__u8 luma_mean = 110;
 	__u8 luma_delta = 20;
 	__u8 spring = 4;
-	__u8 reg8339[2];
 
 	if (sd->ag_cnt < 0)
 		return;
@@ -848,13 +841,13 @@ static void do_autogain(struct gspca_dev *gspca_dev)
 
 			if (gainG > 0x3f)
 				gainG = 0x3f;
-			else if (gainG < 4)
+			else if (gainG < 3)
 				gainG = 3;
 			i2c_write(gspca_dev, gainG, 0x35);
 
-			if (expotimes >= 0x0256)
+			if (expotimes > 0x0256)
 				expotimes = 0x0256;
-			else if (expotimes < 4)
+			else if (expotimes < 3)
 				expotimes = 3;
 			i2c_write(gspca_dev, expotimes | pixelclk, 0x09);
 		}
@@ -862,13 +855,13 @@ static void do_autogain(struct gspca_dev *gspca_dev)
 	case Rev012A:
 		reg_r(gspca_dev, 0x8330, 2);
 		if (gspca_dev->usb_buf[1] > 0x08) {
-			reg8339[0] = ++sd->expo12a;
-			reg8339[1] = 0;
-			reg_w_buf(gspca_dev, 0x8339, reg8339, 2);
+			gspca_dev->usb_buf[0] = ++sd->expo12a;
+			gspca_dev->usb_buf[1] = 0;
+			reg_w_buf(gspca_dev, 0x8339, 2);
 		} else if (gspca_dev->usb_buf[1] < 0x02) {
-			reg8339[0] = --sd->expo12a;
-			reg8339[1] = 0;
-			reg_w_buf(gspca_dev, 0x8339, reg8339, 2);
+			gspca_dev->usb_buf[0] = --sd->expo12a;
+			gspca_dev->usb_buf[1] = 0;
+			reg_w_buf(gspca_dev, 0x8339, 2);
 		}
 		break;
 	}
@@ -881,8 +874,8 @@ static void sd_pkt_scan(struct gspca_dev *gspca_dev,
 {
 	struct sd *sd = (struct sd *) gspca_dev;
 
-	switch (data[0]) {
-	case 0:		/* start of frame */
+	switch (data[0]) {			/* sequence number */
+	case 0:					/* start of frame */
 		frame = gspca_frame_add(gspca_dev, LAST_PACKET, frame,
 					data, 0);
 		data += SPCA561_OFFSET_DATA;
@@ -904,8 +897,7 @@ static void sd_pkt_scan(struct gspca_dev *gspca_dev,
 						frame, data, len);
 		}
 		return;
-	case 0xff:		/* drop */
-/*		gspca_dev->last_packet_type = DISCARD_PACKET; */
+	case 0xff:			/* drop (empty mpackets) */
 		return;
 	}
 	data++;
@@ -914,55 +906,6 @@ static void sd_pkt_scan(struct gspca_dev *gspca_dev,
 }
 
 /* rev 72a only */
-static void setbrightness(struct gspca_dev *gspca_dev)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	__u8 value;
-
-	value = sd->brightness;
-	reg_w_val(gspca_dev->dev, 0x8611, value);
-	reg_w_val(gspca_dev->dev, 0x8612, value);
-	reg_w_val(gspca_dev->dev, 0x8613, value);
-	reg_w_val(gspca_dev->dev, 0x8614, value);
-}
-
-static void getbrightness(struct gspca_dev *gspca_dev)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	__u16 tot;
-
-	tot = 0;
-	reg_r(gspca_dev, 0x8611, 1);
-	tot += gspca_dev->usb_buf[0];
-	reg_r(gspca_dev, 0x8612, 1);
-	tot += gspca_dev->usb_buf[0];
-	reg_r(gspca_dev, 0x8613, 1);
-	tot += gspca_dev->usb_buf[0];
-	reg_r(gspca_dev, 0x8614, 1);
-	tot += gspca_dev->usb_buf[0];
-	sd->brightness = tot >> 2;
-}
-
-/* rev72a only */
-static void getcontrast(struct gspca_dev *gspca_dev)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	__u16 tot;
-
-	tot = 0;
-	reg_r(gspca_dev, 0x8651, 1);
-	tot += gspca_dev->usb_buf[0];
-	reg_r(gspca_dev, 0x8652, 1);
-	tot += gspca_dev->usb_buf[0];
-	reg_r(gspca_dev, 0x8653, 1);
-	tot += gspca_dev->usb_buf[0];
-	reg_r(gspca_dev, 0x8654, 1);
-	tot += gspca_dev->usb_buf[0];
-	sd->contrast = tot << 6;
-	PDEBUG(D_CONF, "get contrast %d", sd->contrast);
-}
-
-/* rev 72a only */
 static int sd_setbrightness(struct gspca_dev *gspca_dev, __s32 val)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
@@ -977,7 +920,6 @@ static int sd_getbrightness(struct gspca_dev *gspca_dev, __s32 *val)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
 
-	getbrightness(gspca_dev);
 	*val = sd->brightness;
 	return 0;
 }
@@ -997,7 +939,6 @@ static int sd_getcontrast(struct gspca_dev *gspca_dev, __s32 *val)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
 
-	getcontrast(gspca_dev);
 	*val = sd->contrast;
 	return 0;
 }
@@ -1020,7 +961,6 @@ static int sd_getautogain(struct gspca_dev *gspca_dev, __s32 *val)
 	return 0;
 }
 
-/* rev12a only */
 static int sd_setwhite(struct gspca_dev *gspca_dev, __s32 val)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
@@ -1135,6 +1075,19 @@ static struct ctrl sd_ctrls_12a[] = {
 
 static struct ctrl sd_ctrls_72a[] = {
 	{
+	    {
+		.id = V4L2_CID_DO_WHITE_BALANCE,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "White Balance",
+		.minimum = WHITE_MIN,
+		.maximum = WHITE_MAX,
+		.step = 1,
+		.default_value = WHITE_DEF,
+	    },
+	    .set = sd_setwhite,
+	    .get = sd_getwhite,
+	},
+	{
 	   {
 		.id = V4L2_CID_BRIGHTNESS,
 		.type = V4L2_CTRL_TYPE_INTEGER,
diff --git a/linux/drivers/media/video/gspca/stk014.c b/linux/drivers/media/video/gspca/stk014.c
index 74f57db53..60de9af87 100644
--- a/linux/drivers/media/video/gspca/stk014.c
+++ b/linux/drivers/media/video/gspca/stk014.c
@@ -109,7 +109,7 @@ static struct ctrl sd_ctrls[] = {
 	},
 };
 
-static struct v4l2_pix_format vga_mode[] = {
+static const struct v4l2_pix_format vga_mode[] = {
 	{320, 240, V4L2_PIX_FMT_JPEG, V4L2_FIELD_NONE,
 		.bytesperline = 320,
 		.sizeimage = 320 * 240 * 3 / 8 + 590,
diff --git a/linux/drivers/media/video/gspca/stv06xx/Kconfig b/linux/drivers/media/video/gspca/stv06xx/Kconfig
new file mode 100644
index 000000000..634ad38d9
--- /dev/null
+++ b/linux/drivers/media/video/gspca/stv06xx/Kconfig
@@ -0,0 +1,9 @@
+config USB_STV06XX
+	tristate "STV06XX USB Camera Driver"
+	depends on USB_GSPCA
+	help
+	  Say Y here if you want support for cameras based on
+	  the ST STV06XX chip.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called gspca_stv06xx.
diff --git a/linux/drivers/media/video/gspca/stv06xx/Makefile b/linux/drivers/media/video/gspca/stv06xx/Makefile
new file mode 100644
index 000000000..8f002b623
--- /dev/null
+++ b/linux/drivers/media/video/gspca/stv06xx/Makefile
@@ -0,0 +1,6 @@
+obj-$(CONFIG_USB_STV06XX) += gspca_stv06xx.o
+
+gspca_stv06xx-objs := stv06xx.o \
+		      stv06xx_vv6410.o \
+		      stv06xx_hdcs.o \
+		      stv06xx_pb0100.o
diff --git a/linux/drivers/media/video/gspca/stv06xx/stv06xx.c b/linux/drivers/media/video/gspca/stv06xx/stv06xx.c
new file mode 100644
index 000000000..29e43718b
--- /dev/null
+++ b/linux/drivers/media/video/gspca/stv06xx/stv06xx.c
@@ -0,0 +1,522 @@
+/*
+ * Copyright (c) 2001 Jean-Fredric Clere, Nikolas Zimmermann, Georg Acher
+ *		      Mark Cave-Ayland, Carlo E Prelz, Dick Streefland
+ * Copyright (c) 2002, 2003 Tuukka Toivonen
+ * Copyright (c) 2008 Erik Andrén
+ *
+ * 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
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ * P/N 861037:      Sensor HDCS1000        ASIC STV0600
+ * P/N 861050-0010: Sensor HDCS1000        ASIC STV0600
+ * P/N 861050-0020: Sensor Photobit PB100  ASIC STV0600-1 - QuickCam Express
+ * P/N 861055:      Sensor ST VV6410       ASIC STV0610   - LEGO cam
+ * P/N 861075-0040: Sensor HDCS1000        ASIC
+ * P/N 961179-0700: Sensor ST VV6410       ASIC STV0602   - Dexxa WebCam USB
+ * P/N 861040-0000: Sensor ST VV6410       ASIC STV0610   - QuickCam Web
+ */
+
+#include "stv06xx_sensor.h"
+
+MODULE_AUTHOR("Erik Andrén");
+MODULE_DESCRIPTION("STV06XX USB Camera Driver");
+MODULE_LICENSE("GPL");
+
+int dump_bridge;
+int dump_sensor;
+
+int stv06xx_write_bridge(struct sd *sd, u16 address, u16 i2c_data)
+{
+	int err;
+	struct usb_device *udev = sd->gspca_dev.dev;
+	__u8 *buf = sd->gspca_dev.usb_buf;
+	u8 len = (i2c_data > 0xff) ? 2 : 1;
+
+	buf[0] = i2c_data & 0xff;
+	buf[1] = (i2c_data >> 8) & 0xff;
+
+	err = usb_control_msg(udev, usb_sndctrlpipe(udev, 0),
+			      0x04, 0x40, address, 0, buf, len,
+			      STV06XX_URB_MSG_TIMEOUT);
+
+
+	PDEBUG(D_CONF, "Written 0x%x to address 0x%x, status: %d",
+	       i2c_data, address, err);
+
+	return (err < 0) ? err : 0;
+}
+
+int stv06xx_read_bridge(struct sd *sd, u16 address, u8 *i2c_data)
+{
+	int err;
+	struct usb_device *udev = sd->gspca_dev.dev;
+	__u8 *buf = sd->gspca_dev.usb_buf;
+
+	err = usb_control_msg(udev, usb_rcvctrlpipe(udev, 0),
+			      0x04, 0xc0, address, 0, buf, 1,
+			      STV06XX_URB_MSG_TIMEOUT);
+
+	*i2c_data = buf[0];
+
+	PDEBUG(D_CONF, "Read 0x%x from address 0x%x, status %d",
+	       *i2c_data, address, err);
+
+	return (err < 0) ? err : 0;
+}
+
+/* Wraps the normal write sensor bytes / words functions for writing a
+   single value */
+int stv06xx_write_sensor(struct sd *sd, u8 address, u16 value)
+{
+	if (sd->sensor->i2c_len == 2) {
+		u16 data[2] = { address, value };
+		return stv06xx_write_sensor_words(sd, data, 1);
+	} else {
+		u8 data[2] = { address, value };
+		return stv06xx_write_sensor_bytes(sd, data, 1);
+	}
+}
+
+static int stv06xx_write_sensor_finish(struct sd *sd)
+{
+	int err = 0;
+
+	if (IS_850(sd)) {
+		struct usb_device *udev = sd->gspca_dev.dev;
+		__u8 *buf = sd->gspca_dev.usb_buf;
+
+		/* Quickam Web needs an extra packet */
+		buf[0] = 0;
+		err = usb_control_msg(udev, usb_sndctrlpipe(udev, 0),
+				      0x04, 0x40, 0x1704, 0, buf, 1,
+				      STV06XX_URB_MSG_TIMEOUT);
+	}
+
+	return (err < 0) ? err : 0;
+}
+
+int stv06xx_write_sensor_bytes(struct sd *sd, const u8 *data, u8 len)
+{
+	int err, i, j;
+	struct usb_device *udev = sd->gspca_dev.dev;
+	__u8 *buf = sd->gspca_dev.usb_buf;
+
+	PDEBUG(D_USBO, "I2C: Command buffer contains %d entries", len);
+	for (i = 0; i < len;) {
+		/* Build the command buffer */
+		memset(buf, 0, I2C_BUFFER_LENGTH);
+		for (j = 0; j < I2C_MAX_BYTES && i < len; j++, i++) {
+			buf[j] = data[2*i];
+			buf[0x10 + j] = data[2*i+1];
+			PDEBUG(D_USBO, "I2C: Writing 0x%02x to reg 0x%02x",
+			data[2*i+1], data[2*i]);
+		}
+		buf[0x20] = sd->sensor->i2c_addr;
+		buf[0x21] = j - 1; /* Number of commands to send - 1 */
+		buf[0x22] = I2C_WRITE_CMD;
+		err = usb_control_msg(udev, usb_sndctrlpipe(udev, 0),
+				      0x04, 0x40, 0x0400, 0, buf,
+				      I2C_BUFFER_LENGTH,
+				      STV06XX_URB_MSG_TIMEOUT);
+				      if (err < 0)
+					return err;
+       }
+       return stv06xx_write_sensor_finish(sd);
+}
+
+int stv06xx_write_sensor_words(struct sd *sd, const u16 *data, u8 len)
+{
+	int err, i, j;
+	struct usb_device *udev = sd->gspca_dev.dev;
+	__u8 *buf = sd->gspca_dev.usb_buf;
+
+	PDEBUG(D_USBO, "I2C: Command buffer contains %d entries", len);
+
+	for (i = 0; i < len;) {
+		/* Build the command buffer */
+		memset(buf, 0, I2C_BUFFER_LENGTH);
+		for (j = 0; j < I2C_MAX_WORDS && i < len; j++, i++) {
+			buf[j] = data[2*i];
+			buf[0x10 + j * 2] = data[2*i+1];
+			buf[0x10 + j * 2 + 1] = data[2*i+1] >> 8;
+			PDEBUG(D_USBO, "I2C: Writing 0x%04x to reg 0x%02x",
+				data[2*i+1], data[2*i]);
+		}
+		buf[0x20] = sd->sensor->i2c_addr;
+		buf[0x21] = j - 1; /* Number of commands to send - 1 */
+		buf[0x22] = I2C_WRITE_CMD;
+		err = usb_control_msg(udev, usb_sndctrlpipe(udev, 0),
+				0x04, 0x40, 0x0400, 0, buf,
+				I2C_BUFFER_LENGTH,
+				STV06XX_URB_MSG_TIMEOUT);
+		if (err < 0)
+			return err;
+	}
+	return stv06xx_write_sensor_finish(sd);
+}
+
+int stv06xx_read_sensor(struct sd *sd, const u8 address, u16 *value)
+{
+	int err;
+	struct usb_device *udev = sd->gspca_dev.dev;
+	__u8 *buf = sd->gspca_dev.usb_buf;
+
+	err = stv06xx_write_bridge(sd, STV_I2C_FLUSH, sd->sensor->i2c_flush);
+	if (err < 0)
+		return err;
+
+	/* Clear mem */
+	memset(buf, 0, I2C_BUFFER_LENGTH);
+
+	buf[0] = address;
+	buf[0x20] = sd->sensor->i2c_addr;
+	buf[0x21] = 0;
+
+	/* Read I2C register */
+	buf[0x22] = I2C_READ_CMD;
+
+	err = usb_control_msg(udev, usb_sndctrlpipe(udev, 0),
+			      0x04, 0x40, 0x1400, 0, buf, I2C_BUFFER_LENGTH,
+			      STV06XX_URB_MSG_TIMEOUT);
+	if (err < 0) {
+		PDEBUG(D_ERR, "I2C Read: error writing address: %d", err);
+		return err;
+	}
+
+	err = usb_control_msg(udev, usb_rcvctrlpipe(udev, 0),
+			      0x04, 0xc0, 0x1410, 0, buf, sd->sensor->i2c_len,
+			      STV06XX_URB_MSG_TIMEOUT);
+	if (sd->sensor->i2c_len == 2)
+		*value = buf[0] | (buf[1] << 8);
+	else
+		*value = buf[0];
+
+	PDEBUG(D_USBO, "I2C: Read 0x%x from address 0x%x, status: %d",
+	       *value, address, err);
+
+	return (err < 0) ? err : 0;
+}
+
+/* Dumps all bridge registers */
+static void stv06xx_dump_bridge(struct sd *sd)
+{
+	int i;
+	u8 data, buf;
+
+	info("Dumping all stv06xx bridge registers");
+	for (i = 0x1400; i < 0x160f; i++) {
+		stv06xx_read_bridge(sd, i, &data);
+
+		info("Read 0x%x from address 0x%x", data, i);
+	}
+
+	for (i = 0x1400; i < 0x160f; i++) {
+		stv06xx_read_bridge(sd, i, &data);
+		buf = data;
+
+		stv06xx_write_bridge(sd, i, 0xff);
+		stv06xx_read_bridge(sd, i, &data);
+		if (data == 0xff)
+			info("Register 0x%x is read/write", i);
+		else if (data != buf)
+			info("Register 0x%x is read/write,"
+			     "but only partially", i);
+		else
+			info("Register 0x%x is read-only", i);
+
+		stv06xx_write_bridge(sd, i, buf);
+	}
+}
+
+/* this function is called at probe and resume time */
+static int stv06xx_init(struct gspca_dev *gspca_dev)
+{
+	struct sd *sd = (struct sd *) gspca_dev;
+	int err;
+
+	PDEBUG(D_PROBE, "Initializing camera");
+
+	/* Let the usb init settle for a bit
+	   before performing the initialization */
+	msleep(250);
+
+	err = sd->sensor->init(sd);
+
+	if (dump_sensor)
+		sd->sensor->dump(sd);
+
+	return (err < 0) ? err : 0;
+}
+
+/* Start the camera */
+static int stv06xx_start(struct gspca_dev *gspca_dev)
+{
+	struct sd *sd = (struct sd *) gspca_dev;
+	int err;
+
+	/* Prepare the sensor for start */
+	err = sd->sensor->start(sd);
+	if (err < 0)
+		goto out;
+
+	/* Start isochronous streaming */
+	err = stv06xx_write_bridge(sd, STV_ISO_ENABLE, 1);
+
+out:
+	if (err < 0)
+		PDEBUG(D_STREAM, "Starting stream failed");
+	else
+		PDEBUG(D_STREAM, "Started streaming");
+
+	return (err < 0) ? err : 0;
+}
+
+static void stv06xx_stopN(struct gspca_dev *gspca_dev)
+{
+	int err;
+	struct sd *sd = (struct sd *) gspca_dev;
+
+	/* stop ISO-streaming */
+	err = stv06xx_write_bridge(sd, STV_ISO_ENABLE, 0);
+	if (err < 0)
+		goto out;
+
+	err = sd->sensor->stop(sd);
+	if (err < 0)
+		goto out;
+
+out:
+	if (err < 0)
+		PDEBUG(D_STREAM, "Failed to stop stream");
+	else
+		PDEBUG(D_STREAM, "Stopped streaming");
+}
+
+/*
+ * Analyse an USB packet of the data stream and store it appropriately.
+ * Each packet contains an integral number of chunks. Each chunk has
+ * 2-bytes identification, followed by 2-bytes that describe the chunk
+ * length. Known/guessed chunk identifications are:
+ * 8001/8005/C001/C005 - Begin new frame
+ * 8002/8006/C002/C006 - End frame
+ * 0200/4200           - Contains actual image data, bayer or compressed
+ * 0005                - 11 bytes of unknown data
+ * 0100                - 2 bytes of unknown data
+ * The 0005 and 0100 chunks seem to appear only in compressed stream.
+ */
+static void stv06xx_pkt_scan(struct gspca_dev *gspca_dev,
+			struct gspca_frame *frame,	/* target */
+			__u8 *data,			/* isoc packet */
+			int len)			/* iso packet length */
+{
+	PDEBUG(D_PACK, "Packet of length %d arrived", len);
+
+	/* A packet may contain several frames
+	   loop until the whole packet is reached */
+	while (len) {
+		int id, chunk_len;
+
+		if (len < 4) {
+			PDEBUG(D_PACK, "Packet is smaller than 4 bytes");
+			return;
+		}
+
+		/* Capture the id */
+		id = (data[0] << 8) | data[1];
+
+		/* Capture the chunk length */
+		chunk_len = (data[2] << 8) | data[3];
+		PDEBUG(D_PACK, "Chunk id: %x, length: %d", id, chunk_len);
+
+		data += 4;
+		len -= 4;
+
+		if (len < chunk_len) {
+			PDEBUG(D_ERR, "URB packet length is smaller"
+				" than the specified chunk length");
+			return;
+		}
+
+		switch (id) {
+		case 0x0200:
+		case 0x4200:
+			PDEBUG(D_PACK, "Frame data packet detected");
+
+			gspca_frame_add(gspca_dev, INTER_PACKET, frame,
+					data, chunk_len);
+			break;
+
+		case 0x8001:
+		case 0x8005:
+		case 0xc001:
+		case 0xc005:
+			PDEBUG(D_PACK, "Starting new frame");
+
+			/* Create a new frame, chunk length should be zero */
+			gspca_frame_add(gspca_dev, FIRST_PACKET,
+					frame, data, 0);
+
+			if (chunk_len)
+				PDEBUG(D_ERR, "Chunk length is "
+					      "non-zero on a SOF");
+			break;
+
+		case 0x8002:
+		case 0x8006:
+		case 0xc002:
+			PDEBUG(D_PACK, "End of frame detected");
+
+			/* Complete the last frame (if any) */
+			gspca_frame_add(gspca_dev, LAST_PACKET, frame, data, 0);
+
+			if (chunk_len)
+				PDEBUG(D_ERR, "Chunk length is "
+					      "non-zero on a EOF");
+			break;
+
+		case 0x0005:
+			PDEBUG(D_PACK, "Chunk 0x005 detected");
+			/* Unknown chunk with 11 bytes of data,
+			   occurs just before end of each frame
+			   in compressed mode */
+			break;
+
+		case 0x0100:
+			PDEBUG(D_PACK, "Chunk 0x0100 detected");
+			/* Unknown chunk with 2 bytes of data,
+			   occurs 2-3 times per USB interrupt */
+			break;
+		default:
+			PDEBUG(D_PACK, "Unknown chunk %d detected", id);
+			/* Unknown chunk */
+		}
+		data    += chunk_len;
+		len     -= chunk_len;
+	}
+}
+
+static int stv06xx_config(struct gspca_dev *gspca_dev,
+			  const struct usb_device_id *id);
+
+/* sub-driver description */
+static const struct sd_desc sd_desc = {
+	.name = MODULE_NAME,
+	.config = stv06xx_config,
+	.init = stv06xx_init,
+	.start = stv06xx_start,
+	.stopN = stv06xx_stopN,
+	.pkt_scan = stv06xx_pkt_scan
+};
+
+/* This function is called at probe time */
+static int stv06xx_config(struct gspca_dev *gspca_dev,
+			  const struct usb_device_id *id)
+{
+	struct sd *sd = (struct sd *) gspca_dev;
+	struct cam *cam;
+
+	PDEBUG(D_PROBE, "Configuring camera");
+
+	cam = &gspca_dev->cam;
+	cam->epaddr = STV_ISOC_ENDPOINT_ADDR;
+	sd->desc = sd_desc;
+	gspca_dev->sd_desc = &sd->desc;
+
+	if (dump_bridge)
+		stv06xx_dump_bridge(sd);
+
+	sd->sensor = &stv06xx_sensor_vv6410;
+	if (!sd->sensor->probe(sd))
+		return 0;
+
+	sd->sensor = &stv06xx_sensor_hdcs1x00;
+	if (!sd->sensor->probe(sd))
+		return 0;
+
+	sd->sensor = &stv06xx_sensor_hdcs1020;
+	if (!sd->sensor->probe(sd))
+		return 0;
+
+	sd->sensor = &stv06xx_sensor_pb0100;
+	if (!sd->sensor->probe(sd))
+		return 0;
+
+	sd->sensor = NULL;
+	return -ENODEV;
+}
+
+
+
+/* -- module initialisation -- */
+static const __devinitdata struct usb_device_id device_table[] = {
+	{USB_DEVICE(0x046d, 0x0840)}, /* QuickCam Express */
+	{USB_DEVICE(0x046d, 0x0850)}, /* LEGO cam / QuickCam Web */
+	{USB_DEVICE(0x046d, 0x0870)}, /* Dexxa WebCam USB */
+	{}
+};
+MODULE_DEVICE_TABLE(usb, device_table);
+
+/* -- device connect -- */
+static int sd_probe(struct usb_interface *intf,
+			const struct usb_device_id *id)
+{
+	PDEBUG(D_PROBE, "Probing for a stv06xx device");
+	return gspca_dev_probe(intf, id, &sd_desc, sizeof(struct sd),
+			       THIS_MODULE);
+}
+
+void sd_disconnect(struct usb_interface *intf)
+{
+	struct gspca_dev *gspca_dev = usb_get_intfdata(intf);
+	struct sd *sd = (struct sd *) gspca_dev;
+	PDEBUG(D_PROBE, "Disconnecting the stv06xx device");
+
+	if (sd->sensor->disconnect)
+		sd->sensor->disconnect(sd);
+	gspca_disconnect(intf);
+}
+
+static struct usb_driver sd_driver = {
+	.name = MODULE_NAME,
+	.id_table = device_table,
+	.probe = sd_probe,
+	.disconnect = sd_disconnect,
+#ifdef CONFIG_PM
+	.suspend = gspca_suspend,
+	.resume = gspca_resume,
+#endif
+};
+
+/* -- module insert / remove -- */
+static int __init sd_mod_init(void)
+{
+	if (usb_register(&sd_driver) < 0)
+		return -1;
+	PDEBUG(D_PROBE, "registered");
+	return 0;
+}
+static void __exit sd_mod_exit(void)
+{
+	usb_deregister(&sd_driver);
+	PDEBUG(D_PROBE, "deregistered");
+}
+
+module_init(sd_mod_init);
+module_exit(sd_mod_exit);
+
+module_param(dump_bridge, bool, S_IRUGO | S_IWUSR);
+MODULE_PARM_DESC(dump_bridge, "Dumps all usb bridge registers at startup");
+
+module_param(dump_sensor, bool, S_IRUGO | S_IWUSR);
+MODULE_PARM_DESC(dump_sensor, "Dumps all sensor registers at startup");
diff --git a/linux/drivers/media/video/gspca/stv06xx/stv06xx.h b/linux/drivers/media/video/gspca/stv06xx/stv06xx.h
new file mode 100644
index 000000000..1207e7d17
--- /dev/null
+++ b/linux/drivers/media/video/gspca/stv06xx/stv06xx.h
@@ -0,0 +1,107 @@
+/*
+ * Copyright (c) 2001 Jean-Fredric Clere, Nikolas Zimmermann, Georg Acher
+ *		      Mark Cave-Ayland, Carlo E Prelz, Dick Streefland
+ * Copyright (c) 2002, 2003 Tuukka Toivonen
+ * Copyright (c) 2008 Erik Andrén
+ *
+ * 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
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ * P/N 861037:      Sensor HDCS1000        ASIC STV0600
+ * P/N 861050-0010: Sensor HDCS1000        ASIC STV0600
+ * P/N 861050-0020: Sensor Photobit PB100  ASIC STV0600-1 - QuickCam Express
+ * P/N 861055:      Sensor ST VV6410       ASIC STV0610   - LEGO cam
+ * P/N 861075-0040: Sensor HDCS1000        ASIC
+ * P/N 961179-0700: Sensor ST VV6410       ASIC STV0602   - Dexxa WebCam USB
+ * P/N 861040-0000: Sensor ST VV6410       ASIC STV0610   - QuickCam Web
+ */
+
+#ifndef STV06XX_H_
+#define STV06XX_H_
+
+#include "gspca.h"
+
+#define MODULE_NAME "STV06xx"
+
+#define STV_ISOC_ENDPOINT_ADDR		0x81
+
+#ifndef V4L2_PIX_FMT_SGRBG8
+#define V4L2_PIX_FMT_SGRBG8 v4l2_fourcc('G', 'R', 'B', 'G')
+#endif
+
+#define STV_REG23 			0x0423
+
+/* Control registers of the STV0600 ASIC */
+#define STV_I2C_PARTNER			0x1420
+#define STV_I2C_VAL_REG_VAL_PAIRS_MIN1	0x1421
+#define STV_I2C_READ_WRITE_TOGGLE	0x1422
+#define STV_I2C_FLUSH			0x1423
+#define STV_I2C_SUCC_READ_REG_VALS	0x1424
+
+#define STV_ISO_ENABLE			0x1440
+#define STV_SCAN_RATE			0x1443
+#define STV_LED_CTRL			0x1445
+#define STV_STV0600_EMULATION		0x1446
+#define STV_REG00			0x1500
+#define STV_REG01			0x1501
+#define STV_REG02			0x1502
+#define STV_REG03			0x1503
+#define STV_REG04			0x1504
+
+#define STV_ISO_SIZE_L			0x15c1
+#define STV_ISO_SIZE_H			0x15c2
+
+/* Refers to the CIF 352x288 and QCIF 176x144 */
+/* 1: 288 lines, 2: 144 lines */
+#define STV_Y_CTRL			0x15c3
+
+/* 0xa: 352 columns, 0x6: 176 columns */
+#define STV_X_CTRL			0x1680
+
+#define STV06XX_URB_MSG_TIMEOUT		5000
+
+#define I2C_MAX_BYTES			16
+#define I2C_MAX_WORDS			8
+
+#define I2C_BUFFER_LENGTH		0x23
+#define I2C_READ_CMD			3
+#define I2C_WRITE_CMD			1
+
+#define LED_ON				1
+#define LED_OFF				0
+
+/* STV06xx device descriptor */
+struct sd {
+	struct gspca_dev gspca_dev;
+
+	/* A pointer to the currently connected sensor */
+	const struct stv06xx_sensor *sensor;
+
+	/* A pointer to the sd_desc struct */
+	struct sd_desc desc;
+
+	/* Sensor private data */
+	void *sensor_priv;
+};
+
+int stv06xx_write_bridge(struct sd *sd, u16 address, u16 i2c_data);
+int stv06xx_read_bridge(struct sd *sd, u16 address, u8 *i2c_data);
+
+int stv06xx_write_sensor_bytes(struct sd *sd, const u8 *data, u8 len);
+int stv06xx_write_sensor_words(struct sd *sd, const u16 *data, u8 len);
+
+int stv06xx_read_sensor(struct sd *sd, const u8 address, u16 *value);
+int stv06xx_write_sensor(struct sd *sd, u8 address, u16 value);
+
+#endif
diff --git a/linux/drivers/media/video/gspca/stv06xx/stv06xx_hdcs.c b/linux/drivers/media/video/gspca/stv06xx/stv06xx_hdcs.c
new file mode 100644
index 000000000..1cfe58504
--- /dev/null
+++ b/linux/drivers/media/video/gspca/stv06xx/stv06xx_hdcs.c
@@ -0,0 +1,533 @@
+/*
+ * Copyright (c) 2001 Jean-Fredric Clere, Nikolas Zimmermann, Georg Acher
+ *		      Mark Cave-Ayland, Carlo E Prelz, Dick Streefland
+ * Copyright (c) 2002, 2003 Tuukka Toivonen
+ * Copyright (c) 2008 Erik Andrén
+ * Copyright (c) 2008 Chia-I Wu
+ *
+ * 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
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ * P/N 861037:      Sensor HDCS1000        ASIC STV0600
+ * P/N 861050-0010: Sensor HDCS1000        ASIC STV0600
+ * P/N 861050-0020: Sensor Photobit PB100  ASIC STV0600-1 - QuickCam Express
+ * P/N 861055:      Sensor ST VV6410       ASIC STV0610   - LEGO cam
+ * P/N 861075-0040: Sensor HDCS1000        ASIC
+ * P/N 961179-0700: Sensor ST VV6410       ASIC STV0602   - Dexxa WebCam USB
+ * P/N 861040-0000: Sensor ST VV6410       ASIC STV0610   - QuickCam Web
+ */
+
+#include "stv06xx_hdcs.h"
+
+enum hdcs_power_state {
+	HDCS_STATE_SLEEP,
+	HDCS_STATE_IDLE,
+	HDCS_STATE_RUN
+};
+
+/* no lock? */
+struct hdcs {
+	enum hdcs_power_state state;
+	int w, h;
+
+	/* visible area of the sensor array */
+	struct {
+		int left, top;
+		int width, height;
+		int border;
+	} array;
+
+	struct {
+		/* Column timing overhead */
+		u8 cto;
+		/* Column processing overhead */
+		u8 cpo;
+		/* Row sample period constant */
+		u16 rs;
+		/* Exposure reset duration */
+		u16 er;
+	} exp;
+
+	int psmp;
+};
+
+static int hdcs_reg_write_seq(struct sd *sd, u8 reg, u8 *vals, u8 len)
+{
+	u8 regs[I2C_MAX_BYTES * 2];
+	int i;
+
+	if (unlikely((len <= 0) || (len >= I2C_MAX_BYTES) ||
+		     (reg + len > 0xff)))
+		return -EINVAL;
+
+	for (i = 0; i < len; i++, reg++) {
+		regs[2*i] = reg;
+		regs[2*i+1] = vals[i];
+	}
+
+	return stv06xx_write_sensor_bytes(sd, regs, len);
+}
+
+static int hdcs_set_state(struct sd *sd, enum hdcs_power_state state)
+{
+	struct hdcs *hdcs = sd->sensor_priv;
+	u8 val;
+	int ret;
+
+	if (hdcs->state == state)
+		return 0;
+
+	/* we need to go idle before running or sleeping */
+	if (hdcs->state != HDCS_STATE_IDLE) {
+		ret = stv06xx_write_sensor(sd, HDCS_REG_CONTROL(sd), 0);
+		if (ret)
+			return ret;
+	}
+
+	hdcs->state = HDCS_STATE_IDLE;
+
+	if (state == HDCS_STATE_IDLE)
+		return 0;
+
+	switch (state) {
+	case HDCS_STATE_SLEEP:
+		val = HDCS_SLEEP_MODE;
+		break;
+
+	case HDCS_STATE_RUN:
+		val = HDCS_RUN_ENABLE;
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	ret = stv06xx_write_sensor(sd, HDCS_REG_CONTROL(sd), val);
+	if (ret < 0)
+		hdcs->state = state;
+
+	return ret;
+}
+
+static int hdcs_reset(struct sd *sd)
+{
+	struct hdcs *hdcs = sd->sensor_priv;
+	int err;
+
+	err = stv06xx_write_sensor(sd, HDCS_REG_CONTROL(sd), 1);
+	if (err < 0)
+		return err;
+
+	err = stv06xx_write_sensor(sd, HDCS_REG_CONTROL(sd), 0);
+	if (err < 0)
+		hdcs->state = HDCS_STATE_IDLE;
+
+	return err;
+}
+
+static int hdcs_get_exposure(struct gspca_dev *gspca_dev, __s32 *val)
+{
+	struct sd *sd = (struct sd *) gspca_dev;
+	struct hdcs *hdcs = sd->sensor_priv;
+
+	/* Column time period */
+	int ct;
+	/* Column processing period */
+	int cp;
+	/* Row processing period */
+	int rp;
+	int cycles;
+	int err;
+	int rowexp;
+	u16 data[2];
+
+	err = stv06xx_read_sensor(sd, HDCS_ROWEXPL, &data[0]);
+	if (err < 0)
+		return err;
+
+	err = stv06xx_read_sensor(sd, HDCS_ROWEXPH, &data[1]);
+	if (err < 0)
+		return err;
+
+	rowexp = (data[1] << 8) | data[0];
+
+	ct = hdcs->exp.cto + hdcs->psmp + (HDCS_ADC_START_SIG_DUR + 2);
+	cp = hdcs->exp.cto + (hdcs->w * ct / 2);
+	rp = hdcs->exp.rs + cp;
+
+	cycles = rp * rowexp;
+	*val = cycles / HDCS_CLK_FREQ_MHZ;
+	PDEBUG(D_V4L2, "Read exposure %d", *val);
+	return 0;
+}
+
+static int hdcs_set_exposure(struct gspca_dev *gspca_dev, __s32 val)
+{
+	struct sd *sd = (struct sd *) gspca_dev;
+	struct hdcs *hdcs = sd->sensor_priv;
+	int rowexp, srowexp;
+	int max_srowexp;
+	/* Column time period */
+	int ct;
+	/* Column processing period */
+	int cp;
+	/* Row processing period */
+	int rp;
+	/* Minimum number of column timing periods
+	   within the column processing period */
+	int mnct;
+	int cycles, err;
+	u8 exp[4];
+
+	cycles = val * HDCS_CLK_FREQ_MHZ;
+
+	ct = hdcs->exp.cto + hdcs->psmp + (HDCS_ADC_START_SIG_DUR + 2);
+	cp = hdcs->exp.cto + (hdcs->w * ct / 2);
+
+	/* the cycles one row takes */
+	rp = hdcs->exp.rs + cp;
+
+	rowexp = cycles / rp;
+
+	/* the remaining cycles */
+	cycles -= rowexp * rp;
+
+	/* calculate sub-row exposure */
+	if (IS_1020(sd)) {
+		/* see HDCS-1020 datasheet 3.5.6.4, p. 63 */
+		srowexp = hdcs->w - (cycles + hdcs->exp.er + 13) / ct;
+
+		mnct = (hdcs->exp.er + 12 + ct - 1) / ct;
+		max_srowexp = hdcs->w - mnct;
+	} else {
+		/* see HDCS-1000 datasheet 3.4.5.5, p. 61 */
+		srowexp = cp - hdcs->exp.er - 6 - cycles;
+
+		mnct = (hdcs->exp.er + 5 + ct - 1) / ct;
+		max_srowexp = cp - mnct * ct - 1;
+	}
+
+	if (srowexp < 0)
+		srowexp = 0;
+	else if (srowexp > max_srowexp)
+		srowexp = max_srowexp;
+
+	if (IS_1020(sd)) {
+		exp[0] = rowexp & 0xff;
+		exp[1] = rowexp >> 8;
+		exp[2] = (srowexp >> 2) & 0xff;
+		/* this clears exposure error flag */
+		exp[3] = 0x1;
+		err = hdcs_reg_write_seq(sd, HDCS_ROWEXPL, exp, 4);
+	} else {
+		exp[0] = rowexp & 0xff;
+		exp[1] = rowexp >> 8;
+		exp[2] = srowexp & 0xff;
+		exp[3] = srowexp >> 8;
+		err = hdcs_reg_write_seq(sd, HDCS_ROWEXPL, exp, 4);
+		if (err < 0)
+			return err;
+
+		/* clear exposure error flag */
+		err = stv06xx_write_sensor(sd,
+		     HDCS_STATUS, BIT(4));
+	}
+	PDEBUG(D_V4L2, "Writing exposure %d, rowexp %d, srowexp %d",
+	       val, rowexp, srowexp);
+	return err;
+}
+
+static int hdcs_set_gains(struct sd *sd, u8 r, u8 g, u8 b)
+{
+	u8 gains[4];
+
+	/* the voltage gain Av = (1 + 19 * val / 127) * (1 + bit7) */
+	if (r > 127)
+		r = 0x80 | (r / 2);
+	if (g > 127)
+		g = 0x80 | (g / 2);
+	if (b > 127)
+		b = 0x80 | (b / 2);
+
+	gains[0] = g;
+	gains[1] = r;
+	gains[2] = b;
+	gains[3] = g;
+
+	return hdcs_reg_write_seq(sd, HDCS_ERECPGA, gains, 4);
+}
+
+static int hdcs_get_gain(struct gspca_dev *gspca_dev, __s32 *val)
+{
+	struct sd *sd = (struct sd *) gspca_dev;
+	int err;
+	u16 data;
+
+	err = stv06xx_read_sensor(sd, HDCS_ERECPGA, &data);
+
+	/* Bit 7 doubles the gain */
+	if (data & 0x80)
+		*val = (data & 0x7f) * 2;
+	else
+		*val = data;
+
+	PDEBUG(D_V4L2, "Read gain %d", *val);
+	return err;
+}
+
+static int hdcs_set_gain(struct gspca_dev *gspca_dev, __s32 val)
+{
+	PDEBUG(D_V4L2, "Writing gain %d", val);
+	return hdcs_set_gains((struct sd *) gspca_dev,
+			       val & 0xff, val & 0xff, val & 0xff);
+}
+
+static int hdcs_set_size(struct sd *sd,
+		unsigned int width, unsigned int height)
+{
+	struct hdcs *hdcs = sd->sensor_priv;
+	u8 win[4];
+	unsigned int x, y;
+	int err;
+
+	/* must be multiple of 4 */
+	width = (width + 3) & ~0x3;
+	height = (height + 3) & ~0x3;
+
+	if (width > hdcs->array.width)
+		width = hdcs->array.width;
+
+	if (IS_1020(sd)) {
+		/* the borders are also invalid */
+		if (height + 2 * hdcs->array.border + HDCS_1020_BOTTOM_Y_SKIP
+				  > hdcs->array.height)
+			height = hdcs->array.height - 2 * hdcs->array.border -
+				HDCS_1020_BOTTOM_Y_SKIP;
+
+		y = (hdcs->array.height - HDCS_1020_BOTTOM_Y_SKIP - height) / 2
+				+ hdcs->array.top;
+	} else if (height > hdcs->array.height) {
+		height = hdcs->array.height;
+		y = hdcs->array.top + (hdcs->array.height - height) / 2;
+	}
+
+	x = hdcs->array.left + (hdcs->array.width - width) / 2;
+
+	win[0] = y / 4;
+	win[1] = x / 4;
+	win[2] = (y + height) / 4 - 1;
+	win[3] = (x + width) / 4 - 1;
+
+	err = hdcs_reg_write_seq(sd, HDCS_FWROW, win, 4);
+	if (err < 0)
+		return err;
+
+	/* Update the current width and height */
+	hdcs->w = width;
+	hdcs->h = height;
+	return err;
+}
+
+static int hdcs_probe_1x00(struct sd *sd)
+{
+	struct hdcs *hdcs;
+	u16 sensor;
+	int ret;
+
+	ret = stv06xx_read_sensor(sd, HDCS_IDENT, &sensor);
+	if (ret < 0 || sensor != 0x08)
+		return -ENODEV;
+
+	info("HDCS-1000/1100 sensor detected");
+
+	sd->gspca_dev.cam.cam_mode = stv06xx_sensor_hdcs1x00.modes;
+	sd->gspca_dev.cam.nmodes = stv06xx_sensor_hdcs1x00.nmodes;
+	sd->desc.ctrls = stv06xx_sensor_hdcs1x00.ctrls;
+	sd->desc.nctrls = stv06xx_sensor_hdcs1x00.nctrls;
+
+	hdcs = kmalloc(sizeof(struct hdcs), GFP_KERNEL);
+	if (!hdcs)
+		return -ENOMEM;
+
+	hdcs->array.left = 8;
+	hdcs->array.top = 8;
+	hdcs->array.width = HDCS_1X00_DEF_WIDTH;
+	hdcs->array.height = HDCS_1X00_DEF_HEIGHT;
+	hdcs->array.border = 4;
+
+	hdcs->exp.cto = 4;
+	hdcs->exp.cpo = 2;
+	hdcs->exp.rs = 186;
+	hdcs->exp.er = 100;
+
+	/*
+	 * Frame rate on HDCS-1000 0x46D:0x840 depends on PSMP:
+	 *  4 = doesn't work at all
+	 *  5 = 7.8 fps,
+	 *  6 = 6.9 fps,
+	 *  8 = 6.3 fps,
+	 * 10 = 5.5 fps,
+	 * 15 = 4.4 fps,
+	 * 31 = 2.8 fps
+	 *
+	 * Frame rate on HDCS-1000 0x46D:0x870 depends on PSMP:
+	 * 15 = doesn't work at all
+	 * 18 = doesn't work at all
+	 * 19 = 7.3 fps
+	 * 20 = 7.4 fps
+	 * 21 = 7.4 fps
+	 * 22 = 7.4 fps
+	 * 24 = 6.3 fps
+	 * 30 = 5.4 fps
+	 */
+	hdcs->psmp = IS_870(sd) ? 20 : 5;
+
+	sd->sensor_priv = hdcs;
+
+	return 0;
+}
+
+static int hdcs_probe_1020(struct sd *sd)
+{
+	struct hdcs *hdcs;
+	u16 sensor;
+	int ret;
+
+	ret = stv06xx_read_sensor(sd, HDCS_IDENT, &sensor);
+	if (ret < 0 || sensor != 0x10)
+		return -ENODEV;
+
+	info("HDCS-1020 sensor detected");
+
+	sd->gspca_dev.cam.cam_mode = stv06xx_sensor_hdcs1020.modes;
+	sd->gspca_dev.cam.nmodes = stv06xx_sensor_hdcs1020.nmodes;
+	sd->desc.ctrls = stv06xx_sensor_hdcs1020.ctrls;
+	sd->desc.nctrls = stv06xx_sensor_hdcs1020.nctrls;
+
+	hdcs = kmalloc(sizeof(struct hdcs), GFP_KERNEL);
+	if (!hdcs)
+		return -ENOMEM;
+
+	/*
+	 * From Andrey's test image: looks like HDCS-1020 upper-left
+	 * visible pixel is at 24,8 (y maybe even smaller?) and lower-right
+	 * visible pixel at 375,299 (x maybe even larger?)
+	 */
+	hdcs->array.left = 24;
+	hdcs->array.top  = 4;
+	hdcs->array.width = HDCS_1020_DEF_WIDTH;
+	hdcs->array.height = 304;
+	hdcs->array.border = 4;
+
+	hdcs->psmp = 6;
+
+	hdcs->exp.cto = 3;
+	hdcs->exp.cpo = 3;
+	hdcs->exp.rs = 155;
+	hdcs->exp.er = 96;
+
+	sd->sensor_priv = hdcs;
+
+	return 0;
+}
+
+static int hdcs_start(struct sd *sd)
+{
+	PDEBUG(D_STREAM, "Starting stream");
+
+	return hdcs_set_state(sd, HDCS_STATE_RUN);
+}
+
+static int hdcs_stop(struct sd *sd)
+{
+	PDEBUG(D_STREAM, "Halting stream");
+
+	return hdcs_set_state(sd, HDCS_STATE_SLEEP);
+}
+
+static void hdcs_disconnect(struct sd *sd)
+{
+	PDEBUG(D_PROBE, "Disconnecting the sensor");
+	kfree(sd->sensor_priv);
+}
+
+static int hdcs_init(struct sd *sd)
+{
+	struct hdcs *hdcs = sd->sensor_priv;
+	int i, err = 0;
+
+	/* Set the STV0602AA in STV0600 emulation mode */
+	if (IS_870(sd))
+		stv06xx_write_bridge(sd, STV_STV0600_EMULATION, 1);
+
+	/* Execute the bridge init */
+	for (i = 0; i < ARRAY_SIZE(stv_bridge_init) && !err; i++) {
+		err = stv06xx_write_bridge(sd, stv_bridge_init[i][0],
+					   stv_bridge_init[i][1]);
+	}
+	if (err < 0)
+		return err;
+
+	/* sensor soft reset */
+	hdcs_reset(sd);
+
+	/* Execute the sensor init */
+	for (i = 0; i < ARRAY_SIZE(stv_sensor_init) && !err; i++) {
+		err = stv06xx_write_sensor(sd, stv_sensor_init[i][0],
+					     stv_sensor_init[i][1]);
+	}
+	if (err < 0)
+		return err;
+
+	/* Enable continous frame capture, bit 2: stop when frame complete */
+	err = stv06xx_write_sensor(sd, HDCS_REG_CONFIG(sd), BIT(3));
+	if (err < 0)
+		return err;
+
+	/* Set PGA sample duration
+	(was 0x7E for IS_870, but caused slow framerate with HDCS-1020) */
+	if (IS_1020(sd))
+		err = stv06xx_write_sensor(sd, HDCS_TCTRL,
+				(HDCS_ADC_START_SIG_DUR << 6) | hdcs->psmp);
+	else
+		err = stv06xx_write_sensor(sd, HDCS_TCTRL,
+				(HDCS_ADC_START_SIG_DUR << 5) | hdcs->psmp);
+	if (err < 0)
+		return err;
+
+	err = hdcs_set_gains(sd, HDCS_DEFAULT_GAIN, HDCS_DEFAULT_GAIN,
+			     HDCS_DEFAULT_GAIN);
+	if (err < 0)
+		return err;
+
+	err = hdcs_set_exposure(&sd->gspca_dev, HDCS_DEFAULT_EXPOSURE);
+	if (err < 0)
+		return err;
+
+	err = hdcs_set_size(sd, hdcs->array.width, hdcs->array.height);
+	return err;
+}
+
+static int hdcs_dump(struct sd *sd)
+{
+	u16 reg, val;
+
+	info("Dumping sensor registers:");
+
+	for (reg = HDCS_IDENT; reg <= HDCS_ROWEXPH; reg++) {
+		stv06xx_read_sensor(sd, reg, &val);
+		info("reg 0x%02x = 0x%02x", reg, val);
+	}
+	return 0;
+}
diff --git a/linux/drivers/media/video/gspca/stv06xx/stv06xx_hdcs.h b/linux/drivers/media/video/gspca/stv06xx/stv06xx_hdcs.h
new file mode 100644
index 000000000..9c7279a4c
--- /dev/null
+++ b/linux/drivers/media/video/gspca/stv06xx/stv06xx_hdcs.h
@@ -0,0 +1,263 @@
+/*
+ * Copyright (c) 2001 Jean-Fredric Clere, Nikolas Zimmermann, Georg Acher
+ *		      Mark Cave-Ayland, Carlo E Prelz, Dick Streefland
+ * Copyright (c) 2002, 2003 Tuukka Toivonen
+ * Copyright (c) 2008 Erik Andrén
+ * Copyright (c) 2008 Chia-I Wu
+ *
+ * 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
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ * P/N 861037:      Sensor HDCS1000        ASIC STV0600
+ * P/N 861050-0010: Sensor HDCS1000        ASIC STV0600
+ * P/N 861050-0020: Sensor Photobit PB100  ASIC STV0600-1 - QuickCam Express
+ * P/N 861055:      Sensor ST VV6410       ASIC STV0610   - LEGO cam
+ * P/N 861075-0040: Sensor HDCS1000        ASIC
+ * P/N 961179-0700: Sensor ST VV6410       ASIC STV0602   - Dexxa WebCam USB
+ * P/N 861040-0000: Sensor ST VV6410       ASIC STV0610   - QuickCam Web
+ */
+
+#ifndef STV06XX_HDCS_H_
+#define STV06XX_HDCS_H_
+
+#include "stv06xx_sensor.h"
+
+#define HDCS_REG_CONFIG(sd)	(IS_1020(sd) ? HDCS20_CONFIG : HDCS00_CONFIG)
+#define HDCS_REG_CONTROL(sd)	(IS_1020(sd) ? HDCS20_CONTROL : HDCS00_CONTROL)
+
+#define HDCS_1X00_DEF_WIDTH	360
+#define HDCS_1X00_DEF_HEIGHT 	296
+
+#define HDCS_1020_DEF_WIDTH	352
+#define HDCS_1020_DEF_HEIGHT	292
+
+#define HDCS_1020_BOTTOM_Y_SKIP	4
+
+#define HDCS_CLK_FREQ_MHZ	25
+
+#define HDCS_ADC_START_SIG_DUR	3
+
+/* LSB bit of I2C or register address signifies write (0) or read (1) */
+/* I2C Registers common for both HDCS-1000/1100 and HDCS-1020 */
+/* Identifications Register */
+#define HDCS_IDENT		(0x00 << 1)
+/* Status Register */
+#define HDCS_STATUS		(0x01 << 1)
+/* Interrupt Mask Register */
+#define HDCS_IMASK		(0x02 << 1)
+/* Pad Control Register */
+#define HDCS_PCTRL		(0x03 << 1)
+/* Pad Drive Control Register */
+#define HDCS_PDRV		(0x04 << 1)
+/* Interface Control Register */
+#define HDCS_ICTRL		(0x05 << 1)
+/* Interface Timing Register */
+#define HDCS_ITMG		(0x06 << 1)
+/* Baud Fraction Register */
+#define HDCS_BFRAC		(0x07 << 1)
+/* Baud Rate Register */
+#define HDCS_BRATE		(0x08 << 1)
+/* ADC Control Register */
+#define HDCS_ADCCTRL		(0x09 << 1)
+/* First Window Row Register */
+#define HDCS_FWROW		(0x0a << 1)
+/* First Window Column Register */
+#define HDCS_FWCOL		(0x0b << 1)
+/* Last Window Row Register */
+#define HDCS_LWROW		(0x0c << 1)
+/* Last Window Column Register */
+#define HDCS_LWCOL		(0x0d << 1)
+/* Timing Control Register */
+#define HDCS_TCTRL		(0x0e << 1)
+/* PGA Gain Register: Even Row, Even Column */
+#define HDCS_ERECPGA		(0x0f << 1)
+/* PGA Gain Register: Even Row, Odd Column */
+#define HDCS_EROCPGA		(0x10 << 1)
+/* PGA Gain Register: Odd Row, Even Column */
+#define HDCS_ORECPGA		(0x11 << 1)
+/* PGA Gain Register: Odd Row, Odd Column */
+#define HDCS_OROCPGA		(0x12 << 1)
+/* Row Exposure Low Register */
+#define HDCS_ROWEXPL		(0x13 << 1)
+/* Row Exposure High Register */
+#define HDCS_ROWEXPH		(0x14 << 1)
+
+/* I2C Registers only for HDCS-1000/1100 */
+/* Sub-Row Exposure Low Register */
+#define HDCS00_SROWEXPL		(0x15 << 1)
+/* Sub-Row Exposure High Register */
+#define HDCS00_SROWEXPH		(0x16 << 1)
+/* Configuration Register */
+#define HDCS00_CONFIG		(0x17 << 1)
+/* Control Register */
+#define HDCS00_CONTROL		(0x18 << 1)
+
+/* I2C Registers only for HDCS-1020 */
+/* Sub-Row Exposure Register */
+#define HDCS20_SROWEXP		(0x15 << 1)
+/* Error Control Register */
+#define HDCS20_ERROR		(0x16 << 1)
+/* Interface Timing 2 Register */
+#define HDCS20_ITMG2		(0x17 << 1)
+/* Interface Control 2 Register	*/
+#define HDCS20_ICTRL2		(0x18 << 1)
+/* Horizontal Blank Register */
+#define HDCS20_HBLANK		(0x19 << 1)
+/* Vertical Blank Register */
+#define HDCS20_VBLANK		(0x1a << 1)
+/* Configuration Register */
+#define HDCS20_CONFIG		(0x1b << 1)
+/* Control Register */
+#define HDCS20_CONTROL		(0x1c << 1)
+
+#define HDCS_RUN_ENABLE		(1 << 2)
+#define HDCS_SLEEP_MODE		(1 << 1)
+
+#define HDCS_DEFAULT_EXPOSURE	5000
+#define HDCS_DEFAULT_GAIN	128
+
+static int hdcs_probe_1x00(struct sd *sd);
+static int hdcs_probe_1020(struct sd *sd);
+static int hdcs_start(struct sd *sd);
+static int hdcs_init(struct sd *sd);
+static int hdcs_stop(struct sd *sd);
+static int hdcs_dump(struct sd *sd);
+static void hdcs_disconnect(struct sd *sd);
+
+static int hdcs_get_exposure(struct gspca_dev *gspca_dev, __s32 *val);
+static int hdcs_set_exposure(struct gspca_dev *gspca_dev, __s32 val);
+static int hdcs_set_gain(struct gspca_dev *gspca_dev, __s32 val);
+static int hdcs_get_gain(struct gspca_dev *gspca_dev, __s32 *val);
+
+const struct stv06xx_sensor stv06xx_sensor_hdcs1x00 = {
+	.name = "HP HDCS-1000/1100",
+	.i2c_flush = 0,
+	.i2c_addr = (0x55 << 1),
+	.i2c_len = 1,
+
+	.init = hdcs_init,
+	.probe = hdcs_probe_1x00,
+	.start = hdcs_start,
+	.stop = hdcs_stop,
+	.disconnect = hdcs_disconnect,
+	.dump = hdcs_dump,
+
+	.nctrls = 2,
+	.ctrls = {
+	{
+		{
+			.id		= V4L2_CID_EXPOSURE,
+			.type		= V4L2_CTRL_TYPE_INTEGER,
+			.name		= "exposure",
+			.minimum	= 0x00,
+			.maximum	= 0xffff,
+			.step		= 0x1,
+			.default_value 	= HDCS_DEFAULT_EXPOSURE,
+			.flags         	= V4L2_CTRL_FLAG_SLIDER
+		},
+		.set = hdcs_set_exposure,
+		.get = hdcs_get_exposure
+	},
+	{
+		{
+			.id		= V4L2_CID_GAIN,
+			.type		= V4L2_CTRL_TYPE_INTEGER,
+			.name		= "gain",
+			.minimum	= 0x00,
+			.maximum	= 0xff,
+			.step		= 0x1,
+			.default_value 	= HDCS_DEFAULT_GAIN,
+			.flags         	= V4L2_CTRL_FLAG_SLIDER
+		},
+		.set = hdcs_set_gain,
+		.get = hdcs_get_gain
+	}
+	},
+
+	.nmodes = 1,
+	.modes = {
+	{
+		HDCS_1X00_DEF_WIDTH,
+		HDCS_1X00_DEF_HEIGHT,
+		V4L2_PIX_FMT_SBGGR8,
+		V4L2_FIELD_NONE,
+		.sizeimage =
+			HDCS_1X00_DEF_WIDTH * HDCS_1X00_DEF_HEIGHT,
+		.bytesperline = HDCS_1X00_DEF_WIDTH,
+		.colorspace = V4L2_COLORSPACE_SRGB,
+		.priv = 1
+	}
+	}
+};
+
+const struct stv06xx_sensor stv06xx_sensor_hdcs1020 = {
+	.name = "HDCS-1020",
+	.i2c_flush = 0,
+	.i2c_addr = (0x55 << 1),
+	.i2c_len = 1,
+
+	.nctrls = 0,
+	.ctrls = {},
+
+	.init = hdcs_init,
+	.probe = hdcs_probe_1020,
+	.start = hdcs_start,
+	.stop = hdcs_stop,
+	.dump = hdcs_dump,
+
+	.nmodes = 1,
+	.modes = {
+	{
+		HDCS_1020_DEF_WIDTH,
+		HDCS_1020_DEF_HEIGHT,
+		V4L2_PIX_FMT_SBGGR8,
+		V4L2_FIELD_NONE,
+		.sizeimage =
+			HDCS_1020_DEF_WIDTH * HDCS_1020_DEF_HEIGHT,
+		.bytesperline = HDCS_1020_DEF_WIDTH,
+		.colorspace = V4L2_COLORSPACE_SRGB,
+		.priv = 1
+	}
+	}
+};
+
+static const u16 stv_bridge_init[][2] = {
+	{STV_ISO_ENABLE, 0},
+	{STV_REG23, 0},
+	{STV_REG00, 0x1d},
+	{STV_REG01, 0xb5},
+	{STV_REG02, 0xa8},
+	{STV_REG03, 0x95},
+	{STV_REG04, 0x07},
+
+	{STV_SCAN_RATE, 0x20},
+	{STV_ISO_SIZE_L, 847},
+	{STV_Y_CTRL, 0x01},
+	{STV_X_CTRL, 0x0a}
+};
+
+static const u8 stv_sensor_init[][2] = {
+	/* Clear status (writing 1 will clear the corresponding status bit) */
+	{HDCS_STATUS, BIT(6) | BIT(5) | BIT(4) | BIT(3) | BIT(2) | BIT(1)},
+	/* Disable all interrupts */
+	{HDCS_IMASK, 0x00},
+	{HDCS_PCTRL, BIT(6) | BIT(5) | BIT(1) | BIT(0)},
+	{HDCS_PDRV,  0x00},
+	{HDCS_ICTRL, BIT(5)},
+	{HDCS_ITMG,  BIT(4) | BIT(1)},
+	/* ADC output resolution to 10 bits */
+	{HDCS_ADCCTRL, 10}
+};
+
+#endif
diff --git a/linux/drivers/media/video/gspca/stv06xx/stv06xx_pb0100.c b/linux/drivers/media/video/gspca/stv06xx/stv06xx_pb0100.c
new file mode 100644
index 000000000..d0a0f8596
--- /dev/null
+++ b/linux/drivers/media/video/gspca/stv06xx/stv06xx_pb0100.c
@@ -0,0 +1,430 @@
+/*
+ * Copyright (c) 2001 Jean-Fredric Clere, Nikolas Zimmermann, Georg Acher
+ *		      Mark Cave-Ayland, Carlo E Prelz, Dick Streefland
+ * Copyright (c) 2002, 2003 Tuukka Toivonen
+ * Copyright (c) 2008 Erik Andrén
+ *
+ * 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
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ * P/N 861037:      Sensor HDCS1000        ASIC STV0600
+ * P/N 861050-0010: Sensor HDCS1000        ASIC STV0600
+ * P/N 861050-0020: Sensor Photobit PB100  ASIC STV0600-1 - QuickCam Express
+ * P/N 861055:      Sensor ST VV6410       ASIC STV0610   - LEGO cam
+ * P/N 861075-0040: Sensor HDCS1000        ASIC
+ * P/N 961179-0700: Sensor ST VV6410       ASIC STV0602   - Dexxa WebCam USB
+ * P/N 861040-0000: Sensor ST VV6410       ASIC STV0610   - QuickCam Web
+ */
+
+/*
+ * The spec file for the PB-0100 suggests the following for best quality
+ * images after the sensor has been reset :
+ *
+ * PB_ADCGAINL      = R60 = 0x03 (3 dec)      : sets low reference of ADC
+						to produce good black level
+ * PB_PREADCTRL     = R32 = 0x1400 (5120 dec) : Enables global gain changes
+						through R53
+ * PB_ADCMINGAIN    = R52 = 0x10 (16 dec)     : Sets the minimum gain for
+						auto-exposure
+ * PB_ADCGLOBALGAIN = R53 = 0x10 (16 dec)     : Sets the global gain
+ * PB_EXPGAIN       = R14 = 0x11 (17 dec)     : Sets the auto-exposure value
+ * PB_UPDATEINT     = R23 = 0x02 (2 dec)      : Sets the speed on
+						auto-exposure routine
+ * PB_CFILLIN       = R5  = 0x0E (14 dec)     : Sets the frame rate
+ */
+
+#include "stv06xx_pb0100.h"
+
+static int pb0100_probe(struct sd *sd)
+{
+	u16 sensor;
+	int i, err;
+	s32 *sensor_settings;
+
+	err = stv06xx_read_sensor(sd, PB_IDENT, &sensor);
+
+	if (err < 0)
+		return -ENODEV;
+
+	if ((sensor >> 8) == 0x64) {
+		sensor_settings = kmalloc(
+				stv06xx_sensor_pb0100.nctrls * sizeof(s32),
+				GFP_KERNEL);
+		if (!sensor_settings)
+			return -ENOMEM;
+
+		info("Photobit pb0100 sensor detected");
+
+		sd->gspca_dev.cam.cam_mode = stv06xx_sensor_pb0100.modes;
+		sd->gspca_dev.cam.nmodes = stv06xx_sensor_pb0100.nmodes;
+		sd->desc.ctrls = stv06xx_sensor_pb0100.ctrls;
+		sd->desc.nctrls = stv06xx_sensor_pb0100.nctrls;
+		for (i = 0; i < stv06xx_sensor_pb0100.nctrls; i++)
+			sensor_settings[i] = stv06xx_sensor_pb0100.
+					     ctrls[i].qctrl.default_value;
+		sd->sensor_priv = sensor_settings;
+
+		return 0;
+	}
+
+	return -ENODEV;
+}
+
+static int pb0100_start(struct sd *sd)
+{
+	int err;
+	struct cam *cam = &sd->gspca_dev.cam;
+	s32 *sensor_settings = sd->sensor_priv;
+	u32 mode = cam->cam_mode[sd->gspca_dev.curr_mode].priv;
+
+	/* Setup sensor window */
+	if (mode & PB0100_CROP_TO_VGA) {
+		stv06xx_write_sensor(sd, PB_RSTART, 30);
+		stv06xx_write_sensor(sd, PB_CSTART, 20);
+		stv06xx_write_sensor(sd, PB_RWSIZE, 240 - 1);
+		stv06xx_write_sensor(sd, PB_CWSIZE, 320 - 1);
+	} else {
+		stv06xx_write_sensor(sd, PB_RSTART, 8);
+		stv06xx_write_sensor(sd, PB_CSTART, 4);
+		stv06xx_write_sensor(sd, PB_RWSIZE, 288 - 1);
+		stv06xx_write_sensor(sd, PB_CWSIZE, 352 - 1);
+	}
+
+	if (mode & PB0100_SUBSAMPLE) {
+		stv06xx_write_bridge(sd, STV_Y_CTRL, 0x02); /* Wrong, FIXME */
+		stv06xx_write_bridge(sd, STV_X_CTRL, 0x06);
+
+		stv06xx_write_bridge(sd, STV_SCAN_RATE, 0x10);
+	} else {
+		stv06xx_write_bridge(sd, STV_Y_CTRL, 0x01);
+		stv06xx_write_bridge(sd, STV_X_CTRL, 0x0a);
+		/* larger -> slower */
+		stv06xx_write_bridge(sd, STV_SCAN_RATE, 0x20);
+	}
+
+	/* set_gain also sets red and blue balance */
+	pb0100_set_gain(&sd->gspca_dev, sensor_settings[GAIN_IDX]);
+	pb0100_set_exposure(&sd->gspca_dev, sensor_settings[EXPOSURE_IDX]);
+	pb0100_set_autogain_target(&sd->gspca_dev,
+				   sensor_settings[AUTOGAIN_TARGET_IDX]);
+	pb0100_set_autogain(&sd->gspca_dev, sensor_settings[AUTOGAIN_IDX]);
+
+	err = stv06xx_write_sensor(sd, PB_CONTROL, BIT(5)|BIT(3)|BIT(1));
+	PDEBUG(D_STREAM, "Started stream, status: %d", err);
+
+	return (err < 0) ? err : 0;
+}
+
+static int pb0100_stop(struct sd *sd)
+{
+	int err;
+
+	err = stv06xx_write_sensor(sd, PB_ABORTFRAME, 1);
+
+	if (err < 0)
+		goto out;
+
+	/* Set bit 1 to zero */
+	err = stv06xx_write_sensor(sd, PB_CONTROL, BIT(5)|BIT(3));
+
+	PDEBUG(D_STREAM, "Halting stream");
+out:
+	return (err < 0) ? err : 0;
+}
+
+/* FIXME: Sort the init commands out and put them into tables,
+	  this is only for getting the camera to work */
+/* FIXME: No error handling for now,
+	  add this once the init has been converted to proper tables */
+static int pb0100_init(struct sd *sd)
+{
+	stv06xx_write_bridge(sd, STV_REG00, 1);
+	stv06xx_write_bridge(sd, STV_SCAN_RATE, 0);
+
+	/* Reset sensor */
+	stv06xx_write_sensor(sd, PB_RESET, 1);
+	stv06xx_write_sensor(sd, PB_RESET, 0);
+
+	/* Disable chip */
+	stv06xx_write_sensor(sd, PB_CONTROL, BIT(5)|BIT(3));
+
+	/* Gain stuff...*/
+	stv06xx_write_sensor(sd, PB_PREADCTRL, BIT(12)|BIT(10)|BIT(6));
+	stv06xx_write_sensor(sd, PB_ADCGLOBALGAIN, 12);
+
+	/* Set up auto-exposure */
+	/* ADC VREF_HI new setting for a transition
+	  from the Expose1 to the Expose2 setting */
+	stv06xx_write_sensor(sd, PB_R28, 12);
+	/* gain max for autoexposure */
+	stv06xx_write_sensor(sd, PB_ADCMAXGAIN, 180);
+	/* gain min for autoexposure  */
+	stv06xx_write_sensor(sd, PB_ADCMINGAIN, 12);
+	/* Maximum frame integration time (programmed into R8)
+	   allowed for auto-exposure routine */
+	stv06xx_write_sensor(sd, PB_R54, 3);
+	/* Minimum frame integration time (programmed into R8)
+	   allowed for auto-exposure routine */
+	stv06xx_write_sensor(sd, PB_R55, 0);
+	stv06xx_write_sensor(sd, PB_UPDATEINT, 1);
+	/* R15  Expose0 (maximum that auto-exposure may use) */
+	stv06xx_write_sensor(sd, PB_R15, 800);
+	/* R17  Expose2 (minimum that auto-exposure may use) */
+	stv06xx_write_sensor(sd, PB_R17, 10);
+
+	stv06xx_write_sensor(sd, PB_EXPGAIN, 0);
+
+	/* 0x14 */
+	stv06xx_write_sensor(sd, PB_VOFFSET, 0);
+	/* 0x0D */
+	stv06xx_write_sensor(sd, PB_ADCGAINH, 11);
+	/* Set black level (important!) */
+	stv06xx_write_sensor(sd, PB_ADCGAINL, 0);
+
+	/* ??? */
+	stv06xx_write_bridge(sd, STV_REG00, 0x11);
+	stv06xx_write_bridge(sd, STV_REG03, 0x45);
+	stv06xx_write_bridge(sd, STV_REG04, 0x07);
+
+	/* ISO-Size (0x27b: 635... why? - HDCS uses 847) */
+	stv06xx_write_bridge(sd, STV_ISO_SIZE_L, 847);
+
+	/* Scan/timing for the sensor */
+	stv06xx_write_sensor(sd, PB_ROWSPEED, BIT(4)|BIT(3)|BIT(1));
+	stv06xx_write_sensor(sd, PB_CFILLIN, 14);
+	stv06xx_write_sensor(sd, PB_VBL, 0);
+	stv06xx_write_sensor(sd, PB_FINTTIME, 0);
+	stv06xx_write_sensor(sd, PB_RINTTIME, 123);
+
+	stv06xx_write_bridge(sd, STV_REG01, 0xc2);
+	stv06xx_write_bridge(sd, STV_REG02, 0xb0);
+	return 0;
+}
+
+static int pb0100_dump(struct sd *sd)
+{
+	return 0;
+}
+
+static int pb0100_get_gain(struct gspca_dev *gspca_dev, __s32 *val)
+{
+	struct sd *sd = (struct sd *) gspca_dev;
+	s32 *sensor_settings = sd->sensor_priv;
+
+	*val = sensor_settings[GAIN_IDX];
+
+	return 0;
+}
+
+static int pb0100_set_gain(struct gspca_dev *gspca_dev, __s32 val)
+{
+	int err;
+	struct sd *sd = (struct sd *) gspca_dev;
+	s32 *sensor_settings = sd->sensor_priv;
+
+	if (sensor_settings[AUTOGAIN_IDX])
+		return -EBUSY;
+
+	sensor_settings[GAIN_IDX] = val;
+	err = stv06xx_write_sensor(sd, PB_G1GAIN, val);
+	if (!err)
+		err = stv06xx_write_sensor(sd, PB_G2GAIN, val);
+	PDEBUG(D_V4L2, "Set green gain to %d, status: %d", val, err);
+
+	if (!err)
+		err = pb0100_set_red_balance(gspca_dev,
+					     sensor_settings[RED_BALANCE_IDX]);
+	if (!err)
+		err = pb0100_set_blue_balance(gspca_dev,
+					    sensor_settings[BLUE_BALANCE_IDX]);
+
+	return err;
+}
+
+static int pb0100_get_red_balance(struct gspca_dev *gspca_dev, __s32 *val)
+{
+	struct sd *sd = (struct sd *) gspca_dev;
+	s32 *sensor_settings = sd->sensor_priv;
+
+	*val = sensor_settings[RED_BALANCE_IDX];
+
+	return 0;
+}
+
+static int pb0100_set_red_balance(struct gspca_dev *gspca_dev, __s32 val)
+{
+	int err;
+	struct sd *sd = (struct sd *) gspca_dev;
+	s32 *sensor_settings = sd->sensor_priv;
+
+	if (sensor_settings[AUTOGAIN_IDX])
+		return -EBUSY;
+
+	sensor_settings[RED_BALANCE_IDX] = val;
+	val += sensor_settings[GAIN_IDX];
+	if (val < 0)
+		val = 0;
+	else if (val > 255)
+		val = 255;
+
+	err = stv06xx_write_sensor(sd, PB_RGAIN, val);
+	PDEBUG(D_V4L2, "Set red gain to %d, status: %d", val, err);
+
+	return err;
+}
+
+static int pb0100_get_blue_balance(struct gspca_dev *gspca_dev, __s32 *val)
+{
+	struct sd *sd = (struct sd *) gspca_dev;
+	s32 *sensor_settings = sd->sensor_priv;
+
+	*val = sensor_settings[BLUE_BALANCE_IDX];
+
+	return 0;
+}
+
+static int pb0100_set_blue_balance(struct gspca_dev *gspca_dev, __s32 val)
+{
+	int err;
+	struct sd *sd = (struct sd *) gspca_dev;
+	s32 *sensor_settings = sd->sensor_priv;
+
+	if (sensor_settings[AUTOGAIN_IDX])
+		return -EBUSY;
+
+	sensor_settings[BLUE_BALANCE_IDX] = val;
+	val += sensor_settings[GAIN_IDX];
+	if (val < 0)
+		val = 0;
+	else if (val > 255)
+		val = 255;
+
+	err = stv06xx_write_sensor(sd, PB_BGAIN, val);
+	PDEBUG(D_V4L2, "Set blue gain to %d, status: %d", val, err);
+
+	return err;
+}
+
+static int pb0100_get_exposure(struct gspca_dev *gspca_dev, __s32 *val)
+{
+	struct sd *sd = (struct sd *) gspca_dev;
+	s32 *sensor_settings = sd->sensor_priv;
+
+	*val = sensor_settings[EXPOSURE_IDX];
+
+	return 0;
+}
+
+static int pb0100_set_exposure(struct gspca_dev *gspca_dev, __s32 val)
+{
+	int err;
+	struct sd *sd = (struct sd *) gspca_dev;
+	s32 *sensor_settings = sd->sensor_priv;
+
+	if (sensor_settings[AUTOGAIN_IDX])
+		return -EBUSY;
+
+	sensor_settings[EXPOSURE_IDX] = val;
+	err = stv06xx_write_sensor(sd, PB_RINTTIME, val);
+	PDEBUG(D_V4L2, "Set exposure to %d, status: %d", val, err);
+
+	return err;
+}
+
+static int pb0100_get_autogain(struct gspca_dev *gspca_dev, __s32 *val)
+{
+	struct sd *sd = (struct sd *) gspca_dev;
+	s32 *sensor_settings = sd->sensor_priv;
+
+	*val = sensor_settings[AUTOGAIN_IDX];
+
+	return 0;
+}
+
+static int pb0100_set_autogain(struct gspca_dev *gspca_dev, __s32 val)
+{
+	int err;
+	struct sd *sd = (struct sd *) gspca_dev;
+	s32 *sensor_settings = sd->sensor_priv;
+
+	sensor_settings[AUTOGAIN_IDX] = val;
+	if (sensor_settings[AUTOGAIN_IDX]) {
+		if (sensor_settings[NATURAL_IDX])
+			val = BIT(6)|BIT(4)|BIT(0);
+		else
+			val = BIT(4)|BIT(0);
+	} else
+		val = 0;
+
+	err = stv06xx_write_sensor(sd, PB_EXPGAIN, val);
+	PDEBUG(D_V4L2, "Set autogain to %d (natural: %d), status: %d",
+	       sensor_settings[AUTOGAIN_IDX], sensor_settings[NATURAL_IDX],
+	       err);
+
+	return err;
+}
+
+static int pb0100_get_autogain_target(struct gspca_dev *gspca_dev, __s32 *val)
+{
+	struct sd *sd = (struct sd *) gspca_dev;
+	s32 *sensor_settings = sd->sensor_priv;
+
+	*val = sensor_settings[AUTOGAIN_TARGET_IDX];
+
+	return 0;
+}
+
+static int pb0100_set_autogain_target(struct gspca_dev *gspca_dev, __s32 val)
+{
+	int err, totalpixels, brightpixels, darkpixels;
+	struct sd *sd = (struct sd *) gspca_dev;
+	s32 *sensor_settings = sd->sensor_priv;
+
+	sensor_settings[AUTOGAIN_TARGET_IDX] = val;
+
+	/* Number of pixels counted by the sensor when subsampling the pixels.
+	 * Slightly larger than the real value to avoid oscillation */
+	totalpixels = gspca_dev->width * gspca_dev->height;
+	totalpixels = totalpixels/(8*8) + totalpixels/(64*64);
+
+	brightpixels = (totalpixels * val) >> 8;
+	darkpixels   = totalpixels - brightpixels;
+	err = stv06xx_write_sensor(sd, PB_R21, brightpixels);
+	if (!err)
+		err = stv06xx_write_sensor(sd, PB_R22, darkpixels);
+
+	PDEBUG(D_V4L2, "Set autogain target to %d, status: %d", val, err);
+
+	return err;
+}
+
+static int pb0100_get_natural(struct gspca_dev *gspca_dev, __s32 *val)
+{
+	struct sd *sd = (struct sd *) gspca_dev;
+	s32 *sensor_settings = sd->sensor_priv;
+
+	*val = sensor_settings[NATURAL_IDX];
+
+	return 0;
+}
+
+static int pb0100_set_natural(struct gspca_dev *gspca_dev, __s32 val)
+{
+	struct sd *sd = (struct sd *) gspca_dev;
+	s32 *sensor_settings = sd->sensor_priv;
+
+	sensor_settings[NATURAL_IDX] = val;
+
+	return pb0100_set_autogain(gspca_dev, sensor_settings[AUTOGAIN_IDX]);
+}
diff --git a/linux/drivers/media/video/gspca/stv06xx/stv06xx_pb0100.h b/linux/drivers/media/video/gspca/stv06xx/stv06xx_pb0100.h
new file mode 100644
index 000000000..4b47270aa
--- /dev/null
+++ b/linux/drivers/media/video/gspca/stv06xx/stv06xx_pb0100.h
@@ -0,0 +1,297 @@
+/*
+ * Copyright (c) 2001 Jean-Fredric Clere, Nikolas Zimmermann, Georg Acher
+ *		      Mark Cave-Ayland, Carlo E Prelz, Dick Streefland
+ * Copyright (c) 2002, 2003 Tuukka Toivonen
+ * Copyright (c) 2008 Erik Andrén
+ *
+ * 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
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ * P/N 861037:      Sensor HDCS1000        ASIC STV0600
+ * P/N 861050-0010: Sensor HDCS1000        ASIC STV0600
+ * P/N 861050-0020: Sensor Photobit PB100  ASIC STV0600-1 - QuickCam Express
+ * P/N 861055:      Sensor ST VV6410       ASIC STV0610   - LEGO cam
+ * P/N 861075-0040: Sensor HDCS1000        ASIC
+ * P/N 961179-0700: Sensor ST VV6410       ASIC STV0602   - Dexxa WebCam USB
+ * P/N 861040-0000: Sensor ST VV6410       ASIC STV0610   - QuickCam Web
+ */
+
+#ifndef STV06XX_PB0100_H_
+#define STV06XX_PB0100_H_
+
+#include "stv06xx_sensor.h"
+
+/* mode priv field flags */
+#define PB0100_CROP_TO_VGA	0x01
+#define PB0100_SUBSAMPLE	0x02
+
+/* I2C Registers */
+#define PB_IDENT		0x00	/* Chip Version */
+#define PB_RSTART		0x01	/* Row Window Start */
+#define PB_CSTART		0x02	/* Column Window Start */
+#define PB_RWSIZE		0x03	/* Row Window Size */
+#define PB_CWSIZE		0x04	/* Column  Window Size */
+#define PB_CFILLIN		0x05	/* Column Fill-In */
+#define PB_VBL			0x06	/* Vertical Blank Count */
+#define PB_CONTROL		0x07	/* Control Mode */
+#define PB_FINTTIME		0x08	/* Integration Time/Frame Unit Count */
+#define PB_RINTTIME		0x09	/* Integration Time/Row Unit Count */
+#define PB_ROWSPEED		0x0a	/* Row Speed Control */
+#define PB_ABORTFRAME		0x0b	/* Abort Frame */
+#define PB_R12			0x0c	/* Reserved */
+#define PB_RESET		0x0d	/* Reset */
+#define PB_EXPGAIN		0x0e	/* Exposure Gain Command */
+#define PB_R15			0x0f	/* Expose0 */
+#define PB_R16			0x10	/* Expose1 */
+#define PB_R17			0x11	/* Expose2 */
+#define PB_R18			0x12	/* Low0_DAC */
+#define PB_R19			0x13	/* Low1_DAC */
+#define PB_R20			0x14	/* Low2_DAC */
+#define PB_R21			0x15	/* Threshold11 */
+#define PB_R22			0x16	/* Threshold0x */
+#define PB_UPDATEINT		0x17	/* Update Interval */
+#define PB_R24			0x18	/* High_DAC */
+#define PB_R25			0x19	/* Trans0H */
+#define PB_R26			0x1a	/* Trans1L */
+#define PB_R27			0x1b	/* Trans1H */
+#define PB_R28			0x1c	/* Trans2L */
+#define PB_R29			0x1d	/* Reserved */
+#define PB_R30			0x1e	/* Reserved */
+#define PB_R31			0x1f	/* Wait to Read */
+#define PB_PREADCTRL		0x20	/* Pixel Read Control Mode */
+#define PB_R33			0x21	/* IREF_VLN */
+#define PB_R34			0x22	/* IREF_VLP */
+#define PB_R35			0x23	/* IREF_VLN_INTEG */
+#define PB_R36			0x24	/* IREF_MASTER */
+#define PB_R37			0x25	/* IDACP */
+#define PB_R38			0x26	/* IDACN */
+#define PB_R39			0x27	/* DAC_Control_Reg */
+#define PB_R40			0x28	/* VCL */
+#define PB_R41			0x29	/* IREF_VLN_ADCIN */
+#define PB_R42			0x2a	/* Reserved */
+#define PB_G1GAIN		0x2b	/* Green 1 Gain */
+#define PB_BGAIN		0x2c	/* Blue Gain */
+#define PB_RGAIN		0x2d	/* Red Gain */
+#define PB_G2GAIN		0x2e	/* Green 2 Gain */
+#define PB_R47			0x2f	/* Dark Row Address */
+#define PB_R48			0x30	/* Dark Row Options */
+#define PB_R49			0x31	/* Reserved */
+#define PB_R50			0x32	/* Image Test Data */
+#define PB_ADCMAXGAIN		0x33	/* Maximum Gain */
+#define PB_ADCMINGAIN		0x34	/* Minimum Gain */
+#define PB_ADCGLOBALGAIN	0x35	/* Global Gain */
+#define PB_R54			0x36	/* Maximum Frame */
+#define PB_R55			0x37	/* Minimum Frame */
+#define PB_R56			0x38	/* Reserved */
+#define PB_VOFFSET		0x39	/* VOFFSET */
+#define PB_R58			0x3a	/* Snap-Shot Sequence Trigger */
+#define PB_ADCGAINH		0x3b	/* VREF_HI */
+#define PB_ADCGAINL		0x3c	/* VREF_LO */
+#define PB_R61			0x3d	/* Reserved */
+#define PB_R62			0x3e	/* Reserved */
+#define PB_R63			0x3f	/* Reserved */
+#define PB_R64			0x40	/* Red/Blue Gain */
+#define PB_R65			0x41	/* Green 2/Green 1 Gain */
+#define PB_R66			0x42	/* VREF_HI/LO */
+#define PB_R67			0x43	/* Integration Time/Row Unit Count */
+#define PB_R240			0xf0	/* ADC Test */
+#define PB_R241			0xf1    /* Chip Enable */
+#define PB_R242			0xf2	/* Reserved */
+
+static int pb0100_probe(struct sd *sd);
+static int pb0100_start(struct sd *sd);
+static int pb0100_init(struct sd *sd);
+static int pb0100_stop(struct sd *sd);
+static int pb0100_dump(struct sd *sd);
+
+/* V4L2 controls supported by the driver */
+static int pb0100_get_gain(struct gspca_dev *gspca_dev, __s32 *val);
+static int pb0100_set_gain(struct gspca_dev *gspca_dev, __s32 val);
+static int pb0100_get_red_balance(struct gspca_dev *gspca_dev, __s32 *val);
+static int pb0100_set_red_balance(struct gspca_dev *gspca_dev, __s32 val);
+static int pb0100_get_blue_balance(struct gspca_dev *gspca_dev, __s32 *val);
+static int pb0100_set_blue_balance(struct gspca_dev *gspca_dev, __s32 val);
+static int pb0100_get_exposure(struct gspca_dev *gspca_dev, __s32 *val);
+static int pb0100_set_exposure(struct gspca_dev *gspca_dev, __s32 val);
+static int pb0100_get_autogain(struct gspca_dev *gspca_dev, __s32 *val);
+static int pb0100_set_autogain(struct gspca_dev *gspca_dev, __s32 val);
+static int pb0100_get_autogain_target(struct gspca_dev *gspca_dev, __s32 *val);
+static int pb0100_set_autogain_target(struct gspca_dev *gspca_dev, __s32 val);
+static int pb0100_get_natural(struct gspca_dev *gspca_dev, __s32 *val);
+static int pb0100_set_natural(struct gspca_dev *gspca_dev, __s32 val);
+
+const struct stv06xx_sensor stv06xx_sensor_pb0100 = {
+	.name = "PB-0100",
+	.i2c_flush = 1,
+	.i2c_addr = 0xba,
+	.i2c_len = 2,
+
+	.nctrls = 7,
+	.ctrls = {
+#define GAIN_IDX 0
+	{
+		{
+			.id		= V4L2_CID_GAIN,
+			.type		= V4L2_CTRL_TYPE_INTEGER,
+			.name		= "Gain",
+			.minimum	= 0,
+			.maximum	= 255,
+			.step		= 1,
+			.default_value  = 128
+		},
+		.set = pb0100_set_gain,
+		.get = pb0100_get_gain
+	},
+#define RED_BALANCE_IDX 1
+	{
+		{
+			.id		= V4L2_CID_RED_BALANCE,
+			.type		= V4L2_CTRL_TYPE_INTEGER,
+			.name		= "Red Balance",
+			.minimum	= -255,
+			.maximum	= 255,
+			.step		= 1,
+			.default_value  = 0
+		},
+		.set = pb0100_set_red_balance,
+		.get = pb0100_get_red_balance
+	},
+#define BLUE_BALANCE_IDX 2
+	{
+		{
+			.id		= V4L2_CID_BLUE_BALANCE,
+			.type		= V4L2_CTRL_TYPE_INTEGER,
+			.name		= "Blue Balance",
+			.minimum	= -255,
+			.maximum	= 255,
+			.step		= 1,
+			.default_value  = 0
+		},
+		.set = pb0100_set_blue_balance,
+		.get = pb0100_get_blue_balance
+	},
+#define EXPOSURE_IDX 3
+	{
+		{
+			.id		= V4L2_CID_EXPOSURE,
+			.type		= V4L2_CTRL_TYPE_INTEGER,
+			.name		= "Exposure",
+			.minimum	= 0,
+			.maximum	= 511,
+			.step		= 1,
+			.default_value  = 12
+		},
+		.set = pb0100_set_exposure,
+		.get = pb0100_get_exposure
+	},
+#define AUTOGAIN_IDX 4
+	{
+		{
+			.id		= V4L2_CID_AUTOGAIN,
+			.type		= V4L2_CTRL_TYPE_BOOLEAN,
+			.name		= "Automatic Gain and Exposure",
+			.minimum	= 0,
+			.maximum	= 1,
+			.step		= 1,
+			.default_value  = 1
+		},
+		.set = pb0100_set_autogain,
+		.get = pb0100_get_autogain
+	},
+#define AUTOGAIN_TARGET_IDX 5
+	{
+		{
+			.id		= V4L2_CTRL_CLASS_USER + 0x1000,
+			.type		= V4L2_CTRL_TYPE_INTEGER,
+			.name		= "Automatic Gain Target",
+			.minimum	= 0,
+			.maximum	= 255,
+			.step		= 1,
+			.default_value  = 128
+		},
+		.set = pb0100_set_autogain_target,
+		.get = pb0100_get_autogain_target
+	},
+#define NATURAL_IDX 6
+	{
+		{
+			.id		= V4L2_CTRL_CLASS_USER + 0x1001,
+			.type		= V4L2_CTRL_TYPE_BOOLEAN,
+			.name		= "Natural Light Source",
+			.minimum	= 0,
+			.maximum	= 1,
+			.step		= 1,
+			.default_value  = 1
+		},
+		.set = pb0100_set_natural,
+		.get = pb0100_get_natural
+	},
+	},
+
+	.init = pb0100_init,
+	.probe = pb0100_probe,
+	.start = pb0100_start,
+	.stop = pb0100_stop,
+	.dump = pb0100_dump,
+
+	.nmodes = 2,
+	.modes = {
+/* low res / subsample modes disabled as they are only half res horizontal,
+   halving the vertical resolution does not seem to work */
+#if 0
+	{
+		160,
+		120,
+		V4L2_PIX_FMT_SGRBG8,
+		V4L2_FIELD_NONE,
+		.sizeimage = 160 * 120 * 2,
+		.bytesperline = 160,
+		.colorspace = V4L2_COLORSPACE_SRGB,
+		.priv = PB0100_CROP_TO_VGA | PB0100_SUBSAMPLE
+	},
+	{
+		176,
+		144,
+		V4L2_PIX_FMT_SGRBG8,
+		V4L2_FIELD_NONE,
+		.sizeimage = 176 * 144 * 2,
+		.bytesperline = 176,
+		.colorspace = V4L2_COLORSPACE_SRGB,
+		.priv = PB0100_SUBSAMPLE
+	},
+#endif
+	{
+		320,
+		240,
+		V4L2_PIX_FMT_SGRBG8,
+		V4L2_FIELD_NONE,
+		.sizeimage = 320 * 240,
+		.bytesperline = 320,
+		.colorspace = V4L2_COLORSPACE_SRGB,
+		.priv = PB0100_CROP_TO_VGA
+	},
+	{
+		352,
+		288,
+		V4L2_PIX_FMT_SGRBG8,
+		V4L2_FIELD_NONE,
+		.sizeimage = 352 * 288,
+		.bytesperline = 352,
+		.colorspace = V4L2_COLORSPACE_SRGB,
+		.priv = 0
+	},
+	}
+};
+
+#endif
diff --git a/linux/drivers/media/video/gspca/stv06xx/stv06xx_sensor.h b/linux/drivers/media/video/gspca/stv06xx/stv06xx_sensor.h
new file mode 100644
index 000000000..c726dacef
--- /dev/null
+++ b/linux/drivers/media/video/gspca/stv06xx/stv06xx_sensor.h
@@ -0,0 +1,92 @@
+/*
+ * Copyright (c) 2001 Jean-Fredric Clere, Nikolas Zimmermann, Georg Acher
+ *		      Mark Cave-Ayland, Carlo E Prelz, Dick Streefland
+ * Copyright (c) 2002, 2003 Tuukka Toivonen
+ * Copyright (c) 2008 Erik Andrén
+ *
+ * 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
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ * P/N 861037:      Sensor HDCS1000        ASIC STV0600
+ * P/N 861050-0010: Sensor HDCS1000        ASIC STV0600
+ * P/N 861050-0020: Sensor Photobit PB100  ASIC STV0600-1 - QuickCam Express
+ * P/N 861055:      Sensor ST VV6410       ASIC STV0610   - LEGO cam
+ * P/N 861075-0040: Sensor HDCS1000        ASIC
+ * P/N 961179-0700: Sensor ST VV6410       ASIC STV0602   - Dexxa WebCam USB
+ * P/N 861040-0000: Sensor ST VV6410       ASIC STV0610   - QuickCam Web
+ */
+
+#ifndef STV06XX_SENSOR_H_
+#define STV06XX_SENSOR_H_
+
+#include "stv06xx.h"
+
+#define IS_850(sd)	((sd)->gspca_dev.dev->descriptor.idProduct == 0x850)
+#define IS_870(sd)	((sd)->gspca_dev.dev->descriptor.idProduct == 0x870)
+#define IS_1020(sd)	((sd)->sensor == &stv06xx_sensor_hdcs1020)
+
+extern const struct stv06xx_sensor stv06xx_sensor_vv6410;
+extern const struct stv06xx_sensor stv06xx_sensor_hdcs1x00;
+extern const struct stv06xx_sensor stv06xx_sensor_hdcs1020;
+extern const struct stv06xx_sensor stv06xx_sensor_pb0100;
+
+#define STV06XX_MAX_CTRLS		(V4L2_CID_LASTP1 - V4L2_CID_BASE + 10)
+
+struct stv06xx_sensor {
+	/* Defines the name of a sensor */
+	char name[32];
+
+	/* Sensor i2c address */
+	u8 i2c_addr;
+
+	/* Flush value*/
+	u8 i2c_flush;
+
+	/* length of an i2c word */
+	u8 i2c_len;
+
+	/* Probes if the sensor is connected */
+	int (*probe)(struct sd *sd);
+
+	/* Performs a initialization sequence */
+	int (*init)(struct sd *sd);
+
+	/* Executed at device disconnect */
+	void (*disconnect)(struct sd *sd);
+
+	/* Reads a sensor register */
+	int (*read_sensor)(struct sd *sd, const u8 address,
+	      u8 *i2c_data, const u8 len);
+
+	/* Writes to a sensor register */
+	int (*write_sensor)(struct sd *sd, const u8 address,
+	      u8 *i2c_data, const u8 len);
+
+	/* Instructs the sensor to start streaming */
+	int (*start)(struct sd *sd);
+
+	/* Instructs the sensor to stop streaming */
+	int (*stop)(struct sd *sd);
+
+	/* Instructs the sensor to dump all its contents */
+	int (*dump)(struct sd *sd);
+
+	int nctrls;
+	struct ctrl ctrls[STV06XX_MAX_CTRLS];
+
+	char nmodes;
+	struct v4l2_pix_format modes[];
+};
+
+#endif
diff --git a/linux/drivers/media/video/gspca/stv06xx/stv06xx_vv6410.c b/linux/drivers/media/video/gspca/stv06xx/stv06xx_vv6410.c
new file mode 100644
index 000000000..1ca91f2a6
--- /dev/null
+++ b/linux/drivers/media/video/gspca/stv06xx/stv06xx_vv6410.c
@@ -0,0 +1,251 @@
+/*
+ * Copyright (c) 2001 Jean-Fredric Clere, Nikolas Zimmermann, Georg Acher
+ *		      Mark Cave-Ayland, Carlo E Prelz, Dick Streefland
+ * Copyright (c) 2002, 2003 Tuukka Toivonen
+ * Copyright (c) 2008 Erik Andrén
+ *
+ * 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
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ * P/N 861037:      Sensor HDCS1000        ASIC STV0600
+ * P/N 861050-0010: Sensor HDCS1000        ASIC STV0600
+ * P/N 861050-0020: Sensor Photobit PB100  ASIC STV0600-1 - QuickCam Express
+ * P/N 861055:      Sensor ST VV6410       ASIC STV0610   - LEGO cam
+ * P/N 861075-0040: Sensor HDCS1000        ASIC
+ * P/N 961179-0700: Sensor ST VV6410       ASIC STV0602   - Dexxa WebCam USB
+ * P/N 861040-0000: Sensor ST VV6410       ASIC STV0610   - QuickCam Web
+ */
+
+#include "stv06xx_vv6410.h"
+
+static int vv6410_probe(struct sd *sd)
+{
+	u16 data;
+	int err;
+
+	err = stv06xx_read_sensor(sd, VV6410_DEVICEH, &data);
+
+	if (err < 0)
+		return -ENODEV;
+
+	if (data == 0x19) {
+		info("vv6410 sensor detected");
+
+		sd->gspca_dev.cam.cam_mode = stv06xx_sensor_vv6410.modes;
+		sd->gspca_dev.cam.nmodes = stv06xx_sensor_vv6410.nmodes;
+		sd->desc.ctrls = stv06xx_sensor_vv6410.ctrls;
+		sd->desc.nctrls = stv06xx_sensor_vv6410.nctrls;
+		return 0;
+	}
+
+	return -ENODEV;
+}
+
+static int vv6410_init(struct sd *sd)
+{
+	int err = 0, i;
+
+	for (i = 0; i < ARRAY_SIZE(stv_bridge_init); i++) {
+		/* if NULL then len contains single value */
+		if (stv_bridge_init[i].data == NULL) {
+			err = stv06xx_write_bridge(sd,
+				stv_bridge_init[i].start,
+				stv_bridge_init[i].len);
+		} else {
+			int j;
+			for (j = 0; j < stv_bridge_init[i].len; j++)
+				err = stv06xx_write_bridge(sd,
+					stv_bridge_init[i].start + j,
+					stv_bridge_init[i].data[j]);
+		}
+	}
+
+	if (err < 0)
+		return err;
+
+	err = stv06xx_write_sensor_bytes(sd, (u8 *) vv6410_sensor_init,
+					 ARRAY_SIZE(vv6410_sensor_init));
+
+	return (err < 0) ? err : 0;
+}
+
+static int vv6410_start(struct sd *sd)
+{
+	int err;
+	struct cam *cam = &sd->gspca_dev.cam;
+	u32 priv = cam->cam_mode[sd->gspca_dev.curr_mode].priv;
+
+	if (priv & VV6410_CROP_TO_QVGA) {
+		PDEBUG(D_CONF, "Cropping to QVGA");
+		stv06xx_write_sensor(sd, VV6410_XENDH, 320 - 1);
+		stv06xx_write_sensor(sd, VV6410_YENDH, 240 - 1);
+	} else {
+		stv06xx_write_sensor(sd, VV6410_XENDH, 360 - 1);
+		stv06xx_write_sensor(sd, VV6410_YENDH, 294 - 1);
+	}
+
+	if (priv & VV6410_SUBSAMPLE) {
+		PDEBUG(D_CONF, "Enabling subsampling");
+		stv06xx_write_bridge(sd, STV_Y_CTRL, 0x02);
+		stv06xx_write_bridge(sd, STV_X_CTRL, 0x06);
+
+		stv06xx_write_bridge(sd, STV_SCAN_RATE, 0x10);
+	} else {
+		stv06xx_write_bridge(sd, STV_Y_CTRL, 0x01);
+		stv06xx_write_bridge(sd, STV_X_CTRL, 0x0a);
+
+		stv06xx_write_bridge(sd, STV_SCAN_RATE, 0x20);
+	}
+
+	/* Turn on LED */
+	err = stv06xx_write_bridge(sd, STV_LED_CTRL, LED_ON);
+	if (err < 0)
+		return err;
+
+	err = stv06xx_write_sensor(sd, VV6410_SETUP0, 0);
+	if (err < 0)
+		return err;
+
+	PDEBUG(D_STREAM, "Starting stream");
+
+	return 0;
+}
+
+static int vv6410_stop(struct sd *sd)
+{
+	int err;
+
+	/* Turn off LED */
+	err = stv06xx_write_bridge(sd, STV_LED_CTRL, LED_OFF);
+	if (err < 0)
+		return err;
+
+	err = stv06xx_write_sensor(sd, VV6410_SETUP0, VV6410_LOW_POWER_MODE);
+	if (err < 0)
+		return err;
+
+	PDEBUG(D_STREAM, "Halting stream");
+
+	return (err < 0) ? err : 0;
+}
+
+static int vv6410_dump(struct sd *sd)
+{
+	u8 i;
+	int err = 0;
+
+	info("Dumping all vv6410 sensor registers");
+	for (i = 0; i < 0xff && !err; i++) {
+		u16 data;
+		err = stv06xx_read_sensor(sd, i, &data);
+		info("Register 0x%x contained 0x%x", i, data);
+	}
+	return (err < 0) ? err : 0;
+}
+
+static int vv6410_get_hflip(struct gspca_dev *gspca_dev, __s32 *val)
+{
+	int err;
+	u16 i2c_data;
+	struct sd *sd = (struct sd *) gspca_dev;
+
+	err = stv06xx_read_sensor(sd, VV6410_DATAFORMAT, &i2c_data);
+
+	*val = (i2c_data & VV6410_HFLIP) ? 1 : 0;
+
+	PDEBUG(D_V4L2, "Read horizontal flip %d", *val);
+
+	return (err < 0) ? err : 0;
+}
+
+static int vv6410_set_hflip(struct gspca_dev *gspca_dev, __s32 val)
+{
+	int err;
+	u16 i2c_data;
+	struct sd *sd = (struct sd *) gspca_dev;
+	err = stv06xx_read_sensor(sd, VV6410_DATAFORMAT, &i2c_data);
+	if (err < 0)
+		return err;
+
+	if (val)
+		i2c_data |= VV6410_HFLIP;
+	else
+		i2c_data &= ~VV6410_HFLIP;
+
+	PDEBUG(D_V4L2, "Set horizontal flip to %d", val);
+	err = stv06xx_write_sensor(sd, VV6410_DATAFORMAT, i2c_data);
+
+	return (err < 0) ? err : 0;
+}
+
+static int vv6410_get_vflip(struct gspca_dev *gspca_dev, __s32 *val)
+{
+	int err;
+	u16 i2c_data;
+	struct sd *sd = (struct sd *) gspca_dev;
+
+	err = stv06xx_read_sensor(sd, VV6410_DATAFORMAT, &i2c_data);
+
+	*val = (i2c_data & VV6410_VFLIP) ? 1 : 0;
+
+	PDEBUG(D_V4L2, "Read vertical flip %d", *val);
+
+	return (err < 0) ? err : 0;
+}
+
+static int vv6410_set_vflip(struct gspca_dev *gspca_dev, __s32 val)
+{
+	int err;
+	u16 i2c_data;
+	struct sd *sd = (struct sd *) gspca_dev;
+	err = stv06xx_read_sensor(sd, VV6410_DATAFORMAT, &i2c_data);
+	if (err < 0)
+		return err;
+
+	if (val)
+		i2c_data |= VV6410_VFLIP;
+	else
+		i2c_data &= ~VV6410_VFLIP;
+
+	PDEBUG(D_V4L2, "Set vertical flip to %d", val);
+	err = stv06xx_write_sensor(sd, VV6410_DATAFORMAT, i2c_data);
+
+	return (err < 0) ? err : 0;
+}
+
+static int vv6410_get_analog_gain(struct gspca_dev *gspca_dev, __s32 *val)
+{
+	int err;
+	u16 i2c_data;
+	struct sd *sd = (struct sd *) gspca_dev;
+
+	err = stv06xx_read_sensor(sd, VV6410_ANALOGGAIN, &i2c_data);
+
+	*val = i2c_data & 0xf;
+
+	PDEBUG(D_V4L2, "Read analog gain %d", *val);
+
+	return (err < 0) ? err : 0;
+}
+
+static int vv6410_set_analog_gain(struct gspca_dev *gspca_dev, __s32 val)
+{
+	int err;
+	struct sd *sd = (struct sd *) gspca_dev;
+
+	PDEBUG(D_V4L2, "Set analog gain to %d", val);
+	err = stv06xx_write_sensor(sd, VV6410_ANALOGGAIN, 0xf0 | (val & 0xf));
+
+	return (err < 0) ? err : 0;
+}
diff --git a/linux/drivers/media/video/gspca/stv06xx/stv06xx_vv6410.h b/linux/drivers/media/video/gspca/stv06xx/stv06xx_vv6410.h
new file mode 100644
index 000000000..3ff8c4ea3
--- /dev/null
+++ b/linux/drivers/media/video/gspca/stv06xx/stv06xx_vv6410.h
@@ -0,0 +1,315 @@
+/*
+ * Copyright (c) 2001 Jean-Fredric Clere, Nikolas Zimmermann, Georg Acher
+ *		      Mark Cave-Ayland, Carlo E Prelz, Dick Streefland
+ * Copyright (c) 2002, 2003 Tuukka Toivonen
+ * Copyright (c) 2008 Erik Andrén
+ *
+ * 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
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ * P/N 861037:      Sensor HDCS1000        ASIC STV0600
+ * P/N 861050-0010: Sensor HDCS1000        ASIC STV0600
+ * P/N 861050-0020: Sensor Photobit PB100  ASIC STV0600-1 - QuickCam Express
+ * P/N 861055:      Sensor ST VV6410       ASIC STV0610   - LEGO cam
+ * P/N 861075-0040: Sensor HDCS1000        ASIC
+ * P/N 961179-0700: Sensor ST VV6410       ASIC STV0602   - Dexxa WebCam USB
+ * P/N 861040-0000: Sensor ST VV6410       ASIC STV0610   - QuickCam Web
+ */
+
+#ifndef STV06XX_VV6410_H_
+#define STV06XX_VV6410_H_
+
+#include "stv06xx_sensor.h"
+
+#define VV6410_COLS			416
+#define VV6410_ROWS			320
+
+/* Status registers */
+/* Chip identification number including revision indicator */
+#define VV6410_DEVICEH			0x00
+#define VV6410_DEVICEL			0x01
+
+/* User can determine whether timed I2C data
+   has been consumed by interrogating flag states */
+#define VV6410_STATUS0			0x02
+
+/* Current line counter value */
+#define VV6410_LINECOUNTH		0x03
+#define VV6410_LINECOUNTL		0x04
+
+/* End x coordinate of image size */
+#define VV6410_XENDH			0x05
+#define VV6410_XENDL			0x06
+
+/* End y coordinate of image size */
+#define VV6410_YENDH			0x07
+#define VV6410_YENDL			0x08
+
+/* This is the average pixel value returned from the
+   dark line offset cancellation algorithm */
+#define VV6410_DARKAVGH			0x09
+#define VV6410_DARKAVGL			0x0a
+
+/* This is the average pixel value returned from the
+   black line offset cancellation algorithm  */
+#define VV6410_BLACKAVGH		0x0b
+#define VV6410_BLACKAVGL		0x0c
+
+/* Flags to indicate whether the x or y image coordinates have been clipped */
+#define VV6410_STATUS1			0x0d
+
+/* Setup registers */
+
+/* Low-power/sleep modes & video timing */
+#define VV6410_SETUP0			0x10
+
+/* Various parameters */
+#define VV6410_SETUP1			0x11
+
+/* Contains pixel counter reset value used by external sync */
+#define VV6410_SYNCVALUE		0x12
+
+/* Frame grabbing modes (FST, LST and QCK) */
+#define VV6410_FGMODES			0x14
+
+/* FST and QCK mapping modes. */
+#define VV6410_PINMAPPING		0x15
+
+/* Data resolution */
+#define VV6410_DATAFORMAT		0x16
+
+/* Output coding formats */
+#define VV6410_OPFORMAT			0x17
+
+/* Various mode select bits */
+#define VV6410_MODESELECT		0x18
+
+/* Exposure registers */
+/* Fine exposure. */
+#define VV6410_FINEH			0x20
+#define VV6410_FINEL			0x21
+
+/* Coarse exposure */
+#define VV6410_COARSEH			0x22
+#define VV6410_COARSEL			0x23
+
+/* Analog gain setting */
+#define VV6410_ANALOGGAIN		0x24
+
+/* Clock division */
+#define VV6410_CLKDIV			0x25
+
+/* Dark line offset cancellation value */
+#define VV6410_DARKOFFSETH		0x2c
+#define VV6410_DARKOFFSETL		0x2d
+
+/* Dark line offset cancellation enable */
+#define VV6410_DARKOFFSETSETUP		0x2e
+
+/* Video timing registers */
+/* Line Length (Pixel Clocks) */
+#define VV6410_LINELENGTHH		0x52
+#define VV6410_LINELENGTHL		0x53
+
+/* X-co-ordinate of top left corner of region of interest (x-offset) */
+#define VV6410_XOFFSETH			0x57
+#define VV6410_XOFFSETL			0x58
+
+/* Y-coordinate of top left corner of region of interest (y-offset) */
+#define VV6410_YOFFSETH			0x59
+#define VV6410_YOFFSETL			0x5a
+
+/* Field length (Lines) */
+#define VV6410_FIELDLENGTHH		0x61
+#define VV6410_FIELDLENGTHL		0x62
+
+/* System registers */
+/* Black offset cancellation default value */
+#define VV6410_BLACKOFFSETH		0x70
+#define VV6410_BLACKOFFSETL		0x71
+
+/* Black offset cancellation setup */
+#define VV6410_BLACKOFFSETSETUP		0x72
+
+/* Analog Control Register 0 */
+#define VV6410_CR0			0x75
+
+/* Analog Control Register 1 */
+#define VV6410_CR1			0x76
+
+/* ADC Setup Register */
+#define VV6410_AS0			0x77
+
+/* Analog Test Register */
+#define VV6410_AT0			0x78
+
+/* Audio Amplifier Setup Register */
+#define VV6410_AT1			0x79
+
+#define VV6410_HFLIP 			(1 << 3)
+#define VV6410_VFLIP 			(1 << 4)
+
+#define VV6410_LOW_POWER_MODE		(1 << 0)
+#define VV6410_SOFT_RESET		(1 << 2)
+#define VV6410_PAL_25_FPS		(0 << 3)
+
+#define VV6410_CLK_DIV_2		(1 << 1)
+
+#define VV6410_FINE_EXPOSURE		320
+#define VV6410_COARSE_EXPOSURE		192
+#define VV6410_DEFAULT_GAIN		5
+
+#define VV6410_SUBSAMPLE		0x01
+#define VV6410_CROP_TO_QVGA		0x02
+
+static int vv6410_probe(struct sd *sd);
+static int vv6410_start(struct sd *sd);
+static int vv6410_init(struct sd *sd);
+static int vv6410_stop(struct sd *sd);
+static int vv6410_dump(struct sd *sd);
+
+/* V4L2 controls supported by the driver */
+static int vv6410_get_hflip(struct gspca_dev *gspca_dev, __s32 *val);
+static int vv6410_set_hflip(struct gspca_dev *gspca_dev, __s32 val);
+static int vv6410_get_vflip(struct gspca_dev *gspca_dev, __s32 *val);
+static int vv6410_set_vflip(struct gspca_dev *gspca_dev, __s32 val);
+static int vv6410_get_analog_gain(struct gspca_dev *gspca_dev, __s32 *val);
+static int vv6410_set_analog_gain(struct gspca_dev *gspca_dev, __s32 val);
+
+const struct stv06xx_sensor stv06xx_sensor_vv6410 = {
+	.name = "ST VV6410",
+	.i2c_flush = 5,
+	.i2c_addr = 0x20,
+	.i2c_len = 1,
+	.init = vv6410_init,
+	.probe = vv6410_probe,
+	.start = vv6410_start,
+	.stop = vv6410_stop,
+	.dump = vv6410_dump,
+
+	.nctrls = 3,
+	.ctrls = {
+	{
+		{
+			.id		= V4L2_CID_HFLIP,
+			.type		= V4L2_CTRL_TYPE_BOOLEAN,
+			.name		= "horizontal flip",
+			.minimum	= 0,
+			.maximum	= 1,
+			.step		= 1,
+			.default_value	= 0
+		},
+		.set = vv6410_set_hflip,
+		.get = vv6410_get_hflip
+	}, {
+		{
+			.id		= V4L2_CID_VFLIP,
+			.type		= V4L2_CTRL_TYPE_BOOLEAN,
+			.name		= "vertical flip",
+			.minimum	= 0,
+			.maximum	= 1,
+			.step		= 1,
+			.default_value 	= 0
+		},
+		.set = vv6410_set_vflip,
+		.get = vv6410_get_vflip
+	}, {
+		{
+			.id		= V4L2_CID_GAIN,
+			.type		= V4L2_CTRL_TYPE_INTEGER,
+			.name		= "analog gain",
+			.minimum	= 0,
+			.maximum	= 15,
+			.step		= 1,
+			.default_value  = 0
+		},
+		.set = vv6410_set_analog_gain,
+		.get = vv6410_get_analog_gain
+	}
+	},
+
+	.nmodes = 1,
+	.modes = {
+	{
+		356,
+		292,
+		V4L2_PIX_FMT_SGRBG8,
+		V4L2_FIELD_NONE,
+		.sizeimage =
+			356 * 292,
+		.bytesperline = 356,
+		.colorspace = V4L2_COLORSPACE_SRGB,
+		.priv = 0
+	}
+	}
+};
+
+/* If NULL, only single value to write, stored in len */
+struct stv_init {
+	const u8 *data;
+	u16 start;
+	u8 len;
+};
+
+static const u8 x1500[] = {	/* 0x1500 - 0x150f */
+	0x0b, 0xa7, 0xb7, 0x00, 0x00
+};
+
+static const u8 x1536[] = {	/* 0x1536 - 0x153b */
+	0x02, 0x00, 0x60, 0x01, 0x20, 0x01
+};
+
+static const u8 x15c1[] = {	/* 0x15c1 - 0x15c2 */
+	0xff, 0x03 /* Output word 0x03ff = 1023 (ISO size) */
+};
+
+static const struct stv_init stv_bridge_init[] = {
+	/* This reg is written twice. Some kind of reset? */
+	{NULL,  0x1620, 0x80},
+	{NULL,  0x1620, 0x00},
+	{NULL,  0x1423, 0x04},
+	{x1500, 0x1500, ARRAY_SIZE(x1500)},
+	{x1536, 0x1536, ARRAY_SIZE(x1536)},
+	{x15c1, 0x15c1, ARRAY_SIZE(x15c1)}
+};
+
+static const u8 vv6410_sensor_init[][2] = {
+	/* Setup registers */
+	{VV6410_SETUP0,		VV6410_SOFT_RESET},
+	{VV6410_SETUP0,		VV6410_LOW_POWER_MODE},
+	/* Use shuffled read-out mode */
+	{VV6410_SETUP1,		BIT(6)},
+	/* All modes to 1 */
+	{VV6410_FGMODES,	BIT(6) | BIT(4) | BIT(2) | BIT(0)},
+	{VV6410_PINMAPPING,	0x00},
+	/* Pre-clock generator divide off */
+	{VV6410_DATAFORMAT,	BIT(7) | BIT(0)},
+
+	/* Exposure registers */
+	{VV6410_FINEH,		VV6410_FINE_EXPOSURE >> 8},
+	{VV6410_FINEL,		VV6410_FINE_EXPOSURE & 0xff},
+	{VV6410_COARSEH,	VV6410_COARSE_EXPOSURE >> 8},
+	{VV6410_COARSEL,	VV6410_COARSE_EXPOSURE & 0xff},
+	{VV6410_ANALOGGAIN,	0xf0 | VV6410_DEFAULT_GAIN},
+	{VV6410_CLKDIV,		VV6410_CLK_DIV_2},
+
+	/* System registers */
+	/* Enable voltage doubler */
+	{VV6410_AS0,		BIT(6) | BIT(4) | BIT(3) | BIT(2) | BIT(1)},
+	{VV6410_AT0,		0x00},
+	/* Power up audio, differential */
+	{VV6410_AT1,		BIT(4)|BIT(0)},
+};
+
+#endif
diff --git a/linux/drivers/media/video/gspca/sunplus.c b/linux/drivers/media/video/gspca/sunplus.c
index 3a972ff1c..3692062d0 100644
--- a/linux/drivers/media/video/gspca/sunplus.c
+++ b/linux/drivers/media/video/gspca/sunplus.c
@@ -123,7 +123,7 @@ static struct ctrl sd_ctrls[] = {
 	},
 };
 
-static struct v4l2_pix_format vga_mode[] = {
+static const struct v4l2_pix_format vga_mode[] = {
 	{320, 240, V4L2_PIX_FMT_JPEG, V4L2_FIELD_NONE,
 		.bytesperline = 320,
 		.sizeimage = 320 * 240 * 3 / 8 + 590,
@@ -136,7 +136,7 @@ static struct v4l2_pix_format vga_mode[] = {
 		.priv = 1},
 };
 
-static struct v4l2_pix_format custom_mode[] = {
+static const struct v4l2_pix_format custom_mode[] = {
 	{320, 240, V4L2_PIX_FMT_JPEG, V4L2_FIELD_NONE,
 		.bytesperline = 320,
 		.sizeimage = 320 * 240 * 3 / 8 + 590,
@@ -149,7 +149,7 @@ static struct v4l2_pix_format custom_mode[] = {
 		.priv = 1},
 };
 
-static struct v4l2_pix_format vga_mode2[] = {
+static const struct v4l2_pix_format vga_mode2[] = {
 	{176, 144, V4L2_PIX_FMT_JPEG, V4L2_FIELD_NONE,
 		.bytesperline = 176,
 		.sizeimage = 176 * 144 * 3 / 8 + 590,
diff --git a/linux/drivers/media/video/gspca/t613.c b/linux/drivers/media/video/gspca/t613.c
index d2d6ad9d1..2d032aa0e 100644
--- a/linux/drivers/media/video/gspca/t613.c
+++ b/linux/drivers/media/video/gspca/t613.c
@@ -233,7 +233,7 @@ static char *effects_control[] = {
 	"Negative",
 };
 
-static struct v4l2_pix_format vga_mode_t16[] = {
+static const struct v4l2_pix_format vga_mode_t16[] = {
 	{160, 120, V4L2_PIX_FMT_JPEG, V4L2_FIELD_NONE,
 		.bytesperline = 160,
 		.sizeimage = 160 * 120 * 4 / 8 + 590,
@@ -499,7 +499,7 @@ static void om6802_sensor_init(struct gspca_dev *gspca_dev)
 	reg_w_buf(gspca_dev, sensor_reset, sizeof sensor_reset);
 	msleep(5);
 	i = 4;
-	while (--i < 0) {
+	while (--i > 0) {
 		byte = reg_r(gspca_dev, 0x0060);
 		if (!(byte & 0x01))
 			break;
diff --git a/linux/drivers/media/video/gspca/tv8532.c b/linux/drivers/media/video/gspca/tv8532.c
index 4e18f90ba..7d11a0d3e 100644
--- a/linux/drivers/media/video/gspca/tv8532.c
+++ b/linux/drivers/media/video/gspca/tv8532.c
@@ -73,7 +73,7 @@ static struct ctrl sd_ctrls[] = {
 	 },
 };
 
-static struct v4l2_pix_format sif_mode[] = {
+static const struct v4l2_pix_format sif_mode[] = {
 	{176, 144, V4L2_PIX_FMT_SBGGR8, V4L2_FIELD_NONE,
 		.bytesperline = 176,
 		.sizeimage = 176 * 144,
diff --git a/linux/drivers/media/video/gspca/vc032x.c b/linux/drivers/media/video/gspca/vc032x.c
index 4cc1f69f1..82dc818f4 100644
--- a/linux/drivers/media/video/gspca/vc032x.c
+++ b/linux/drivers/media/video/gspca/vc032x.c
@@ -126,7 +126,7 @@ static struct ctrl sd_ctrls[] = {
 	 },
 };
 
-static struct v4l2_pix_format vc0321_mode[] = {
+static const struct v4l2_pix_format vc0321_mode[] = {
 	{320, 240, V4L2_PIX_FMT_YVYU, V4L2_FIELD_NONE,
 		.bytesperline = 320,
 		.sizeimage = 320 * 240 * 2,
@@ -138,7 +138,7 @@ static struct v4l2_pix_format vc0321_mode[] = {
 		.colorspace = V4L2_COLORSPACE_SRGB,
 		.priv = 0},
 };
-static struct v4l2_pix_format vc0323_mode[] = {
+static const struct v4l2_pix_format vc0323_mode[] = {
 	{320, 240, V4L2_PIX_FMT_JPEG, V4L2_FIELD_NONE,
 		.bytesperline = 320,
 		.sizeimage = 320 * 240 * 3 / 8 + 590,
@@ -151,7 +151,7 @@ static struct v4l2_pix_format vc0323_mode[] = {
 		.priv = 0},
 };
 
-static struct v4l2_pix_format svga_mode[] = {
+static const struct v4l2_pix_format svga_mode[] = {
 	{800, 600, V4L2_PIX_FMT_JPEG, V4L2_FIELD_NONE,
 		.bytesperline = 800,
 		.sizeimage = 800 * 600 * 1 / 4 + 590,
@@ -2452,6 +2452,7 @@ static const struct sd_desc sd_desc = {
 static const __devinitdata struct usb_device_id device_table[] = {
 	{USB_DEVICE(0x046d, 0x0892), .driver_info = BRIDGE_VC0321},
 	{USB_DEVICE(0x046d, 0x0896), .driver_info = BRIDGE_VC0321},
+	{USB_DEVICE(0x046d, 0x0897), .driver_info = BRIDGE_VC0321},
 	{USB_DEVICE(0x0ac8, 0x0321), .driver_info = BRIDGE_VC0321},
 	{USB_DEVICE(0x0ac8, 0x0323), .driver_info = BRIDGE_VC0323},
 	{USB_DEVICE(0x0ac8, 0x0328), .driver_info = BRIDGE_VC0321},
diff --git a/linux/drivers/media/video/gspca/zc3xx.c b/linux/drivers/media/video/gspca/zc3xx.c
index 678070b93..79d2a776d 100644
--- a/linux/drivers/media/video/gspca/zc3xx.c
+++ b/linux/drivers/media/video/gspca/zc3xx.c
@@ -173,7 +173,7 @@ static struct ctrl sd_ctrls[] = {
 	},
 };
 
-static struct v4l2_pix_format vga_mode[] = {
+static const struct v4l2_pix_format vga_mode[] = {
 	{320, 240, V4L2_PIX_FMT_JPEG, V4L2_FIELD_NONE,
 		.bytesperline = 320,
 		.sizeimage = 320 * 240 * 3 / 8 + 590,
@@ -186,7 +186,7 @@ static struct v4l2_pix_format vga_mode[] = {
 		.priv = 0},
 };
 
-static struct v4l2_pix_format sif_mode[] = {
+static const struct v4l2_pix_format sif_mode[] = {
 	{176, 144, V4L2_PIX_FMT_JPEG, V4L2_FIELD_NONE,
 		.bytesperline = 176,
 		.sizeimage = 176 * 144 * 3 / 8 + 590,
@@ -7542,6 +7542,7 @@ static const __devinitdata struct usb_device_id device_table[] = {
 	{USB_DEVICE(0x0458, 0x700c)},
 	{USB_DEVICE(0x0458, 0x700f)},
 	{USB_DEVICE(0x0461, 0x0a00)},
+	{USB_DEVICE(0x046d, 0x089d), .driver_info = SENSOR_MC501CB},
 	{USB_DEVICE(0x046d, 0x08a0)},
 	{USB_DEVICE(0x046d, 0x08a1)},
 	{USB_DEVICE(0x046d, 0x08a2)},
diff --git a/linux/drivers/media/video/mt9m001.c b/linux/drivers/media/video/mt9m001.c
index edacba723..dd1435d41 100644
--- a/linux/drivers/media/video/mt9m001.c
+++ b/linux/drivers/media/video/mt9m001.c
@@ -272,17 +272,16 @@ static int mt9m001_set_bus_param(struct soc_camera_device *icd,
 static unsigned long mt9m001_query_bus_param(struct soc_camera_device *icd)
 {
 	struct mt9m001 *mt9m001 = container_of(icd, struct mt9m001, icd);
-	unsigned int width_flag = SOCAM_DATAWIDTH_10;
+	struct soc_camera_link *icl = mt9m001->client->dev.platform_data;
+	/* MT9M001 has all capture_format parameters fixed */
+	unsigned long flags = SOCAM_DATAWIDTH_10 | SOCAM_PCLK_SAMPLE_RISING |
+		SOCAM_HSYNC_ACTIVE_HIGH | SOCAM_VSYNC_ACTIVE_HIGH |
+		SOCAM_MASTER;
 
 	if (bus_switch_possible(mt9m001))
-		width_flag |= SOCAM_DATAWIDTH_8;
+		flags |= SOCAM_DATAWIDTH_8;
 
-	/* MT9M001 has all capture_format parameters fixed */
-	return SOCAM_PCLK_SAMPLE_RISING |
-		SOCAM_HSYNC_ACTIVE_HIGH |
-		SOCAM_VSYNC_ACTIVE_HIGH |
-		SOCAM_MASTER |
-		width_flag;
+	return soc_camera_apply_sensor_flags(icl, flags);
 }
 
 static int mt9m001_set_fmt(struct soc_camera_device *icd,
@@ -328,15 +327,17 @@ static int mt9m001_set_fmt(struct soc_camera_device *icd,
 static int mt9m001_try_fmt(struct soc_camera_device *icd,
 			   struct v4l2_format *f)
 {
-	if (f->fmt.pix.height < 32 + icd->y_skip_top)
-		f->fmt.pix.height = 32 + icd->y_skip_top;
-	if (f->fmt.pix.height > 1024 + icd->y_skip_top)
-		f->fmt.pix.height = 1024 + icd->y_skip_top;
-	if (f->fmt.pix.width < 48)
-		f->fmt.pix.width = 48;
-	if (f->fmt.pix.width > 1280)
-		f->fmt.pix.width = 1280;
-	f->fmt.pix.width &= ~0x01; /* has to be even, unsure why was ~3 */
+	struct v4l2_pix_format *pix = &f->fmt.pix;
+
+	if (pix->height < 32 + icd->y_skip_top)
+		pix->height = 32 + icd->y_skip_top;
+	if (pix->height > 1024 + icd->y_skip_top)
+		pix->height = 1024 + icd->y_skip_top;
+	if (pix->width < 48)
+		pix->width = 48;
+	if (pix->width > 1280)
+		pix->width = 1280;
+	pix->width &= ~0x01; /* has to be even, unsure why was ~3 */
 
 	return 0;
 }
@@ -578,6 +579,7 @@ static int mt9m001_set_control(struct soc_camera_device *icd, struct v4l2_contro
 static int mt9m001_video_probe(struct soc_camera_device *icd)
 {
 	struct mt9m001 *mt9m001 = container_of(icd, struct mt9m001, icd);
+	struct soc_camera_link *icl = mt9m001->client->dev.platform_data;
 	s32 data;
 	int ret;
 
@@ -588,7 +590,7 @@ static int mt9m001_video_probe(struct soc_camera_device *icd)
 		return -ENODEV;
 
 	/* Enable the chip */
-	data = reg_write(&mt9m001->icd, MT9M001_CHIP_ENABLE, 1);
+	data = reg_write(icd, MT9M001_CHIP_ENABLE, 1);
 	dev_dbg(&icd->dev, "write: %d\n", data);
 
 	/* Read out the chip version register */
@@ -600,7 +602,7 @@ static int mt9m001_video_probe(struct soc_camera_device *icd)
 	case 0x8421:
 		mt9m001->model = V4L2_IDENT_MT9M001C12ST;
 		icd->formats = mt9m001_colour_formats;
-		if (mt9m001->client->dev.platform_data)
+		if (gpio_is_valid(icl->gpio))
 			icd->num_formats = ARRAY_SIZE(mt9m001_colour_formats);
 		else
 			icd->num_formats = 1;
@@ -608,7 +610,7 @@ static int mt9m001_video_probe(struct soc_camera_device *icd)
 	case 0x8431:
 		mt9m001->model = V4L2_IDENT_MT9M001C12STM;
 		icd->formats = mt9m001_monochrome_formats;
-		if (mt9m001->client->dev.platform_data)
+		if (gpio_is_valid(icl->gpio))
 			icd->num_formats = ARRAY_SIZE(mt9m001_monochrome_formats);
 		else
 			icd->num_formats = 1;
@@ -640,8 +642,8 @@ static void mt9m001_video_remove(struct soc_camera_device *icd)
 	struct mt9m001 *mt9m001 = container_of(icd, struct mt9m001, icd);
 
 	dev_dbg(&icd->dev, "Video %x removed: %p, %p\n", mt9m001->client->addr,
-		mt9m001->icd.dev.parent, mt9m001->icd.vdev);
-	soc_camera_video_stop(&mt9m001->icd);
+		icd->dev.parent, icd->vdev);
+	soc_camera_video_stop(icd);
 }
 
 #if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 26)
diff --git a/linux/drivers/media/video/mt9m111.c b/linux/drivers/media/video/mt9m111.c
index 1fde94514..39f2e83cf 100644
--- a/linux/drivers/media/video/mt9m111.c
+++ b/linux/drivers/media/video/mt9m111.c
@@ -1,5 +1,5 @@
 /*
- * Driver for MT9M111 CMOS Image Sensor from Micron
+ * Driver for MT9M111/MT9M112 CMOS Image Sensor from Micron
  *
  * Copyright (C) 2008, Robert Jarzmik <robert.jarzmik@free.fr>
  *
@@ -19,7 +19,7 @@
 #include <media/soc_camera.h>
 
 /*
- * mt9m111 i2c address is 0x5d or 0x48 (depending on SAddr pin)
+ * mt9m111 and mt9m112 i2c address is 0x5d or 0x48 (depending on SAddr pin)
  * The platform has to define i2c_board_info and call i2c_register_board_info()
  */
 
@@ -90,7 +90,7 @@
 #define MT9M111_OUTPUT_FORMAT_CTRL2_B	0x19b
 
 #define MT9M111_OPMODE_AUTOEXPO_EN	(1 << 14)
-
+#define MT9M111_OPMODE_AUTOWHITEBAL_EN	(1 << 1)
 
 #define MT9M111_OUTFMT_PROCESSED_BAYER	(1 << 14)
 #define MT9M111_OUTFMT_BYPASS_IFP	(1 << 10)
@@ -128,9 +128,14 @@
 	.colorspace = _colorspace }
 #define RGB_FMT(_name, _depth, _fourcc) \
 	COL_FMT(_name, _depth, _fourcc, V4L2_COLORSPACE_SRGB)
+#define JPG_FMT(_name, _depth, _fourcc) \
+	COL_FMT(_name, _depth, _fourcc, V4L2_COLORSPACE_JPEG)
 
 static const struct soc_camera_data_format mt9m111_colour_formats[] = {
-	COL_FMT("YCrYCb 8 bit", 8, V4L2_PIX_FMT_YUYV, V4L2_COLORSPACE_JPEG),
+	JPG_FMT("CbYCrY 16 bit", 16, V4L2_PIX_FMT_UYVY),
+	JPG_FMT("CrYCbY 16 bit", 16, V4L2_PIX_FMT_VYUY),
+	JPG_FMT("YCbYCr 16 bit", 16, V4L2_PIX_FMT_YUYV),
+	JPG_FMT("YCrYCb 16 bit", 16, V4L2_PIX_FMT_YVYU),
 	RGB_FMT("RGB 565", 16, V4L2_PIX_FMT_RGB565),
 	RGB_FMT("RGB 555", 16, V4L2_PIX_FMT_RGB555),
 	RGB_FMT("Bayer (sRGB) 10 bit", 10, V4L2_PIX_FMT_SBGGR16),
@@ -145,7 +150,7 @@ enum mt9m111_context {
 struct mt9m111 {
 	struct i2c_client *client;
 	struct soc_camera_device icd;
-	int model;	/* V4L2_IDENT_MT9M111* codes from v4l2-chip-ident.h */
+	int model;	/* V4L2_IDENT_MT9M11x* codes from v4l2-chip-ident.h */
 	enum mt9m111_context context;
 	unsigned int left, top, width, height;
 	u32 pixfmt;
@@ -158,6 +163,7 @@ struct mt9m111 {
 	unsigned int swap_rgb_red_blue:1;
 	unsigned int swap_yuv_y_chromas:1;
 	unsigned int swap_yuv_cb_cr:1;
+	unsigned int autowhitebalance:1;
 };
 
 static int reg_page_map_set(struct i2c_client *client, const u16 reg)
@@ -410,9 +416,13 @@ static int mt9m111_stop_capture(struct soc_camera_device *icd)
 
 static unsigned long mt9m111_query_bus_param(struct soc_camera_device *icd)
 {
-	return SOCAM_MASTER | SOCAM_PCLK_SAMPLE_RISING |
+	struct mt9m111 *mt9m111 = container_of(icd, struct mt9m111, icd);
+	struct soc_camera_link *icl = mt9m111->client->dev.platform_data;
+	unsigned long flags = SOCAM_MASTER | SOCAM_PCLK_SAMPLE_RISING |
 		SOCAM_HSYNC_ACTIVE_HIGH | SOCAM_VSYNC_ACTIVE_HIGH |
 		SOCAM_DATAWIDTH_8;
+
+	return soc_camera_apply_sensor_flags(icl, flags);
 }
 
 static int mt9m111_set_bus_param(struct soc_camera_device *icd, unsigned long f)
@@ -438,7 +448,24 @@ static int mt9m111_set_pixfmt(struct soc_camera_device *icd, u32 pixfmt)
 	case V4L2_PIX_FMT_RGB565:
 		ret = mt9m111_setfmt_rgb565(icd);
 		break;
+	case V4L2_PIX_FMT_UYVY:
+		mt9m111->swap_yuv_y_chromas = 0;
+		mt9m111->swap_yuv_cb_cr = 0;
+		ret = mt9m111_setfmt_yuv(icd);
+		break;
+	case V4L2_PIX_FMT_VYUY:
+		mt9m111->swap_yuv_y_chromas = 0;
+		mt9m111->swap_yuv_cb_cr = 1;
+		ret = mt9m111_setfmt_yuv(icd);
+		break;
 	case V4L2_PIX_FMT_YUYV:
+		mt9m111->swap_yuv_y_chromas = 1;
+		mt9m111->swap_yuv_cb_cr = 0;
+		ret = mt9m111_setfmt_yuv(icd);
+		break;
+	case V4L2_PIX_FMT_YVYU:
+		mt9m111->swap_yuv_y_chromas = 1;
+		mt9m111->swap_yuv_cb_cr = 1;
 		ret = mt9m111_setfmt_yuv(icd);
 		break;
 	default:
@@ -476,10 +503,12 @@ static int mt9m111_set_fmt(struct soc_camera_device *icd,
 static int mt9m111_try_fmt(struct soc_camera_device *icd,
 			   struct v4l2_format *f)
 {
-	if (f->fmt.pix.height > MT9M111_MAX_HEIGHT)
-		f->fmt.pix.height = MT9M111_MAX_HEIGHT;
-	if (f->fmt.pix.width > MT9M111_MAX_WIDTH)
-		f->fmt.pix.width = MT9M111_MAX_WIDTH;
+	struct v4l2_pix_format *pix = &f->fmt.pix;
+
+	if (pix->height > MT9M111_MAX_HEIGHT)
+		pix->height = MT9M111_MAX_HEIGHT;
+	if (pix->width > MT9M111_MAX_WIDTH)
+		pix->width = MT9M111_MAX_WIDTH;
 
 	return 0;
 }
@@ -634,18 +663,15 @@ static int mt9m111_set_flip(struct soc_camera_device *icd, int flip, int mask)
 
 static int mt9m111_get_global_gain(struct soc_camera_device *icd)
 {
-	unsigned int data, gain;
+	int data;
 
 	data = reg_read(GLOBAL_GAIN);
 	if (data >= 0)
-		gain = ((data & (1 << 10)) * 2)
-			| ((data & (1 << 9)) * 2)
-			| (data & 0x2f);
-	else
-		gain = data;
-
-	return gain;
+		return (data & 0x2f) * (1 << ((data >> 10) & 1)) *
+			(1 << ((data >> 9) & 1));
+	return data;
 }
+
 static int mt9m111_set_global_gain(struct soc_camera_device *icd, int gain)
 {
 	u16 val;
@@ -679,6 +705,23 @@ static int mt9m111_set_autoexposure(struct soc_camera_device *icd, int on)
 
 	return ret;
 }
+
+static int mt9m111_set_autowhitebalance(struct soc_camera_device *icd, int on)
+{
+	struct mt9m111 *mt9m111 = container_of(icd, struct mt9m111, icd);
+	int ret;
+
+	if (on)
+		ret = reg_set(OPER_MODE_CTRL, MT9M111_OPMODE_AUTOWHITEBAL_EN);
+	else
+		ret = reg_clear(OPER_MODE_CTRL, MT9M111_OPMODE_AUTOWHITEBAL_EN);
+
+	if (!ret)
+		mt9m111->autowhitebalance = on;
+
+	return ret;
+}
+
 static int mt9m111_get_control(struct soc_camera_device *icd,
 			       struct v4l2_control *ctrl)
 {
@@ -715,6 +758,9 @@ static int mt9m111_get_control(struct soc_camera_device *icd,
 	case V4L2_CID_EXPOSURE_AUTO:
 		ctrl->value = mt9m111->autoexposure;
 		break;
+	case V4L2_CID_AUTO_WHITE_BALANCE:
+		ctrl->value = mt9m111->autowhitebalance;
+		break;
 	}
 	return 0;
 }
@@ -748,6 +794,9 @@ static int mt9m111_set_control(struct soc_camera_device *icd,
 	case V4L2_CID_EXPOSURE_AUTO:
 		ret =  mt9m111_set_autoexposure(icd, ctrl->value);
 		break;
+	case V4L2_CID_AUTO_WHITE_BALANCE:
+		ret =  mt9m111_set_autowhitebalance(icd, ctrl->value);
+		break;
 	default:
 		ret = -EINVAL;
 	}
@@ -766,6 +815,7 @@ static int mt9m111_restore_state(struct soc_camera_device *icd)
 	mt9m111_set_flip(icd, mt9m111->vflip, MT9M111_RMB_MIRROR_ROWS);
 	mt9m111_set_global_gain(icd, icd->gain);
 	mt9m111_set_autoexposure(icd, mt9m111->autoexposure);
+	mt9m111_set_autowhitebalance(icd, mt9m111->autowhitebalance);
 	return 0;
 }
 
@@ -798,7 +848,7 @@ static int mt9m111_init(struct soc_camera_device *icd)
 	if (!ret)
 		ret = mt9m111_set_autoexposure(icd, mt9m111->autoexposure);
 	if (ret)
-		dev_err(&icd->dev, "mt9m111 init failed: %d\n", ret);
+		dev_err(&icd->dev, "mt9m11x init failed: %d\n", ret);
 	return ret;
 }
 
@@ -808,7 +858,7 @@ static int mt9m111_release(struct soc_camera_device *icd)
 
 	ret = mt9m111_disable(icd);
 	if (ret < 0)
-		dev_err(&icd->dev, "mt9m111 release failed: %d\n", ret);
+		dev_err(&icd->dev, "mt9m11x release failed: %d\n", ret);
 
 	return ret;
 }
@@ -841,25 +891,30 @@ static int mt9m111_video_probe(struct soc_camera_device *icd)
 	data = reg_read(CHIP_VERSION);
 
 	switch (data) {
-	case 0x143a:
+	case 0x143a: /* MT9M111 */
 		mt9m111->model = V4L2_IDENT_MT9M111;
-		icd->formats = mt9m111_colour_formats;
-		icd->num_formats = ARRAY_SIZE(mt9m111_colour_formats);
+		break;
+	case 0x148c: /* MT9M112 */
+		mt9m111->model = V4L2_IDENT_MT9M112;
 		break;
 	default:
 		ret = -ENODEV;
 		dev_err(&icd->dev,
-			"No MT9M111 chip detected, register read %x\n", data);
+			"No MT9M11x chip detected, register read %x\n", data);
 		goto ei2c;
 	}
 
-	dev_info(&icd->dev, "Detected a MT9M111 chip ID 0x143a\n");
+	icd->formats = mt9m111_colour_formats;
+	icd->num_formats = ARRAY_SIZE(mt9m111_colour_formats);
+
+	dev_info(&icd->dev, "Detected a MT9M11x chip ID %x\n", data);
 
 	ret = soc_camera_video_start(icd);
 	if (ret)
 		goto eisis;
 
 	mt9m111->autoexposure = 1;
+	mt9m111->autowhitebalance = 1;
 
 	mt9m111->swap_rgb_even_odd = 1;
 	mt9m111->swap_rgb_red_blue = 1;
@@ -893,7 +948,7 @@ static int mt9m111_probe(struct i2c_client *client,
 	int ret;
 
 	if (!icl) {
-		dev_err(&client->dev, "MT9M111 driver needs platform data\n");
+		dev_err(&client->dev, "MT9M11x driver needs platform data\n");
 		return -EINVAL;
 	}
 
@@ -976,6 +1031,6 @@ static void __exit mt9m111_mod_exit(void)
 module_init(mt9m111_mod_init);
 module_exit(mt9m111_mod_exit);
 
-MODULE_DESCRIPTION("Micron MT9M111 Camera driver");
+MODULE_DESCRIPTION("Micron MT9M111/MT9M112 Camera driver");
 MODULE_AUTHOR("Robert Jarzmik");
 MODULE_LICENSE("GPL");
diff --git a/linux/drivers/media/video/mt9t031.c b/linux/drivers/media/video/mt9t031.c
new file mode 100644
index 000000000..1a9d53966
--- /dev/null
+++ b/linux/drivers/media/video/mt9t031.c
@@ -0,0 +1,736 @@
+/*
+ * Driver for MT9T031 CMOS Image Sensor from Micron
+ *
+ * Copyright (C) 2008, Guennadi Liakhovetski, DENX Software Engineering <lg@denx.de>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/videodev2.h>
+#include <linux/slab.h>
+#include <linux/i2c.h>
+#include <linux/log2.h>
+
+#include <media/v4l2-common.h>
+#include <media/v4l2-chip-ident.h>
+#include <media/soc_camera.h>
+
+/* mt9t031 i2c address 0x5d
+ * The platform has to define i2c_board_info
+ * and call i2c_register_board_info() */
+
+/* mt9t031 selected register addresses */
+#define MT9T031_CHIP_VERSION		0x00
+#define MT9T031_ROW_START		0x01
+#define MT9T031_COLUMN_START		0x02
+#define MT9T031_WINDOW_HEIGHT		0x03
+#define MT9T031_WINDOW_WIDTH		0x04
+#define MT9T031_HORIZONTAL_BLANKING	0x05
+#define MT9T031_VERTICAL_BLANKING	0x06
+#define MT9T031_OUTPUT_CONTROL		0x07
+#define MT9T031_SHUTTER_WIDTH_UPPER	0x08
+#define MT9T031_SHUTTER_WIDTH		0x09
+#define MT9T031_PIXEL_CLOCK_CONTROL	0x0a
+#define MT9T031_FRAME_RESTART		0x0b
+#define MT9T031_SHUTTER_DELAY		0x0c
+#define MT9T031_RESET			0x0d
+#define MT9T031_READ_MODE_1		0x1e
+#define MT9T031_READ_MODE_2		0x20
+#define MT9T031_READ_MODE_3		0x21
+#define MT9T031_ROW_ADDRESS_MODE	0x22
+#define MT9T031_COLUMN_ADDRESS_MODE	0x23
+#define MT9T031_GLOBAL_GAIN		0x35
+#define MT9T031_CHIP_ENABLE		0xF8
+
+#define MT9T031_MAX_HEIGHT		1536
+#define MT9T031_MAX_WIDTH		2048
+#define MT9T031_MIN_HEIGHT		2
+#define MT9T031_MIN_WIDTH		2
+#define MT9T031_HORIZONTAL_BLANK	142
+#define MT9T031_VERTICAL_BLANK		25
+#define MT9T031_COLUMN_SKIP		32
+#define MT9T031_ROW_SKIP		20
+
+#define MT9T031_BUS_PARAM	(SOCAM_PCLK_SAMPLE_RISING |	\
+	SOCAM_PCLK_SAMPLE_FALLING | SOCAM_HSYNC_ACTIVE_HIGH |	\
+	SOCAM_VSYNC_ACTIVE_HIGH | SOCAM_DATA_ACTIVE_HIGH |	\
+	SOCAM_MASTER | SOCAM_DATAWIDTH_10)
+
+static const struct soc_camera_data_format mt9t031_colour_formats[] = {
+	{
+		.name		= "Bayer (sRGB) 10 bit",
+		.depth		= 10,
+		.fourcc		= V4L2_PIX_FMT_SGRBG10,
+		.colorspace	= V4L2_COLORSPACE_SRGB,
+	}
+};
+
+struct mt9t031 {
+	struct i2c_client *client;
+	struct soc_camera_device icd;
+	int model;	/* V4L2_IDENT_MT9T031* codes from v4l2-chip-ident.h */
+	unsigned char autoexposure;
+	u16 xskip;
+	u16 yskip;
+};
+
+static int reg_read(struct soc_camera_device *icd, const u8 reg)
+{
+	struct mt9t031 *mt9t031 = container_of(icd, struct mt9t031, icd);
+	struct i2c_client *client = mt9t031->client;
+	s32 data = i2c_smbus_read_word_data(client, reg);
+	return data < 0 ? data : swab16(data);
+}
+
+static int reg_write(struct soc_camera_device *icd, const u8 reg,
+		     const u16 data)
+{
+	struct mt9t031 *mt9t031 = container_of(icd, struct mt9t031, icd);
+	return i2c_smbus_write_word_data(mt9t031->client, reg, swab16(data));
+}
+
+static int reg_set(struct soc_camera_device *icd, const u8 reg,
+		   const u16 data)
+{
+	int ret;
+
+	ret = reg_read(icd, reg);
+	if (ret < 0)
+		return ret;
+	return reg_write(icd, reg, ret | data);
+}
+
+static int reg_clear(struct soc_camera_device *icd, const u8 reg,
+		     const u16 data)
+{
+	int ret;
+
+	ret = reg_read(icd, reg);
+	if (ret < 0)
+		return ret;
+	return reg_write(icd, reg, ret & ~data);
+}
+
+static int set_shutter(struct soc_camera_device *icd, const u32 data)
+{
+	int ret;
+
+	ret = reg_write(icd, MT9T031_SHUTTER_WIDTH_UPPER, data >> 16);
+
+	if (ret >= 0)
+		ret = reg_write(icd, MT9T031_SHUTTER_WIDTH, data & 0xffff);
+
+	return ret;
+}
+
+static int get_shutter(struct soc_camera_device *icd, u32 *data)
+{
+	int ret;
+
+	ret = reg_read(icd, MT9T031_SHUTTER_WIDTH_UPPER);
+	*data = ret << 16;
+
+	if (ret >= 0)
+		ret = reg_read(icd, MT9T031_SHUTTER_WIDTH);
+	*data |= ret & 0xffff;
+
+	return ret < 0 ? ret : 0;
+}
+
+static int mt9t031_init(struct soc_camera_device *icd)
+{
+	int ret;
+
+	/* Disable chip output, synchronous option update */
+	dev_dbg(icd->vdev->parent, "%s\n", __func__);
+
+	ret = reg_write(icd, MT9T031_RESET, 1);
+	if (ret >= 0)
+		ret = reg_write(icd, MT9T031_RESET, 0);
+	if (ret >= 0)
+		ret = reg_clear(icd, MT9T031_OUTPUT_CONTROL, 3);
+
+	return ret >= 0 ? 0 : -EIO;
+}
+
+static int mt9t031_release(struct soc_camera_device *icd)
+{
+	/* Disable the chip */
+	reg_clear(icd, MT9T031_OUTPUT_CONTROL, 3);
+	return 0;
+}
+
+static int mt9t031_start_capture(struct soc_camera_device *icd)
+{
+	/* Switch to master "normal" mode */
+	if (reg_set(icd, MT9T031_OUTPUT_CONTROL, 3) < 0)
+		return -EIO;
+	return 0;
+}
+
+static int mt9t031_stop_capture(struct soc_camera_device *icd)
+{
+	/* Stop sensor readout */
+	if (reg_clear(icd, MT9T031_OUTPUT_CONTROL, 3) < 0)
+		return -EIO;
+	return 0;
+}
+
+static int mt9t031_set_bus_param(struct soc_camera_device *icd,
+				 unsigned long flags)
+{
+	/* The caller should have queried our parameters, check anyway */
+	if (flags & ~MT9T031_BUS_PARAM)
+		return -EINVAL;
+
+	if (flags & SOCAM_PCLK_SAMPLE_FALLING)
+		reg_set(icd, MT9T031_PIXEL_CLOCK_CONTROL, 0x8000);
+	else
+		reg_clear(icd, MT9T031_PIXEL_CLOCK_CONTROL, 0x8000);
+
+	return 0;
+}
+
+static unsigned long mt9t031_query_bus_param(struct soc_camera_device *icd)
+{
+	struct mt9t031 *mt9t031 = container_of(icd, struct mt9t031, icd);
+	struct soc_camera_link *icl = mt9t031->client->dev.platform_data;
+
+	return soc_camera_apply_sensor_flags(icl, MT9T031_BUS_PARAM);
+}
+
+static int mt9t031_set_fmt(struct soc_camera_device *icd,
+			   __u32 pixfmt, struct v4l2_rect *rect)
+{
+	struct mt9t031 *mt9t031 = container_of(icd, struct mt9t031, icd);
+	int ret;
+	const u16 hblank = MT9T031_HORIZONTAL_BLANK,
+		vblank = MT9T031_VERTICAL_BLANK;
+	u16 xbin, xskip = mt9t031->xskip, ybin, yskip = mt9t031->yskip,
+		width = rect->width * xskip, height = rect->height * yskip;
+
+	if (pixfmt) {
+		/* S_FMT - use binning and skipping for scaling, recalculate */
+		/* Is this more optimal than just a division? */
+		for (xskip = 8; xskip > 1; xskip--)
+			if (rect->width * xskip <= icd->width_max)
+				break;
+
+		for (yskip = 8; yskip > 1; yskip--)
+			if (rect->height * yskip <= icd->height_max)
+				break;
+
+		width = rect->width * xskip;
+		height = rect->height * yskip;
+
+		dev_dbg(&icd->dev, "xskip %u, width %u, yskip %u, height %u\n",
+			xskip, width, yskip, height);
+	}
+
+	xbin = min(xskip, (u16)3);
+	ybin = min(yskip, (u16)3);
+
+	/* Make sure we don't exceed frame limits */
+	if (rect->left + width > icd->width_max)
+		rect->left = (icd->width_max - width) / 2;
+
+	if (rect->top + height > icd->height_max)
+		rect->top = (icd->height_max - height) / 2;
+
+	/* Could just do roundup(rect->left, [xy]bin); but this is cheaper */
+	switch (xbin) {
+	case 2:
+		rect->left = (rect->left + 1) & ~1;
+		break;
+	case 3:
+		rect->left = roundup(rect->left, 3);
+	}
+
+	switch (ybin) {
+	case 2:
+		rect->top = (rect->top + 1) & ~1;
+		break;
+	case 3:
+		rect->top = roundup(rect->top, 3);
+	}
+
+	/* Blanking and start values - default... */
+	ret = reg_write(icd, MT9T031_HORIZONTAL_BLANKING, hblank);
+	if (ret >= 0)
+		ret = reg_write(icd, MT9T031_VERTICAL_BLANKING, vblank);
+
+	if (pixfmt) {
+		/* Binning, skipping */
+		if (ret >= 0)
+			ret = reg_write(icd, MT9T031_COLUMN_ADDRESS_MODE,
+					((xbin - 1) << 4) | (xskip - 1));
+		if (ret >= 0)
+			ret = reg_write(icd, MT9T031_ROW_ADDRESS_MODE,
+					((ybin - 1) << 4) | (yskip - 1));
+	}
+	dev_dbg(&icd->dev, "new left %u, top %u\n", rect->left, rect->top);
+
+	/* The caller provides a supported format, as guaranteed by
+	 * icd->try_fmt_cap(), soc_camera_s_crop() and soc_camera_cropcap() */
+	if (ret >= 0)
+		ret = reg_write(icd, MT9T031_COLUMN_START, rect->left);
+	if (ret >= 0)
+		ret = reg_write(icd, MT9T031_ROW_START, rect->top);
+	if (ret >= 0)
+		ret = reg_write(icd, MT9T031_WINDOW_WIDTH, width - 1);
+	if (ret >= 0)
+		ret = reg_write(icd, MT9T031_WINDOW_HEIGHT,
+				height + icd->y_skip_top - 1);
+	if (ret >= 0 && mt9t031->autoexposure) {
+		ret = set_shutter(icd, height + icd->y_skip_top + vblank);
+		if (ret >= 0) {
+			const u32 shutter_max = MT9T031_MAX_HEIGHT + vblank;
+			const struct v4l2_queryctrl *qctrl =
+				soc_camera_find_qctrl(icd->ops,
+						      V4L2_CID_EXPOSURE);
+			icd->exposure = (shutter_max / 2 + (height +
+					 icd->y_skip_top + vblank - 1) *
+					 (qctrl->maximum - qctrl->minimum)) /
+				shutter_max + qctrl->minimum;
+		}
+	}
+
+	if (!ret && pixfmt) {
+		mt9t031->xskip = xskip;
+		mt9t031->yskip = yskip;
+	}
+
+	return ret < 0 ? ret : 0;
+}
+
+static int mt9t031_try_fmt(struct soc_camera_device *icd,
+			   struct v4l2_format *f)
+{
+	struct v4l2_pix_format *pix = &f->fmt.pix;
+
+	if (pix->height < icd->height_min)
+		pix->height = icd->height_min;
+	if (pix->height > icd->height_max)
+		pix->height = icd->height_max;
+	if (pix->width < icd->width_min)
+		pix->width = icd->width_min;
+	if (pix->width > icd->width_max)
+		pix->width = icd->width_max;
+
+	pix->width &= ~0x01; /* has to be even */
+	pix->height &= ~0x01; /* has to be even */
+
+	return 0;
+}
+
+static int mt9t031_get_chip_id(struct soc_camera_device *icd,
+			       struct v4l2_chip_ident *id)
+{
+	struct mt9t031 *mt9t031 = container_of(icd, struct mt9t031, icd);
+
+	if (id->match_type != V4L2_CHIP_MATCH_I2C_ADDR)
+		return -EINVAL;
+
+	if (id->match_chip != mt9t031->client->addr)
+		return -ENODEV;
+
+	id->ident	= mt9t031->model;
+	id->revision	= 0;
+
+	return 0;
+}
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int mt9t031_get_register(struct soc_camera_device *icd,
+				struct v4l2_register *reg)
+{
+	struct mt9t031 *mt9t031 = container_of(icd, struct mt9t031, icd);
+
+	if (reg->match_type != V4L2_CHIP_MATCH_I2C_ADDR || reg->reg > 0xff)
+		return -EINVAL;
+
+	if (reg->match_chip != mt9t031->client->addr)
+		return -ENODEV;
+
+	reg->val = reg_read(icd, reg->reg);
+
+	if (reg->val > 0xffff)
+		return -EIO;
+
+	return 0;
+}
+
+static int mt9t031_set_register(struct soc_camera_device *icd,
+				struct v4l2_register *reg)
+{
+	struct mt9t031 *mt9t031 = container_of(icd, struct mt9t031, icd);
+
+	if (reg->match_type != V4L2_CHIP_MATCH_I2C_ADDR || reg->reg > 0xff)
+		return -EINVAL;
+
+	if (reg->match_chip != mt9t031->client->addr)
+		return -ENODEV;
+
+	if (reg_write(icd, reg->reg, reg->val) < 0)
+		return -EIO;
+
+	return 0;
+}
+#endif
+
+static const struct v4l2_queryctrl mt9t031_controls[] = {
+	{
+		.id		= V4L2_CID_VFLIP,
+		.type		= V4L2_CTRL_TYPE_BOOLEAN,
+		.name		= "Flip Vertically",
+		.minimum	= 0,
+		.maximum	= 1,
+		.step		= 1,
+		.default_value	= 0,
+	}, {
+		.id		= V4L2_CID_GAIN,
+		.type		= V4L2_CTRL_TYPE_INTEGER,
+		.name		= "Gain",
+		.minimum	= 0,
+		.maximum	= 127,
+		.step		= 1,
+		.default_value	= 64,
+		.flags		= V4L2_CTRL_FLAG_SLIDER,
+	}, {
+		.id		= V4L2_CID_EXPOSURE,
+		.type		= V4L2_CTRL_TYPE_INTEGER,
+		.name		= "Exposure",
+		.minimum	= 1,
+		.maximum	= 255,
+		.step		= 1,
+		.default_value	= 255,
+		.flags		= V4L2_CTRL_FLAG_SLIDER,
+	}, {
+		.id		= V4L2_CID_EXPOSURE_AUTO,
+		.type		= V4L2_CTRL_TYPE_BOOLEAN,
+		.name		= "Automatic Exposure",
+		.minimum	= 0,
+		.maximum	= 1,
+		.step		= 1,
+		.default_value	= 1,
+	}
+};
+
+static int mt9t031_video_probe(struct soc_camera_device *);
+static void mt9t031_video_remove(struct soc_camera_device *);
+static int mt9t031_get_control(struct soc_camera_device *, struct v4l2_control *);
+static int mt9t031_set_control(struct soc_camera_device *, struct v4l2_control *);
+
+static struct soc_camera_ops mt9t031_ops = {
+	.owner			= THIS_MODULE,
+	.probe			= mt9t031_video_probe,
+	.remove			= mt9t031_video_remove,
+	.init			= mt9t031_init,
+	.release		= mt9t031_release,
+	.start_capture		= mt9t031_start_capture,
+	.stop_capture		= mt9t031_stop_capture,
+	.set_fmt		= mt9t031_set_fmt,
+	.try_fmt		= mt9t031_try_fmt,
+	.set_bus_param		= mt9t031_set_bus_param,
+	.query_bus_param	= mt9t031_query_bus_param,
+	.controls		= mt9t031_controls,
+	.num_controls		= ARRAY_SIZE(mt9t031_controls),
+	.get_control		= mt9t031_get_control,
+	.set_control		= mt9t031_set_control,
+	.get_chip_id		= mt9t031_get_chip_id,
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.get_register		= mt9t031_get_register,
+	.set_register		= mt9t031_set_register,
+#endif
+};
+
+static int mt9t031_get_control(struct soc_camera_device *icd, struct v4l2_control *ctrl)
+{
+	struct mt9t031 *mt9t031 = container_of(icd, struct mt9t031, icd);
+	int data;
+
+	switch (ctrl->id) {
+	case V4L2_CID_VFLIP:
+		data = reg_read(icd, MT9T031_READ_MODE_2);
+		if (data < 0)
+			return -EIO;
+		ctrl->value = !!(data & 0x8000);
+		break;
+	case V4L2_CID_HFLIP:
+		data = reg_read(icd, MT9T031_READ_MODE_2);
+		if (data < 0)
+			return -EIO;
+		ctrl->value = !!(data & 0x4000);
+		break;
+	case V4L2_CID_EXPOSURE_AUTO:
+		ctrl->value = mt9t031->autoexposure;
+		break;
+	}
+	return 0;
+}
+
+static int mt9t031_set_control(struct soc_camera_device *icd, struct v4l2_control *ctrl)
+{
+	struct mt9t031 *mt9t031 = container_of(icd, struct mt9t031, icd);
+	const struct v4l2_queryctrl *qctrl;
+	int data;
+
+	qctrl = soc_camera_find_qctrl(&mt9t031_ops, ctrl->id);
+
+	if (!qctrl)
+		return -EINVAL;
+
+	switch (ctrl->id) {
+	case V4L2_CID_VFLIP:
+		if (ctrl->value)
+			data = reg_set(icd, MT9T031_READ_MODE_2, 0x8000);
+		else
+			data = reg_clear(icd, MT9T031_READ_MODE_2, 0x8000);
+		if (data < 0)
+			return -EIO;
+		break;
+	case V4L2_CID_HFLIP:
+		if (ctrl->value)
+			data = reg_set(icd, MT9T031_READ_MODE_2, 0x4000);
+		else
+			data = reg_clear(icd, MT9T031_READ_MODE_2, 0x4000);
+		if (data < 0)
+			return -EIO;
+		break;
+	case V4L2_CID_GAIN:
+		if (ctrl->value > qctrl->maximum || ctrl->value < qctrl->minimum)
+			return -EINVAL;
+		/* See Datasheet Table 7, Gain settings. */
+		if (ctrl->value <= qctrl->default_value) {
+			/* Pack it into 0..1 step 0.125, register values 0..8 */
+			unsigned long range = qctrl->default_value - qctrl->minimum;
+			data = ((ctrl->value - qctrl->minimum) * 8 + range / 2) / range;
+
+			dev_dbg(&icd->dev, "Setting gain %d\n", data);
+			data = reg_write(icd, MT9T031_GLOBAL_GAIN, data);
+			if (data < 0)
+				return -EIO;
+		} else {
+			/* Pack it into 1.125..15 variable step, register values 9..67 */
+			/* We assume qctrl->maximum - qctrl->default_value - 1 > 0 */
+			unsigned long range = qctrl->maximum - qctrl->default_value - 1;
+			unsigned long gain = ((ctrl->value - qctrl->default_value - 1) *
+					       111 + range / 2) / range + 9;
+
+			if (gain <= 32)
+				data = gain;
+			else if (gain <= 64)
+				data = ((gain - 32) * 16 + 16) / 32 + 80;
+			else
+				data = ((gain - 64) * 7 + 28) / 56 + 96;
+
+			dev_dbg(&icd->dev, "Setting gain from %d to %d\n",
+				 reg_read(icd, MT9T031_GLOBAL_GAIN), data);
+			data = reg_write(icd, MT9T031_GLOBAL_GAIN, data);
+			if (data < 0)
+				return -EIO;
+		}
+
+		/* Success */
+		icd->gain = ctrl->value;
+		break;
+	case V4L2_CID_EXPOSURE:
+		/* mt9t031 has maximum == default */
+		if (ctrl->value > qctrl->maximum || ctrl->value < qctrl->minimum)
+			return -EINVAL;
+		else {
+			const unsigned long range = qctrl->maximum - qctrl->minimum;
+			const u32 shutter = ((ctrl->value - qctrl->minimum) * 1048 +
+					     range / 2) / range + 1;
+			u32 old;
+
+			get_shutter(icd, &old);
+			dev_dbg(&icd->dev, "Setting shutter width from %u to %u\n",
+				old, shutter);
+			if (set_shutter(icd, shutter) < 0)
+				return -EIO;
+			icd->exposure = ctrl->value;
+			mt9t031->autoexposure = 0;
+		}
+		break;
+	case V4L2_CID_EXPOSURE_AUTO:
+		if (ctrl->value) {
+			const u16 vblank = MT9T031_VERTICAL_BLANK;
+			const u32 shutter_max = MT9T031_MAX_HEIGHT + vblank;
+			if (set_shutter(icd, icd->height +
+					icd->y_skip_top + vblank) < 0)
+				return -EIO;
+			qctrl = soc_camera_find_qctrl(icd->ops, V4L2_CID_EXPOSURE);
+			icd->exposure = (shutter_max / 2 + (icd->height +
+					 icd->y_skip_top + vblank - 1) *
+					 (qctrl->maximum - qctrl->minimum)) /
+				shutter_max + qctrl->minimum;
+			mt9t031->autoexposure = 1;
+		} else
+			mt9t031->autoexposure = 0;
+		break;
+	}
+	return 0;
+}
+
+/* Interface active, can use i2c. If it fails, it can indeed mean, that
+ * this wasn't our capture interface, so, we wait for the right one */
+static int mt9t031_video_probe(struct soc_camera_device *icd)
+{
+	struct mt9t031 *mt9t031 = container_of(icd, struct mt9t031, icd);
+	s32 data;
+	int ret;
+
+	/* We must have a parent by now. And it cannot be a wrong one.
+	 * So this entire test is completely redundant. */
+	if (!icd->dev.parent ||
+	    to_soc_camera_host(icd->dev.parent)->nr != icd->iface)
+		return -ENODEV;
+
+	/* Enable the chip */
+	data = reg_write(icd, MT9T031_CHIP_ENABLE, 1);
+	dev_dbg(&icd->dev, "write: %d\n", data);
+
+	/* Read out the chip version register */
+	data = reg_read(icd, MT9T031_CHIP_VERSION);
+
+	switch (data) {
+	case 0x1621:
+		mt9t031->model = V4L2_IDENT_MT9T031;
+		icd->formats = mt9t031_colour_formats;
+		icd->num_formats = ARRAY_SIZE(mt9t031_colour_formats);
+		break;
+	default:
+		ret = -ENODEV;
+		dev_err(&icd->dev,
+			"No MT9T031 chip detected, register read %x\n", data);
+		goto ei2c;
+	}
+
+	dev_info(&icd->dev, "Detected a MT9T031 chip ID %x\n", data);
+
+	/* Now that we know the model, we can start video */
+	ret = soc_camera_video_start(icd);
+	if (ret)
+		goto evstart;
+
+	return 0;
+
+evstart:
+ei2c:
+	return ret;
+}
+
+static void mt9t031_video_remove(struct soc_camera_device *icd)
+{
+	struct mt9t031 *mt9t031 = container_of(icd, struct mt9t031, icd);
+
+	dev_dbg(&icd->dev, "Video %x removed: %p, %p\n", mt9t031->client->addr,
+		icd->dev.parent, icd->vdev);
+	soc_camera_video_stop(icd);
+}
+
+static int mt9t031_probe(struct i2c_client *client,
+			 const struct i2c_device_id *did)
+{
+	struct mt9t031 *mt9t031;
+	struct soc_camera_device *icd;
+	struct i2c_adapter *adapter = to_i2c_adapter(client->dev.parent);
+	struct soc_camera_link *icl = client->dev.platform_data;
+	int ret;
+
+	if (!icl) {
+		dev_err(&client->dev, "MT9T031 driver needs platform data\n");
+		return -EINVAL;
+	}
+
+	if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_WORD_DATA)) {
+		dev_warn(&adapter->dev,
+			 "I2C-Adapter doesn't support I2C_FUNC_SMBUS_WORD\n");
+		return -EIO;
+	}
+
+	mt9t031 = kzalloc(sizeof(struct mt9t031), GFP_KERNEL);
+	if (!mt9t031)
+		return -ENOMEM;
+
+	mt9t031->client = client;
+	i2c_set_clientdata(client, mt9t031);
+
+	/* Second stage probe - when a capture adapter is there */
+	icd = &mt9t031->icd;
+	icd->ops	= &mt9t031_ops;
+	icd->control	= &client->dev;
+	icd->x_min	= MT9T031_COLUMN_SKIP;
+	icd->y_min	= MT9T031_ROW_SKIP;
+	icd->x_current	= icd->x_min;
+	icd->y_current	= icd->y_min;
+	icd->width_min	= MT9T031_MIN_WIDTH;
+	icd->width_max	= MT9T031_MAX_WIDTH;
+	icd->height_min	= MT9T031_MIN_HEIGHT;
+	icd->height_max	= MT9T031_MAX_HEIGHT;
+	icd->y_skip_top	= 0;
+	icd->iface	= icl->bus_id;
+	/* Simulated autoexposure. If enabled, we calculate shutter width
+	 * ourselves in the driver based on vertical blanking and frame width */
+	mt9t031->autoexposure = 1;
+
+	mt9t031->xskip = 1;
+	mt9t031->yskip = 1;
+
+	ret = soc_camera_device_register(icd);
+	if (ret)
+		goto eisdr;
+
+	return 0;
+
+eisdr:
+	i2c_set_clientdata(client, NULL);
+	kfree(mt9t031);
+	return ret;
+}
+
+static int mt9t031_remove(struct i2c_client *client)
+{
+	struct mt9t031 *mt9t031 = i2c_get_clientdata(client);
+
+	soc_camera_device_unregister(&mt9t031->icd);
+	i2c_set_clientdata(client, NULL);
+	kfree(mt9t031);
+
+	return 0;
+}
+
+static const struct i2c_device_id mt9t031_id[] = {
+	{ "mt9t031", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, mt9t031_id);
+
+static struct i2c_driver mt9t031_i2c_driver = {
+	.driver = {
+		.name = "mt9t031",
+	},
+	.probe		= mt9t031_probe,
+	.remove		= mt9t031_remove,
+	.id_table	= mt9t031_id,
+};
+
+static int __init mt9t031_mod_init(void)
+{
+	return i2c_add_driver(&mt9t031_i2c_driver);
+}
+
+static void __exit mt9t031_mod_exit(void)
+{
+	i2c_del_driver(&mt9t031_i2c_driver);
+}
+
+module_init(mt9t031_mod_init);
+module_exit(mt9t031_mod_exit);
+
+MODULE_DESCRIPTION("Micron MT9T031 Camera driver");
+MODULE_AUTHOR("Guennadi Liakhovetski <lg@denx.de>");
+MODULE_LICENSE("GPL v2");
diff --git a/linux/drivers/media/video/mt9v022.c b/linux/drivers/media/video/mt9v022.c
index 1ca28c087..e2579559d 100644
--- a/linux/drivers/media/video/mt9v022.c
+++ b/linux/drivers/media/video/mt9v022.c
@@ -273,6 +273,7 @@ static int mt9v022_set_bus_param(struct soc_camera_device *icd,
 				 unsigned long flags)
 {
 	struct mt9v022 *mt9v022 = container_of(icd, struct mt9v022, icd);
+	struct soc_camera_link *icl = mt9v022->client->dev.platform_data;
 	unsigned int width_flag = flags & SOCAM_DATAWIDTH_MASK;
 	int ret;
 	u16 pixclk = 0;
@@ -296,6 +297,8 @@ static int mt9v022_set_bus_param(struct soc_camera_device *icd,
 		mt9v022->datawidth = width_flag == SOCAM_DATAWIDTH_8 ? 8 : 10;
 	}
 
+	flags = soc_camera_apply_sensor_flags(icl, flags);
+
 	if (flags & SOCAM_PCLK_SAMPLE_RISING)
 		pixclk |= 0x10;
 
@@ -403,15 +406,17 @@ static int mt9v022_set_fmt(struct soc_camera_device *icd,
 static int mt9v022_try_fmt(struct soc_camera_device *icd,
 			   struct v4l2_format *f)
 {
-	if (f->fmt.pix.height < 32 + icd->y_skip_top)
-		f->fmt.pix.height = 32 + icd->y_skip_top;
-	if (f->fmt.pix.height > 480 + icd->y_skip_top)
-		f->fmt.pix.height = 480 + icd->y_skip_top;
-	if (f->fmt.pix.width < 48)
-		f->fmt.pix.width = 48;
-	if (f->fmt.pix.width > 752)
-		f->fmt.pix.width = 752;
-	f->fmt.pix.width &= ~0x03; /* ? */
+	struct v4l2_pix_format *pix = &f->fmt.pix;
+
+	if (pix->height < 32 + icd->y_skip_top)
+		pix->height = 32 + icd->y_skip_top;
+	if (pix->height > 480 + icd->y_skip_top)
+		pix->height = 480 + icd->y_skip_top;
+	if (pix->width < 48)
+		pix->width = 48;
+	if (pix->width > 752)
+		pix->width = 752;
+	pix->width &= ~0x03; /* ? */
 
 	return 0;
 }
@@ -690,6 +695,7 @@ static int mt9v022_set_control(struct soc_camera_device *icd,
 static int mt9v022_video_probe(struct soc_camera_device *icd)
 {
 	struct mt9v022 *mt9v022 = container_of(icd, struct mt9v022, icd);
+	struct soc_camera_link *icl = mt9v022->client->dev.platform_data;
 	s32 data;
 	int ret;
 
@@ -725,7 +731,7 @@ static int mt9v022_video_probe(struct soc_camera_device *icd)
 		ret = reg_write(icd, MT9V022_PIXEL_OPERATION_MODE, 4 | 0x11);
 		mt9v022->model = V4L2_IDENT_MT9V022IX7ATC;
 		icd->formats = mt9v022_colour_formats;
-		if (mt9v022->client->dev.platform_data)
+		if (gpio_is_valid(icl->gpio))
 			icd->num_formats = ARRAY_SIZE(mt9v022_colour_formats);
 		else
 			icd->num_formats = 1;
@@ -733,7 +739,7 @@ static int mt9v022_video_probe(struct soc_camera_device *icd)
 		ret = reg_write(icd, MT9V022_PIXEL_OPERATION_MODE, 0x11);
 		mt9v022->model = V4L2_IDENT_MT9V022IX7ATM;
 		icd->formats = mt9v022_monochrome_formats;
-		if (mt9v022->client->dev.platform_data)
+		if (gpio_is_valid(icl->gpio))
 			icd->num_formats = ARRAY_SIZE(mt9v022_monochrome_formats);
 		else
 			icd->num_formats = 1;
@@ -760,8 +766,8 @@ static void mt9v022_video_remove(struct soc_camera_device *icd)
 	struct mt9v022 *mt9v022 = container_of(icd, struct mt9v022, icd);
 
 	dev_dbg(&icd->dev, "Video %x removed: %p, %p\n", mt9v022->client->addr,
-		mt9v022->icd.dev.parent, mt9v022->icd.vdev);
-	soc_camera_video_stop(&mt9v022->icd);
+		icd->dev.parent, icd->vdev);
+	soc_camera_video_stop(icd);
 }
 
 static int mt9v022_probe(struct i2c_client *client,
diff --git a/linux/drivers/media/video/ov772x.c b/linux/drivers/media/video/ov772x.c
index c023bbc84..932444623 100644
--- a/linux/drivers/media/video/ov772x.c
+++ b/linux/drivers/media/video/ov772x.c
@@ -51,6 +51,7 @@
 #define COM8        0x13 /* Common control 8 */
 #define COM9        0x14 /* Common control 9 */
 #define COM10       0x15 /* Common control 10 */
+#define REG16       0x16 /* Register 16 */
 #define HSTART      0x17 /* Horizontal sensor size */
 #define HSIZE       0x18 /* Horizontal frame (HREF column) end high 8-bit */
 #define VSTART      0x19 /* Vertical frame (row) start high 8-bit */
@@ -65,6 +66,7 @@
 #define AEW         0x24 /* AGC/AEC - Stable operating region (upper limit) */
 #define AEB         0x25 /* AGC/AEC - Stable operating region (lower limit) */
 #define VPT         0x26 /* AGC/AEC Fast mode operating region */
+#define REG28       0x28 /* Register 28 */
 #define HOUTSIZE    0x29 /* Horizontal data output size MSBs */
 #define EXHCH       0x2A /* Dummy pixel insert MSB */
 #define EXHCL       0x2B /* Dummy pixel insert LSB */
@@ -94,6 +96,7 @@
 #define TGT_R       0x43 /* BLC red  channel target value */
 #define TGT_GB      0x44 /* BLC Gb   channel target value */
 #define TGT_GR      0x45 /* BLC Gr   channel target value */
+/* for ov7720 */
 #define LCC0        0x46 /* Lens correction control 0 */
 #define LCC1        0x47 /* Lens correction option 1 - X coordinate */
 #define LCC2        0x48 /* Lens correction option 2 - Y coordinate */
@@ -101,6 +104,15 @@
 #define LCC4        0x4A /* Lens correction option 4 - radius of the circular */
 #define LCC5        0x4B /* Lens correction option 5 */
 #define LCC6        0x4C /* Lens correction option 6 */
+/* for ov7725 */
+#define LC_CTR      0x46 /* Lens correction control */
+#define LC_XC       0x47 /* X coordinate of lens correction center relative */
+#define LC_YC       0x48 /* Y coordinate of lens correction center relative */
+#define LC_COEF     0x49 /* Lens correction coefficient */
+#define LC_RADI     0x4A /* Lens correction radius */
+#define LC_COEFB    0x4B /* Lens B channel compensation coefficient */
+#define LC_COEFR    0x4C /* Lens R channel compensation coefficient */
+
 #define FIXGAIN     0x4D /* Analog fix gain amplifer */
 #define AREF0       0x4E /* Sensor reference control */
 #define AREF1       0x4F /* Sensor reference current control */
@@ -182,8 +194,13 @@
 #define SDE         0xA6 /* Special digital effect control */
 #define USAT        0xA7 /* U component saturation control */
 #define VSAT        0xA8 /* V component saturation control */
+/* for ov7720 */
 #define HUE0        0xA9 /* Hue control 0 */
 #define HUE1        0xAA /* Hue control 1 */
+/* for ov7725 */
+#define HUECOS      0xA9 /* Cosine value */
+#define HUESIN      0xAA /* Sine value */
+
 #define SIGN        0xAB /* Sign bit for Hue and contrast */
 #define DSPAUTO     0xAC /* DSP auto function ON/OFF control */
 
@@ -346,6 +363,13 @@
 #define OP_SWAP_RGB 0x00000002
 
 /*
+ * ID
+ */
+#define OV7720  0x7720
+#define OV7725  0x7721
+#define VERSION(pid, ver) ((pid<<8)|(ver&0xFF))
+
+/*
  * struct
  */
 struct regval_list {
@@ -374,34 +398,22 @@ struct ov772x_priv {
 	struct soc_camera_device          icd;
 	const struct ov772x_color_format *fmt;
 	const struct ov772x_win_size     *win;
+	int                               model;
 };
 
 #define ENDMARKER { 0xff, 0xff }
 
-static const struct regval_list ov772x_default_regs[] =
-{
-	{ COM3,  0x00 },
-	{ COM4,  PLL_4x | 0x01 },
-	{ 0x16,  0x00 }, /* Mystery */
-	{ COM11, 0x10 }, /* Mystery */
-	{ 0x28,  0x00 }, /* Mystery */
-	{ HREF,  0x00 },
-	{ COM13, 0xe2 }, /* Mystery */
-	{ AREF0, 0xef },
-	{ AREF2, 0x60 },
-	{ AREF6, 0x7a },
-	ENDMARKER,
-};
-
 /*
  * register setting for color format
  */
 static const struct regval_list ov772x_RGB555_regs[] = {
+	{ COM3, 0x00 },
 	{ COM7, FMT_RGB555 | OFMT_RGB },
 	ENDMARKER,
 };
 
 static const struct regval_list ov772x_RGB565_regs[] = {
+	{ COM3, 0x00 },
 	{ COM7, FMT_RGB565 | OFMT_RGB },
 	ENDMARKER,
 };
@@ -413,6 +425,7 @@ static const struct regval_list ov772x_YYUV_regs[] = {
 };
 
 static const struct regval_list ov772x_UVYY_regs[] = {
+	{ COM3, 0x00 },
 	{ COM7, OFMT_YUV },
 	ENDMARKER,
 };
@@ -624,7 +637,6 @@ static int ov772x_start_capture(struct soc_camera_device *icd)
 	struct ov772x_priv *priv = container_of(icd, struct ov772x_priv, icd);
 	int                 ret;
 
-
 	if (!priv->win)
 		priv->win = &ov772x_win_vga;
 	if (!priv->fmt)
@@ -634,9 +646,6 @@ static int ov772x_start_capture(struct soc_camera_device *icd)
 	 * reset hardware
 	 */
 	ov772x_reset(priv->client);
-	ret = ov772x_write_array(priv->client, ov772x_default_regs);
-	if (ret < 0)
-		goto start_end;
 
 	/*
 	 * set color format
@@ -680,7 +689,7 @@ static int ov772x_start_capture(struct soc_camera_device *icd)
 			goto start_end;
 	}
 
-	dev_info(&icd->dev,
+	dev_dbg(&icd->dev,
 		 "format %s, win %s\n", priv->fmt->name, priv->win->name);
 
 start_end:
@@ -706,18 +715,20 @@ static int ov772x_set_bus_param(struct soc_camera_device *icd,
 static unsigned long ov772x_query_bus_param(struct soc_camera_device *icd)
 {
 	struct ov772x_priv *priv = container_of(icd, struct ov772x_priv, icd);
-
-	return  SOCAM_PCLK_SAMPLE_RISING |
-		SOCAM_HSYNC_ACTIVE_HIGH  |
-		SOCAM_VSYNC_ACTIVE_HIGH  |
-		SOCAM_MASTER             |
+	struct soc_camera_link *icl = priv->client->dev.platform_data;
+	unsigned long flags = SOCAM_PCLK_SAMPLE_RISING | SOCAM_MASTER |
+		SOCAM_VSYNC_ACTIVE_HIGH | SOCAM_HSYNC_ACTIVE_HIGH |
 		priv->info->buswidth;
+
+	return soc_camera_apply_sensor_flags(icl, flags);
 }
 
 static int ov772x_get_chip_id(struct soc_camera_device *icd,
 			      struct v4l2_chip_ident   *id)
 {
-	id->ident    = V4L2_IDENT_OV772X;
+	struct ov772x_priv *priv = container_of(icd, struct ov772x_priv, icd);
+
+	id->ident    = priv->model;
 	id->revision = 0;
 
 	return 0;
@@ -755,6 +766,27 @@ static int ov772x_set_register(struct soc_camera_device *icd,
 }
 #endif
 
+static const struct ov772x_win_size*
+ov772x_select_win(u32 width, u32 height)
+{
+	__u32 diff;
+	const struct ov772x_win_size *win;
+
+	/* default is QVGA */
+	diff = abs(width - ov772x_win_qvga.width) +
+		abs(height - ov772x_win_qvga.height);
+	win = &ov772x_win_qvga;
+
+	/* VGA */
+	if (diff >
+	    abs(width  - ov772x_win_vga.width) +
+	    abs(height - ov772x_win_vga.height))
+		win = &ov772x_win_vga;
+
+	return win;
+}
+
+
 static int ov772x_set_fmt(struct soc_camera_device *icd,
 			  __u32                     pixfmt,
 			  struct v4l2_rect         *rect)
@@ -775,34 +807,28 @@ static int ov772x_set_fmt(struct soc_camera_device *icd,
 		}
 	}
 
+	/*
+	 * select win
+	 */
+	priv->win = ov772x_select_win(rect->width, rect->height);
+
 	return ret;
 }
 
 static int ov772x_try_fmt(struct soc_camera_device *icd,
 			  struct v4l2_format       *f)
 {
-	struct v4l2_pix_format *pix  = &f->fmt.pix;
-	struct ov772x_priv     *priv;
+	struct v4l2_pix_format *pix = &f->fmt.pix;
+	const struct ov772x_win_size *win;
 
-	priv = container_of(icd, struct ov772x_priv, icd);
-
-	/* QVGA */
-	if (pix->width  <= ov772x_win_qvga.width ||
-	    pix->height <= ov772x_win_qvga.height) {
-		priv->win   = &ov772x_win_qvga;
-		pix->width  =  ov772x_win_qvga.width;
-		pix->height =  ov772x_win_qvga.height;
-	}
-
-	/* VGA */
-	else if (pix->width  <= ov772x_win_vga.width ||
-		 pix->height <= ov772x_win_vga.height) {
-		priv->win   = &ov772x_win_vga;
-		pix->width  =  ov772x_win_vga.width;
-		pix->height =  ov772x_win_vga.height;
-	}
+	/*
+	 * select suitable win
+	 */
+	win = ov772x_select_win(pix->width, pix->height);
 
-	pix->field = V4L2_FIELD_NONE;
+	pix->width  = win->width;
+	pix->height = win->height;
+	pix->field  = V4L2_FIELD_NONE;
 
 	return 0;
 }
@@ -811,6 +837,7 @@ static int ov772x_video_probe(struct soc_camera_device *icd)
 {
 	struct ov772x_priv *priv = container_of(icd, struct ov772x_priv, icd);
 	u8                  pid, ver;
+	const char         *devname;
 
 	/*
 	 * We must have a parent by now. And it cannot be a wrong one.
@@ -837,15 +864,25 @@ static int ov772x_video_probe(struct soc_camera_device *icd)
 	 */
 	pid = i2c_smbus_read_byte_data(priv->client, PID);
 	ver = i2c_smbus_read_byte_data(priv->client, VER);
-	if (pid != 0x77 ||
-	    ver != 0x21) {
+
+	switch (VERSION(pid, ver)) {
+	case OV7720:
+		devname     = "ov7720";
+		priv->model = V4L2_IDENT_OV7720;
+		break;
+	case OV7725:
+		devname     = "ov7725";
+		priv->model = V4L2_IDENT_OV7725;
+		break;
+	default:
 		dev_err(&icd->dev,
 			"Product ID error %x:%x\n", pid, ver);
 		return -ENODEV;
 	}
 
 	dev_info(&icd->dev,
-		 "ov772x Product ID %0x:%0x Manufacturer ID %x:%x\n",
+		 "%s Product ID %0x:%0x Manufacturer ID %x:%x\n",
+		 devname,
 		 pid,
 		 ver,
 		 i2c_smbus_read_byte_data(priv->client, MIDH),
@@ -924,8 +961,10 @@ static int ov772x_probe(struct i2c_client *client,
 
 	ret = soc_camera_device_register(icd);
 
-	if (ret)
+	if (ret) {
+		i2c_set_clientdata(client, NULL);
 		kfree(priv);
+	}
 
 	return ret;
 }
@@ -935,13 +974,14 @@ static int ov772x_remove(struct i2c_client *client)
 	struct ov772x_priv *priv = i2c_get_clientdata(client);
 
 	soc_camera_device_unregister(&priv->icd);
+	i2c_set_clientdata(client, NULL);
 	kfree(priv);
 	return 0;
 }
 
 #if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 26)
 static const struct i2c_device_id ov772x_id[] = {
-	{"ov772x", 0},
+	{ "ov772x", 0 },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, ov772x_id);
@@ -964,7 +1004,6 @@ static struct i2c_driver ov772x_i2c_driver = {
 
 static int __init ov772x_module_init(void)
 {
-	printk(KERN_INFO "ov772x driver\n");
 	return i2c_add_driver(&ov772x_i2c_driver);
 }
 
diff --git a/linux/drivers/media/video/pxa_camera.c b/linux/drivers/media/video/pxa_camera.c
index 210a99780..bb151032b 100644
--- a/linux/drivers/media/video/pxa_camera.c
+++ b/linux/drivers/media/video/pxa_camera.c
@@ -25,7 +25,6 @@
 #include <linux/version.h>
 #include <linux/device.h>
 #include <linux/platform_device.h>
-#include <linux/mutex.h>
 #include <linux/clk.h>
 
 #include <media/v4l2-common.h>
@@ -47,6 +46,101 @@
 #define PXA_CAM_VERSION_CODE KERNEL_VERSION(0, 0, 5)
 #define PXA_CAM_DRV_NAME "pxa27x-camera"
 
+/* Camera Interface */
+#define CICR0		0x0000
+#define CICR1		0x0004
+#define CICR2		0x0008
+#define CICR3		0x000C
+#define CICR4		0x0010
+#define CISR		0x0014
+#define CIFR		0x0018
+#define CITOR		0x001C
+#define CIBR0		0x0028
+#define CIBR1		0x0030
+#define CIBR2		0x0038
+
+#define CICR0_DMAEN	(1 << 31)	/* DMA request enable */
+#define CICR0_PAR_EN	(1 << 30)	/* Parity enable */
+#define CICR0_SL_CAP_EN	(1 << 29)	/* Capture enable for slave mode */
+#define CICR0_ENB	(1 << 28)	/* Camera interface enable */
+#define CICR0_DIS	(1 << 27)	/* Camera interface disable */
+#define CICR0_SIM	(0x7 << 24)	/* Sensor interface mode mask */
+#define CICR0_TOM	(1 << 9)	/* Time-out mask */
+#define CICR0_RDAVM	(1 << 8)	/* Receive-data-available mask */
+#define CICR0_FEM	(1 << 7)	/* FIFO-empty mask */
+#define CICR0_EOLM	(1 << 6)	/* End-of-line mask */
+#define CICR0_PERRM	(1 << 5)	/* Parity-error mask */
+#define CICR0_QDM	(1 << 4)	/* Quick-disable mask */
+#define CICR0_CDM	(1 << 3)	/* Disable-done mask */
+#define CICR0_SOFM	(1 << 2)	/* Start-of-frame mask */
+#define CICR0_EOFM	(1 << 1)	/* End-of-frame mask */
+#define CICR0_FOM	(1 << 0)	/* FIFO-overrun mask */
+
+#define CICR1_TBIT	(1 << 31)	/* Transparency bit */
+#define CICR1_RGBT_CONV	(0x3 << 29)	/* RGBT conversion mask */
+#define CICR1_PPL	(0x7ff << 15)	/* Pixels per line mask */
+#define CICR1_RGB_CONV	(0x7 << 12)	/* RGB conversion mask */
+#define CICR1_RGB_F	(1 << 11)	/* RGB format */
+#define CICR1_YCBCR_F	(1 << 10)	/* YCbCr format */
+#define CICR1_RGB_BPP	(0x7 << 7)	/* RGB bis per pixel mask */
+#define CICR1_RAW_BPP	(0x3 << 5)	/* Raw bis per pixel mask */
+#define CICR1_COLOR_SP	(0x3 << 3)	/* Color space mask */
+#define CICR1_DW	(0x7 << 0)	/* Data width mask */
+
+#define CICR2_BLW	(0xff << 24)	/* Beginning-of-line pixel clock
+					   wait count mask */
+#define CICR2_ELW	(0xff << 16)	/* End-of-line pixel clock
+					   wait count mask */
+#define CICR2_HSW	(0x3f << 10)	/* Horizontal sync pulse width mask */
+#define CICR2_BFPW	(0x3f << 3)	/* Beginning-of-frame pixel clock
+					   wait count mask */
+#define CICR2_FSW	(0x7 << 0)	/* Frame stabilization
+					   wait count mask */
+
+#define CICR3_BFW	(0xff << 24)	/* Beginning-of-frame line clock
+					   wait count mask */
+#define CICR3_EFW	(0xff << 16)	/* End-of-frame line clock
+					   wait count mask */
+#define CICR3_VSW	(0x3f << 10)	/* Vertical sync pulse width mask */
+#define CICR3_BFPW	(0x3f << 3)	/* Beginning-of-frame pixel clock
+					   wait count mask */
+#define CICR3_LPF	(0x7ff << 0)	/* Lines per frame mask */
+
+#define CICR4_MCLK_DLY	(0x3 << 24)	/* MCLK Data Capture Delay mask */
+#define CICR4_PCLK_EN	(1 << 23)	/* Pixel clock enable */
+#define CICR4_PCP	(1 << 22)	/* Pixel clock polarity */
+#define CICR4_HSP	(1 << 21)	/* Horizontal sync polarity */
+#define CICR4_VSP	(1 << 20)	/* Vertical sync polarity */
+#define CICR4_MCLK_EN	(1 << 19)	/* MCLK enable */
+#define CICR4_FR_RATE	(0x7 << 8)	/* Frame rate mask */
+#define CICR4_DIV	(0xff << 0)	/* Clock divisor mask */
+
+#define CISR_FTO	(1 << 15)	/* FIFO time-out */
+#define CISR_RDAV_2	(1 << 14)	/* Channel 2 receive data available */
+#define CISR_RDAV_1	(1 << 13)	/* Channel 1 receive data available */
+#define CISR_RDAV_0	(1 << 12)	/* Channel 0 receive data available */
+#define CISR_FEMPTY_2	(1 << 11)	/* Channel 2 FIFO empty */
+#define CISR_FEMPTY_1	(1 << 10)	/* Channel 1 FIFO empty */
+#define CISR_FEMPTY_0	(1 << 9)	/* Channel 0 FIFO empty */
+#define CISR_EOL	(1 << 8)	/* End of line */
+#define CISR_PAR_ERR	(1 << 7)	/* Parity error */
+#define CISR_CQD	(1 << 6)	/* Camera interface quick disable */
+#define CISR_CDD	(1 << 5)	/* Camera interface disable done */
+#define CISR_SOF	(1 << 4)	/* Start of frame */
+#define CISR_EOF	(1 << 3)	/* End of frame */
+#define CISR_IFO_2	(1 << 2)	/* FIFO overrun for Channel 2 */
+#define CISR_IFO_1	(1 << 1)	/* FIFO overrun for Channel 1 */
+#define CISR_IFO_0	(1 << 0)	/* FIFO overrun for Channel 0 */
+
+#define CIFR_FLVL2	(0x7f << 23)	/* FIFO 2 level mask */
+#define CIFR_FLVL1	(0x7f << 16)	/* FIFO 1 level mask */
+#define CIFR_FLVL0	(0xff << 8)	/* FIFO 0 level mask */
+#define CIFR_THL_0	(0x3 << 4)	/* Threshold Level for Channel 0 FIFO */
+#define CIFR_RESET_F	(1 << 3)	/* Reset input FIFOs */
+#define CIFR_FEN2	(1 << 2)	/* FIFO enable for channel 2 */
+#define CIFR_FEN1	(1 << 1)	/* FIFO enable for channel 1 */
+#define CIFR_FEN0	(1 << 0)	/* FIFO enable for channel 0 */
+
 #define CICR0_SIM_MP	(0 << 24)
 #define CICR0_SIM_SP	(1 << 24)
 #define CICR0_SIM_MS	(2 << 24)
@@ -74,8 +168,6 @@
 			CICR0_PERRM | CICR0_QDM | CICR0_CDM | CICR0_SOFM | \
 			CICR0_EOFM | CICR0_FOM)
 
-static DEFINE_MUTEX(camera_lock);
-
 /*
  * Structures
  */
@@ -125,7 +217,9 @@ struct pxa_camera_dev {
 	struct pxacamera_platform_data *pdata;
 	struct resource		*res;
 	unsigned long		platform_flags;
-	unsigned long		platform_mclk_10khz;
+	unsigned long		ciclk;
+	unsigned long		mclk;
+	u32			mclk_divisor;
 
 	struct list_head	capture;
 
@@ -148,8 +242,7 @@ static int pxa_videobuf_setup(struct videobuf_queue *vq, unsigned int *count,
 			      unsigned int *size)
 {
 	struct soc_camera_device *icd = vq->priv_data;
-	struct soc_camera_host *ici =
-		to_soc_camera_host(icd->dev.parent);
+	struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 	struct pxa_camera_dev *pcdev = ici->priv;
 
 	dev_dbg(&icd->dev, "count=%d, size=%d\n", *count, *size);
@@ -175,8 +268,7 @@ static int pxa_videobuf_setup(struct videobuf_queue *vq, unsigned int *count,
 static void free_buffer(struct videobuf_queue *vq, struct pxa_buffer *buf)
 {
 	struct soc_camera_device *icd = vq->priv_data;
-	struct soc_camera_host *ici =
-		to_soc_camera_host(icd->dev.parent);
+	struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 	struct pxa_camera_dev *pcdev = ici->priv;
 	struct videobuf_dmabuf *dma = videobuf_to_dma(&buf->vb);
 	int i;
@@ -252,8 +344,7 @@ static int pxa_videobuf_prepare(struct videobuf_queue *vq,
 		struct videobuf_buffer *vb, enum v4l2_field field)
 {
 	struct soc_camera_device *icd = vq->priv_data;
-	struct soc_camera_host *ici =
-		to_soc_camera_host(icd->dev.parent);
+	struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 	struct pxa_camera_dev *pcdev = ici->priv;
 	struct pxa_buffer *buf = container_of(vb, struct pxa_buffer, vb);
 	int ret;
@@ -372,8 +463,7 @@ static void pxa_videobuf_queue(struct videobuf_queue *vq,
 			       struct videobuf_buffer *vb)
 {
 	struct soc_camera_device *icd = vq->priv_data;
-	struct soc_camera_host *ici =
-		to_soc_camera_host(icd->dev.parent);
+	struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 	struct pxa_camera_dev *pcdev = ici->priv;
 	struct pxa_buffer *buf = container_of(vb, struct pxa_buffer, vb);
 	struct pxa_buffer *active;
@@ -390,7 +480,10 @@ static void pxa_videobuf_queue(struct videobuf_queue *vq,
 	active = pcdev->active;
 
 	if (!active) {
-		CIFR |= CIFR_RESET_F;
+		unsigned long cifr, cicr0;
+
+		cifr = __raw_readl(pcdev->base + CIFR) | CIFR_RESET_F;
+		__raw_writel(cifr, pcdev->base + CIFR);
 
 		for (i = 0; i < pcdev->channels; i++) {
 			DDADR(pcdev->dma_chans[i]) = buf->dmas[i].sg_dma;
@@ -399,7 +492,9 @@ static void pxa_videobuf_queue(struct videobuf_queue *vq,
 		}
 
 		pcdev->active = buf;
-		CICR0 |= CICR0_ENB;
+
+		cicr0 = __raw_readl(pcdev->base + CICR0) | CICR0_ENB;
+		__raw_writel(cicr0, pcdev->base + CICR0);
 	} else {
 		struct pxa_cam_dma *buf_dma;
 		struct pxa_cam_dma *act_dma;
@@ -483,6 +578,8 @@ static void pxa_camera_wakeup(struct pxa_camera_dev *pcdev,
 			      struct videobuf_buffer *vb,
 			      struct pxa_buffer *buf)
 {
+	unsigned long cicr0;
+
 	/* _init is used to debug races, see comment in pxa_camera_reqbufs() */
 	list_del_init(&vb->queue);
 	vb->state = VIDEOBUF_DONE;
@@ -495,7 +592,9 @@ static void pxa_camera_wakeup(struct pxa_camera_dev *pcdev,
 		DCSR(pcdev->dma_chans[0]) = 0;
 		DCSR(pcdev->dma_chans[1]) = 0;
 		DCSR(pcdev->dma_chans[2]) = 0;
-		CICR0 &= ~CICR0_ENB;
+
+		cicr0 = __raw_readl(pcdev->base + CICR0) & ~CICR0_ENB;
+		__raw_writel(cicr0, pcdev->base + CICR0);
 		return;
 	}
 
@@ -510,6 +609,7 @@ static void pxa_camera_dma_irq(int channel, struct pxa_camera_dev *pcdev,
 	unsigned long flags;
 	u32 status, camera_status, overrun;
 	struct videobuf_buffer *vb;
+	unsigned long cifr, cicr0;
 
 	spin_lock_irqsave(&pcdev->lock, flags);
 
@@ -532,22 +632,26 @@ static void pxa_camera_dma_irq(int channel, struct pxa_camera_dev *pcdev,
 		goto out;
 	}
 
-	camera_status = CISR;
+	camera_status = __raw_readl(pcdev->base + CISR);
 	overrun = CISR_IFO_0;
 	if (pcdev->channels == 3)
 		overrun |= CISR_IFO_1 | CISR_IFO_2;
 	if (camera_status & overrun) {
 		dev_dbg(pcdev->dev, "FIFO overrun! CISR: %x\n", camera_status);
 		/* Stop the Capture Interface */
-		CICR0 &= ~CICR0_ENB;
+		cicr0 = __raw_readl(pcdev->base + CICR0) & ~CICR0_ENB;
+		__raw_writel(cicr0, pcdev->base + CICR0);
+
 		/* Stop DMA */
 		DCSR(channel) = 0;
 		/* Reset the FIFOs */
-		CIFR |= CIFR_RESET_F;
+		cifr = __raw_readl(pcdev->base + CIFR) | CIFR_RESET_F;
+		__raw_writel(cifr, pcdev->base + CIFR);
 		/* Enable End-Of-Frame Interrupt */
-		CICR0 &= ~CICR0_EOFM;
+		cicr0 &= ~CICR0_EOFM;
+		__raw_writel(cicr0, pcdev->base + CICR0);
 		/* Restart the Capture Interface */
-		CICR0 |= CICR0_ENB;
+		__raw_writel(cicr0 | CICR0_ENB, pcdev->base + CICR0);
 		goto out;
 	}
 
@@ -603,24 +707,43 @@ static void pxa_camera_init_videobuf(struct videobuf_queue *q,
 				sizeof(struct pxa_buffer), icd);
 }
 
-static int mclk_get_divisor(struct pxa_camera_dev *pcdev)
+static u32 mclk_get_divisor(struct pxa_camera_dev *pcdev)
 {
-	unsigned int mclk_10khz = pcdev->platform_mclk_10khz;
-	unsigned long div;
+	unsigned long mclk = pcdev->mclk;
+	u32 div;
 	unsigned long lcdclk;
 
-	lcdclk = clk_get_rate(pcdev->clk) / 10000;
+	lcdclk = clk_get_rate(pcdev->clk);
+	pcdev->ciclk = lcdclk;
+
+	/* mclk <= ciclk / 4 (27.4.2) */
+	if (mclk > lcdclk / 4) {
+		mclk = lcdclk / 4;
+		dev_warn(pcdev->dev, "Limiting master clock to %lu\n", mclk);
+	}
+
+	/* We verify mclk != 0, so if anyone breaks it, here comes their Oops */
+	div = (lcdclk + 2 * mclk - 1) / (2 * mclk) - 1;
 
-	/* We verify platform_mclk_10khz != 0, so if anyone breaks it, here
-	 * they get a nice Oops */
-	div = (lcdclk + 2 * mclk_10khz - 1) / (2 * mclk_10khz) - 1;
+	/* If we're not supplying MCLK, leave it at 0 */
+	if (pcdev->platform_flags & PXA_CAMERA_MCLK_EN)
+		pcdev->mclk = lcdclk / (2 * (div + 1));
 
-	dev_dbg(pcdev->dev, "LCD clock %lukHz, target freq %dkHz, "
-		"divisor %lu\n", lcdclk * 10, mclk_10khz * 10, div);
+	dev_dbg(pcdev->dev, "LCD clock %luHz, target freq %luHz, "
+		"divisor %u\n", lcdclk, mclk, div);
 
 	return div;
 }
 
+static void recalculate_fifo_timeout(struct pxa_camera_dev *pcdev,
+				     unsigned long pclk)
+{
+	/* We want a timeout > 1 pixel time, not ">=" */
+	u32 ciclk_per_pixel = pcdev->ciclk / pclk + 1;
+
+	__raw_writel(ciclk_per_pixel, pcdev->base + CITOR);
+}
+
 static void pxa_camera_activate(struct pxa_camera_dev *pcdev)
 {
 	struct pxacamera_platform_data *pdata = pcdev->pdata;
@@ -634,7 +757,8 @@ static void pxa_camera_activate(struct pxa_camera_dev *pcdev)
 		pdata->init(pcdev->dev);
 	}
 
-	CICR0 = 0x3FF;   /* disable all interrupts */
+	/* disable all interrupts */
+	__raw_writel(0x3ff, pcdev->base + CICR0);
 
 	if (pcdev->platform_flags & PXA_CAMERA_PCLK_EN)
 		cicr4 |= CICR4_PCLK_EN;
@@ -647,7 +771,14 @@ static void pxa_camera_activate(struct pxa_camera_dev *pcdev)
 	if (pcdev->platform_flags & PXA_CAMERA_VSP)
 		cicr4 |= CICR4_VSP;
 
-	CICR4 = mclk_get_divisor(pcdev) | cicr4;
+	__raw_writel(pcdev->mclk_divisor | cicr4, pcdev->base + CICR4);
+
+	if (pcdev->platform_flags & PXA_CAMERA_MCLK_EN)
+		/* Initialise the timeout under the assumption pclk = mclk */
+		recalculate_fifo_timeout(pcdev, pcdev->mclk);
+	else
+		/* "Safe default" - 13MHz */
+		recalculate_fifo_timeout(pcdev, 13000000);
 
 	clk_enable(pcdev->clk);
 }
@@ -660,14 +791,15 @@ static void pxa_camera_deactivate(struct pxa_camera_dev *pcdev)
 static irqreturn_t pxa_camera_irq(int irq, void *data)
 {
 	struct pxa_camera_dev *pcdev = data;
-	unsigned int status = CISR;
+	unsigned long status, cicr0;
 
-	dev_dbg(pcdev->dev, "Camera interrupt status 0x%x\n", status);
+	status = __raw_readl(pcdev->base + CISR);
+	dev_dbg(pcdev->dev, "Camera interrupt status 0x%lx\n", status);
 
 	if (!status)
 		return IRQ_NONE;
 
-	CISR = status;
+	__raw_writel(status, pcdev->base + CISR);
 
 	if (status & CISR_EOF) {
 		int i;
@@ -676,22 +808,24 @@ static irqreturn_t pxa_camera_irq(int irq, void *data)
 				pcdev->active->dmas[i].sg_dma;
 			DCSR(pcdev->dma_chans[i]) = DCSR_RUN;
 		}
-		CICR0 |= CICR0_EOFM;
+		cicr0 = __raw_readl(pcdev->base + CICR0) | CICR0_EOFM;
+		__raw_writel(cicr0, pcdev->base + CICR0);
 	}
 
 	return IRQ_HANDLED;
 }
 
-/* The following two functions absolutely depend on the fact, that
- * there can be only one camera on PXA quick capture interface */
+/*
+ * The following two functions absolutely depend on the fact, that
+ * there can be only one camera on PXA quick capture interface
+ * Called with .video_lock held
+ */
 static int pxa_camera_add_device(struct soc_camera_device *icd)
 {
 	struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 	struct pxa_camera_dev *pcdev = ici->priv;
 	int ret;
 
-	mutex_lock(&camera_lock);
-
 	if (pcdev->icd) {
 		ret = -EBUSY;
 		goto ebusy;
@@ -707,11 +841,10 @@ static int pxa_camera_add_device(struct soc_camera_device *icd)
 		pcdev->icd = icd;
 
 ebusy:
-	mutex_unlock(&camera_lock);
-
 	return ret;
 }
 
+/* Called with .video_lock held */
 static void pxa_camera_remove_device(struct soc_camera_device *icd)
 {
 	struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
@@ -723,7 +856,7 @@ static void pxa_camera_remove_device(struct soc_camera_device *icd)
 		 icd->devnum);
 
 	/* disable capture, disable interrupts */
-	CICR0 = 0x3ff;
+	__raw_writel(0x3ff, pcdev->base + CICR0);
 
 	/* Stop DMA engine */
 	DCSR(pcdev->dma_chans[0]) = 0;
@@ -780,11 +913,10 @@ static int test_platform_param(struct pxa_camera_dev *pcdev,
 
 static int pxa_camera_set_bus_param(struct soc_camera_device *icd, __u32 pixfmt)
 {
-	struct soc_camera_host *ici =
-		to_soc_camera_host(icd->dev.parent);
+	struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 	struct pxa_camera_dev *pcdev = ici->priv;
 	unsigned long dw, bpp, bus_flags, camera_flags, common_flags;
-	u32 cicr0, cicr1, cicr4 = 0;
+	u32 cicr0, cicr1, cicr2, cicr3, cicr4 = 0;
 	int ret = test_platform_param(pcdev, icd->buswidth, &bus_flags);
 
 	if (ret < 0)
@@ -857,9 +989,9 @@ static int pxa_camera_set_bus_param(struct soc_camera_device *icd, __u32 pixfmt)
 	if (common_flags & SOCAM_VSYNC_ACTIVE_LOW)
 		cicr4 |= CICR4_VSP;
 
-	cicr0 = CICR0;
+	cicr0 = __raw_readl(pcdev->base + CICR0);
 	if (cicr0 & CICR0_ENB)
-		CICR0 = cicr0 & ~CICR0_ENB;
+		__raw_writel(cicr0 & ~CICR0_ENB, pcdev->base + CICR0);
 
 	cicr1 = CICR1_PPL_VAL(icd->width - 1) | bpp | dw;
 
@@ -889,16 +1021,21 @@ static int pxa_camera_set_bus_param(struct soc_camera_device *icd, __u32 pixfmt)
 		break;
 	}
 
-	CICR1 = cicr1;
-	CICR2 = 0;
-	CICR3 = CICR3_LPF_VAL(icd->height - 1) |
+	cicr2 = 0;
+	cicr3 = CICR3_LPF_VAL(icd->height - 1) |
 		CICR3_BFW_VAL(min((unsigned short)255, icd->y_skip_top));
-	CICR4 = mclk_get_divisor(pcdev) | cicr4;
+	cicr4 |= pcdev->mclk_divisor;
+
+	__raw_writel(cicr1, pcdev->base + CICR1);
+	__raw_writel(cicr2, pcdev->base + CICR2);
+	__raw_writel(cicr3, pcdev->base + CICR3);
+	__raw_writel(cicr4, pcdev->base + CICR4);
 
 	/* CIF interrupts are not used, only DMA */
-	CICR0 = (pcdev->platform_flags & PXA_CAMERA_MASTER ?
-		 CICR0_SIM_MP : (CICR0_SL_CAP_EN | CICR0_SIM_SP)) |
-		CICR0_DMAEN | CICR0_IRQ_MASK | (cicr0 & CICR0_ENB);
+	cicr0 = (cicr0 & CICR0_ENB) | (pcdev->platform_flags & PXA_CAMERA_MASTER ?
+		CICR0_SIM_MP : (CICR0_SL_CAP_EN | CICR0_SIM_SP));
+	cicr0 |= CICR0_DMAEN | CICR0_IRQ_MASK;
+	__raw_writel(cicr0, pcdev->base + CICR0);
 
 	return 0;
 }
@@ -906,8 +1043,7 @@ static int pxa_camera_set_bus_param(struct soc_camera_device *icd, __u32 pixfmt)
 static int pxa_camera_try_bus_param(struct soc_camera_device *icd,
 				    unsigned char buswidth)
 {
-	struct soc_camera_host *ici =
-		to_soc_camera_host(icd->dev.parent);
+	struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 	struct pxa_camera_dev *pcdev = ici->priv;
 	unsigned long bus_flags, camera_flags;
 	int ret = test_platform_param(pcdev, buswidth, &bus_flags);
@@ -1023,8 +1159,13 @@ static int pxa_camera_set_fmt(struct soc_camera_device *icd,
 			      __u32 pixfmt, struct v4l2_rect *rect)
 {
 	struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+	struct pxa_camera_dev *pcdev = ici->priv;
 	const struct soc_camera_data_format *host_fmt, *cam_fmt = NULL;
 	const struct soc_camera_format_xlate *xlate;
+	struct soc_camera_sense sense = {
+		.master_clock = pcdev->mclk,
+		.pixel_clock_max = pcdev->ciclk / 4,
+	};
 	int ret, buswidth;
 
 	xlate = soc_camera_xlate_by_fourcc(icd, pixfmt);
@@ -1037,6 +1178,10 @@ static int pxa_camera_set_fmt(struct soc_camera_device *icd,
 	host_fmt = xlate->host_fmt;
 	cam_fmt = xlate->cam_fmt;
 
+	/* If PCLK is used to latch data from the sensor, check sense */
+	if (pcdev->platform_flags & PXA_CAMERA_PCLK_EN)
+		icd->sense = &sense;
+
 	switch (pixfmt) {
 	case 0:				/* Only geometry change */
 		ret = icd->ops->set_fmt(icd, pixfmt, rect);
@@ -1045,9 +1190,20 @@ static int pxa_camera_set_fmt(struct soc_camera_device *icd,
 		ret = icd->ops->set_fmt(icd, cam_fmt->fourcc, rect);
 	}
 
-	if (ret < 0)
+	icd->sense = NULL;
+
+	if (ret < 0) {
 		dev_warn(&ici->dev, "Failed to configure for format %x\n",
 			 pixfmt);
+	} else if (sense.flags & SOCAM_SENSE_PCLK_CHANGED) {
+		if (sense.pixel_clock > sense.pixel_clock_max) {
+			dev_err(&ici->dev,
+				"pixel clock %lu set by the camera too high!",
+				sense.pixel_clock);
+			return -EIO;
+		}
+		recalculate_fifo_timeout(pcdev, sense.pixel_clock);
+	}
 
 	if (pixfmt && !ret) {
 		icd->buswidth = buswidth;
@@ -1064,6 +1220,8 @@ static int pxa_camera_try_fmt(struct soc_camera_device *icd,
 	const struct soc_camera_format_xlate *xlate;
 	struct v4l2_pix_format *pix = &f->fmt.pix;
 	__u32 pixfmt = pix->pixelformat;
+	enum v4l2_field field;
+	int ret;
 
 	xlate = soc_camera_xlate_by_fourcc(icd, pixfmt);
 	if (!xlate) {
@@ -1086,8 +1244,22 @@ static int pxa_camera_try_fmt(struct soc_camera_device *icd,
 		DIV_ROUND_UP(xlate->host_fmt->depth, 8);
 	pix->sizeimage = pix->height * pix->bytesperline;
 
+	/* camera has to see its format, but the user the original one */
+	pix->pixelformat = xlate->cam_fmt->fourcc;
 	/* limit to sensor capabilities */
-	return icd->ops->try_fmt(icd, f);
+	ret = icd->ops->try_fmt(icd, f);
+	pix->pixelformat = xlate->host_fmt->fourcc;
+
+	field = pix->field;
+
+	if (field == V4L2_FIELD_ANY) {
+		pix->field = V4L2_FIELD_NONE;
+	} else if (field != V4L2_FIELD_NONE) {
+		dev_err(&icd->dev, "Field type %d unsupported.\n", field);
+		return -EINVAL;
+	}
+
+	return ret;
 }
 
 static int pxa_camera_reqbufs(struct soc_camera_file *icf,
@@ -1139,16 +1311,15 @@ static int pxa_camera_querycap(struct soc_camera_host *ici,
 
 static int pxa_camera_suspend(struct soc_camera_device *icd, pm_message_t state)
 {
-	struct soc_camera_host *ici =
-		to_soc_camera_host(icd->dev.parent);
+	struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 	struct pxa_camera_dev *pcdev = ici->priv;
 	int i = 0, ret = 0;
 
-	pcdev->save_cicr[i++] = CICR0;
-	pcdev->save_cicr[i++] = CICR1;
-	pcdev->save_cicr[i++] = CICR2;
-	pcdev->save_cicr[i++] = CICR3;
-	pcdev->save_cicr[i++] = CICR4;
+	pcdev->save_cicr[i++] = __raw_readl(pcdev->base + CICR0);
+	pcdev->save_cicr[i++] = __raw_readl(pcdev->base + CICR1);
+	pcdev->save_cicr[i++] = __raw_readl(pcdev->base + CICR2);
+	pcdev->save_cicr[i++] = __raw_readl(pcdev->base + CICR3);
+	pcdev->save_cicr[i++] = __raw_readl(pcdev->base + CICR4);
 
 	if ((pcdev->icd) && (pcdev->icd->ops->suspend))
 		ret = pcdev->icd->ops->suspend(pcdev->icd, state);
@@ -1158,8 +1329,7 @@ static int pxa_camera_suspend(struct soc_camera_device *icd, pm_message_t state)
 
 static int pxa_camera_resume(struct soc_camera_device *icd)
 {
-	struct soc_camera_host *ici =
-		to_soc_camera_host(icd->dev.parent);
+	struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 	struct pxa_camera_dev *pcdev = ici->priv;
 	int i = 0, ret = 0;
 
@@ -1167,23 +1337,27 @@ static int pxa_camera_resume(struct soc_camera_device *icd)
 	DRCMR(69) = pcdev->dma_chans[1] | DRCMR_MAPVLD;
 	DRCMR(70) = pcdev->dma_chans[2] | DRCMR_MAPVLD;
 
-	CICR0 = pcdev->save_cicr[i++] & ~CICR0_ENB;
-	CICR1 = pcdev->save_cicr[i++];
-	CICR2 = pcdev->save_cicr[i++];
-	CICR3 = pcdev->save_cicr[i++];
-	CICR4 = pcdev->save_cicr[i++];
+	__raw_writel(pcdev->save_cicr[i++] & ~CICR0_ENB, pcdev->base + CICR0);
+	__raw_writel(pcdev->save_cicr[i++], pcdev->base + CICR1);
+	__raw_writel(pcdev->save_cicr[i++], pcdev->base + CICR2);
+	__raw_writel(pcdev->save_cicr[i++], pcdev->base + CICR3);
+	__raw_writel(pcdev->save_cicr[i++], pcdev->base + CICR4);
 
 	if ((pcdev->icd) && (pcdev->icd->ops->resume))
 		ret = pcdev->icd->ops->resume(pcdev->icd);
 
 	/* Restart frame capture if active buffer exists */
 	if (!ret && pcdev->active) {
+		unsigned long cifr, cicr0;
+
 		/* Reset the FIFOs */
-		CIFR |= CIFR_RESET_F;
-		/* Enable End-Of-Frame Interrupt */
-		CICR0 &= ~CICR0_EOFM;
-		/* Restart the Capture Interface */
-		CICR0 |= CICR0_ENB;
+		cifr = __raw_readl(pcdev->base + CIFR) | CIFR_RESET_F;
+		__raw_writel(cifr, pcdev->base + CIFR);
+
+		cicr0 = __raw_readl(pcdev->base + CICR0);
+		cicr0 &= ~CICR0_EOFM;	/* Enable End-Of-Frame Interrupt */
+		cicr0 |= CICR0_ENB;	/* Restart the Capture Interface */
+		__raw_writel(cicr0, pcdev->base + CICR0);
 	}
 
 	return ret;
@@ -1252,14 +1426,17 @@ static int pxa_camera_probe(struct platform_device *pdev)
 			 "data widths, using default 10 bit\n");
 		pcdev->platform_flags |= PXA_CAMERA_DATAWIDTH_10;
 	}
-	pcdev->platform_mclk_10khz = pcdev->pdata->mclk_10khz;
-	if (!pcdev->platform_mclk_10khz) {
+	pcdev->mclk = pcdev->pdata->mclk_10khz * 10000;
+	if (!pcdev->mclk) {
 		dev_warn(&pdev->dev,
-			 "mclk_10khz == 0! Please, fix your platform data. "
+			 "mclk == 0! Please, fix your platform data. "
 			 "Using default 20MHz\n");
-		pcdev->platform_mclk_10khz = 2000;
+		pcdev->mclk = 20000000;
 	}
 
+	pcdev->dev = &pdev->dev;
+	pcdev->mclk_divisor = mclk_get_divisor(pcdev);
+
 	INIT_LIST_HEAD(&pcdev->capture);
 	spin_lock_init(&pcdev->lock);
 
@@ -1279,7 +1456,6 @@ static int pxa_camera_probe(struct platform_device *pdev)
 	}
 	pcdev->irq = irq;
 	pcdev->base = base;
-	pcdev->dev = &pdev->dev;
 
 	/* request dma */
 	err = pxa_request_dma("CI_Y", DMA_PRIO_HIGH,
diff --git a/linux/drivers/media/video/sh_mobile_ceu_camera.c b/linux/drivers/media/video/sh_mobile_ceu_camera.c
index bd2a1754b..ae98eaf5f 100644
--- a/linux/drivers/media/video/sh_mobile_ceu_camera.c
+++ b/linux/drivers/media/video/sh_mobile_ceu_camera.c
@@ -29,8 +29,8 @@
 #include <linux/version.h>
 #include <linux/device.h>
 #include <linux/platform_device.h>
-#include <linux/mutex.h>
 #include <linux/videodev2.h>
+#include <linux/clk.h>
 
 #include <media/v4l2-common.h>
 #include <media/v4l2-dev.h>
@@ -75,8 +75,6 @@
 #define CDBYR2 0x98 /* Capture data bottom-field address Y register 2 */
 #define CDBCR2 0x9c /* Capture data bottom-field address C register 2 */
 
-static DEFINE_MUTEX(camera_lock);
-
 /* per video frame buffer */
 struct sh_mobile_ceu_buffer {
 	struct videobuf_buffer vb; /* v4l buffer must be first */
@@ -90,24 +88,27 @@ struct sh_mobile_ceu_dev {
 
 	unsigned int irq;
 	void __iomem *base;
+	struct clk *clk;
 	unsigned long video_limit;
 
 	/* lock used to protect videobuf */
 	spinlock_t lock;
 	struct list_head capture;
 	struct videobuf_buffer *active;
+	int is_interlace;
 
 	struct sh_mobile_ceu_info *pdata;
+
+	const struct soc_camera_data_format *camera_fmt;
 };
 
 static void ceu_write(struct sh_mobile_ceu_dev *priv,
-		      unsigned long reg_offs, unsigned long data)
+		      unsigned long reg_offs, u32 data)
 {
 	iowrite32(data, priv->base + reg_offs);
 }
 
-static unsigned long ceu_read(struct sh_mobile_ceu_dev *priv,
-			      unsigned long reg_offs)
+static u32 ceu_read(struct sh_mobile_ceu_dev *priv, unsigned long reg_offs)
 {
 	return ioread32(priv->base + reg_offs);
 }
@@ -155,21 +156,52 @@ static void free_buffer(struct videobuf_queue *vq,
 	buf->vb.state = VIDEOBUF_NEEDS_INIT;
 }
 
+#define CEU_CETCR_MAGIC 0x0317f313 /* acknowledge magical interrupt sources */
+#define CEU_CETCR_IGRW (1 << 4) /* prohibited register access interrupt bit */
+#define CEU_CEIER_CPEIE (1 << 0) /* one-frame capture end interrupt */
+#define CEU_CAPCR_CTNCP (1 << 16) /* continuous capture mode (if set) */
+
+
 static void sh_mobile_ceu_capture(struct sh_mobile_ceu_dev *pcdev)
 {
-	ceu_write(pcdev, CEIER, ceu_read(pcdev, CEIER) & ~1);
-	ceu_write(pcdev, CETCR, ~ceu_read(pcdev, CETCR) & 0x0317f313);
-	ceu_write(pcdev, CEIER, ceu_read(pcdev, CEIER) | 1);
-
-	ceu_write(pcdev, CAPCR, ceu_read(pcdev, CAPCR) & ~0x10000);
+	struct soc_camera_device *icd = pcdev->icd;
+	dma_addr_t phys_addr_top, phys_addr_bottom;
 
-	ceu_write(pcdev, CETCR, 0x0317f313 ^ 0x10);
+	/* The hardware is _very_ picky about this sequence. Especially
+	 * the CEU_CETCR_MAGIC value. It seems like we need to acknowledge
+	 * several not-so-well documented interrupt sources in CETCR.
+	 */
+	ceu_write(pcdev, CEIER, ceu_read(pcdev, CEIER) & ~CEU_CEIER_CPEIE);
+	ceu_write(pcdev, CETCR, ~ceu_read(pcdev, CETCR) & CEU_CETCR_MAGIC);
+	ceu_write(pcdev, CEIER, ceu_read(pcdev, CEIER) | CEU_CEIER_CPEIE);
+	ceu_write(pcdev, CAPCR, ceu_read(pcdev, CAPCR) & ~CEU_CAPCR_CTNCP);
+	ceu_write(pcdev, CETCR, CEU_CETCR_MAGIC ^ CEU_CETCR_IGRW);
+
+	if (!pcdev->active)
+		return;
+
+	phys_addr_top = videobuf_to_dma_contig(pcdev->active);
+	ceu_write(pcdev, CDAYR, phys_addr_top);
+	if (pcdev->is_interlace) {
+		phys_addr_bottom = phys_addr_top + icd->width;
+		ceu_write(pcdev, CDBYR, phys_addr_bottom);
+	}
 
-	if (pcdev->active) {
-		pcdev->active->state = VIDEOBUF_ACTIVE;
-		ceu_write(pcdev, CDAYR, videobuf_to_dma_contig(pcdev->active));
-		ceu_write(pcdev, CAPSR, 0x1); /* start capture */
+	switch (icd->current_fmt->fourcc) {
+	case V4L2_PIX_FMT_NV12:
+	case V4L2_PIX_FMT_NV21:
+	case V4L2_PIX_FMT_NV16:
+	case V4L2_PIX_FMT_NV61:
+		phys_addr_top += icd->width * icd->height;
+		ceu_write(pcdev, CDACR, phys_addr_top);
+		if (pcdev->is_interlace) {
+			phys_addr_bottom = phys_addr_top + icd->width;
+			ceu_write(pcdev, CDBCR, phys_addr_bottom);
+		}
 	}
+
+	pcdev->active->state = VIDEOBUF_ACTIVE;
+	ceu_write(pcdev, CAPSR, 0x1); /* start capture */
 }
 
 static int sh_mobile_ceu_videobuf_prepare(struct videobuf_queue *vq,
@@ -291,14 +323,13 @@ static irqreturn_t sh_mobile_ceu_irq(int irq, void *data)
 	return IRQ_HANDLED;
 }
 
+/* Called with .video_lock held */
 static int sh_mobile_ceu_add_device(struct soc_camera_device *icd)
 {
 	struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 	struct sh_mobile_ceu_dev *pcdev = ici->priv;
 	int ret = -EBUSY;
 
-	mutex_lock(&camera_lock);
-
 	if (pcdev->icd)
 		goto err;
 
@@ -310,17 +341,18 @@ static int sh_mobile_ceu_add_device(struct soc_camera_device *icd)
 	if (ret)
 		goto err;
 
+	clk_enable(pcdev->clk);
+
 	ceu_write(pcdev, CAPSR, 1 << 16); /* reset */
 	while (ceu_read(pcdev, CSTSR) & 1)
 		msleep(1);
 
 	pcdev->icd = icd;
 err:
-	mutex_unlock(&camera_lock);
-
 	return ret;
 }
 
+/* Called with .video_lock held */
 static void sh_mobile_ceu_remove_device(struct soc_camera_device *icd)
 {
 	struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
@@ -343,6 +375,8 @@ static void sh_mobile_ceu_remove_device(struct soc_camera_device *icd)
 	}
 	spin_unlock_irqrestore(&pcdev->lock, flags);
 
+	clk_disable(pcdev->clk);
+
 	icd->ops->release(icd);
 
 	dev_info(&icd->dev,
@@ -357,8 +391,9 @@ static int sh_mobile_ceu_set_bus_param(struct soc_camera_device *icd,
 {
 	struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 	struct sh_mobile_ceu_dev *pcdev = ici->priv;
-	int ret, buswidth, width, cfszr_width, cdwdr_width;
+	int ret, buswidth, width, height, cfszr_width, cdwdr_width;
 	unsigned long camera_flags, common_flags, value;
+	int yuv_mode, yuv_lineskip;
 
 	camera_flags = icd->ops->query_bus_param(icd);
 	common_flags = soc_camera_bus_param_compatible(camera_flags,
@@ -384,27 +419,71 @@ static int sh_mobile_ceu_set_bus_param(struct soc_camera_device *icd,
 	ceu_write(pcdev, CRCNTR, 0);
 	ceu_write(pcdev, CRCMPR, 0);
 
-	value = 0x00000010;
-	value |= (common_flags & SOCAM_VSYNC_ACTIVE_LOW) ? (1 << 1) : 0;
-	value |= (common_flags & SOCAM_HSYNC_ACTIVE_LOW) ? (1 << 0) : 0;
-	value |= (buswidth == 16) ? (1 << 12) : 0;
+	value = 0x00000010; /* data fetch by default */
+	yuv_mode = yuv_lineskip = 0;
+
+	switch (icd->current_fmt->fourcc) {
+	case V4L2_PIX_FMT_NV12:
+	case V4L2_PIX_FMT_NV21:
+		yuv_lineskip = 1; /* skip for NV12/21, no skip for NV16/61 */
+		/* fall-through */
+	case V4L2_PIX_FMT_NV16:
+	case V4L2_PIX_FMT_NV61:
+		yuv_mode = 1;
+		switch (pcdev->camera_fmt->fourcc) {
+		case V4L2_PIX_FMT_UYVY:
+			value = 0x00000000; /* Cb0, Y0, Cr0, Y1 */
+			break;
+		case V4L2_PIX_FMT_VYUY:
+			value = 0x00000100; /* Cr0, Y0, Cb0, Y1 */
+			break;
+		case V4L2_PIX_FMT_YUYV:
+			value = 0x00000200; /* Y0, Cb0, Y1, Cr0 */
+			break;
+		case V4L2_PIX_FMT_YVYU:
+			value = 0x00000300; /* Y0, Cr0, Y1, Cb0 */
+			break;
+		default:
+			BUG();
+		}
+	}
+
+	if (icd->current_fmt->fourcc == V4L2_PIX_FMT_NV21 ||
+	    icd->current_fmt->fourcc == V4L2_PIX_FMT_NV61)
+		value ^= 0x00000100; /* swap U, V to change from NV1x->NVx1 */
+
+	value |= common_flags & SOCAM_VSYNC_ACTIVE_LOW ? 1 << 1 : 0;
+	value |= common_flags & SOCAM_HSYNC_ACTIVE_LOW ? 1 << 0 : 0;
+	value |= buswidth == 16 ? 1 << 12 : 0;
 	ceu_write(pcdev, CAMCR, value);
 
 	ceu_write(pcdev, CAPCR, 0x00300000);
-	ceu_write(pcdev, CAIFR, 0);
+	ceu_write(pcdev, CAIFR, (pcdev->is_interlace) ? 0x101 : 0);
 
 	mdelay(1);
 
-	width = icd->width * (icd->current_fmt->depth / 8);
-	width = (buswidth == 16) ? width / 2 : width;
-	cfszr_width = (buswidth == 8) ? width / 2 : width;
-	cdwdr_width = (buswidth == 16) ? width * 2 : width;
+	if (yuv_mode) {
+		width = icd->width * 2;
+		width = buswidth == 16 ? width / 2 : width;
+		cfszr_width = cdwdr_width = icd->width;
+	} else {
+		width = icd->width * ((icd->current_fmt->depth + 7) >> 3);
+		width = buswidth == 16 ? width / 2 : width;
+		cfszr_width = buswidth == 8 ? width / 2 : width;
+		cdwdr_width = buswidth == 16 ? width * 2 : width;
+	}
+
+	height = icd->height;
+	if (pcdev->is_interlace) {
+		height /= 2;
+		cdwdr_width *= 2;
+	}
 
 	ceu_write(pcdev, CAMOR, 0);
-	ceu_write(pcdev, CAPWR, (icd->height << 16) | width);
-	ceu_write(pcdev, CFLCR, 0); /* data fetch mode - no scaling */
-	ceu_write(pcdev, CFSZR, (icd->height << 16) | cfszr_width);
-	ceu_write(pcdev, CLFCR, 0); /* data fetch mode - no lowpass filter */
+	ceu_write(pcdev, CAPWR, (height << 16) | width);
+	ceu_write(pcdev, CFLCR, 0); /* no scaling */
+	ceu_write(pcdev, CFSZR, (height << 16) | cfszr_width);
+	ceu_write(pcdev, CLFCR, 0); /* no lowpass filter */
 
 	/* A few words about byte order (observed in Big Endian mode)
 	 *
@@ -418,19 +497,20 @@ static int sh_mobile_ceu_set_bus_param(struct soc_camera_device *icd,
 	 * using 7 we swap the data bytes to match the incoming order:
 	 * D0, D1, D2, D3, D4, D5, D6, D7
 	 */
-	ceu_write(pcdev, CDOCR, 0x00000017);
+	value = 0x00000017;
+	if (yuv_lineskip)
+		value &= ~0x00000010; /* convert 4:2:2 -> 4:2:0 */
+
+	ceu_write(pcdev, CDOCR, value);
 
 	ceu_write(pcdev, CDWDR, cdwdr_width);
 	ceu_write(pcdev, CFWCR, 0); /* keep "datafetch firewall" disabled */
 
 	/* not in bundle mode: skip CBDSR, CDAYR2, CDACR2, CDBYR2, CDBCR2 */
-	/* in data fetch mode: no need for CDACR, CDBYR, CDBCR */
-
 	return 0;
 }
 
-static int sh_mobile_ceu_try_bus_param(struct soc_camera_device *icd,
-				       __u32 pixfmt)
+static int sh_mobile_ceu_try_bus_param(struct soc_camera_device *icd)
 {
 	struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 	struct sh_mobile_ceu_dev *pcdev = ici->priv;
@@ -445,25 +525,104 @@ static int sh_mobile_ceu_try_bus_param(struct soc_camera_device *icd,
 	return 0;
 }
 
+static const struct soc_camera_data_format sh_mobile_ceu_formats[] = {
+	{
+		.name		= "NV12",
+		.depth		= 12,
+		.fourcc		= V4L2_PIX_FMT_NV12,
+		.colorspace	= V4L2_COLORSPACE_JPEG,
+	},
+	{
+		.name		= "NV21",
+		.depth		= 12,
+		.fourcc		= V4L2_PIX_FMT_NV21,
+		.colorspace	= V4L2_COLORSPACE_JPEG,
+	},
+	{
+		.name		= "NV16",
+		.depth		= 16,
+		.fourcc		= V4L2_PIX_FMT_NV16,
+		.colorspace	= V4L2_COLORSPACE_JPEG,
+	},
+	{
+		.name		= "NV61",
+		.depth		= 16,
+		.fourcc		= V4L2_PIX_FMT_NV61,
+		.colorspace	= V4L2_COLORSPACE_JPEG,
+	},
+};
+
+static int sh_mobile_ceu_get_formats(struct soc_camera_device *icd, int idx,
+				     struct soc_camera_format_xlate *xlate)
+{
+	struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+	int ret, k, n;
+	int formats = 0;
+
+	ret = sh_mobile_ceu_try_bus_param(icd);
+	if (ret < 0)
+		return 0;
+
+	switch (icd->formats[idx].fourcc) {
+	case V4L2_PIX_FMT_UYVY:
+	case V4L2_PIX_FMT_VYUY:
+	case V4L2_PIX_FMT_YUYV:
+	case V4L2_PIX_FMT_YVYU:
+		n = ARRAY_SIZE(sh_mobile_ceu_formats);
+		formats += n;
+		for (k = 0; xlate && k < n; k++) {
+			xlate->host_fmt = &sh_mobile_ceu_formats[k];
+			xlate->cam_fmt = icd->formats + idx;
+			xlate->buswidth = icd->formats[idx].depth;
+			xlate++;
+			dev_dbg(&ici->dev, "Providing format %s using %s\n",
+				sh_mobile_ceu_formats[k].name,
+				icd->formats[idx].name);
+		}
+	default:
+		/* Generic pass-through */
+		formats++;
+		if (xlate) {
+			xlate->host_fmt = icd->formats + idx;
+			xlate->cam_fmt = icd->formats + idx;
+			xlate->buswidth = icd->formats[idx].depth;
+			xlate++;
+			dev_dbg(&ici->dev,
+				"Providing format %s in pass-through mode\n",
+				icd->formats[idx].name);
+		}
+	}
+
+	return formats;
+}
+
 static int sh_mobile_ceu_set_fmt(struct soc_camera_device *icd,
 				 __u32 pixfmt, struct v4l2_rect *rect)
 {
-	const struct soc_camera_data_format *cam_fmt = NULL;
+	struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+	struct sh_mobile_ceu_dev *pcdev = ici->priv;
+	const struct soc_camera_format_xlate *xlate;
 	int ret;
 
-	/*
-	 * TODO: find a suitable supported by the SoC output format, check
-	 * whether the sensor supports one of acceptable input formats.
-	 */
-	if (pixfmt) {
-		cam_fmt = soc_camera_format_by_fourcc(icd, pixfmt);
-		if (!cam_fmt)
-			return -EINVAL;
+	xlate = soc_camera_xlate_by_fourcc(icd, pixfmt);
+	if (!xlate) {
+		dev_warn(&ici->dev, "Format %x not found\n", pixfmt);
+		return -EINVAL;
+	}
+
+	switch (pixfmt) {
+	case 0:				/* Only geometry change */
+		ret = icd->ops->set_fmt(icd, pixfmt, rect);
+		break;
+	default:
+		ret = icd->ops->set_fmt(icd, xlate->cam_fmt->fourcc, rect);
 	}
 
-	ret = icd->ops->set_fmt(icd, pixfmt, rect);
-	if (pixfmt && !ret)
-		icd->current_fmt = cam_fmt;
+	if (pixfmt && !ret) {
+		icd->buswidth = xlate->buswidth;
+		icd->current_fmt = xlate->host_fmt;
+		pcdev->camera_fmt = xlate->cam_fmt;
+	}
 
 	return ret;
 }
@@ -471,19 +630,17 @@ static int sh_mobile_ceu_set_fmt(struct soc_camera_device *icd,
 static int sh_mobile_ceu_try_fmt(struct soc_camera_device *icd,
 				 struct v4l2_format *f)
 {
-	const struct soc_camera_data_format *cam_fmt;
-	int ret = sh_mobile_ceu_try_bus_param(icd, f->fmt.pix.pixelformat);
-
-	if (ret < 0)
-		return ret;
+	struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+	struct sh_mobile_ceu_dev *pcdev = ici->priv;
+	const struct soc_camera_format_xlate *xlate;
+	__u32 pixfmt = f->fmt.pix.pixelformat;
+	int ret;
 
-	/*
-	 * TODO: find a suitable supported by the SoC output format, check
-	 * whether the sensor supports one of acceptable input formats.
-	 */
-	cam_fmt = soc_camera_format_by_fourcc(icd, f->fmt.pix.pixelformat);
-	if (!cam_fmt)
+	xlate = soc_camera_xlate_by_fourcc(icd, pixfmt);
+	if (!xlate) {
+		dev_warn(&ici->dev, "Format %x not found\n", pixfmt);
 		return -EINVAL;
+	}
 
 	/* FIXME: calculate using depth and bus width */
 
@@ -499,11 +656,30 @@ static int sh_mobile_ceu_try_fmt(struct soc_camera_device *icd,
 	f->fmt.pix.height &= ~0x03;
 
 	f->fmt.pix.bytesperline = f->fmt.pix.width *
-		DIV_ROUND_UP(cam_fmt->depth, 8);
+		DIV_ROUND_UP(xlate->host_fmt->depth, 8);
 	f->fmt.pix.sizeimage = f->fmt.pix.height * f->fmt.pix.bytesperline;
 
 	/* limit to sensor capabilities */
-	return icd->ops->try_fmt(icd, f);
+	ret = icd->ops->try_fmt(icd, f);
+	if (ret < 0)
+		return ret;
+
+	switch (f->fmt.pix.field) {
+	case V4L2_FIELD_INTERLACED:
+		pcdev->is_interlace = 1;
+		break;
+	case V4L2_FIELD_ANY:
+		f->fmt.pix.field = V4L2_FIELD_NONE;
+		/* fall-through */
+	case V4L2_FIELD_NONE:
+		pcdev->is_interlace = 0;
+		break;
+	default:
+		ret = -EINVAL;
+		break;
+	}
+
+	return ret;
 }
 
 static int sh_mobile_ceu_reqbufs(struct soc_camera_file *icf,
@@ -562,7 +738,7 @@ static void sh_mobile_ceu_init_videobuf(struct videobuf_queue *q,
 				       &sh_mobile_ceu_videobuf_ops,
 				       &ici->dev, &pcdev->lock,
 				       V4L2_BUF_TYPE_VIDEO_CAPTURE,
-				       V4L2_FIELD_NONE,
+				       V4L2_FIELD_ANY,
 				       sizeof(struct sh_mobile_ceu_buffer),
 				       icd);
 }
@@ -571,6 +747,7 @@ static struct soc_camera_host_ops sh_mobile_ceu_host_ops = {
 	.owner		= THIS_MODULE,
 	.add		= sh_mobile_ceu_add_device,
 	.remove		= sh_mobile_ceu_remove_device,
+	.get_formats	= sh_mobile_ceu_get_formats,
 	.set_fmt	= sh_mobile_ceu_set_fmt,
 	.try_fmt	= sh_mobile_ceu_try_fmt,
 	.reqbufs	= sh_mobile_ceu_reqbufs,
@@ -585,6 +762,7 @@ static int sh_mobile_ceu_probe(struct platform_device *pdev)
 	struct sh_mobile_ceu_dev *pcdev;
 	struct resource *res;
 	void __iomem *base;
+	char clk_name[8];
 	unsigned int irq;
 	int err = 0;
 
@@ -650,6 +828,14 @@ static int sh_mobile_ceu_probe(struct platform_device *pdev)
 		goto exit_release_mem;
 	}
 
+	snprintf(clk_name, sizeof(clk_name), "ceu%d", pdev->id);
+	pcdev->clk = clk_get(&pdev->dev, clk_name);
+	if (IS_ERR(pcdev->clk)) {
+		dev_err(&pdev->dev, "cannot get clock \"%s\"\n", clk_name);
+		err = PTR_ERR(pcdev->clk);
+		goto exit_free_irq;
+	}
+
 	pcdev->ici.priv = pcdev;
 	pcdev->ici.dev.parent = &pdev->dev;
 	pcdev->ici.nr = pdev->id;
@@ -658,10 +844,12 @@ static int sh_mobile_ceu_probe(struct platform_device *pdev)
 
 	err = soc_camera_host_register(&pcdev->ici);
 	if (err)
-		goto exit_free_irq;
+		goto exit_free_clk;
 
 	return 0;
 
+exit_free_clk:
+	clk_put(pcdev->clk);
 exit_free_irq:
 	free_irq(pcdev->irq, pcdev);
 exit_release_mem:
@@ -680,6 +868,7 @@ static int sh_mobile_ceu_remove(struct platform_device *pdev)
 	struct sh_mobile_ceu_dev *pcdev = platform_get_drvdata(pdev);
 
 	soc_camera_host_unregister(&pcdev->ici);
+	clk_put(pcdev->clk);
 	free_irq(pcdev->irq, pcdev);
 	if (platform_get_resource(pdev, IORESOURCE_MEM, 1))
 		dma_release_declared_memory(&pdev->dev);
diff --git a/linux/drivers/media/video/soc_camera.c b/linux/drivers/media/video/soc_camera.c
index 4a9c52c37..ba28c844f 100644
--- a/linux/drivers/media/video/soc_camera.c
+++ b/linux/drivers/media/video/soc_camera.c
@@ -34,7 +34,6 @@
 static LIST_HEAD(hosts);
 static LIST_HEAD(devices);
 static DEFINE_MUTEX(list_lock);
-static DEFINE_MUTEX(video_lock);
 
 const struct soc_camera_data_format *soc_camera_format_by_fourcc(
 	struct soc_camera_device *icd, unsigned int fourcc)
@@ -60,48 +59,73 @@ const struct soc_camera_format_xlate *soc_camera_xlate_by_fourcc(
 }
 EXPORT_SYMBOL(soc_camera_xlate_by_fourcc);
 
+/**
+ * soc_camera_apply_sensor_flags() - apply platform SOCAM_SENSOR_INVERT_* flags
+ * @icl:	camera platform parameters
+ * @flags:	flags to be inverted according to platform configuration
+ * @return:	resulting flags
+ */
+unsigned long soc_camera_apply_sensor_flags(struct soc_camera_link *icl,
+					    unsigned long flags)
+{
+	unsigned long f;
+
+	/* If only one of the two polarities is supported, switch to the opposite */
+	if (icl->flags & SOCAM_SENSOR_INVERT_HSYNC) {
+		f = flags & (SOCAM_HSYNC_ACTIVE_HIGH | SOCAM_HSYNC_ACTIVE_LOW);
+		if (f == SOCAM_HSYNC_ACTIVE_HIGH || f == SOCAM_HSYNC_ACTIVE_LOW)
+			flags ^= SOCAM_HSYNC_ACTIVE_HIGH | SOCAM_HSYNC_ACTIVE_LOW;
+	}
+
+	if (icl->flags & SOCAM_SENSOR_INVERT_VSYNC) {
+		f = flags & (SOCAM_VSYNC_ACTIVE_HIGH | SOCAM_VSYNC_ACTIVE_LOW);
+		if (f == SOCAM_VSYNC_ACTIVE_HIGH || f == SOCAM_VSYNC_ACTIVE_LOW)
+			flags ^= SOCAM_VSYNC_ACTIVE_HIGH | SOCAM_VSYNC_ACTIVE_LOW;
+	}
+
+	if (icl->flags & SOCAM_SENSOR_INVERT_PCLK) {
+		f = flags & (SOCAM_PCLK_SAMPLE_RISING | SOCAM_PCLK_SAMPLE_FALLING);
+		if (f == SOCAM_PCLK_SAMPLE_RISING || f == SOCAM_PCLK_SAMPLE_FALLING)
+			flags ^= SOCAM_PCLK_SAMPLE_RISING | SOCAM_PCLK_SAMPLE_FALLING;
+	}
+
+	return flags;
+}
+EXPORT_SYMBOL(soc_camera_apply_sensor_flags);
+
 static int soc_camera_try_fmt_vid_cap(struct file *file, void *priv,
 				      struct v4l2_format *f)
 {
 	struct soc_camera_file *icf = file->private_data;
 	struct soc_camera_device *icd = icf->icd;
-	struct soc_camera_host *ici =
-		to_soc_camera_host(icd->dev.parent);
-	enum v4l2_field field;
-	int ret;
+	struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 
 	WARN_ON(priv != file->private_data);
 
-	/*
-	 * TODO: this might also have to migrate to host-drivers, if anyone
-	 * wishes to support other fields
-	 */
-	field = f->fmt.pix.field;
-
-	if (field == V4L2_FIELD_ANY) {
-		f->fmt.pix.field = V4L2_FIELD_NONE;
-	} else if (field != V4L2_FIELD_NONE) {
-		dev_err(&icd->dev, "Field type invalid.\n");
-		return -EINVAL;
-	}
-
 	/* limit format to hardware capabilities */
-	ret = ici->ops->try_fmt(icd, f);
-
-	return ret;
+	return ici->ops->try_fmt(icd, f);
 }
 
 static int soc_camera_enum_input(struct file *file, void *priv,
 				 struct v4l2_input *inp)
 {
+	struct soc_camera_file *icf = file->private_data;
+	struct soc_camera_device *icd = icf->icd;
+	int ret = 0;
+
 	if (inp->index != 0)
 		return -EINVAL;
 
-	inp->type = V4L2_INPUT_TYPE_CAMERA;
-	inp->std = V4L2_STD_UNKNOWN;
-	strcpy(inp->name, "Camera");
+	if (icd->ops->enum_input)
+		ret = icd->ops->enum_input(icd, inp);
+	else {
+		/* default is camera */
+		inp->type = V4L2_INPUT_TYPE_CAMERA;
+		inp->std  = V4L2_STD_UNKNOWN;
+		strcpy(inp->name, "Camera");
+	}
 
-	return 0;
+	return ret;
 }
 
 static int soc_camera_g_input(struct file *file, void *priv, unsigned int *i)
@@ -121,7 +145,14 @@ static int soc_camera_s_input(struct file *file, void *priv, unsigned int i)
 
 static int soc_camera_s_std(struct file *file, void *priv, v4l2_std_id *a)
 {
-	return 0;
+	struct soc_camera_file *icf = file->private_data;
+	struct soc_camera_device *icd = icf->icd;
+	int ret = 0;
+
+	if (icd->ops->set_std)
+		ret = icd->ops->set_std(icd, a);
+
+	return ret;
 }
 
 static int soc_camera_reqbufs(struct file *file, void *priv,
@@ -130,8 +161,7 @@ static int soc_camera_reqbufs(struct file *file, void *priv,
 	int ret;
 	struct soc_camera_file *icf = file->private_data;
 	struct soc_camera_device *icd = icf->icd;
-	struct soc_camera_host *ici =
-		to_soc_camera_host(icd->dev.parent);
+	struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 
 	WARN_ON(priv != file->private_data);
 
@@ -239,8 +269,10 @@ static int soc_camera_open(struct inode *inode, struct file *file)
 	if (!icf)
 		return -ENOMEM;
 
-	/* Protect against icd->remove() until we module_get() both drivers. */
-	mutex_lock(&video_lock);
+	/*
+	 * It is safe to dereference these pointers now as long as a user has
+	 * the video device open - we are protected by the held cdev reference.
+	 */
 
 	vdev = video_devdata(file);
 	icd = container_of(vdev->parent, struct soc_camera_device, dev);
@@ -258,6 +290,9 @@ static int soc_camera_open(struct inode *inode, struct file *file)
 		goto emgi;
 	}
 
+	/* Protect against icd->remove() until we module_get() both drivers. */
+	mutex_lock(&icd->video_lock);
+
 	icf->icd = icd;
 	icd->use_count++;
 
@@ -273,7 +308,7 @@ static int soc_camera_open(struct inode *inode, struct file *file)
 		}
 	}
 
-	mutex_unlock(&video_lock);
+	mutex_unlock(&icd->video_lock);
 
 	file->private_data = icf;
 	dev_dbg(&icd->dev, "camera device open\n");
@@ -282,16 +317,16 @@ static int soc_camera_open(struct inode *inode, struct file *file)
 
 	return 0;
 
-	/* All errors are entered with the video_lock held */
+	/* First two errors are entered with the .video_lock held */
 eiciadd:
 	soc_camera_free_user_formats(icd);
 eiufmt:
 	icd->use_count--;
+	mutex_unlock(&icd->video_lock);
 	module_put(ici->ops->owner);
 emgi:
 	module_put(icd->ops->owner);
 emgd:
-	mutex_unlock(&video_lock);
 	vfree(icf);
 	return ret;
 }
@@ -303,15 +338,16 @@ static int soc_camera_close(struct inode *inode, struct file *file)
 	struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 	struct video_device *vdev = icd->vdev;
 
-	mutex_lock(&video_lock);
+	mutex_lock(&icd->video_lock);
 	icd->use_count--;
 	if (!icd->use_count) {
 		ici->ops->remove(icd);
 		soc_camera_free_user_formats(icd);
 	}
+	mutex_unlock(&icd->video_lock);
+
 	module_put(icd->ops->owner);
 	module_put(ici->ops->owner);
-	mutex_unlock(&video_lock);
 
 	vfree(icf);
 
@@ -355,8 +391,7 @@ static unsigned int soc_camera_poll(struct file *file, poll_table *pt)
 {
 	struct soc_camera_file *icf = file->private_data;
 	struct soc_camera_device *icd = icf->icd;
-	struct soc_camera_host *ici =
-		to_soc_camera_host(icd->dev.parent);
+	struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 
 	if (list_empty(&icf->vb_vidq.stream)) {
 		dev_err(&icd->dev, "Trying to poll with no queued buffers!\n");
@@ -382,9 +417,9 @@ static int soc_camera_s_fmt_vid_cap(struct file *file, void *priv,
 {
 	struct soc_camera_file *icf = file->private_data;
 	struct soc_camera_device *icd = icf->icd;
-	struct soc_camera_host *ici =
-		to_soc_camera_host(icd->dev.parent);
-	__u32 pixfmt = f->fmt.pix.pixelformat;
+	struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+	struct v4l2_pix_format *pix = &f->fmt.pix;
+	__u32 pixfmt = pix->pixelformat;
 	int ret;
 	struct v4l2_rect rect;
 
@@ -394,23 +429,32 @@ static int soc_camera_s_fmt_vid_cap(struct file *file, void *priv,
 	if (ret < 0)
 		return ret;
 
+	mutex_lock(&icf->vb_vidq.vb_lock);
+
+	if (videobuf_queue_is_busy(&icf->vb_vidq)) {
+		dev_err(&icd->dev, "S_FMT denied: queue busy\n");
+		ret = -EBUSY;
+		goto unlock;
+	}
+
 	rect.left	= icd->x_current;
 	rect.top	= icd->y_current;
-	rect.width	= f->fmt.pix.width;
-	rect.height	= f->fmt.pix.height;
-	ret = ici->ops->set_fmt(icd, f->fmt.pix.pixelformat, &rect);
+	rect.width	= pix->width;
+	rect.height	= pix->height;
+	ret = ici->ops->set_fmt(icd, pix->pixelformat, &rect);
 	if (ret < 0) {
-		return ret;
+		goto unlock;
 	} else if (!icd->current_fmt ||
 		   icd->current_fmt->fourcc != pixfmt) {
 		dev_err(&ici->dev,
 			"Host driver hasn't set up current format correctly!\n");
-		return -EINVAL;
+		ret = -EINVAL;
+		goto unlock;
 	}
 
 	icd->width		= rect.width;
 	icd->height		= rect.height;
-	icf->vb_vidq.field	= f->fmt.pix.field;
+	icf->vb_vidq.field	= pix->field;
 	if (f->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
 		dev_warn(&icd->dev, "Attention! Wrong buf-type %d\n",
 			 f->type);
@@ -419,7 +463,12 @@ static int soc_camera_s_fmt_vid_cap(struct file *file, void *priv,
 		icd->width, icd->height);
 
 	/* set physical bus parameters */
-	return ici->ops->set_bus_param(icd, pixfmt);
+	ret = ici->ops->set_bus_param(icd, pixfmt);
+
+unlock:
+	mutex_unlock(&icf->vb_vidq.vb_lock);
+
+	return ret;
 }
 
 static int soc_camera_enum_fmt_vid_cap(struct file *file, void  *priv,
@@ -446,16 +495,17 @@ static int soc_camera_g_fmt_vid_cap(struct file *file, void *priv,
 {
 	struct soc_camera_file *icf = file->private_data;
 	struct soc_camera_device *icd = icf->icd;
+	struct v4l2_pix_format *pix = &f->fmt.pix;
 
 	WARN_ON(priv != file->private_data);
 
-	f->fmt.pix.width	= icd->width;
-	f->fmt.pix.height	= icd->height;
-	f->fmt.pix.field	= icf->vb_vidq.field;
-	f->fmt.pix.pixelformat	= icd->current_fmt->fourcc;
-	f->fmt.pix.bytesperline	= f->fmt.pix.width *
+	pix->width		= icd->width;
+	pix->height		= icd->height;
+	pix->field		= icf->vb_vidq.field;
+	pix->pixelformat	= icd->current_fmt->fourcc;
+	pix->bytesperline	= pix->width *
 		DIV_ROUND_UP(icd->current_fmt->depth, 8);
-	f->fmt.pix.sizeimage	= f->fmt.pix.height * f->fmt.pix.bytesperline;
+	pix->sizeimage		= pix->height * pix->bytesperline;
 	dev_dbg(&icd->dev, "current_fmt->fourcc: 0x%08x\n",
 		icd->current_fmt->fourcc);
 	return 0;
@@ -466,8 +516,7 @@ static int soc_camera_querycap(struct file *file, void  *priv,
 {
 	struct soc_camera_file *icf = file->private_data;
 	struct soc_camera_device *icd = icf->icd;
-	struct soc_camera_host *ici =
-		to_soc_camera_host(icd->dev.parent);
+	struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 
 	WARN_ON(priv != file->private_data);
 
@@ -480,6 +529,7 @@ static int soc_camera_streamon(struct file *file, void *priv,
 {
 	struct soc_camera_file *icf = file->private_data;
 	struct soc_camera_device *icd = icf->icd;
+	int ret;
 
 	WARN_ON(priv != file->private_data);
 
@@ -488,10 +538,16 @@ static int soc_camera_streamon(struct file *file, void *priv,
 	if (i != V4L2_BUF_TYPE_VIDEO_CAPTURE)
 		return -EINVAL;
 
+	mutex_lock(&icd->video_lock);
+
 	icd->ops->start_capture(icd);
 
 	/* This calls buf_queue from host driver's videobuf_queue_ops */
-	return videobuf_streamon(&icf->vb_vidq);
+	ret = videobuf_streamon(&icf->vb_vidq);
+
+	mutex_unlock(&icd->video_lock);
+
+	return ret;
 }
 
 static int soc_camera_streamoff(struct file *file, void *priv,
@@ -507,12 +563,16 @@ static int soc_camera_streamoff(struct file *file, void *priv,
 	if (i != V4L2_BUF_TYPE_VIDEO_CAPTURE)
 		return -EINVAL;
 
+	mutex_lock(&icd->video_lock);
+
 	/* This calls buf_release from host driver's videobuf_queue_ops for all
 	 * remaining buffers. When the last buffer is freed, stop capture */
 	videobuf_streamoff(&icf->vb_vidq);
 
 	icd->ops->stop_capture(icd);
 
+	mutex_unlock(&icd->video_lock);
+
 	return 0;
 }
 
@@ -618,13 +678,15 @@ static int soc_camera_s_crop(struct file *file, void *fh,
 {
 	struct soc_camera_file *icf = file->private_data;
 	struct soc_camera_device *icd = icf->icd;
-	struct soc_camera_host *ici =
-		to_soc_camera_host(icd->dev.parent);
+	struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 	int ret;
 
 	if (a->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
 		return -EINVAL;
 
+	/* Cropping is allowed during a running capture, guard consistency */
+	mutex_lock(&icf->vb_vidq.vb_lock);
+
 	ret = ici->ops->set_fmt(icd, 0, &a->c);
 	if (!ret) {
 		icd->width	= a->c.width;
@@ -633,6 +695,8 @@ static int soc_camera_s_crop(struct file *file, void *fh,
 		icd->y_current	= a->c.top;
 	}
 
+	mutex_unlock(&icf->vb_vidq.vb_lock);
+
 	return ret;
 }
 
@@ -743,18 +807,35 @@ static int scan_add_device(struct soc_camera_device *icd)
 static int soc_camera_probe(struct device *dev)
 {
 	struct soc_camera_device *icd = to_soc_camera_dev(dev);
-	struct soc_camera_host *ici =
-		to_soc_camera_host(icd->dev.parent);
+	struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 	int ret;
 
-	if (!icd->ops->probe)
-		return -ENODEV;
+	/*
+	 * Possible race scenario:
+	 * modprobe <camera-host-driver> triggers __func__
+	 * at this moment respective <camera-sensor-driver> gets rmmod'ed
+	 * to protect take module references.
+	 */
+
+	if (!try_module_get(icd->ops->owner)) {
+		dev_err(&icd->dev, "Couldn't lock sensor driver.\n");
+		ret = -EINVAL;
+		goto emgd;
+	}
+
+	if (!try_module_get(ici->ops->owner)) {
+		dev_err(&icd->dev, "Couldn't lock capture bus driver.\n");
+		ret = -EINVAL;
+		goto emgi;
+	}
+
+	mutex_lock(&icd->video_lock);
 
 	/* We only call ->add() here to activate and probe the camera.
 	 * We shall ->remove() and deactivate it immediately afterwards. */
 	ret = ici->ops->add(icd);
 	if (ret < 0)
-		return ret;
+		goto eiadd;
 
 	ret = icd->ops->probe(icd);
 	if (ret >= 0) {
@@ -768,6 +849,12 @@ static int soc_camera_probe(struct device *dev)
 	}
 	ici->ops->remove(icd);
 
+eiadd:
+	mutex_unlock(&icd->video_lock);
+	module_put(ici->ops->owner);
+emgi:
+	module_put(icd->ops->owner);
+emgd:
 	return ret;
 }
 
@@ -830,7 +917,16 @@ int soc_camera_host_register(struct soc_camera_host *ici)
 	int ret;
 	struct soc_camera_host *ix;
 
-	if (!ici->ops->init_videobuf || !ici->ops->add || !ici->ops->remove)
+	if (!ici || !ici->ops ||
+	    !ici->ops->try_fmt ||
+	    !ici->ops->set_fmt ||
+	    !ici->ops->set_bus_param ||
+	    !ici->ops->querycap ||
+	    !ici->ops->init_videobuf ||
+	    !ici->ops->reqbufs ||
+	    !ici->ops->add ||
+	    !ici->ops->remove ||
+	    !ici->ops->poll)
 		return -EINVAL;
 
 	/* Number might be equal to the platform device ID */
@@ -898,7 +994,16 @@ int soc_camera_device_register(struct soc_camera_device *icd)
 	struct soc_camera_device *ix;
 	int num = -1, i;
 
-	if (!icd)
+	if (!icd || !icd->ops ||
+	    !icd->ops->probe ||
+	    !icd->ops->init ||
+	    !icd->ops->release ||
+	    !icd->ops->start_capture ||
+	    !icd->ops->stop_capture ||
+	    !icd->ops->set_fmt ||
+	    !icd->ops->try_fmt ||
+	    !icd->ops->query_bus_param ||
+	    !icd->ops->set_bus_param)
 		return -EINVAL;
 
 	for (i = 0; i < 256 && num < 0; i++) {
@@ -920,7 +1025,10 @@ int soc_camera_device_register(struct soc_camera_device *icd)
 	icd->dev.bus = &soc_camera_bus_type;
 	dev_set_name(&icd->dev, "%u-%u", icd->iface, icd->devnum);
 
-	icd->dev.release = dummy_release;
+	icd->dev.release	= dummy_release;
+	icd->use_count		= 0;
+	icd->host_priv		= NULL;
+	mutex_init(&icd->video_lock);
 
 	return scan_add_device(icd);
 }
@@ -967,6 +1075,10 @@ static const struct v4l2_ioctl_ops soc_camera_ioctl_ops = {
 #endif
 };
 
+/*
+ * Usually called from the struct soc_camera_ops .probe() method, i.e., from
+ * soc_camera_probe() above with .video_lock held
+ */
 int soc_camera_video_start(struct soc_camera_device *icd)
 {
 	struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
@@ -982,7 +1094,7 @@ int soc_camera_video_start(struct soc_camera_device *icd)
 	dev_dbg(&ici->dev, "Allocated video_device %p\n", vdev);
 
 	strlcpy(vdev->name, ici->drv_name, sizeof(vdev->name));
-	/* Maybe better &ici->dev */
+
 	vdev->parent		= &icd->dev;
 	vdev->current_norm	= V4L2_STD_UNKNOWN;
 	vdev->fops		= &soc_camera_fops;
@@ -1016,10 +1128,10 @@ void soc_camera_video_stop(struct soc_camera_device *icd)
 	if (!icd->dev.parent || !vdev)
 		return;
 
-	mutex_lock(&video_lock);
+	mutex_lock(&icd->video_lock);
 	video_unregister_device(vdev);
 	icd->vdev = NULL;
-	mutex_unlock(&video_lock);
+	mutex_unlock(&icd->video_lock);
 }
 EXPORT_SYMBOL(soc_camera_video_stop);
 
diff --git a/linux/drivers/media/video/soc_camera_platform.c b/linux/drivers/media/video/soc_camera_platform.c
index c23871e4c..013ab06e3 100644
--- a/linux/drivers/media/video/soc_camera_platform.c
+++ b/linux/drivers/media/video/soc_camera_platform.c
@@ -89,9 +89,10 @@ static int soc_camera_platform_try_fmt(struct soc_camera_device *icd,
 				       struct v4l2_format *f)
 {
 	struct soc_camera_platform_info *p = soc_camera_platform_get_info(icd);
+	struct v4l2_pix_format *pix = &f->fmt.pix;
 
-	f->fmt.pix.width = p->format.width;
-	f->fmt.pix.height = p->format.height;
+	pix->width = p->format.width;
+	pix->height = p->format.height;
 	return 0;
 }
 
diff --git a/linux/drivers/media/video/stk-webcam.c b/linux/drivers/media/video/stk-webcam.c
index 9615b7678..bfd0ddb63 100644
--- a/linux/drivers/media/video/stk-webcam.c
+++ b/linux/drivers/media/video/stk-webcam.c
@@ -1263,6 +1263,25 @@ static int stk_vidioc_g_parm(struct file *filp,
 	return 0;
 }
 
+static int stk_vidioc_enum_framesizes(struct file *filp,
+		void *priv, struct v4l2_frmsizeenum *frms)
+{
+	if (frms->index >= ARRAY_SIZE(stk_sizes))
+		return -EINVAL;
+	switch (frms->pixel_format) {
+	case V4L2_PIX_FMT_RGB565:
+	case V4L2_PIX_FMT_RGB565X:
+	case V4L2_PIX_FMT_UYVY:
+	case V4L2_PIX_FMT_YUYV:
+	case V4L2_PIX_FMT_SBGGR8:
+		frms->type = V4L2_FRMSIZE_TYPE_DISCRETE;
+		frms->discrete.width = stk_sizes[frms->index].w;
+		frms->discrete.height = stk_sizes[frms->index].h;
+		return 0;
+	default: return -EINVAL;
+	}
+}
+
 static struct file_operations v4l_stk_fops = {
 	.owner = THIS_MODULE,
 	.open = v4l_stk_open,
@@ -1297,6 +1316,7 @@ static const struct v4l2_ioctl_ops v4l_stk_ioctl_ops = {
 	.vidioc_g_ctrl = stk_vidioc_g_ctrl,
 	.vidioc_s_ctrl = stk_vidioc_s_ctrl,
 	.vidioc_g_parm = stk_vidioc_g_parm,
+	.vidioc_enum_framesizes = stk_vidioc_enum_framesizes,
 };
 
 static void stk_v4l_dev_release(struct video_device *vd)
diff --git a/linux/drivers/media/video/tw9910.c b/linux/drivers/media/video/tw9910.c
new file mode 100644
index 000000000..d5cdc4be1
--- /dev/null
+++ b/linux/drivers/media/video/tw9910.c
@@ -0,0 +1,951 @@
+/*
+ * tw9910 Video Driver
+ *
+ * Copyright (C) 2008 Renesas Solutions Corp.
+ * Kuninori Morimoto <morimoto.kuninori@renesas.com>
+ *
+ * Based on ov772x driver,
+ *
+ * Copyright (C) 2008 Kuninori Morimoto <morimoto.kuninori@renesas.com>
+ * Copyright 2006-7 Jonathan Corbet <corbet@lwn.net>
+ * Copyright (C) 2008 Magnus Damm
+ * Copyright (C) 2008, Guennadi Liakhovetski <kernel@pengutronix.de>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <linux/kernel.h>
+#include <linux/delay.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-chip-ident.h>
+#include <media/v4l2-common.h>
+#include <media/soc_camera.h>
+#include <media/tw9910.h>
+
+#define GET_ID(val)  ((val & 0xF8) >> 3)
+#define GET_ReV(val) (val & 0x07)
+
+/*
+ * register offset
+ */
+#define ID		0x00 /* Product ID Code Register */
+#define STATUS1		0x01 /* Chip Status Register I */
+#define INFORM		0x02 /* Input Format */
+#define OPFORM		0x03 /* Output Format Control Register */
+#define DLYCTR		0x04 /* Hysteresis and HSYNC Delay Control */
+#define OUTCTR1		0x05 /* Output Control I */
+#define ACNTL1		0x06 /* Analog Control Register 1 */
+#define CROP_HI		0x07 /* Cropping Register, High */
+#define VDELAY_LO	0x08 /* Vertical Delay Register, Low */
+#define VACTIVE_LO	0x09 /* Vertical Active Register, Low */
+#define HDELAY_LO	0x0A /* Horizontal Delay Register, Low */
+#define HACTIVE_LO	0x0B /* Horizontal Active Register, Low */
+#define CNTRL1		0x0C /* Control Register I */
+#define VSCALE_LO	0x0D /* Vertical Scaling Register, Low */
+#define SCALE_HI	0x0E /* Scaling Register, High */
+#define HSCALE_LO	0x0F /* Horizontal Scaling Register, Low */
+#define BRIGHT		0x10 /* BRIGHTNESS Control Register */
+#define CONTRAST	0x11 /* CONTRAST Control Register */
+#define SHARPNESS	0x12 /* SHARPNESS Control Register I */
+#define SAT_U		0x13 /* Chroma (U) Gain Register */
+#define SAT_V		0x14 /* Chroma (V) Gain Register */
+#define HUE		0x15 /* Hue Control Register */
+#define CORING1		0x17
+#define CORING2		0x18 /* Coring and IF compensation */
+#define VBICNTL		0x19 /* VBI Control Register */
+#define ACNTL2		0x1A /* Analog Control 2 */
+#define OUTCTR2		0x1B /* Output Control 2 */
+#define SDT		0x1C /* Standard Selection */
+#define SDTR		0x1D /* Standard Recognition */
+#define TEST		0x1F /* Test Control Register */
+#define CLMPG		0x20 /* Clamping Gain */
+#define IAGC		0x21 /* Individual AGC Gain */
+#define AGCGAIN		0x22 /* AGC Gain */
+#define PEAKWT		0x23 /* White Peak Threshold */
+#define CLMPL		0x24 /* Clamp level */
+#define SYNCT		0x25 /* Sync Amplitude */
+#define MISSCNT		0x26 /* Sync Miss Count Register */
+#define PCLAMP		0x27 /* Clamp Position Register */
+#define VCNTL1		0x28 /* Vertical Control I */
+#define VCNTL2		0x29 /* Vertical Control II */
+#define CKILL		0x2A /* Color Killer Level Control */
+#define COMB		0x2B /* Comb Filter Control */
+#define LDLY		0x2C /* Luma Delay and H Filter Control */
+#define MISC1		0x2D /* Miscellaneous Control I */
+#define LOOP		0x2E /* LOOP Control Register */
+#define MISC2		0x2F /* Miscellaneous Control II */
+#define MVSN		0x30 /* Macrovision Detection */
+#define STATUS2		0x31 /* Chip STATUS II */
+#define HFREF		0x32 /* H monitor */
+#define CLMD		0x33 /* CLAMP MODE */
+#define IDCNTL		0x34 /* ID Detection Control */
+#define CLCNTL1		0x35 /* Clamp Control I */
+#define ANAPLLCTL	0x4C
+#define VBIMIN		0x4D
+#define HSLOWCTL	0x4E
+#define WSS3		0x4F
+#define FILLDATA	0x50
+#define SDID		0x51
+#define DID		0x52
+#define WSS1		0x53
+#define WSS2		0x54
+#define VVBI		0x55
+#define LCTL6		0x56
+#define LCTL7		0x57
+#define LCTL8		0x58
+#define LCTL9		0x59
+#define LCTL10		0x5A
+#define LCTL11		0x5B
+#define LCTL12		0x5C
+#define LCTL13		0x5D
+#define LCTL14		0x5E
+#define LCTL15		0x5F
+#define LCTL16		0x60
+#define LCTL17		0x61
+#define LCTL18		0x62
+#define LCTL19		0x63
+#define LCTL20		0x64
+#define LCTL21		0x65
+#define LCTL22		0x66
+#define LCTL23		0x67
+#define LCTL24		0x68
+#define LCTL25		0x69
+#define LCTL26		0x6A
+#define HSGEGIN		0x6B
+#define HSEND		0x6C
+#define OVSDLY		0x6D
+#define OVSEND		0x6E
+#define VBIDELAY	0x6F
+
+/*
+ * register detail
+ */
+
+/* INFORM */
+#define FC27_ON     0x40 /* 1 : Input crystal clock frequency is 27MHz */
+#define FC27_FF     0x00 /* 0 : Square pixel mode. */
+			 /*     Must use 24.54MHz for 60Hz field rate */
+			 /*     source or 29.5MHz for 50Hz field rate */
+#define IFSEL_S     0x10 /* 01 : S-video decoding */
+#define IFSEL_C     0x00 /* 00 : Composite video decoding */
+			 /* Y input video selection */
+#define YSEL_M0     0x00 /*  00 : Mux0 selected */
+#define YSEL_M1     0x04 /*  01 : Mux1 selected */
+#define YSEL_M2     0x08 /*  10 : Mux2 selected */
+#define YSEL_M3     0x10 /*  11 : Mux3 selected */
+
+/* OPFORM */
+#define MODE        0x80 /* 0 : CCIR601 compatible YCrCb 4:2:2 format */
+			 /* 1 : ITU-R-656 compatible data sequence format */
+#define LEN         0x40 /* 0 : 8-bit YCrCb 4:2:2 output format */
+			 /* 1 : 16-bit YCrCb 4:2:2 output format.*/
+#define LLCMODE     0x20 /* 1 : LLC output mode. */
+			 /* 0 : free-run output mode */
+#define AINC        0x10 /* Serial interface auto-indexing control */
+			 /* 0 : auto-increment */
+			 /* 1 : non-auto */
+#define VSCTL       0x08 /* 1 : Vertical out ctrl by DVALID */
+			 /* 0 : Vertical out ctrl by HACTIVE and DVALID */
+#define OEN         0x04 /* Output Enable together with TRI_SEL. */
+
+/* OUTCTR1 */
+#define VSP_LO      0x00 /* 0 : VS pin output polarity is active low */
+#define VSP_HI      0x80 /* 1 : VS pin output polarity is active high. */
+			 /* VS pin output control */
+#define VSSL_VSYNC  0x00 /*   0 : VSYNC  */
+#define VSSL_VACT   0x10 /*   1 : VACT   */
+#define VSSL_FIELD  0x20 /*   2 : FIELD  */
+#define VSSL_VVALID 0x30 /*   3 : VVALID */
+#define VSSL_ZERO   0x70 /*   7 : 0      */
+#define HSP_LOW     0x00 /* 0 : HS pin output polarity is active low */
+#define HSP_HI      0x08 /* 1 : HS pin output polarity is active high.*/
+			 /* HS pin output control */
+#define HSSL_HACT   0x00 /*   0 : HACT   */
+#define HSSL_HSYNC  0x01 /*   1 : HSYNC  */
+#define HSSL_DVALID 0x02 /*   2 : DVALID */
+#define HSSL_HLOCK  0x03 /*   3 : HLOCK  */
+#define HSSL_ASYNCW 0x04 /*   4 : ASYNCW */
+#define HSSL_ZERO   0x07 /*   7 : 0      */
+
+/* ACNTL1 */
+#define SRESET      0x80 /* resets the device to its default state
+			  * but all register content remain unchanged.
+			  * This bit is self-resetting.
+			  */
+
+/* VBICNTL */
+/* RTSEL : control the real time signal
+*          output from the MPOUT pin
+*/
+#define RTSEL_MASK  0x07
+#define RTSEL_VLOSS 0x00 /* 0000 = Video loss */
+#define RTSEL_HLOCK 0x01 /* 0001 = H-lock */
+#define RTSEL_SLOCK 0x02 /* 0010 = S-lock */
+#define RTSEL_VLOCK 0x03 /* 0011 = V-lock */
+#define RTSEL_MONO  0x04 /* 0100 = MONO */
+#define RTSEL_DET50 0x05 /* 0101 = DET50 */
+#define RTSEL_FIELD 0x06 /* 0110 = FIELD */
+#define RTSEL_RTCO  0x07 /* 0111 = RTCO ( Real Time Control ) */
+
+/*
+ * structure
+ */
+
+struct regval_list {
+	unsigned char reg_num;
+	unsigned char value;
+};
+
+struct tw9910_scale_ctrl {
+	char           *name;
+	unsigned short  width;
+	unsigned short  height;
+	u16             hscale;
+	u16             vscale;
+};
+
+struct tw9910_cropping_ctrl {
+	u16 vdelay;
+	u16 vactive;
+	u16 hdelay;
+	u16 hactive;
+};
+
+struct tw9910_hsync_ctrl {
+	u16 start;
+	u16 end;
+};
+
+struct tw9910_priv {
+	struct tw9910_video_info       *info;
+	struct i2c_client              *client;
+	struct soc_camera_device        icd;
+	const struct tw9910_scale_ctrl *scale;
+};
+
+/*
+ * register settings
+ */
+
+#define ENDMARKER { 0xff, 0xff }
+
+static const struct regval_list tw9910_default_regs[] =
+{
+	{ OPFORM,  0x00 },
+	{ OUTCTR1, VSP_LO | VSSL_VVALID | HSP_HI | HSSL_HSYNC },
+	ENDMARKER,
+};
+
+static const struct soc_camera_data_format tw9910_color_fmt[] = {
+	{
+		.name       = "VYUY",
+		.fourcc     = V4L2_PIX_FMT_VYUY,
+		.depth      = 16,
+		.colorspace = V4L2_COLORSPACE_SMPTE170M,
+	}
+};
+
+static const struct tw9910_scale_ctrl tw9910_ntsc_scales[] = {
+	{
+		.name   = "NTSC SQ",
+		.width  = 640,
+		.height = 480,
+		.hscale = 0x0100,
+		.vscale = 0x0100,
+	},
+	{
+		.name   = "NTSC CCIR601",
+		.width  = 720,
+		.height = 480,
+		.hscale = 0x0100,
+		.vscale = 0x0100,
+	},
+	{
+		.name   = "NTSC SQ (CIF)",
+		.width  = 320,
+		.height = 240,
+		.hscale = 0x0200,
+		.vscale = 0x0200,
+	},
+	{
+		.name   = "NTSC CCIR601 (CIF)",
+		.width  = 360,
+		.height = 240,
+		.hscale = 0x0200,
+		.vscale = 0x0200,
+	},
+	{
+		.name   = "NTSC SQ (QCIF)",
+		.width  = 160,
+		.height = 120,
+		.hscale = 0x0400,
+		.vscale = 0x0400,
+	},
+	{
+		.name   = "NTSC CCIR601 (QCIF)",
+		.width  = 180,
+		.height = 120,
+		.hscale = 0x0400,
+		.vscale = 0x0400,
+	},
+};
+
+static const struct tw9910_scale_ctrl tw9910_pal_scales[] = {
+	{
+		.name   = "PAL SQ",
+		.width  = 768,
+		.height = 576,
+		.hscale = 0x0100,
+		.vscale = 0x0100,
+	},
+	{
+		.name   = "PAL CCIR601",
+		.width  = 720,
+		.height = 576,
+		.hscale = 0x0100,
+		.vscale = 0x0100,
+	},
+	{
+		.name   = "PAL SQ (CIF)",
+		.width  = 384,
+		.height = 288,
+		.hscale = 0x0200,
+		.vscale = 0x0200,
+	},
+	{
+		.name   = "PAL CCIR601 (CIF)",
+		.width  = 360,
+		.height = 288,
+		.hscale = 0x0200,
+		.vscale = 0x0200,
+	},
+	{
+		.name   = "PAL SQ (QCIF)",
+		.width  = 192,
+		.height = 144,
+		.hscale = 0x0400,
+		.vscale = 0x0400,
+	},
+	{
+		.name   = "PAL CCIR601 (QCIF)",
+		.width  = 180,
+		.height = 144,
+		.hscale = 0x0400,
+		.vscale = 0x0400,
+	},
+};
+
+static const struct tw9910_cropping_ctrl tw9910_cropping_ctrl = {
+	.vdelay  = 0x0012,
+	.vactive = 0x00F0,
+	.hdelay  = 0x0010,
+	.hactive = 0x02D0,
+};
+
+static const struct tw9910_hsync_ctrl tw9910_hsync_ctrl = {
+	.start = 0x0260,
+	.end   = 0x0300,
+};
+
+/*
+ * general function
+ */
+static int tw9910_set_scale(struct i2c_client *client,
+			    const struct tw9910_scale_ctrl *scale)
+{
+	int ret;
+
+	ret = i2c_smbus_write_byte_data(client, SCALE_HI,
+					(scale->vscale & 0x0F00) >> 4 |
+					(scale->hscale & 0x0F00) >> 8);
+	if (ret < 0)
+		return ret;
+
+	ret = i2c_smbus_write_byte_data(client, HSCALE_LO,
+					scale->hscale & 0x00FF);
+	if (ret < 0)
+		return ret;
+
+	ret = i2c_smbus_write_byte_data(client, VSCALE_LO,
+					scale->vscale & 0x00FF);
+
+	return ret;
+}
+
+static int tw9910_set_cropping(struct i2c_client *client,
+			       const struct tw9910_cropping_ctrl *cropping)
+{
+	int ret;
+
+	ret = i2c_smbus_write_byte_data(client, CROP_HI,
+					(cropping->vdelay  & 0x0300) >> 2 |
+					(cropping->vactive & 0x0300) >> 4 |
+					(cropping->hdelay  & 0x0300) >> 6 |
+					(cropping->hactive & 0x0300) >> 8);
+	if (ret < 0)
+		return ret;
+
+	ret = i2c_smbus_write_byte_data(client, VDELAY_LO,
+					cropping->vdelay & 0x00FF);
+	if (ret < 0)
+		return ret;
+
+	ret = i2c_smbus_write_byte_data(client, VACTIVE_LO,
+					cropping->vactive & 0x00FF);
+	if (ret < 0)
+		return ret;
+
+	ret = i2c_smbus_write_byte_data(client, HDELAY_LO,
+					cropping->hdelay & 0x00FF);
+	if (ret < 0)
+		return ret;
+
+	ret = i2c_smbus_write_byte_data(client, HACTIVE_LO,
+					cropping->hactive & 0x00FF);
+
+	return ret;
+}
+
+static int tw9910_set_hsync(struct i2c_client *client,
+			    const struct tw9910_hsync_ctrl *hsync)
+{
+	int ret;
+
+	/* bit 10 - 3 */
+	ret = i2c_smbus_write_byte_data(client, HSGEGIN,
+					(hsync->start & 0x07F8) >> 3);
+	if (ret < 0)
+		return ret;
+
+	/* bit 10 - 3 */
+	ret = i2c_smbus_write_byte_data(client, HSEND,
+					(hsync->end & 0x07F8) >> 3);
+	if (ret < 0)
+		return ret;
+
+	/* bit 2 - 0 */
+	ret = i2c_smbus_read_byte_data(client, HSLOWCTL);
+	if (ret < 0)
+		return ret;
+
+	ret = i2c_smbus_write_byte_data(client, HSLOWCTL,
+					(ret & 0x88)                 |
+					(hsync->start & 0x0007) << 4 |
+					(hsync->end   & 0x0007));
+
+	return ret;
+}
+
+static int tw9910_write_array(struct i2c_client *client,
+			      const struct regval_list *vals)
+{
+	while (vals->reg_num != 0xff) {
+		int ret = i2c_smbus_write_byte_data(client,
+						    vals->reg_num,
+						    vals->value);
+		if (ret < 0)
+			return ret;
+		vals++;
+	}
+	return 0;
+}
+
+static int tw9910_mask_set(struct i2c_client *client, u8 command,
+			   u8 mask, u8 set)
+{
+	s32 val = i2c_smbus_read_byte_data(client, command);
+
+	val &= ~mask;
+	val |=  set;
+
+	return i2c_smbus_write_byte_data(client, command, val);
+}
+
+static void tw9910_reset(struct i2c_client *client)
+{
+	i2c_smbus_write_byte_data(client, ACNTL1, SRESET);
+	msleep(1);
+}
+
+static const struct tw9910_scale_ctrl*
+tw9910_select_norm(struct soc_camera_device *icd, u32 width, u32 height)
+{
+	const struct tw9910_scale_ctrl *scale;
+	const struct tw9910_scale_ctrl *ret = NULL;
+	v4l2_std_id norm = icd->vdev->current_norm;
+	__u32 diff = 0xffffffff, tmp;
+	int size, i;
+
+	if (norm & V4L2_STD_NTSC) {
+		scale = tw9910_ntsc_scales;
+		size = ARRAY_SIZE(tw9910_ntsc_scales);
+	} else if (norm & V4L2_STD_PAL) {
+		scale = tw9910_pal_scales;
+		size = ARRAY_SIZE(tw9910_pal_scales);
+	} else {
+		return NULL;
+	}
+
+	for (i = 0; i < size; i++) {
+		tmp = abs(width - scale[i].width) +
+			abs(height - scale[i].height);
+		if (tmp < diff) {
+			diff = tmp;
+			ret = scale + i;
+		}
+	}
+
+	return ret;
+}
+
+/*
+ * soc_camera_ops function
+ */
+static int tw9910_init(struct soc_camera_device *icd)
+{
+	struct tw9910_priv *priv = container_of(icd, struct tw9910_priv, icd);
+	int ret = 0;
+
+	if (priv->info->link.power) {
+		ret = priv->info->link.power(&priv->client->dev, 1);
+		if (ret < 0)
+			return ret;
+	}
+
+	if (priv->info->link.reset)
+		ret = priv->info->link.reset(&priv->client->dev);
+
+	return ret;
+}
+
+static int tw9910_release(struct soc_camera_device *icd)
+{
+	struct tw9910_priv *priv = container_of(icd, struct tw9910_priv, icd);
+	int ret = 0;
+
+	if (priv->info->link.power)
+		ret = priv->info->link.power(&priv->client->dev, 0);
+
+	return ret;
+}
+
+static int tw9910_start_capture(struct soc_camera_device *icd)
+{
+	struct tw9910_priv *priv = container_of(icd, struct tw9910_priv, icd);
+
+	if (!priv->scale) {
+		dev_err(&icd->dev, "norm select error\n");
+		return -EPERM;
+	}
+
+	dev_dbg(&icd->dev, "%s %dx%d\n",
+		 priv->scale->name,
+		 priv->scale->width,
+		 priv->scale->height);
+
+	return 0;
+}
+
+static int tw9910_stop_capture(struct soc_camera_device *icd)
+{
+	return 0;
+}
+
+static int tw9910_set_bus_param(struct soc_camera_device *icd,
+				unsigned long flags)
+{
+	return 0;
+}
+
+static unsigned long tw9910_query_bus_param(struct soc_camera_device *icd)
+{
+	struct tw9910_priv *priv = container_of(icd, struct tw9910_priv, icd);
+	struct soc_camera_link *icl = priv->client->dev.platform_data;
+	unsigned long flags = SOCAM_PCLK_SAMPLE_RISING | SOCAM_MASTER |
+		SOCAM_VSYNC_ACTIVE_HIGH | SOCAM_HSYNC_ACTIVE_HIGH |
+		SOCAM_DATA_ACTIVE_HIGH | priv->info->buswidth;
+
+	return soc_camera_apply_sensor_flags(icl, flags);
+}
+
+static int tw9910_get_chip_id(struct soc_camera_device *icd,
+			      struct v4l2_chip_ident *id)
+{
+	id->ident = V4L2_IDENT_TW9910;
+	id->revision = 0;
+
+	return 0;
+}
+
+static int tw9910_set_std(struct soc_camera_device *icd,
+			  v4l2_std_id *a)
+{
+	int ret = -EINVAL;
+
+	if (*a & (V4L2_STD_NTSC | V4L2_STD_PAL))
+		ret = 0;
+
+	return ret;
+}
+
+static int tw9910_enum_input(struct soc_camera_device *icd,
+			     struct v4l2_input *inp)
+{
+	inp->type = V4L2_INPUT_TYPE_TUNER;
+	inp->std  = V4L2_STD_UNKNOWN;
+	strcpy(inp->name, "Video");
+
+	return 0;
+}
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int tw9910_get_register(struct soc_camera_device *icd,
+			       struct v4l2_register *reg)
+{
+	struct tw9910_priv *priv = container_of(icd, struct tw9910_priv, icd);
+	int ret;
+
+	if (reg->reg > 0xff)
+		return -EINVAL;
+
+	ret = i2c_smbus_read_byte_data(priv->client, reg->reg);
+	if (ret < 0)
+		return ret;
+
+	/* ret      = int
+	 * reg->val = __u64
+	 */
+	reg->val = (__u64)ret;
+
+	return 0;
+}
+
+static int tw9910_set_register(struct soc_camera_device *icd,
+			       struct v4l2_register *reg)
+{
+	struct tw9910_priv *priv = container_of(icd, struct tw9910_priv, icd);
+
+	if (reg->reg > 0xff ||
+	    reg->val > 0xff)
+		return -EINVAL;
+
+	return i2c_smbus_write_byte_data(priv->client, reg->reg, reg->val);
+}
+#endif
+
+static int tw9910_set_fmt(struct soc_camera_device *icd, __u32 pixfmt,
+			      struct v4l2_rect *rect)
+{
+	struct tw9910_priv *priv = container_of(icd, struct tw9910_priv, icd);
+	int                 ret  = -EINVAL;
+	u8                  val;
+
+	/*
+	 * select suitable norm
+	 */
+	priv->scale = tw9910_select_norm(icd, rect->width, rect->height);
+	if (!priv->scale)
+		goto tw9910_set_fmt_error;
+
+	/*
+	 * reset hardware
+	 */
+	tw9910_reset(priv->client);
+	ret = tw9910_write_array(priv->client, tw9910_default_regs);
+	if (ret < 0)
+		goto tw9910_set_fmt_error;
+
+	/*
+	 * set bus width
+	 */
+	val = 0x00;
+	if (SOCAM_DATAWIDTH_16 == priv->info->buswidth)
+		val = LEN;
+
+	ret = tw9910_mask_set(priv->client, OPFORM, LEN, val);
+	if (ret < 0)
+		goto tw9910_set_fmt_error;
+
+	/*
+	 * select MPOUT behavior
+	 */
+	switch (priv->info->mpout) {
+	case TW9910_MPO_VLOSS:
+		val = RTSEL_VLOSS; break;
+	case TW9910_MPO_HLOCK:
+		val = RTSEL_HLOCK; break;
+	case TW9910_MPO_SLOCK:
+		val = RTSEL_SLOCK; break;
+	case TW9910_MPO_VLOCK:
+		val = RTSEL_VLOCK; break;
+	case TW9910_MPO_MONO:
+		val = RTSEL_MONO;  break;
+	case TW9910_MPO_DET50:
+		val = RTSEL_DET50; break;
+	case TW9910_MPO_FIELD:
+		val = RTSEL_FIELD; break;
+	case TW9910_MPO_RTCO:
+		val = RTSEL_RTCO;  break;
+	default:
+		val = 0;
+	}
+
+	ret = tw9910_mask_set(priv->client, VBICNTL, RTSEL_MASK, val);
+	if (ret < 0)
+		goto tw9910_set_fmt_error;
+
+	/*
+	 * set scale
+	 */
+	ret = tw9910_set_scale(priv->client, priv->scale);
+	if (ret < 0)
+		goto tw9910_set_fmt_error;
+
+	/*
+	 * set cropping
+	 */
+	ret = tw9910_set_cropping(priv->client, &tw9910_cropping_ctrl);
+	if (ret < 0)
+		goto tw9910_set_fmt_error;
+
+	/*
+	 * set hsync
+	 */
+	ret = tw9910_set_hsync(priv->client, &tw9910_hsync_ctrl);
+	if (ret < 0)
+		goto tw9910_set_fmt_error;
+
+	return ret;
+
+tw9910_set_fmt_error:
+
+	tw9910_reset(priv->client);
+	priv->scale = NULL;
+
+	return ret;
+}
+
+static int tw9910_try_fmt(struct soc_camera_device *icd,
+			      struct v4l2_format *f)
+{
+	struct v4l2_pix_format *pix = &f->fmt.pix;
+	const struct tw9910_scale_ctrl *scale;
+
+	if (V4L2_FIELD_ANY == pix->field) {
+		pix->field = V4L2_FIELD_INTERLACED;
+	} else if (V4L2_FIELD_INTERLACED != pix->field) {
+		dev_err(&icd->dev, "Field type invalid.\n");
+		return -EINVAL;
+	}
+
+	/*
+	 * select suitable norm
+	 */
+	scale = tw9910_select_norm(icd, pix->width, pix->height);
+	if (!scale)
+		return -EINVAL;
+
+	pix->width  = scale->width;
+	pix->height = scale->height;
+
+	return 0;
+}
+
+static int tw9910_video_probe(struct soc_camera_device *icd)
+{
+	struct tw9910_priv *priv = container_of(icd, struct tw9910_priv, icd);
+	s32 val;
+	int ret;
+
+	/*
+	 * We must have a parent by now. And it cannot be a wrong one.
+	 * So this entire test is completely redundant.
+	 */
+	if (!icd->dev.parent ||
+	    to_soc_camera_host(icd->dev.parent)->nr != icd->iface)
+		return -ENODEV;
+
+	/*
+	 * tw9910 only use 8 or 16 bit bus width
+	 */
+	if (SOCAM_DATAWIDTH_16 != priv->info->buswidth &&
+	    SOCAM_DATAWIDTH_8  != priv->info->buswidth) {
+		dev_err(&icd->dev, "bus width error\n");
+		return -ENODEV;
+	}
+
+	icd->formats     = tw9910_color_fmt;
+	icd->num_formats = ARRAY_SIZE(tw9910_color_fmt);
+
+	/*
+	 * check and show Product ID
+	 */
+	val = i2c_smbus_read_byte_data(priv->client, ID);
+	if (0x0B != GET_ID(val) ||
+	    0x00 != GET_ReV(val)) {
+		dev_err(&icd->dev,
+			"Product ID error %x:%x\n", GET_ID(val), GET_ReV(val));
+		return -ENODEV;
+	}
+
+	dev_info(&icd->dev,
+		 "tw9910 Product ID %0x:%0x\n", GET_ID(val), GET_ReV(val));
+
+	ret = soc_camera_video_start(icd);
+	if (ret < 0)
+		return ret;
+
+	icd->vdev->tvnorms      = V4L2_STD_NTSC | V4L2_STD_PAL;
+	icd->vdev->current_norm = V4L2_STD_NTSC;
+
+	return ret;
+}
+
+static void tw9910_video_remove(struct soc_camera_device *icd)
+{
+	soc_camera_video_stop(icd);
+}
+
+static struct soc_camera_ops tw9910_ops = {
+	.owner			= THIS_MODULE,
+	.probe			= tw9910_video_probe,
+	.remove			= tw9910_video_remove,
+	.init			= tw9910_init,
+	.release		= tw9910_release,
+	.start_capture		= tw9910_start_capture,
+	.stop_capture		= tw9910_stop_capture,
+	.set_fmt		= tw9910_set_fmt,
+	.try_fmt		= tw9910_try_fmt,
+	.set_bus_param		= tw9910_set_bus_param,
+	.query_bus_param	= tw9910_query_bus_param,
+	.get_chip_id		= tw9910_get_chip_id,
+	.set_std		= tw9910_set_std,
+	.enum_input		= tw9910_enum_input,
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.get_register		= tw9910_get_register,
+	.set_register		= tw9910_set_register,
+#endif
+};
+
+/*
+ * i2c_driver function
+ */
+
+static int tw9910_probe(struct i2c_client *client,
+			const struct i2c_device_id *did)
+
+{
+	struct tw9910_priv             *priv;
+	struct tw9910_video_info       *info;
+	struct soc_camera_device       *icd;
+	const struct tw9910_scale_ctrl *scale;
+	int                             i, ret;
+
+	info = client->dev.platform_data;
+	if (!info)
+		return -EINVAL;
+
+	if (!i2c_check_functionality(to_i2c_adapter(client->dev.parent),
+				     I2C_FUNC_SMBUS_BYTE_DATA)) {
+		dev_err(&client->dev,
+			"I2C-Adapter doesn't support "
+			"I2C_FUNC_SMBUS_BYTE_DATA\n");
+		return -EIO;
+	}
+
+	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	priv->info   = info;
+	priv->client = client;
+	i2c_set_clientdata(client, priv);
+
+	icd          = &priv->icd;
+	icd->ops     = &tw9910_ops;
+	icd->control = &client->dev;
+	icd->iface   = info->link.bus_id;
+
+	/*
+	 * set width and height
+	 */
+	icd->width_max  = tw9910_ntsc_scales[0].width; /* set default */
+	icd->width_min  = tw9910_ntsc_scales[0].width;
+	icd->height_max = tw9910_ntsc_scales[0].height;
+	icd->height_min = tw9910_ntsc_scales[0].height;
+
+	scale = tw9910_ntsc_scales;
+	for (i = 0; i < ARRAY_SIZE(tw9910_ntsc_scales); i++) {
+		icd->width_max  = max(scale[i].width,  icd->width_max);
+		icd->width_min  = min(scale[i].width,  icd->width_min);
+		icd->height_max = max(scale[i].height, icd->height_max);
+		icd->height_min = min(scale[i].height, icd->height_min);
+	}
+	scale = tw9910_pal_scales;
+	for (i = 0; i < ARRAY_SIZE(tw9910_pal_scales); i++) {
+		icd->width_max  = max(scale[i].width,  icd->width_max);
+		icd->width_min  = min(scale[i].width,  icd->width_min);
+		icd->height_max = max(scale[i].height, icd->height_max);
+		icd->height_min = min(scale[i].height, icd->height_min);
+	}
+
+	ret = soc_camera_device_register(icd);
+
+	if (ret) {
+		i2c_set_clientdata(client, NULL);
+		kfree(priv);
+	}
+
+	return ret;
+}
+
+static int tw9910_remove(struct i2c_client *client)
+{
+	struct tw9910_priv *priv = i2c_get_clientdata(client);
+
+	soc_camera_device_unregister(&priv->icd);
+	i2c_set_clientdata(client, NULL);
+	kfree(priv);
+	return 0;
+}
+
+static const struct i2c_device_id tw9910_id[] = {
+	{ "tw9910", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, tw9910_id);
+
+static struct i2c_driver tw9910_i2c_driver = {
+	.driver = {
+		.name = "tw9910",
+	},
+	.probe    = tw9910_probe,
+	.remove   = tw9910_remove,
+	.id_table = tw9910_id,
+};
+
+/*
+ * module function
+ */
+static int __init tw9910_module_init(void)
+{
+	return i2c_add_driver(&tw9910_i2c_driver);
+}
+
+static void __exit tw9910_module_exit(void)
+{
+	i2c_del_driver(&tw9910_i2c_driver);
+}
+
+module_init(tw9910_module_init);
+module_exit(tw9910_module_exit);
+
+MODULE_DESCRIPTION("SoC Camera driver for tw9910");
+MODULE_AUTHOR("Kuninori Morimoto");
+MODULE_LICENSE("GPL v2");