V4L/DVB (9091): gspca: Subdriver m5602 (ALi) added.

This patch adds support for the ALi m5602 usb bridge and is based on
the gspca framework.
It contains code for communicating with 5 different sensors:
OmniVision OV9650, Pixel Plus PO1030, Samsung S5K83A, S5K4AA and
finally Micron MT9M111.
Signed-off-by: Erik Andren <erik.andren@gmail.com>
Signed-off-by: Jean-Francois Moine <moinejf@free.fr>
[mchehab@redhat.com: fix m5602/Makefile]
[mchehab@redhat.com: extern debug caused conflicts. Renamed to m5602_debug]
Signed-off-by: Mauro Carvalho Chehab <mchehab@redhat.com>

Documentation/video4linux/gspca.txt

@@ -7,6 +7,7 @@ The modules are:
 xxxx		vend:prod
 ----
 spca501		0000:0000	MystFromOri Unknow Camera
+m5602		0402:5602	ALi Video Camera Controller
 spca501		040a:0002	Kodak DVC-325
 spca500		040a:0300	Kodak EZ200
 zc3xx		041e:041e	Creative WebCam Live!

Documentation/video4linux/m5602.txt

+This document describes the ALi m5602 bridge connected
+to the following supported sensors:
+OmniVision OV9650,
+Samsung s5k83a,
+Samsung s5k4aa,
+Micron mt9m111,
+Pixel plus PO1030
+
+This driver mimics the windows drivers, which have a braindead implementation sending bayer-encoded frames at VGA resolution.
+In a perfect world we should be able to reprogram the m5602 and the connected sensor in hardware instead, supporting a range of resolutions and pixelformats
+
+Anyway, have fun and please report any bugs to m560x-driver-devel@lists.sourceforge.net

drivers/media/video/gspca/Kconfig

@@ -11,3 +11,6 @@ config USB_GSPCA
 
 	  To compile this driver as modules, choose M here: the
 	  modules will be called gspca_xxxx.
+
+source "drivers/media/video/gspca/m5602/Kconfig"
+

drivers/media/video/gspca/Makefile

@@ -28,3 +28,6 @@ gspca_t613-objs := t613.o
 gspca_tv8532-objs := tv8532.o
 gspca_vc032x-objs := vc032x.o
 gspca_zc3xx-objs := zc3xx.o
+
+obj-$(CONFIG_USB_M5602)         += m5602/
+

drivers/media/video/gspca/m5602/Kconfig

+config USB_M5602
+	tristate "USB ALi m5602 Webcam support"
+	depends on VIDEO_V4L2 && USB_GSPCA
+	help
+	  Say Y here if you want support for cameras based on the
+	  ALi m5602 connected to various image sensors.
+
+	  See <file:Documentation/video4linux/m5602.txt> for more info.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called gspca-m5602.

drivers/media/video/gspca/m5602/Makefile

+obj-$(CONFIG_USB_M5602) += gspca_m5602.o
+
+gspca_m5602-objs := m5602_core.o \
+		    m5602_ov9650.o \
+		    m5602_mt9m111.o \
+		    m5602_po1030.o \
+		    m5602_s5k83a.o \
+		    m5602_s5k4aa.o
+
+EXTRA_CFLAGS += -Idrivers/media/video/gspca
+

drivers/media/video/gspca/m5602/m5602_bridge.h

