media: ov5640: fix frame interval enumeration

Driver must reject frame interval enumeration of unsupported resolution.
This was detected by v4l2-compliance format ioctl test:
v4l2-compliance Format ioctls:
    info: found 2 frameintervals for pixel format 4745504a and size 176x144
  fail: v4l2-test-formats.cpp(123):
                           found frame intervals for invalid size 177x144
    test VIDIOC_ENUM_FMT/FRAMESIZES/FRAMEINTERVALS: FAIL

Signed-off-by: Hugues Fruchet <hugues.fruchet@st.com>
Signed-off-by: Sakari Ailus <sakari.ailus@linux.intel.com>
Signed-off-by: Mauro Carvalho Chehab <mchehab+samsung@kernel.org>

1 files changed, 16 insertions, 18 deletions

diff --git a/drivers/media/i2c/ov5640.c b/drivers/media/i2c/ov5640.c
index e0a6264a6a12..1ecbb7a4a2ee 100644
--- a/drivers/media/i2c/ov5640.c
+++ b/drivers/media/i2c/ov5640.c
@@ -1413,24 +1413,16 @@ static const struct ov5640_mode_info *
 ov5640_find_mode(struct ov5640_dev *sensor, enum ov5640_frame_rate fr,
 		 int width, int height, bool nearest)
 {
-	const struct ov5640_mode_info *mode = NULL;
-	int i;
-
-	for (i = OV5640_NUM_MODES - 1; i >= 0; i--) {
-		mode = &ov5640_mode_data[fr][i];
-
-		if (!mode->reg_data)
-			continue;
+	const struct ov5640_mode_info *mode;
 
-		if ((nearest && mode->hact <= width &&
-		     mode->vact <= height) ||
-		    (!nearest && mode->hact == width &&
-		     mode->vact == height))
-			break;
-	}
+	mode = v4l2_find_nearest_size(ov5640_mode_data[fr],
+				      ARRAY_SIZE(ov5640_mode_data[fr]),
+				      hact, vact,
+				      width, height);
 
-	if (nearest && i < 0)
-		mode = &ov5640_mode_data[fr][0];
+	if (!mode ||
+	    (!nearest && (mode->hact != width || mode->vact != height)))
+		return NULL;
 
 	return mode;
 }
@@ -2509,8 +2501,14 @@ static int ov5640_s_frame_interval(struct v4l2_subdev *sd,
 
 	sensor->current_fr = frame_rate;
 	sensor->frame_interval = fi->interval;
-	sensor->current_mode = ov5640_find_mode(sensor, frame_rate, mode->hact,
-						mode->vact, true);
+	mode = ov5640_find_mode(sensor, frame_rate, mode->hact,
+				mode->vact, true);
+	if (!mode) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	sensor->current_mode = mode;
 	sensor->pending_mode_change = true;
 out:
 	mutex_unlock(&sensor->lock);