1 files changed, 212 insertions, 9 deletions

diff --git a/drivers/media/video/mt9v011.c b/drivers/media/video/mt9v011.c
index 1fe8fc9183a7..cc85f77a5706 100644
--- a/drivers/media/video/mt9v011.c
+++ b/drivers/media/video/mt9v011.c
@@ -8,6 +8,7 @@
 #include <linux/i2c.h>
 #include <linux/videodev2.h>
 #include <linux/delay.h>
+#include <asm/div64.h>
 #include <media/v4l2-device.h>
 #include "mt9v011.h"
 #include <media/v4l2-i2c-drv.h>
@@ -51,12 +52,34 @@ static struct v4l2_queryctrl mt9v011_qctrl[] = {
                 .step = 1,
                 .default_value = 0,
                 .flags = 0,
-        },
+        }, {
+                .id      = V4L2_CID_HFLIP,
+                .type    = V4L2_CTRL_TYPE_BOOLEAN,
+                .name    = "Mirror",
+                .minimum = 0,
+                .maximum = 1,
+                .step    = 1,
+                .default_value = 0,
+                .flags = 0,
+        }, {
+                .id      = V4L2_CID_VFLIP,
+                .type    = V4L2_CTRL_TYPE_BOOLEAN,
+                .name    = "Vflip",
+                .minimum = 0,
+                .maximum = 1,
+                .step    = 1,
+                .default_value = 0,
+                .flags = 0,
+        }, {
+        }
 };
 
 struct mt9v011 {
         struct v4l2_subdev sd;
         unsigned width, height;
+        unsigned xtal;
+        unsigned hflip:1;
+        unsigned vflip:1;
 
         u16 global_gain, red_bal, blue_bal;
 };
@@ -129,9 +152,8 @@ static const struct i2c_reg_value mt9v011_init_default[] = {
 
                 { R0A_MT9V011_CLK_SPEED, 0x0000 },
                 { R1E_MT9V011_DIGITAL_ZOOM,  0x0000 },
-                { R20_MT9V011_READ_MODE, 0x1000 },
 
-                { R07_MT9V011_OUT_CTRL, 0x000a },       /* chip enable */
+                { R07_MT9V011_OUT_CTRL, 0x0002 },       /* chip enable */
 };
 
 static void set_balance(struct v4l2_subdev *sd)
@@ -154,6 +176,76 @@ static void set_balance(struct v4l2_subdev *sd)
         mt9v011_write(sd, R2D_MT9V011_RED_GAIN, red_gain);
 }
 
+static void calc_fps(struct v4l2_subdev *sd, u32 *numerator, u32 *denominator)
+{
+        struct mt9v011 *core = to_mt9v011(sd);
+        unsigned height, width, hblank, vblank, speed;
+        unsigned row_time, t_time;
+        u64 frames_per_ms;
+        unsigned tmp;
+
+        height = mt9v011_read(sd, R03_MT9V011_HEIGHT);
+        width = mt9v011_read(sd, R04_MT9V011_WIDTH);
+        hblank = mt9v011_read(sd, R05_MT9V011_HBLANK);
+        vblank = mt9v011_read(sd, R06_MT9V011_VBLANK);
+        speed = mt9v011_read(sd, R0A_MT9V011_CLK_SPEED);
+
+        row_time = (width + 113 + hblank) * (speed + 2);
+        t_time = row_time * (height + vblank + 1);
+
+        frames_per_ms = core->xtal * 1000l;
+        do_div(frames_per_ms, t_time);
+        tmp = frames_per_ms;
+
+        v4l2_dbg(1, debug, sd, "Programmed to %u.%03u fps (%d pixel clcks)\n",
+                tmp / 1000, tmp % 1000, t_time);
+
+        if (numerator && denominator) {
+                *numerator = 1000;
+                *denominator = (u32)frames_per_ms;
+        }
+}
+
+static u16 calc_speed(struct v4l2_subdev *sd, u32 numerator, u32 denominator)
+{
+        struct mt9v011 *core = to_mt9v011(sd);
+        unsigned height, width, hblank, vblank;
+        unsigned row_time, line_time;
+        u64 t_time, speed;
+
+        /* Avoid bogus calculus */
+        if (!numerator || !denominator)
+                return 0;
+
+        height = mt9v011_read(sd, R03_MT9V011_HEIGHT);
+        width = mt9v011_read(sd, R04_MT9V011_WIDTH);
+        hblank = mt9v011_read(sd, R05_MT9V011_HBLANK);
+        vblank = mt9v011_read(sd, R06_MT9V011_VBLANK);
+
+        row_time = width + 113 + hblank;
+        line_time = height + vblank + 1;
+
+        t_time = core->xtal * ((u64)numerator);
+        /* round to the closest value */
+        t_time += denominator / 2;
+        do_div(t_time, denominator);
+
+        speed = t_time;
+        do_div(speed, row_time * line_time);
+
+        /* Avoid having a negative value for speed */
+        if (speed < 2)
+                speed = 0;
+        else
+                speed -= 2;
+
+        /* Avoid speed overflow */
+        if (speed > 15)
+                return 15;
+
+        return (u16)speed;
+}
+
 static void set_res(struct v4l2_subdev *sd)
 {
         struct mt9v011 *core = to_mt9v011(sd);
@@ -175,12 +267,28 @@ static void set_res(struct v4l2_subdev *sd)
         mt9v011_write(sd, R04_MT9V011_WIDTH, core->width);
         mt9v011_write(sd, R05_MT9V011_HBLANK, 771 - core->width);
 
-        vstart = 8 + (640 - core->height) / 2;
+        vstart = 8 + (480 - core->height) / 2;
         mt9v011_write(sd, R01_MT9V011_ROWSTART, vstart);
         mt9v011_write(sd, R03_MT9V011_HEIGHT, core->height);
         mt9v011_write(sd, R06_MT9V011_VBLANK, 508 - core->height);
+
+        calc_fps(sd, NULL, NULL);
 };
 
