media: atomisp-mt9m114: use v4l2_find_nearest_size()

Instead of reinventing the wheel, use v4l2_find_nearest_size()
in order to get the closest resolution.

This should address a bug where the wrong resolution was
selected.

Signed-off-by: Mauro Carvalho Chehab <mchehab+huawei@kernel.org>

diff --git a/drivers/staging/media/atomisp/i2c/atomisp-mt9m114.c b/drivers/staging/media/atomisp/i2c/atomisp-mt9m114.c
index 49f4090..00d6842 100644 (file)
--- a/drivers/staging/media/atomisp/i2c/atomisp-mt9m114.c
+++ b/drivers/staging/media/atomisp/i2c/atomisp-mt9m114.c
@@ -579,107 +579,6 @@ static int mt9m114_s_power(struct v4l2_subdev *sd, int power)
        return mt9m114_init_common(sd);
 }
 
-/*
- * distance - calculate the distance
- * @res: resolution
- * @w: width
- * @h: height
- *
- * Get the gap between resolution and w/h.
- * res->width/height smaller than w/h wouldn't be considered.
- * Returns the value of gap or -1 if fail.
- */
-#define LARGEST_ALLOWED_RATIO_MISMATCH 600
-static int distance(struct mt9m114_res_struct const *res, u32 w, u32 h)
-{
-       unsigned int w_ratio;
-       unsigned int h_ratio;
-       int match;
-
-       if (w == 0)
-               return -1;
-       w_ratio = (res->width << 13) / w;
-       if (h == 0)
-               return -1;
-       h_ratio = (res->height << 13) / h;
-       if (h_ratio == 0)
-               return -1;
-       match   = abs(((w_ratio << 13) / h_ratio) - 8192);
-
-       if ((w_ratio < 8192) || (h_ratio < 8192) ||
-           (match > LARGEST_ALLOWED_RATIO_MISMATCH))
-               return -1;
-
-       return w_ratio + h_ratio;
-}
-
-/* Return the nearest higher resolution index */
-static int nearest_resolution_index(int w, int h)
-{
-       int i;
-       int idx = -1;
-       int dist;
-       int min_dist = INT_MAX;
-       const struct mt9m114_res_struct *tmp_res = NULL;
-
-       for (i = 0; i < ARRAY_SIZE(mt9m114_res); i++) {
-               tmp_res = &mt9m114_res[i];
-               dist = distance(tmp_res, w, h);
-               if (dist == -1)
-                       continue;
-               if (dist < min_dist) {
-                       min_dist = dist;
-                       idx = i;
-               }
-       }
-
-       return idx;
-}
-
-static int mt9m114_try_res(u32 *w, u32 *h)
-{
-       int idx = 0;
-
-       if ((*w > MT9M114_RES_960P_SIZE_H)
-           || (*h > MT9M114_RES_960P_SIZE_V)) {
-               *w = MT9M114_RES_960P_SIZE_H;
-               *h = MT9M114_RES_960P_SIZE_V;
-       } else {
-               idx = nearest_resolution_index(*w, *h);
-
-               /*
-                * nearest_resolution_index() doesn't return smaller
-                *  resolutions. If it fails, it means the requested
-                *  resolution is higher than wecan support. Fallback
-                *  to highest possible resolution in this case.
-                */
-               if (idx == -1)
-                       idx = ARRAY_SIZE(mt9m114_res) - 1;
-
-               *w = mt9m114_res[idx].width;
-               *h = mt9m114_res[idx].height;
-       }
-
-       return 0;
-}
-
-static struct mt9m114_res_struct *mt9m114_to_res(u32 w, u32 h)
-{
-       int  index;
-
-       for (index = 0; index < N_RES; index++) {
-               if ((mt9m114_res[index].width == w) &&
-                   (mt9m114_res[index].height == h))
-                       break;
-       }
-
-       /* No mode found */
-       if (index >= N_RES)
-               return NULL;
-
-       return &mt9m114_res[index];
-}
-
 static int mt9m114_res2size(struct v4l2_subdev *sd, int *h_size, int *v_size)
 {
        struct mt9m114_device *dev = to_mt9m114_sensor(sd);
@@ -829,7 +728,7 @@ static int mt9m114_set_fmt(struct v4l2_subdev *sd,
        struct v4l2_mbus_framefmt *fmt = &format->format;
        struct i2c_client *c = v4l2_get_subdevdata(sd);
        struct mt9m114_device *dev = to_mt9m114_sensor(sd);
-       struct mt9m114_res_struct *res_index;
+       struct mt9m114_res_struct *res;
        u32 width = fmt->width;
        u32 height = fmt->height;
        struct camera_mipi_info *mt9m114_info = NULL;
@@ -845,20 +744,21 @@ static int mt9m114_set_fmt(struct v4l2_subdev *sd,
        if (!mt9m114_info)
                return -EINVAL;
 
-       mt9m114_try_res(&width, &height);
+       res = v4l2_find_nearest_size(mt9m114_res,
+                                    ARRAY_SIZE(mt9m114_res), width,
+                                    height, fmt->width, fmt->height);
+       if (!res)
+               res = &mt9m114_res[N_RES - 1];
+
+       fmt->width = res->width;
+       fmt->height = res->height;
+
        if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
                sd_state->pads->try_fmt = *fmt;
                return 0;
        }
-       res_index = mt9m114_to_res(width, height);
-
-       /* Sanity check */
-       if (unlikely(!res_index)) {
-               WARN_ON(1);
-               return -EINVAL;
-       }
 
-       switch (res_index->res) {
+       switch (res->res) {
        case MT9M114_RES_736P:
                ret = mt9m114_write_reg_array(c, mt9m114_736P_init, NO_POLLING);
                ret += misensor_rmw_reg(c, MISENSOR_16BIT, MISENSOR_READ_MODE,
@@ -876,7 +776,7 @@ static int mt9m114_set_fmt(struct v4l2_subdev *sd,
                                        MISENSOR_R_MODE_MASK, MISENSOR_NORMAL_SET);
                break;
        default:
-               v4l2_err(sd, "set resolution: %d failed!\n", res_index->res);
+               v4l2_err(sd, "set resolution: %d failed!\n", res->res);
                return -EINVAL;
        }
 
@@ -890,7 +790,7 @@ static int mt9m114_set_fmt(struct v4l2_subdev *sd,
        if (mt9m114_set_suspend(sd))
                return -EINVAL;
 
-       if (dev->res != res_index->res) {
+       if (dev->res != res->res) {
                int index;
 
                /* Switch to different size */
@@ -922,7 +822,7 @@ static int mt9m114_set_fmt(struct v4l2_subdev *sd,
                }
        }
        ret = mt9m114_get_intg_factor(c, mt9m114_info,
-                                     &mt9m114_res[res_index->res]);
+                                     &mt9m114_res[res->res]);
        if (ret) {
                dev_err(&c->dev, "failed to get integration_factor\n");
                return -EINVAL;
@@ -931,7 +831,7 @@ static int mt9m114_set_fmt(struct v4l2_subdev *sd,
         * mt9m114 - we don't poll for context switch
         * because it does not happen with streaming disabled.
         */
-       dev->res = res_index->res;
+       dev->res = res->res;
 
        fmt->width = width;
        fmt->height = height;