+/*
+ * USB Driver for ALi m5602 based webcams
+ *
+ * Copyright (C) 2008 Erik Andren
+ * Copyright (C) 2007 Ilyes Gouta. Based on the m5603x Linux Driver Project.
+ * Copyright (C) 2005 m5603x Linux Driver Project <m5602@x3ng.com.br>
+ *
+ * Portions of code to USB interface and ALi driver software,
+ * Copyright (c) 2006 Willem Duinker
+ * v4l2 interface modeled after the V4L2 driver
+ * for SN9C10x PC Camera Controllers
+ *
+ * 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, version 2.
+ *
+ */
+
+#ifndef M5602_BRIDGE_H_
+#define M5602_BRIDGE_H_
+
+#include "gspca.h"
+
+#define MODULE_NAME "ALi m5602"
+
+/*****************************************************************************/
+
+#undef PDEBUG
+#undef info
+#undef err
+
+#define err(format, arg...) printk(KERN_ERR KBUILD_MODNAME ": " \
+	format "\n" , ## arg)
+#define info(format, arg...) printk(KERN_INFO KBUILD_MODNAME ": " \
+	format "\n" , ## arg)
+
+/* Debug parameters */
+#define DBG_INIT 0x1
+#define DBG_PROBE 0x2
+#define DBG_V4L2 0x4
+#define DBG_TRACE 0x8
+#define DBG_DATA 0x10
+#define DBG_V4L2_CID 0x20
+#define DBG_GSPCA 0x40
+
+#define PDEBUG(level, fmt, args...) \
+	do { \
+		if (m5602_debug & level)     \
+			info("[%s:%d] " fmt, __func__, __LINE__ , \
+			## args); \
+	} while (0)
+
+/*****************************************************************************/
+
+#define M5602_XB_SENSOR_TYPE 0x00
+#define M5602_XB_SENSOR_CTRL 0x01
+#define M5602_XB_LINE_OF_FRAME_H 0x02
+#define M5602_XB_LINE_OF_FRAME_L 0x03
+#define M5602_XB_PIX_OF_LINE_H 0x04
+#define M5602_XB_PIX_OF_LINE_L 0x05
+#define M5602_XB_VSYNC_PARA 0x06
+#define M5602_XB_HSYNC_PARA 0x07
+#define M5602_XB_TEST_MODE_1 0x08
+#define M5602_XB_TEST_MODE_2 0x09
+#define M5602_XB_SIG_INI 0x0a
+#define M5602_XB_DS_PARA 0x0e
+#define M5602_XB_TRIG_PARA 0x0f
+#define M5602_XB_CLK_PD 0x10
+#define M5602_XB_MCU_CLK_CTRL 0x12
+#define M5602_XB_MCU_CLK_DIV 0x13
+#define M5602_XB_SEN_CLK_CTRL 0x14
+#define M5602_XB_SEN_CLK_DIV 0x15
+#define M5602_XB_AUD_CLK_CTRL 0x16
+#define M5602_XB_AUD_CLK_DIV 0x17
+#define M5602_XB_DEVCTR1 0x41
+#define M5602_XB_EPSETR0 0x42
+#define M5602_XB_EPAFCTR 0x47
+#define M5602_XB_EPBFCTR 0x49
+#define M5602_XB_EPEFCTR 0x4f
+#define M5602_XB_TEST_REG 0x53
+#define M5602_XB_ALT2SIZE 0x54
+#define M5602_XB_ALT3SIZE 0x55
+#define M5602_XB_OBSFRAME 0x56
+#define M5602_XB_PWR_CTL 0x59
+#define M5602_XB_ADC_CTRL 0x60
+#define M5602_XB_ADC_DATA 0x61
+#define M5602_XB_MISC_CTRL 0x62
+#define M5602_XB_SNAPSHOT 0x63
+#define M5602_XB_SCRATCH_1 0x64
+#define M5602_XB_SCRATCH_2 0x65
+#define M5602_XB_SCRATCH_3 0x66
+#define M5602_XB_SCRATCH_4 0x67
+#define M5602_XB_I2C_CTRL 0x68
+#define M5602_XB_I2C_CLK_DIV 0x69
+#define M5602_XB_I2C_DEV_ADDR 0x6a
+#define M5602_XB_I2C_REG_ADDR 0x6b
+#define M5602_XB_I2C_DATA 0x6c
+#define M5602_XB_I2C_STATUS 0x6d
+#define M5602_XB_GPIO_DAT_H 0x70
+#define M5602_XB_GPIO_DAT_L 0x71
+#define M5602_XB_GPIO_DIR_H 0x72
+#define M5602_XB_GPIO_DIR_L 0x73
+#define M5602_XB_GPIO_EN_H 0x74
+#define M5602_XB_GPIO_EN_L 0x75
+#define M5602_XB_GPIO_DAT 0x76
+#define M5602_XB_GPIO_DIR 0x77
+#define M5602_XB_MISC_CTL 0x70
+
+#define I2C_BUSY 0x80
+
+/*****************************************************************************/
+
+/* Driver info */
+#define DRIVER_AUTHOR "ALi m5602 Linux Driver Project"
+#define DRIVER_DESC "ALi m5602 webcam driver"
+
+#define M5602_ISOC_ENDPOINT_ADDR 0x81
+#define M5602_INTR_ENDPOINT_ADDR 0x82
+
+#define M5602_MAX_FRAMES	32
+#define M5602_URBS		2
+#define M5602_ISOC_PACKETS	14
+
+#define M5602_URB_TIMEOUT	msecs_to_jiffies(2 * M5602_ISOC_PACKETS)
+#define M5602_URB_MSG_TIMEOUT   5000
+#define M5602_FRAME_TIMEOUT	2
+
+/*****************************************************************************/
+
+/* A skeleton used for sending messages to the m5602 bridge */
+static const unsigned char bridge_urb_skeleton[] = {
+	0x13, 0x00, 0x81, 0x00
+};
+
+/* A skeleton used for sending messages to the sensor */
+static const unsigned char sensor_urb_skeleton[] = {
+	0x23, M5602_XB_GPIO_EN_H, 0x81, 0x06,
+	0x23, M5602_XB_MISC_CTRL, 0x81, 0x80,
+	0x13, M5602_XB_I2C_DEV_ADDR, 0x81, 0x00,
+	0x13, M5602_XB_I2C_REG_ADDR, 0x81, 0x00,
+	0x13, M5602_XB_I2C_DATA, 0x81, 0x00,
+	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;
+
+	/* The name of the m5602 camera */
+	char *name;
+
+	/* A pointer to the currently connected sensor */
+	struct m5602_sensor *sensor;
+
+	/* The current frame's id, used to detect frame boundaries */
+	u8 frame_id;
+
+	/* The current frame count */
+	u32 frame_count;
+};
+
+int m5602_read_bridge(
+	struct sd *sd, u8 address, u8 *i2c_data);
+
+int m5602_write_bridge(
+	struct sd *sd, u8 address, u8 i2c_data);
+
+int m5602_configure(struct gspca_dev *gspca_dev,
+			   const struct usb_device_id *id);
+
+int m5602_init(struct gspca_dev *gspca_dev);
+
+void m5602_start_transfer(struct gspca_dev *gspca_dev);
+
+void m5602_stop_transfer(struct gspca_dev *gspca_dev);
+
+void m5602_urb_complete(struct gspca_dev *gspca_dev, struct gspca_frame *frame,
+			__u8 *data, int len);
+
+#endif

drivers/media/video/gspca/m5602/m5602_core.c

+/*
+ * USB Driver for ALi m5602 based webcams
+ *
+ * Copyright (C) 2008 Erik Andren
+ * Copyright (C) 2007 Ilyes Gouta. Based on the m5603x Linux Driver Project.
+ * Copyright (C) 2005 m5603x Linux Driver Project <m5602@x3ng.com.br>
+ *
+ * Portions of code to USB interface and ALi driver software,
+ * Copyright (c) 2006 Willem Duinker
+ * v4l2 interface modeled after the V4L2 driver
+ * for SN9C10x PC Camera Controllers
+ *
+ * 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, version 2.
+ *
+ */
+
+#include "m5602_ov9650.h"
+#include "m5602_mt9m111.h"
+#include "m5602_po1030.h"
+#include "m5602_s5k83a.h"
+#include "m5602_s5k4aa.h"
+
+/* Kernel module parameters */
+int force_sensor;
+int dump_bridge;
+int dump_sensor;
+unsigned int m5602_debug;
+
+static const __devinitdata struct usb_device_id m5602_table[] = {
+	{USB_DEVICE(0x0402, 0x5602)},
+	{}
+};
+
+MODULE_DEVICE_TABLE(usb, m5602_table);
+
+/* sub-driver description, the ctrl and nctrl is filled at probe time */
+static struct sd_desc sd_desc = {
+	.name		= MODULE_NAME,
+	.config		= m5602_configure,
+	.init		= m5602_init,
+	.start		= m5602_start_transfer,
+	.stopN		= m5602_stop_transfer,
+	.pkt_scan	= m5602_urb_complete
+};
+
+/* Reads a byte from the m5602 */
+int m5602_read_bridge(struct sd *sd, u8 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, 0x14,
+			      0x8100 + address, buf,
+			      1, M5602_URB_MSG_TIMEOUT);
+	*i2c_data = buf[0];
+
+	PDEBUG(DBG_TRACE, "Reading bridge register 0x%x containing 0x%x",
+	       address, *i2c_data);
+
+	/* usb_control_msg(...) returns the number of bytes sent upon success,
+	mask that and return zero upon success instead*/
+	return (err < 0) ? err : 0;
+}
+
+/* Writes a byte to to the m5602 */
+int m5602_write_bridge(struct sd *sd, u8 address, u8 i2c_data)
+{
+	int err;
+	struct usb_device *udev = sd->gspca_dev.dev;
+	__u8 *buf = sd->gspca_dev.usb_buf;
+
+	PDEBUG(DBG_TRACE, "Writing bridge register 0x%x with 0x%x",
+	       address, i2c_data);
+
+	memcpy(buf, bridge_urb_skeleton,
+	       sizeof(bridge_urb_skeleton));
+	buf[1] = address;
+	buf[3] = i2c_data;
+
+	err = usb_control_msg(udev, usb_sndctrlpipe(udev, 0),
+				0x04, 0x40, 0x19,
+				0x0000, buf,
+				4, M5602_URB_MSG_TIMEOUT);
+
+	/* usb_control_msg(...) returns the number of bytes sent upon success,
+	   mask that and return zero upon success instead */
+	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)
+{
+	int i;
+	for (i = 0; i < 0x80; i++) {
+		unsigned char val = 0;
+		m5602_read_bridge(sd, i, &val);
+		info("ALi m5602 address 0x%x contains 0x%x", i, val);
+	}
+	info("Warning: The camera probably won't work until it's power cycled");
+}
+
+int m5602_probe_sensor(struct sd *sd)
+{
+	/* Try the po1030 */
+	sd->sensor = &po1030;
+	if (!sd->sensor->probe(sd)) {
+		sd_desc.ctrls = po1030.ctrls;
+		sd_desc.nctrls = po1030.nctrls;
+		return 0;
+	}
+
+	/* Try the mt9m111 sensor */
+	sd->sensor = &mt9m111;
+	if (!sd->sensor->probe(sd)) {
+		sd_desc.ctrls = mt9m111.ctrls;
+		sd_desc.nctrls = mt9m111.nctrls;
+		return 0;
+	}
+
+	/* Try the s5k4aa */
+	sd->sensor = &s5k4aa;
+	if (!sd->sensor->probe(sd)) {
+		sd_desc.ctrls = s5k4aa.ctrls;
+		sd_desc.nctrls = s5k4aa.nctrls;
+		return 0;
+	}
+
+	/* Try the ov9650 */
+	sd->sensor = &ov9650;
+	if (!sd->sensor->probe(sd)) {
+		sd_desc.ctrls = ov9650.ctrls;
+		sd_desc.nctrls = ov9650.nctrls;
+		return 0;
+	}
+
+	/* Try the s5k83a */
+	sd->sensor = &s5k83a;
+	if (!sd->sensor->probe(sd)) {
+		sd_desc.ctrls = s5k83a.ctrls;
+		sd_desc.nctrls = s5k83a.nctrls;
+		return 0;
+	}
+
+
+	/* More sensor probe function goes here */
+	info("Failed to find a sensor");
+	sd->sensor = NULL;
+	return -ENODEV;
+}
+
+int m5602_init(struct gspca_dev *gspca_dev)
+{
+	struct sd *sd = (struct sd *) gspca_dev;
+	int err;
+
+	PDEBUG(DBG_TRACE, "Initializing ALi m5602 webcam");
+	/* Run the init sequence */
+	err = sd->sensor->init(sd);
+
+	return err;
+}
+
+void m5602_start_transfer(struct gspca_dev *gspca_dev)
+{
+	struct sd *sd = (struct sd *) gspca_dev;
+	__u8 *buf = sd->gspca_dev.usb_buf;
+
+	/* Send start command to the camera */
+	const u8 buffer[4] = {0x13, 0xf9, 0x0f, 0x01};
+	memcpy(buf, buffer, sizeof(buffer));
+	usb_control_msg(gspca_dev->dev, usb_sndctrlpipe(gspca_dev->dev, 0),
+			0x04, 0x40, 0x19, 0x0000, buf,
+			4, M5602_URB_MSG_TIMEOUT);
+
+	PDEBUG(DBG_V4L2, "Transfer started");
+}
+
+void m5602_urb_complete(struct gspca_dev *gspca_dev, struct gspca_frame *frame,
+			__u8 *data, int len)
+{
+	struct sd *sd = (struct sd *) gspca_dev;
+
+	if (len < 6) {
+		PDEBUG(DBG_DATA, "Packet is less than 6 bytes");
+		return;
+	}
+
+	/* Frame delimiter: ff xx xx xx ff ff */
+	if (data[0] == 0xff && data[4] == 0xff && data[5] == 0xff &&
+	    data[2] != sd->frame_id) {
+		PDEBUG(DBG_DATA, "Frame delimiter detected");
+		sd->frame_id = data[2];
+
+		/* Remove the extra fluff appended on each header */
+		data += 6;
+		len -= 6;
+
+		/* Complete the last frame (if any) */
+		frame = gspca_frame_add(gspca_dev, LAST_PACKET,
+					frame, data, 0);
+
+		/* Create a new frame */
+		gspca_frame_add(gspca_dev, FIRST_PACKET, frame, data, len);
+
+		PDEBUG(DBG_V4L2, "Starting new frame. First urb contained %d",
+		       len);
+
+	} else {
+		int cur_frame_len = frame->data_end - frame->data;
+
+		/* Remove urb header */
+		data += 4;
+		len -= 4;
+
+		if (cur_frame_len + len <= frame->v4l2_buf.length) {
+			PDEBUG(DBG_DATA, "Continuing frame copying %d bytes",
+			       len);
+
+			gspca_frame_add(gspca_dev, INTER_PACKET, frame,
+					data, len);
+		} else if (frame->v4l2_buf.length - cur_frame_len > 0) {
+			/* Add the remaining data up to frame size */
+			gspca_frame_add(gspca_dev, INTER_PACKET, frame, data,
+					frame->v4l2_buf.length - cur_frame_len);
+		}
+	}
+}
+
+void m5602_stop_transfer(struct gspca_dev *gspca_dev)
+{
+	/* Is there are a command to stop a data transfer? */
+}
+
+/* this function is called at probe time */
+int m5602_configure(struct gspca_dev *gspca_dev,
+			   const struct usb_device_id *id)
+{
+	struct sd *sd = (struct sd *) gspca_dev;
+	struct cam *cam;
+	int err;
+
+	PDEBUG(DBG_GSPCA, "m5602_configure start");
+	cam = &gspca_dev->cam;
+	cam->epaddr = M5602_ISOC_ENDPOINT_ADDR;
+
+	if (dump_bridge)
+		m5602_dump_bridge(sd);
+
+	/* Probe sensor */
+	err = m5602_probe_sensor(sd);
+	if (err)
+		goto fail;
+
+	PDEBUG(DBG_GSPCA, "m5602_configure end");
+	return 0;
+
+fail:
+	PDEBUG(DBG_GSPCA, "m5602_configure failed");
+	cam->cam_mode = NULL;
+	cam->nmodes = 0;
+
+	return err;
+}
+
+static int m5602_probe(struct usb_interface *intf,
+		       const struct usb_device_id *id)
+{
+	return gspca_dev_probe(intf, id, &sd_desc, sizeof(struct sd),
+			       THIS_MODULE);
+}
+
+static struct usb_driver sd_driver = {
+	.name = MODULE_NAME,
+	.id_table = m5602_table,
+	.probe = m5602_probe,
+#ifdef CONFIG_PM
+	.suspend = gspca_suspend,
+	.resume = gspca_resume,
+#endif
+	.disconnect = gspca_disconnect
+};
+
+/* -- module insert / remove -- */
+static int __init mod_m5602_init(void)
+{
+	if (usb_register(&sd_driver) < 0)
+		return -1;
+	PDEBUG(D_PROBE, "m5602 module registered");
+	return 0;
+}
+static void __exit mod_m5602_exit(void)
+{
+	usb_deregister(&sd_driver);
+	PDEBUG(D_PROBE, "m5602 module deregistered");
+}
+
+module_init(mod_m5602_init);
+module_exit(mod_m5602_exit);
+
+MODULE_AUTHOR(DRIVER_AUTHOR);
+MODULE_DESCRIPTION(DRIVER_DESC);
+MODULE_LICENSE("GPL");
+module_param_named(debug, m5602_debug, int, S_IRUGO | S_IWUSR);
+MODULE_PARM_DESC(debug, "toggles debug on/off");
+
+module_param(force_sensor, int, S_IRUGO | S_IWUSR);
+MODULE_PARM_DESC(force_sensor,
+		"force detection of sensor, "
+		"1 = OV9650, 2 = S5K83A, 3 = S5K4AA, 4 = MT9M111, 5 = PO1030");
+
+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 usb sensor registers "
+		"at startup providing a sensor is found");