+static void set_read_mode(struct v4l2_subdev *sd)
+{
+        struct mt9v011 *core = to_mt9v011(sd);
+        unsigned mode = 0x1000;
+
+        if (core->hflip)
+                mode |= 0x4000;
+
+        if (core->vflip)
+                mode |= 0x8000;
+
+        mt9v011_write(sd, R20_MT9V011_READ_MODE, mode);
+}
+
 static int mt9v011_reset(struct v4l2_subdev *sd, u32 val)
 {
         int i;
@@ -191,6 +299,7 @@ static int mt9v011_reset(struct v4l2_subdev *sd, u32 val)
 
         set_balance(sd);
         set_res(sd);
+        set_read_mode(sd);
 
         return 0;
 };
@@ -211,10 +320,33 @@ static int mt9v011_g_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
         case V4L2_CID_BLUE_BALANCE:
                 ctrl->value = core->blue_bal;
                 return 0;
+        case V4L2_CID_HFLIP:
+                ctrl->value = core->hflip ? 1 : 0;
+                return 0;
+        case V4L2_CID_VFLIP:
+                ctrl->value = core->vflip ? 1 : 0;
+                return 0;
         }
         return -EINVAL;
 }
 
+static int mt9v011_queryctrl(struct v4l2_subdev *sd, struct v4l2_queryctrl *qc)
+{
+        int i;
+
+        v4l2_dbg(1, debug, sd, "queryctrl called\n");
+
+        for (i = 0; i < ARRAY_SIZE(mt9v011_qctrl); i++)
+                if (qc->id && qc->id == mt9v011_qctrl[i].id) {
+                        memcpy(qc, &(mt9v011_qctrl[i]),
+                               sizeof(*qc));
+                        return 0;
+                }
+
+        return -EINVAL;
+}
+
+
 static int mt9v011_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
 {
         struct mt9v011 *core = to_mt9v011(sd);
@@ -242,6 +374,14 @@ static int mt9v011_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
         case V4L2_CID_BLUE_BALANCE:
                 core->blue_bal = ctrl->value;
                 break;
+        case V4L2_CID_HFLIP:
+                core->hflip = ctrl->value;
+                set_read_mode(sd);
+                return 0;
+        case V4L2_CID_VFLIP:
+                core->vflip = ctrl->value;
+                set_read_mode(sd);
+                return 0;
         default:
                 return -EINVAL;
         }
@@ -276,6 +416,44 @@ static int mt9v011_try_fmt(struct v4l2_subdev *sd, struct v4l2_format *fmt)
         return 0;
 }
 
