1 files changed, 87 insertions, 2 deletions

diff --git a/drivers/media/video/mt9v011.c b/drivers/media/video/mt9v011.c
index c14bf47d6928..8de1316a276d 100644
--- a/drivers/media/video/mt9v011.c
+++ b/drivers/media/video/mt9v011.c
@@ -156,7 +156,7 @@ 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)
+static void calc_fps(struct v4l2_subdev *sd, u32 *numerator, u32 *denominator)
 {
         struct mt9v011 *core = to_mt9v011(sd);
         unsigned height, width, hblank, vblank, speed;
@@ -179,6 +179,51 @@ static void calc_fps(struct v4l2_subdev *sd)
 
         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)
@@ -207,7 +252,7 @@ static void set_res(struct v4l2_subdev *sd)
         mt9v011_write(sd, R03_MT9V011_HEIGHT, core->height);
         mt9v011_write(sd, R06_MT9V011_VBLANK, 508 - core->height);
 
-        calc_fps(sd);
+        calc_fps(sd, NULL, NULL);
 };
 
 static int mt9v011_reset(struct v4l2_subdev *sd, u32 val)
@@ -322,6 +367,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;
@@ -419,6 +502,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 = {