1 files changed, 56 insertions, 13 deletions

diff --git a/drivers/media/platform/qcom/camss/camss-csid.c b/drivers/media/platform/qcom/camss/camss-csid.c
index db960da560c9..cf543fa2c497 100644
--- a/drivers/media/platform/qcom/camss/camss-csid.c
+++ b/drivers/media/platform/qcom/camss/camss-csid.c
@@ -300,6 +300,47 @@ static const struct csid_format csid_formats_8x96[] = {
 	}
 };
 
+static u32 csid_find_code(u32 *code, unsigned int n_code,
+			  unsigned int index, u32 req_code)
+{
+	int i;
+
+	if (!req_code && (index >= n_code))
+		return 0;
+
+	for (i = 0; i < n_code; i++)
+		if (req_code) {
+			if (req_code == code[i])
+				return req_code;
+		} else {
+			if (i == index)
+				return code[i];
+		}
+
+	return code[0];
+}
+
+static u32 csid_src_pad_code(struct csid_device *csid, u32 sink_code,
+			     unsigned int index, u32 src_req_code)
+{
+	if (csid->camss->version == CAMSS_8x16) {
+		if (index > 0)
+			return 0;
+
+		return sink_code;
+	} else if (csid->camss->version == CAMSS_8x96) {
+		switch (sink_code) {
+		default:
+			if (index > 0)
+				return 0;
+
+			return sink_code;
+		}
+	} else {
+		return 0;
+	}
+}
+
 static const struct csid_format *csid_get_fmt_entry(
 					const struct csid_format *formats,
 					unsigned int nformat,
@@ -674,15 +715,15 @@ static void csid_try_format(struct csid_device *csid,
 
 	case MSM_CSID_PAD_SRC:
 		if (csid->testgen_mode->cur.val == 0) {
-			/* Test generator is disabled, keep pad formats */
-			/* in sync - set and return a format same as sink pad */
-			struct v4l2_mbus_framefmt format;
+			/* Test generator is disabled, */
+			/* keep pad formats in sync */
+			u32 code = fmt->code;
 
-			format = *__csid_get_format(csid, cfg,
-						    MSM_CSID_PAD_SINK, which);
-			*fmt = format;
+			*fmt = *__csid_get_format(csid, cfg,
+						      MSM_CSID_PAD_SINK, which);
+			fmt->code = csid_src_pad_code(csid, fmt->code, 0, code);
 		} else {
-			/* Test generator is enabled, set format on source*/
+			/* Test generator is enabled, set format on source */
 			/* pad to allow test generator usage */
 
 			for (i = 0; i < csid->nformats; i++)
@@ -716,7 +757,6 @@ static int csid_enum_mbus_code(struct v4l2_subdev *sd,
 			       struct v4l2_subdev_mbus_code_enum *code)
 {
 	struct csid_device *csid = v4l2_get_subdevdata(sd);
-	struct v4l2_mbus_framefmt *format;
 
 	if (code->pad == MSM_CSID_PAD_SINK) {
 		if (code->index >= csid->nformats)
@@ -725,13 +765,16 @@ static int csid_enum_mbus_code(struct v4l2_subdev *sd,
 		code->code = csid->formats[code->index].code;
 	} else {
 		if (csid->testgen_mode->cur.val == 0) {
-			if (code->index > 0)
-				return -EINVAL;
+			struct v4l2_mbus_framefmt *sink_fmt;
 
-			format = __csid_get_format(csid, cfg, MSM_CSID_PAD_SINK,
-						   code->which);
+			sink_fmt = __csid_get_format(csid, cfg,
+						     MSM_CSID_PAD_SINK,
+						     code->which);
 
-			code->code = format->code;
+			code->code = csid_src_pad_code(csid, sink_fmt->code,
+						       code->index, 0);
+			if (!code->code)
+				return -EINVAL;
 		} else {
 			if (code->index >= csid->nformats)
 				return -EINVAL;