armbian_build/patch/kernel/archive/sunxi-5.18/patches.megous/media-ov5640-Fix-mode-setting-semantics.patch

From f49279e6f8fc849afc424ae5f3356e472618887a Mon Sep 17 00:00:00 2001
From: Ondrej Jirman <megi@xff.cz>
Date: Mon, 28 Mar 2022 18:14:04 +0200
Subject: [PATCH 057/515] media: ov5640: Fix mode setting semantics

Previously setting the format was not possible when the current
frame-rate exceeded the limit of the new format. When setting a format
the current frame-rate should be ignored completely and instead set to
the maximum. After that a frame-rate can be set within the limits of
the current format.
---
 drivers/media/i2c/ov5640.c | 168 ++++++++++++++-----------------------
 1 file changed, 65 insertions(+), 103 deletions(-)

diff --git a/drivers/media/i2c/ov5640.c b/drivers/media/i2c/ov5640.c
index e70fa2442411..81c355e6fa83 100644
--- a/drivers/media/i2c/ov5640.c
+++ b/drivers/media/i2c/ov5640.c
@@ -1557,28 +1557,6 @@ static int ov5640_set_virtual_channel(struct ov5640_dev *sensor)
 	return ov5640_write_reg(sensor, OV5640_REG_DEBUG_MODE, temp);
 }
 
-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;
-
-	mode = v4l2_find_nearest_size(ov5640_mode_data,
-				      ARRAY_SIZE(ov5640_mode_data),
-				      hact, vact,
-				      width, height);
-
-	if (!mode ||
-	    (!nearest && (mode->hact != width || mode->vact != height)))
-		return NULL;
-
-	/* Check to see if the current mode exceeds the max frame rate */
-	if (ov5640_framerates[fr] > ov5640_framerates[mode->max_fps])
-		return NULL;
-
-	return mode;
-}
-
 static u64 ov5640_calc_pixel_rate(struct ov5640_dev *sensor)
 {
 	u64 rate;
@@ -2222,46 +2200,6 @@ static int ov5640_s_power(struct v4l2_subdev *sd, int on)
 	return ret;
 }
 
-static int ov5640_try_frame_interval(struct ov5640_dev *sensor,
-				     struct v4l2_fract *fi,
-				     u32 width, u32 height)
-{
-	const struct ov5640_mode_info *mode;
-	enum ov5640_frame_rate rate = OV5640_2_FPS;
-	int minfps, maxfps, best_fps, fps;
-	int i;
-
-	minfps = ov5640_framerates[OV5640_2_FPS];
-	maxfps = ov5640_framerates[OV5640_60_FPS];
-
-	if (fi->numerator == 0) {
-		fi->denominator = maxfps;
-		fi->numerator = 1;
-		rate = OV5640_60_FPS;
-		goto find_mode;
-	}
-
-	fps = clamp_val(DIV_ROUND_CLOSEST(fi->denominator, fi->numerator),
-			minfps, maxfps);
-
-	best_fps = minfps;
-	for (i = 0; i < ARRAY_SIZE(ov5640_framerates); i++) {
-		int curr_fps = ov5640_framerates[i];
-
-		if (abs(curr_fps - fps) < abs(best_fps - fps)) {
-			best_fps = curr_fps;
-			rate = i;
-		}
-	}
-
-	fi->numerator = 1;
-	fi->denominator = best_fps;
-
-find_mode:
-	mode = ov5640_find_mode(sensor, rate, width, height, false);
-	return mode ? rate : -EINVAL;
-}
-
 static int ov5640_get_fmt(struct v4l2_subdev *sd,
 			  struct v4l2_subdev_state *sd_state,
 			  struct v4l2_subdev_format *format)
@@ -2287,18 +2225,19 @@ static int ov5640_get_fmt(struct v4l2_subdev *sd,
 	return 0;
 }
 
