V4L/DVB (10012): m5602: Start to unify read/write sensor functions

First step into unifying the read and write sensor functions

Signed-off-by: Erik Andrén <erik.andren@gmail.com>
Signed-off-by: Mauro Carvalho Chehab <mchehab@redhat.com>

7 files changed, 52 insertions, 0 deletions

diff --git a/drivers/media/video/gspca/m5602/m5602_core.c b/drivers/media/video/gspca/m5602/m5602_core.c
index b2a5ad4fea59..d69e3c4ff97d 100644
--- a/drivers/media/video/gspca/m5602/m5602_core.c
+++ b/drivers/media/video/gspca/m5602/m5602_core.c
@@ -80,6 +80,50 @@ int m5602_write_bridge(struct sd *sd, u8 address, u8 i2c_data)
         return (err < 0) ? err : 0;
 }
 
+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)
diff --git a/drivers/media/video/gspca/m5602/m5602_mt9m111.h b/drivers/media/video/gspca/m5602/m5602_mt9m111.h
index 0fe4d6327897..2c7b2a49c44f 100644
--- a/drivers/media/video/gspca/m5602/m5602_mt9m111.h
+++ b/drivers/media/video/gspca/m5602/m5602_mt9m111.h
@@ -104,6 +104,7 @@ static struct m5602_sensor mt9m111 = {
         .name = "MT9M111",
 
         .i2c_slave_id = 0xba,
+        .i2c_regW = 2,
 
         .probe = mt9m111_probe,
         .init = mt9m111_init,
diff --git a/drivers/media/video/gspca/m5602/m5602_ov9650.h b/drivers/media/video/gspca/m5602/m5602_ov9650.h
index 32fc440c5c13..293be3f7f761 100644
--- a/drivers/media/video/gspca/m5602/m5602_ov9650.h
+++ b/drivers/media/video/gspca/m5602/m5602_ov9650.h
@@ -153,6 +153,7 @@ 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,
         .power_down = ov9650_power_down,
diff --git a/drivers/media/video/gspca/m5602/m5602_po1030.h b/drivers/media/video/gspca/m5602/m5602_po1030.h
index 02072ccf9258..3a49d15bd355 100644
--- a/drivers/media/video/gspca/m5602/m5602_po1030.h
+++ b/drivers/media/video/gspca/m5602/m5602_po1030.h
@@ -150,6 +150,7 @@ static struct m5602_sensor po1030 = {
         .name = "PO1030",
 
         .i2c_slave_id = 0xdc,
+        .i2c_regW = 1,
 
         .probe = po1030_probe,
         .init = po1030_init,
diff --git a/drivers/media/video/gspca/m5602/m5602_s5k4aa.h b/drivers/media/video/gspca/m5602/m5602_s5k4aa.h
index 701d30292ac3..3d04ff9b264c 100644
--- a/drivers/media/video/gspca/m5602/m5602_s5k4aa.h
+++ b/drivers/media/video/gspca/m5602/m5602_s5k4aa.h
@@ -87,6 +87,7 @@ static struct m5602_sensor s5k4aa = {
         .init = s5k4aa_init,
         .power_down = s5k4aa_power_down,
         .i2c_slave_id = 0x5a,
+        .i2c_regW = 2,
         .nctrls = 4,
         .ctrls = {
         {
diff --git a/drivers/media/video/gspca/m5602/m5602_s5k83a.h b/drivers/media/video/gspca/m5602/m5602_s5k83a.h
index 793c967875d7..6a884d73ee79 100644
--- a/drivers/media/video/gspca/m5602/m5602_s5k83a.h
+++ b/drivers/media/video/gspca/m5602/m5602_s5k83a.h
@@ -69,6 +69,7 @@ static struct m5602_sensor s5k83a = {
         .init = s5k83a_init,
         .power_down = s5k83a_power_down,
         .i2c_slave_id = 0x5a,
+        .i2c_regW = 2,
         .nctrls = 5,
         .ctrls = {
         {
diff --git a/drivers/media/video/gspca/m5602/m5602_sensor.h b/drivers/media/video/gspca/m5602/m5602_sensor.h
index c0900023680e..23c0ef9489fe 100644
--- a/drivers/media/video/gspca/m5602/m5602_sensor.h
+++ b/drivers/media/video/gspca/m5602/m5602_sensor.h
@@ -49,6 +49,9 @@ 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);