drivers/media/video/gspca/m5602/m5602_mt9m111.c

+/*
+ * Driver for the mt9m111 sensor
+ *
+ * Copyright (C) 2008 Erik Andren
+ * Copyright (C) 2007 Ilyes Gouta. Based on the m5603x Linux Driver Project.
+ * Copyright (C) 2005 m5603x Linux Driver Project <m5602@x3ng.com.br>
+ *
+ * Portions of code to USB interface and ALi driver software,
+ * Copyright (c) 2006 Willem Duinker
+ * v4l2 interface modeled after the V4L2 driver
+ * for SN9C10x PC Camera Controllers
+ *
+ * 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, version 2.
+ *
+ */
+
+#include "m5602_mt9m111.h"
+
+int mt9m111_probe(struct sd *sd)
+{
+	u8 data[2] = {0x00, 0x00};
+	int i;
+
+	if (force_sensor) {
+		if (force_sensor == MT9M111_SENSOR) {
+			info("Forcing a %s sensor", mt9m111.name);
+			goto sensor_found;
+		}
+		/* If we want to force another sensor, don't try to probe this
+		 * one */
+		return -ENODEV;
+	}
+
+	info("Probing for a mt9m111 sensor");
+
+	/* Do the preinit */
+	for (i = 0; i < ARRAY_SIZE(preinit_mt9m111); i++) {
+		if (preinit_mt9m111[i][0] == BRIDGE) {
+			m5602_write_bridge(sd,
+				preinit_mt9m111[i][1],
+				preinit_mt9m111[i][2]);
+		} else {
+			data[0] = preinit_mt9m111[i][2];
+			data[1] = preinit_mt9m111[i][3];
+			mt9m111_write_sensor(sd,
+				preinit_mt9m111[i][1], data, 2);
+		}
+	}
+
+	if (mt9m111_read_sensor(sd, MT9M111_SC_CHIPVER, data, 2))
+		return -ENODEV;
+
+	if ((data[0] == 0x14) && (data[1] == 0x3a)) {
+		info("Detected a mt9m111 sensor");
+		goto sensor_found;
+	}
+
+	return -ENODEV;
+
+sensor_found:
+	sd->gspca_dev.cam.cam_mode = mt9m111.modes;
+	sd->gspca_dev.cam.nmodes = mt9m111.nmodes;
+	return 0;
+}
+
+int mt9m111_init(struct sd *sd)
+{
+	int i, err;
+
+	/* Init the sensor */
+	for (i = 0; i < ARRAY_SIZE(init_mt9m111); i++) {
+		u8 data[2];
+
+		if (init_mt9m111[i][0] == BRIDGE) {
+			err = m5602_write_bridge(sd,
+				init_mt9m111[i][1],
+				init_mt9m111[i][2]);
+		} else {
+			data[0] = init_mt9m111[i][2];
+			data[1] = init_mt9m111[i][3];
+			err = mt9m111_write_sensor(sd,
+				init_mt9m111[i][1], data, 2);
+		}
+	}
+
+	if (dump_sensor)
+		mt9m111_dump_registers(sd);
+
+	return (err < 0) ? err : 0;
+}
+
+int mt9m111_power_down(struct sd *sd)
+{
+	return 0;
+}
+
+int mt9m111_get_vflip(struct gspca_dev *gspca_dev, __s32 *val)
+{
+	int err;
+	u8 data[2] = {0x00, 0x00};
+	struct sd *sd = (struct sd *) gspca_dev;
+
+	err = mt9m111_read_sensor(sd, MT9M111_SC_R_MODE_CONTEXT_B,
+				  data, 2);
+	*val = data[0] & MT9M111_RMB_MIRROR_ROWS;
+	PDEBUG(DBG_V4L2_CID, "Read vertical flip %d", *val);
+
+	return (err < 0) ? err : 0;
+}
+
+int mt9m111_set_vflip(struct gspca_dev *gspca_dev, __s32 val)
+{
+	int err;
+	u8 data[2] = {0x00, 0x00};
+	struct sd *sd = (struct sd *) gspca_dev;
+
+	PDEBUG(DBG_V4L2_CID, "Set vertical flip to %d", val);
+
+	/* Set the correct page map */
+	err = mt9m111_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);
+	if (err < 0)
+		goto out;
+
+	data[0] = (data[0] & 0xfe) | val;
+	err = mt9m111_write_sensor(sd, MT9M111_SC_R_MODE_CONTEXT_B,
+				   data, 2);
+out:
+	return (err < 0) ? err : 0;
+}
+
+int mt9m111_get_hflip(struct gspca_dev *gspca_dev, __s32 *val)
+{
+	int err;
+	u8 data[2] = {0x00, 0x00};
+	struct sd *sd = (struct sd *) gspca_dev;
+
+	err = mt9m111_read_sensor(sd, MT9M111_SC_R_MODE_CONTEXT_B,
+				  data, 2);
+	*val = data[0] & MT9M111_RMB_MIRROR_COLS;
+	PDEBUG(DBG_V4L2_CID, "Read horizontal flip %d", *val);
+
+	return (err < 0) ? err : 0;
+}
+
+int mt9m111_set_hflip(struct gspca_dev *gspca_dev, __s32 val)
+{
+	int err;
+	u8 data[2] = {0x00, 0x00};
+	struct sd *sd = (struct sd *) gspca_dev;
+
+	PDEBUG(DBG_V4L2_CID, "Set horizontal flip to %d", val);
+
+	/* Set the correct page map */
+	err = mt9m111_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);
+	if (err < 0)
+		goto out;
+
+	data[0] = (data[0] & 0xfd) | ((val << 1) & 0x02);
+	err = mt9m111_write_sensor(sd, MT9M111_SC_R_MODE_CONTEXT_B,
+					data, 2);
+out:
+	return (err < 0) ? err : 0;
+}
+
+int mt9m111_get_gain(struct gspca_dev *gspca_dev, __s32 *val)
+{
+	int err, tmp;
+	u8 data[2] = {0x00, 0x00};
+	struct sd *sd = (struct sd *) gspca_dev;
+
+	err = mt9m111_read_sensor(sd, MT9M111_SC_GLOBAL_GAIN, data, 2);
+	tmp = ((data[1] << 8) | data[0]);
+
+	*val = ((tmp & (1 << 10)) * 2) |
+	      ((tmp & (1 <<  9)) * 2) |
+	      ((tmp & (1 <<  8)) * 2) |
+	       (tmp & 0x7f);
+
+	PDEBUG(DBG_V4L2_CID, "Read gain %d", *val);
+
+	return (err < 0) ? err : 0;
+}
+
+int mt9m111_set_gain(struct gspca_dev *gspca_dev, __s32 val)
+{
+	int err, tmp;
+	u8 data[2] = {0x00, 0x00};
+	struct sd *sd = (struct sd *) gspca_dev;
+
+	/* Set the correct page map */
+	err = mt9m111_write_sensor(sd, MT9M111_PAGE_MAP, data, 2);
+	if (err < 0)
+		goto out;
+
+	if (val >= INITIAL_MAX_GAIN * 2 * 2 * 2)
+		return -EINVAL;
+
+	if ((val >= INITIAL_MAX_GAIN * 2 * 2) &&
+	    (val < (INITIAL_MAX_GAIN - 1) * 2 * 2 * 2))
+		tmp = (1 << 10) | (val << 9) |
+				(val << 8) | (val / 8);
+	else if ((val >= INITIAL_MAX_GAIN * 2) &&
+		 (val <  INITIAL_MAX_GAIN * 2 * 2))
+		tmp = (1 << 9) | (1 << 8) | (val / 4);
+	else if ((val >= INITIAL_MAX_GAIN) &&
+		 (val < INITIAL_MAX_GAIN * 2))
+		tmp = (1 << 8) | (val / 2);
+	else
+		tmp = val;
+
+	data[1] = (tmp & 0xff00) >> 8;
+	data[0] = (tmp & 0xff);
+	PDEBUG(DBG_V4L2_CID, "tmp=%d, data[1]=%d, data[0]=%d", tmp,
+	       data[1], data[0]);
+
+	err = mt9m111_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(DBG_TRACE, "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(DBG_TRACE, "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;
+}
+
+void mt9m111_dump_registers(struct sd *sd)
+{
+	u8 address, value[2] = {0x00, 0x00};
+
+	info("Dumping the mt9m111 register state");
+
+	info("Dumping the mt9m111 sensor core registers");
+	value[1] = MT9M111_SENSOR_CORE;
+	mt9m111_write_sensor(sd, MT9M111_PAGE_MAP, value, 2);
+	for (address = 0; address < 0xff; address++) {
+		mt9m111_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);
+	for (address = 0; address < 0xff; address++) {
+		mt9m111_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);
+	for (address = 0; address < 0xff; address++) {
+		mt9m111_read_sensor(sd, address, value, 2);
+		info("register 0x%x contains 0x%x%x",
+		     address, value[0], value[1]);
+	}
+
+	info("mt9m111 register state dump complete");
+}

drivers/media/video/gspca/m5602/m5602_po1030.c

+/*
+ * Driver for the po1030 sensor
+ *
+ * Copyright (c) 2008 Erik Andren
+ * Copyright (c) 2007 Ilyes Gouta. Based on the m5603x Linux Driver Project.
+ * Copyright (c) 2005 m5603x Linux Driver Project <m5602@x3ng.com.br>
+ *
+ * Portions of code to USB interface and ALi driver software,
+ * Copyright (c) 2006 Willem Duinker
+ * v4l2 interface modeled after the V4L2 driver
+ * for SN9C10x PC Camera Controllers
+ *
+ * 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, version 2.
+ *
+ */
+
+#include "m5602_po1030.h"
+
+int po1030_probe(struct sd *sd)
+{
+	u8 prod_id = 0, ver_id = 0, i;
+
+	if (force_sensor) {
+		if (force_sensor == PO1030_SENSOR) {
+			info("Forcing a %s sensor", po1030.name);
+			goto sensor_found;
+		}
+		/* If we want to force another sensor, don't try to probe this
+		 * one */
+		return -ENODEV;
+	}
+
+	info("Probing for a po1030 sensor");
+
+	/* Run the pre-init to actually probe the unit */
+	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,
+				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))
+		return -ENODEV;
+
+	if (po1030_read_sensor(sd, 0x4, &ver_id, 1))
+		return -ENODEV;
+
+	if ((prod_id == 0x02) && (ver_id == 0xef)) {
+		info("Detected a po1030 sensor");
+		goto sensor_found;
+	}
+	return -ENODEV;
+
+sensor_found:
+	sd->gspca_dev.cam.cam_mode = po1030.modes;
+	sd->gspca_dev.cam.nmodes = po1030.nmodes;
+	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(DBG_TRACE, "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(DBG_TRACE, "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++) {
+		u8 data[2] = {0x00, 0x00};
+
+		switch (init_po1030[i][0]) {
+		case BRIDGE:
+			err = m5602_write_bridge(sd,
+				init_po1030[i][1],
+				init_po1030[i][2]);
+			break;
+
+		case SENSOR:
+			data[0] = init_po1030[i][2];
+			err = po1030_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;
+		}
+	}
+
+	if (dump_sensor)
+		po1030_dump_registers(sd);
+
+	return (err < 0) ? err : 0;
+}
+
+int po1030_get_exposure(struct gspca_dev *gspca_dev, __s32 *val)
+{
+	struct sd *sd = (struct sd *) gspca_dev;
+	u8 i2c_data;
+	int err;
+
+	err = po1030_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,
+				 &i2c_data, 1);
+	*val |= i2c_data;
+
+	PDEBUG(DBG_V4L2_CID, "Exposure read as %d", *val);
+out:
+	return (err < 0) ? err : 0;
+}
+
+int po1030_set_exposure(struct gspca_dev *gspca_dev, __s32 val)
+{
+	struct sd *sd = (struct sd *) gspca_dev;
+	u8 i2c_data;
+	int err;
+
+	PDEBUG(DBG_V4L2, "Set exposure to %d", val & 0xffff);
+
+	i2c_data = ((val & 0xff00) >> 8);
+	PDEBUG(DBG_V4L2, "Set exposure to high byte to 0x%x",
+	       i2c_data);
+
+	err = po1030_write_sensor(sd, PO1030_REG_INTEGLINES_H,
+				  &i2c_data, 1);
+	if (err < 0)
+		goto out;
+
+	i2c_data = (val & 0xff);
+	PDEBUG(DBG_V4L2, "Set exposure to low byte to 0x%x",
+	       i2c_data);
+	err = po1030_write_sensor(sd, PO1030_REG_INTEGLINES_M,
+				  &i2c_data, 1);
+
+out:
+	return (err < 0) ? err : 0;
+}
+
+int po1030_get_gain(struct gspca_dev *gspca_dev, __s32 *val)
+{
+	struct sd *sd = (struct sd *) gspca_dev;
+	u8 i2c_data;
+	int err;
+
+	err = po1030_read_sensor(sd, PO1030_REG_GLOBALGAIN,
+				 &i2c_data, 1);
+	*val = i2c_data;
+	PDEBUG(DBG_V4L2_CID, "Read global gain %d", *val);
+
+	return (err < 0) ? err : 0;
+}
+
+int po1030_set_gain(struct gspca_dev *gspca_dev, __s32 val)
+{
+	struct sd *sd = (struct sd *) gspca_dev;
+	u8 i2c_data;
+	int err;
+
+	i2c_data = val & 0xff;
+	PDEBUG(DBG_V4L2, "Set global gain to %d", i2c_data);
+	err = po1030_write_sensor(sd, PO1030_REG_GLOBALGAIN,
+				  &i2c_data, 1);
+	return (err < 0) ? err : 0;
+}
+
+int po1030_get_red_balance(struct gspca_dev *gspca_dev, __s32 *val)
+{
+	struct sd *sd = (struct sd *) gspca_dev;
+	u8 i2c_data;
+	int err;
+
+	err = po1030_read_sensor(sd, PO1030_REG_RED_GAIN,
+				 &i2c_data, 1);
+	*val = i2c_data;
+	PDEBUG(DBG_V4L2_CID, "Read red gain %d", *val);
+	return (err < 0) ? err : 0;
+}
+
+int po1030_set_red_balance(struct gspca_dev *gspca_dev, __s32 val)
+{
+	struct sd *sd = (struct sd *) gspca_dev;
+	u8 i2c_data;
+	int err;
+
+	i2c_data = val & 0xff;
+	PDEBUG(DBG_V4L2, "Set red gain to %d", i2c_data);
+	err = po1030_write_sensor(sd, PO1030_REG_RED_GAIN,
+				  &i2c_data, 1);
+	return (err < 0) ? err : 0;
+}
+
+int po1030_get_blue_balance(struct gspca_dev *gspca_dev, __s32 *val)
+{
+	struct sd *sd = (struct sd *) gspca_dev;
+	u8 i2c_data;
+	int err;
+
+	err = po1030_read_sensor(sd, PO1030_REG_BLUE_GAIN,
+				 &i2c_data, 1);
+	*val = i2c_data;
+	PDEBUG(DBG_V4L2_CID, "Read blue gain %d", *val);
+
+	return (err < 0) ? err : 0;
+}
+
+int po1030_set_blue_balance(struct gspca_dev *gspca_dev, __s32 val)
+{
+	struct sd *sd = (struct sd *) gspca_dev;
+	u8 i2c_data;
+	int err;
+	i2c_data = val & 0xff;
+	PDEBUG(DBG_V4L2, "Set blue gain to %d", i2c_data);
+	err = po1030_write_sensor(sd, PO1030_REG_BLUE_GAIN,
+				  &i2c_data, 1);
+
+	return (err < 0) ? err : 0;
+}
+
+int po1030_power_down(struct sd *sd)
+{
+	return 0;
+}
+
+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);
+		info("register 0x%x contains 0x%x",
+		     address, value);
+	}
+
+	info("po1030 register state dump complete");
+
+	info("Probing for which registers that are read/write");
+	for (address = 0; address < 0xff; address++) {
+		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);
+
+		if (ctrl_value == test_value[0])
+			info("register 0x%x is writeable", address);
+		else
+			info("register 0x%x is read only", address);
+
+		/* Restore original value */
+		po1030_write_sensor(sd, address, &old_value, 1);
+	}
+}

