1 files changed, 36 insertions, 54 deletions

diff --git a/drivers/media/platform/qcom/camss/camss-video.c b/drivers/media/platform/qcom/camss/camss-video.c
index 114c3ae4a4ab..bd9334af1c73 100644
--- a/drivers/media/platform/qcom/camss/camss-video.c
+++ b/drivers/media/platform/qcom/camss/camss-video.c
@@ -535,20 +535,38 @@ static int video_querycap(struct file *file, void *fh,
 	return 0;
 }
 
-/*
- *  Returns the index in the video->formats[] array of the element which
- *  has the "ndx"th unique value of pixelformat field.
- *  If not found (no more unique pixelformat's) returns -EINVAL.
- */
-static int video_get_unique_pixelformat_by_index(struct camss_video *video,
-						 int ndx)
+static int video_enum_fmt(struct file *file, void *fh, struct v4l2_fmtdesc *f)
 {
+	struct camss_video *video = video_drvdata(file);
 	int i, j, k;
+	u32 mcode = f->mbus_code;
 
-	/* find index "i" of "k"th unique pixelformat in formats array */
+	if (f->type != video->type)
+		return -EINVAL;
+
+	if (f->index >= video->nformats)
+		return -EINVAL;
+
+	/*
+	 * Find index "i" of "k"th unique pixelformat in formats array.
+	 *
+	 * If f->mbus_code passed to video_enum_fmt() is not zero, a device
+	 * with V4L2_CAP_IO_MC capability restricts enumeration to only the
+	 * pixel formats that can be produced from that media bus code.
+	 * This is implemented by skipping video->formats[] entries with
+	 * code != f->mbus_code (if f->mbus_code is not zero).
+	 * If the f->mbus_code passed to video_enum_fmt() is not supported,
+	 * -EINVAL is returned.
+	 * If f->mbus_code is zero, all the pixel formats are enumerated.
+	 */
 	k = -1;
 	for (i = 0; i < video->nformats; i++) {
+		if (mcode != 0 && video->formats[i].code != mcode)
+			continue;
+
 		for (j = 0; j < i; j++) {
+			if (mcode != 0 && video->formats[j].code != mcode)
+				continue;
 			if (video->formats[i].pixelformat ==
 					video->formats[j].pixelformat)
 				break;
@@ -557,53 +575,16 @@ static int video_get_unique_pixelformat_by_index(struct camss_video *video,
 		if (j == i)
 			k++;
 
-		if (k == ndx)
-			return i;
-	}
-
-	return -EINVAL;
-}
-
-/*
- *  Returns the index in the video->formats[] array of the element which
- *  has code equal to mcode.
- *  If not found returns -EINVAL.
- */
-static int video_get_pixelformat_by_mbus_code(struct camss_video *video,
-					      u32 mcode)
-{
-	int i;
-
-	for (i = 0; i < video->nformats; i++) {
-		if (video->formats[i].code == mcode)
-			return i;
-	}
-
-	return -EINVAL;
-}
-
-static int video_enum_fmt(struct file *file, void *fh, struct v4l2_fmtdesc *f)
-{
-	struct camss_video *video = video_drvdata(file);
-	int i;
-
-	if (f->type != video->type)
-		return -EINVAL;
-
-	if (f->index >= video->nformats)
-		return -EINVAL;
-
-	if (f->mbus_code) {
-		/* Each entry in formats[] table has unique mbus_code */
-		if (f->index > 0)
-			return -EINVAL;
-
-		i = video_get_pixelformat_by_mbus_code(video, f->mbus_code);
-	} else {
-		i = video_get_unique_pixelformat_by_index(video, f->index);
+		if (k == f->index)
+			break;
 	}
 
-	if (i < 0)
+	if (k < f->index)
+		/*
+		 * All the unique pixel formats matching the arguments
+		 * have been enumerated (k >= 0 and f->index > 0), or
+		 * no pixel formats match the non-zero f->mbus_code (k == -1).
+		 */
 		return -EINVAL;
 
 	f->pixelformat = video->formats[i].pixelformat;
@@ -970,7 +951,8 @@ int msm_video_register(struct camss_video *video, struct v4l2_device *v4l2_dev,
 			video->formats = formats_rdi_8x16;
 			video->nformats = ARRAY_SIZE(formats_rdi_8x16);
 		}
-	} else if (video->camss->version == CAMSS_8x96) {
+	} else if (video->camss->version == CAMSS_8x96 ||
+		   video->camss->version == CAMSS_660) {
 		if (is_pix) {
 			video->formats = formats_pix_8x96;
 			video->nformats = ARRAY_SIZE(formats_pix_8x96);