-static int ov5640_try_fmt_internal(struct v4l2_subdev *sd,
-				   struct v4l2_mbus_framefmt *fmt,
-				   enum ov5640_frame_rate fr,
+static int ov5640_try_fmt_internal(struct v4l2_mbus_framefmt *fmt,
 				   const struct ov5640_mode_info **new_mode)
 {
-	struct ov5640_dev *sensor = to_ov5640_dev(sd);
 	const struct ov5640_mode_info *mode;
 	int i;
 
-	mode = ov5640_find_mode(sensor, fr, fmt->width, fmt->height, true);
+	mode = v4l2_find_nearest_size(ov5640_mode_data,
+				      ARRAY_SIZE(ov5640_mode_data),
+				      hact, vact,
+				      fmt->width, fmt->height);
 	if (!mode)
 		return -EINVAL;
+
 	fmt->width = mode->hact;
 	fmt->height = mode->vact;
 
@@ -2339,28 +2278,29 @@ static int ov5640_set_fmt(struct v4l2_subdev *sd,
 		goto out;
 	}
 
-	ret = ov5640_try_fmt_internal(sd, mbus_fmt,
-				      sensor->current_fr, &new_mode);
+	ret = ov5640_try_fmt_internal(mbus_fmt, &new_mode);
 	if (ret)
 		goto out;
 
 	if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
 		*v4l2_subdev_get_try_format(sd, sd_state, 0) = *mbus_fmt;
-		goto out;
-	}
+	} else {
+		if (new_mode != sensor->current_mode) {
+			sensor->current_mode = new_mode;
+			sensor->pending_mode_change = true;
 
-	if (new_mode != sensor->current_mode) {
-		sensor->current_mode = new_mode;
-		sensor->pending_mode_change = true;
-	}
-	if (mbus_fmt->code != sensor->fmt.code)
-		sensor->pending_fmt_change = true;
+			// Reset frame rate to maximum
+			sensor->current_fr = sensor->current_mode->max_fps;
+		}
+
+		if (mbus_fmt->code != sensor->fmt.code)
+			sensor->pending_fmt_change = true;
 
-	/* update format even if code is unchanged, resolution might change */
-	sensor->fmt = *mbus_fmt;
+		sensor->fmt = *mbus_fmt;
 
-	__v4l2_ctrl_s_ctrl_int64(sensor->ctrls.pixel_rate,
-				 ov5640_calc_pixel_rate(sensor));
+		__v4l2_ctrl_s_ctrl_int64(sensor->ctrls.pixel_rate,
+					 ov5640_calc_pixel_rate(sensor));
+	}
 out:
 	mutex_unlock(&sensor->lock);
 	return ret;
@@ -2881,7 +2821,6 @@ static int ov5640_enum_frame_interval(
 {
 	struct ov5640_dev *sensor = to_ov5640_dev(sd);
 	struct v4l2_fract tpf;
-	int ret;
 
 	if (fie->pad != 0)
 		return -EINVAL;
@@ -2891,9 +2830,7 @@ static int ov5640_enum_frame_interval(
 	tpf.numerator = 1;
 	tpf.denominator = ov5640_framerates[fie->index];
 
-	ret = ov5640_try_frame_interval(sensor, &tpf,
-					fie->width, fie->height);
-	if (ret < 0)
+	if (fie->index > sensor->current_mode->max_fps)
 		return -EINVAL;
 
 	fie->interval = tpf;
@@ -2912,12 +2849,52 @@ static int ov5640_g_frame_interval(struct v4l2_subdev *sd,
 	return 0;
 }
 
+static enum ov5640_frame_rate ov5640_find_interval(struct ov5640_dev *sensor,
+						   struct v4l2_fract *fi)
+{
+	enum ov5640_frame_rate max_rate, rate;
+	int minfps, maxfps, best_fps, fps;
+	int i;
+
+	max_rate = sensor->current_mode->max_fps;
+	minfps = ov5640_framerates[OV5640_2_FPS];
+	maxfps = ov5640_framerates[max_rate];
+
+	if (fi->numerator == 0) {
+		fi->denominator = maxfps;
+		fi->numerator = 1;
+		return max_rate;
+	}
+
+	fps = clamp_val(DIV_ROUND_CLOSEST(fi->denominator, fi->numerator),
+			minfps, maxfps);
+
+	best_fps = minfps;
+	rate = OV5640_2_FPS;
+	for (i = 0; i < ARRAY_SIZE(ov5640_framerates); i++) {
+		int curr_fps;
+		if (i > max_rate)
+			break;
+
+		curr_fps = ov5640_framerates[i];
+		if (abs(curr_fps - fps) < abs(best_fps - fps)) {
+			best_fps = curr_fps;
+			rate = i;
+		}
+	}
+
+	fi->numerator = 1;
+	fi->denominator = best_fps;
+	return rate;
+}
+
 static int ov5640_s_frame_interval(struct v4l2_subdev *sd,
 				   struct v4l2_subdev_frame_interval *fi)
 {
 	struct ov5640_dev *sensor = to_ov5640_dev(sd);
 	const struct ov5640_mode_info *mode;
-	int frame_rate, ret = 0;
+	enum ov5640_frame_rate frame_rate;
+	int ret = 0;
 
 	if (fi->pad != 0)
 		return -EINVAL;
@@ -2931,26 +2908,11 @@ static int ov5640_s_frame_interval(struct v4l2_subdev *sd,
 
 	mode = sensor->current_mode;
 
-	frame_rate = ov5640_try_frame_interval(sensor, &fi->interval,
-					       mode->hact, mode->vact);
-	if (frame_rate < 0) {
-		/* Always return a valid frame interval value */
-		fi->interval = sensor->frame_interval;
-		goto out;
-	}
-
-	mode = ov5640_find_mode(sensor, frame_rate, mode->hact,
-				mode->vact, true);
-	if (!mode) {
-		ret = -EINVAL;
-		goto out;
-	}
+	frame_rate = ov5640_find_interval(sensor, &fi->interval);
 
-	if (mode != sensor->current_mode ||
-	    frame_rate != sensor->current_fr) {
+	if (frame_rate != sensor->current_fr) {
 		sensor->current_fr = frame_rate;
 		sensor->frame_interval = fi->interval;
-		sensor->current_mode = mode;
 		sensor->pending_mode_change = true;
 
 		__v4l2_ctrl_s_ctrl_int64(sensor->ctrls.pixel_rate,
-- 
2.35.3