+static int mt9v011_g_parm(struct v4l2_subdev *sd, struct v4l2_streamparm *parms)
+{
+        struct v4l2_captureparm *cp = &parms->parm.capture;
+
+        if (parms->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+                return -EINVAL;
+
+        memset(cp, 0, sizeof(struct v4l2_captureparm));
+        cp->capability = V4L2_CAP_TIMEPERFRAME;
+        calc_fps(sd,
+                 &cp->timeperframe.numerator,
+                 &cp->timeperframe.denominator);
+
+        return 0;
+}
+
+static int mt9v011_s_parm(struct v4l2_subdev *sd, struct v4l2_streamparm *parms)
+{
+        struct v4l2_captureparm *cp = &parms->parm.capture;
+        struct v4l2_fract *tpf = &cp->timeperframe;
+        u16 speed;
+
+        if (parms->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+                return -EINVAL;
+        if (cp->extendedmode != 0)
+                return -EINVAL;
+
+        speed = calc_speed(sd, tpf->numerator, tpf->denominator);
+
+        mt9v011_write(sd, R0A_MT9V011_CLK_SPEED, speed);
+        v4l2_dbg(1, debug, sd, "Setting speed to %d\n", speed);
+
+        /* Recalculate and update fps info */
+        calc_fps(sd, &tpf->numerator, &tpf->denominator);
+
+        return 0;
+}
+
 static int mt9v011_s_fmt(struct v4l2_subdev *sd, struct v4l2_format *fmt)
 {
         struct v4l2_pix_format *pix = &fmt->fmt.pix;
@@ -294,6 +472,22 @@ static int mt9v011_s_fmt(struct v4l2_subdev *sd, struct v4l2_format *fmt)
         return 0;
 }
 
+static int mt9v011_s_config(struct v4l2_subdev *sd, int dumb, void *data)
+{
+        struct mt9v011 *core = to_mt9v011(sd);
+        unsigned *xtal = data;
+
+        v4l2_dbg(1, debug, sd, "s_config called\n");
+
+        if (xtal) {
+                core->xtal = *xtal;
+                v4l2_dbg(1, debug, sd, "xtal set to %d.%03d MHz\n",
+                         *xtal / 1000000, (*xtal / 1000) % 1000);
+        }
+
+        return 0;
+}
+
 
 #ifdef CONFIG_VIDEO_ADV_DEBUG
 static int mt9v011_g_register(struct v4l2_subdev *sd,
@@ -331,16 +525,21 @@ static int mt9v011_s_register(struct v4l2_subdev *sd,
 static int mt9v011_g_chip_ident(struct v4l2_subdev *sd,
                                 struct v4l2_dbg_chip_ident *chip)
 {
+        u16 version;
         struct i2c_client *client = v4l2_get_subdevdata(sd);
 
+        version = mt9v011_read(sd, R00_MT9V011_CHIP_VERSION);
+
         return v4l2_chip_ident_i2c_client(client, chip, V4L2_IDENT_MT9V011,
-                                          MT9V011_VERSION);
+                                          version);
 }
 
 static const struct v4l2_subdev_core_ops mt9v011_core_ops = {
+        .queryctrl = mt9v011_queryctrl,
         .g_ctrl = mt9v011_g_ctrl,
         .s_ctrl = mt9v011_s_ctrl,
         .reset = mt9v011_reset,
+        .s_config = mt9v011_s_config,
         .g_chip_ident = mt9v011_g_chip_ident,
 #ifdef CONFIG_VIDEO_ADV_DEBUG
         .g_register = mt9v011_g_register,
@@ -352,6 +551,8 @@ static const struct v4l2_subdev_video_ops mt9v011_video_ops = {
         .enum_fmt = mt9v011_enum_fmt,
         .try_fmt = mt9v011_try_fmt,
         .s_fmt = mt9v011_s_fmt,
+        .g_parm = mt9v011_g_parm,
+        .s_parm = mt9v011_s_parm,
 };
 
 static const struct v4l2_subdev_ops mt9v011_ops = {
@@ -385,8 +586,9 @@ static int mt9v011_probe(struct i2c_client *c,
 
         /* Check if the sensor is really a MT9V011 */
         version = mt9v011_read(sd, R00_MT9V011_CHIP_VERSION);
-        if (version != MT9V011_VERSION) {
-                v4l2_info(sd, "*** unknown micron chip detected (0x%04x.\n",
+        if ((version != MT9V011_VERSION) &&
+            (version != MT9V011_REV_B_VERSION)) {
+                v4l2_info(sd, "*** unknown micron chip detected (0x%04x).\n",
                           version);
                 kfree(core);
                 return -EINVAL;
@@ -395,9 +597,10 @@ static int mt9v011_probe(struct i2c_client *c,
         core->global_gain = 0x0024;
         core->width  = 640;
         core->height = 480;
+        core->xtal = 27000000;  /* Hz */
 
-        v4l_info(c, "chip found @ 0x%02x (%s)\n",
-                 c->addr << 1, c->adapter->name);
+        v4l_info(c, "chip found @ 0x%02x (%s - chip version 0x%04x)\n",
+                 c->addr << 1, c->adapter->name, version);
 
         return 0;
 }