drivers/media/video/gspca/m5602/m5602_s5k83a.c

+/*
+ * Driver for the s5k83a sensor
+ *
+ * Copyright (C) 2008 Erik Andren
+ * Copyright (C) 2007 Ilyes Gouta. Based on the m5603x Linux Driver Project.
+ * Copyright (C) 2005 m5603x Linux Driver Project <m5602@x3ng.com.br>
+ *
+ * Portions of code to USB interface and ALi driver software,
+ * Copyright (c) 2006 Willem Duinker
+ * v4l2 interface modeled after the V4L2 driver
+ * for SN9C10x PC Camera Controllers
+ *
+ * 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, version 2.
+ *
+ */
+
+#include "m5602_s5k83a.h"
+
+int s5k83a_probe(struct sd *sd)
+{
+	u8 prod_id = 0, ver_id = 0;
+	int i, err = 0;
+
+	if (force_sensor) {
+		if (force_sensor == S5K83A_SENSOR) {
+			info("Forcing a %s sensor", s5k83a.name);
+			goto sensor_found;
+		}
+		/* If we want to force another sensor, don't try to probe this
+		 * one */
+		return -ENODEV;
+	}
+
+	info("Probing for a s5k83a sensor");
+
+	/* Preinit the sensor */
+	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],
+				data, 2);
+		else
+			err = m5602_write_bridge(sd, preinit_s5k83a[i][1],
+				data[0]);
+	}
+
+	/* 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))
+		return -ENODEV;
+
+	if (s5k83a_read_sensor(sd, 0x01, &ver_id, 1))
+		return -ENODEV;
+
+	if ((prod_id == 0xff) || (ver_id == 0xff))
+		return -ENODEV;
+	else
+		info("Detected a s5k83a sensor");
+
+sensor_found:
+	sd->gspca_dev.cam.cam_mode = s5k83a.modes;
+	sd->gspca_dev.cam.nmodes = s5k83a.nmodes;
+	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(DBG_TRACE, "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(DBG_TRACE, "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;
+
+	for (i = 0; i < ARRAY_SIZE(init_s5k83a) && !err; i++) {
+		u8 data[2] = {0x00, 0x00};
+
+		switch (init_s5k83a[i][0]) {
+		case BRIDGE:
+			err = m5602_write_bridge(sd,
+					init_s5k83a[i][1],
+					init_s5k83a[i][2]);
+			break;
+
+		case SENSOR:
+			data[0] = init_s5k83a[i][2];
+			err = s5k83a_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,
+				init_s5k83a[i][1], data, 2);
+			break;
+		default:
+			info("Invalid stream command, exiting init");
+			return -EINVAL;
+		}
+	}
+
+	if (dump_sensor)
+		s5k83a_dump_registers(sd);
+
+	return (err < 0) ? err : 0;
+}
+
+int s5k83a_power_down(struct sd *sd)
+{
+	return 0;
+}
+
+void s5k83a_dump_registers(struct sd *sd)
+{
+	int address;
+	u8 page, old_page;
+	s5k83a_read_sensor(sd, S5K83A_PAGE_MAP, &old_page, 1);
+
+	for (page = 0; page < 16; page++) {
+		s5k83a_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);
+			info("register 0x%x contains 0x%x",
+			     address, val);
+		}
+	}
+	info("s5k83a register state dump complete");
+
+	for (page = 0; page < 16; page++) {
+		s5k83a_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);
+
+			if (ctrl_val == test_val)
+				info("register 0x%x is writeable", address);
+			else
+				info("register 0x%x is read only", address);
+
+			/* Restore original val */
+			s5k83a_write_sensor(sd, address, &old_val, 1);
+		}
+	}
+	info("Read/write register probing complete");
+	s5k83a_write_sensor(sd, S5K83A_PAGE_MAP, &old_page, 1);
+}
+
+int s5k83a_get_brightness(struct gspca_dev *gspca_dev, __s32 *val)
+{
+	int err;
+	u8 data[2];
+	struct sd *sd = (struct sd *) gspca_dev;
+
+	err = s5k83a_read_sensor(sd, S5K83A_BRIGHTNESS, data, 2);
+	data[1] = data[1] << 1;
+	*val = data[1];
+
+	return (err < 0) ? err : 0;
+}
+
+int s5k83a_set_brightness(struct gspca_dev *gspca_dev, __s32 val)
+{
+	int err;
+	u8 data[2];
+	struct sd *sd = (struct sd *) gspca_dev;
+
+	data[0] = 0x00;
+	data[1] = 0x20;
+	err = s5k83a_write_sensor(sd, 0x14, data, 2);
+	if (err < 0)
+		return err;
+
+	data[0] = 0x01;
+	data[1] = 0x00;
+	err = s5k83a_write_sensor(sd, 0x0d, data, 2);
+	if (err < 0)
+		return err;
+
+	/* 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);
+
+	return (err < 0) ? err : 0;
+}
+
+int s5k83a_get_whiteness(struct gspca_dev *gspca_dev, __s32 *val)
+{
+	int err;
+	u8 data;
+	struct sd *sd = (struct sd *) gspca_dev;
+
+	err = s5k83a_read_sensor(sd, S5K83A_WHITENESS, &data, 1);
+
+	*val = data;
+	return (err < 0) ? err : 0;
+}
+
+int s5k83a_set_whiteness(struct gspca_dev *gspca_dev, __s32 val)
+{
+	int err;
+	u8 data[1];
+	struct sd *sd = (struct sd *) gspca_dev;
+
+	data[0] = val;
+	err = s5k83a_write_sensor(sd, S5K83A_WHITENESS, data, 1);
+
+	return (err < 0) ? err : 0;
+}
+
+int s5k83a_get_gain(struct gspca_dev *gspca_dev, __s32 *val)
+{
+	int err;
+	u8 data[2];
+	struct sd *sd = (struct sd *) gspca_dev;
+
+	err = s5k83a_read_sensor(sd, S5K83A_GAIN, data, 2);
+
+	data[1] = data[1] & 0x3f;
+	if (data[1] > S5K83A_MAXIMUM_GAIN)
+		data[1] = S5K83A_MAXIMUM_GAIN;
+
+	*val = data[1];
+
+	return (err < 0) ? err : 0;
+}
+
+int s5k83a_set_gain(struct gspca_dev *gspca_dev, __s32 val)
+{
+	int err = 0;
+	u8 data[2];
+	struct sd *sd = (struct sd *) gspca_dev;
+
+	data[0] = 0;
+	data[1] = val;
+	err = s5k83a_write_sensor(sd, S5K83A_GAIN, data, 2);
+
+	return (err < 0) ? err : 0;
+}