From 7580dc7daa2ac363c20674545be200802e4f6dce Mon Sep 17 00:00:00 2001
From: John Cox <jc@kynesim.co.uk>
Date: Wed, 1 Sep 2021 16:34:50 +0100
Subject: [PATCH] media: rpivid: Avoid returning EINVAL to a G_FMT
 ioctl

V4L2 spec says that G/S/TRY_FMT IOCTLs should never return errors for
anything other than wrong buffer types. Improve the capture format
function such that this is so and unsupported values get converted
to supported ones properly.

Signed-off-by: John Cox <jc@kynesim.co.uk>
---
 drivers/staging/media/rpivid/rpivid.c       |  1 -
 drivers/staging/media/rpivid/rpivid.h       |  2 -
 drivers/staging/media/rpivid/rpivid_video.c | 99 +++++++++++----------
 drivers/staging/media/rpivid/rpivid_video.h |  3 +-
 4 files changed, 54 insertions(+), 51 deletions(-)

--- a/drivers/staging/media/rpivid/rpivid.c
+++ b/drivers/staging/media/rpivid/rpivid.c
@@ -249,7 +249,6 @@ static int rpivid_open(struct file *file
 	/* The only bit of format info that we can guess now is H265 src
 	 * Everything else we need more info for
 	 */
-	ctx->src_fmt.pixelformat = RPIVID_SRC_PIXELFORMAT_DEFAULT;
 	rpivid_prepare_src_format(&ctx->src_fmt);
 
 	v4l2_fh_add(&ctx->fh);
--- a/drivers/staging/media/rpivid/rpivid.h
+++ b/drivers/staging/media/rpivid/rpivid.h
@@ -35,8 +35,6 @@
 
 #define RPIVID_QUIRK_NO_DMA_OFFSET	BIT(0)
 
-#define RPIVID_SRC_PIXELFORMAT_DEFAULT	V4L2_PIX_FMT_HEVC_SLICE
-
 enum rpivid_irq_status {
 	RPIVID_IRQ_NONE,
 	RPIVID_IRQ_ERROR,
--- a/drivers/staging/media/rpivid/rpivid_video.c
+++ b/drivers/staging/media/rpivid/rpivid_video.c
@@ -27,6 +27,8 @@
 
 #define RPIVID_MIN_WIDTH	16U
 #define RPIVID_MIN_HEIGHT	16U
+#define RPIVID_DEFAULT_WIDTH	1920U
+#define RPIVID_DEFAULT_HEIGHT	1088U
 #define RPIVID_MAX_WIDTH	4096U
 #define RPIVID_MAX_HEIGHT	4096U
 
@@ -70,25 +72,22 @@ size_t rpivid_bit_buf_size(unsigned int
 	return rpivid_round_up_size(bits_alloc);
 }
 
-int rpivid_prepare_src_format(struct v4l2_pix_format_mplane *pix_fmt)
+void rpivid_prepare_src_format(struct v4l2_pix_format_mplane *pix_fmt)
 {
 	size_t size;
 	u32 w;
 	u32 h;
 
-	if (pix_fmt->pixelformat != V4L2_PIX_FMT_HEVC_SLICE)
-		return -EINVAL;
-
 	w = pix_fmt->width;
 	h = pix_fmt->height;
 	if (!w || !h) {
-		w = 1920;
-		h = 1080;
+		w = RPIVID_DEFAULT_WIDTH;
+		h = RPIVID_DEFAULT_HEIGHT;
 	}
-	if (w > 4096)
-		w = 4096;
-	if (h > 4096)
-		h = 4096;
+	if (w > RPIVID_MAX_WIDTH)
+		w = RPIVID_MAX_WIDTH;
+	if (h > RPIVID_MAX_HEIGHT)
+		h = RPIVID_MAX_HEIGHT;
 
 	if (!pix_fmt->plane_fmt[0].sizeimage ||
 	    pix_fmt->plane_fmt[0].sizeimage > SZ_32M) {
@@ -98,6 +97,7 @@ int rpivid_prepare_src_format(struct v4l
 	/* Set a minimum */
 	size = max_t(u32, SZ_4K, pix_fmt->plane_fmt[0].sizeimage);
 
+	pix_fmt->pixelformat = V4L2_PIX_FMT_HEVC_SLICE;
 	pix_fmt->width = w;
 	pix_fmt->height = h;
 	pix_fmt->num_planes = 1;
@@ -105,22 +105,33 @@ int rpivid_prepare_src_format(struct v4l
 	/* Zero bytes per line for encoded source. */
 	pix_fmt->plane_fmt[0].bytesperline = 0;
 	pix_fmt->plane_fmt[0].sizeimage = size;
-
-	return 0;
 }
 
-int rpivid_prepare_dst_format(struct v4l2_pix_format_mplane *pix_fmt)
+/* Take any pix_format and make it valid */
+static void rpivid_prepare_dst_format(struct v4l2_pix_format_mplane *pix_fmt)
 {
 	unsigned int width = pix_fmt->width;
 	unsigned int height = pix_fmt->height;
 	unsigned int sizeimage = pix_fmt->plane_fmt[0].sizeimage;
 	unsigned int bytesperline = pix_fmt->plane_fmt[0].bytesperline;
 
-	switch (pix_fmt->pixelformat) {
+	if (!width)
+		width = RPIVID_DEFAULT_WIDTH;
+	if (width > RPIVID_MAX_WIDTH)
+		width = RPIVID_MAX_WIDTH;
+	if (!height)
+		height = RPIVID_DEFAULT_HEIGHT;
+	if (height > RPIVID_MAX_HEIGHT)
+		height = RPIVID_MAX_HEIGHT;
+
 	/* For column formats set bytesperline to column height (stride2) */
+	switch (pix_fmt->pixelformat) {
+	default:
+		pix_fmt->pixelformat = V4L2_PIX_FMT_NV12_COL128;
+		fallthrough;
 	case V4L2_PIX_FMT_NV12_COL128:
 		/* Width rounds up to columns */
-		width = ALIGN(min(width, RPIVID_MAX_WIDTH), 128);
+		width = ALIGN(width, 128);
 
 		/* 16 aligned height - not sure we even need that */
 		height = ALIGN(height, 16);
@@ -140,7 +151,7 @@ int rpivid_prepare_dst_format(struct v4l
 		/* width in pixels (3 pels = 4 bytes) rounded to 128 byte
 		 * columns
 		 */
-		width = ALIGN(((min(width, RPIVID_MAX_WIDTH) + 2) / 3), 32) * 3;
+		width = ALIGN(((width + 2) / 3), 32) * 3;
 
 		/* 16-aligned height. */
 		height = ALIGN(height, 16);
@@ -157,9 +168,6 @@ int rpivid_prepare_dst_format(struct v4l
 		sizeimage = constrain2x(sizeimage,
 					bytesperline * width * 4 / 3);
 		break;
-
-	default:
-		return -EINVAL;
 	}
 
 	pix_fmt->width = width;
@@ -169,7 +177,6 @@ int rpivid_prepare_dst_format(struct v4l
 	pix_fmt->plane_fmt[0].bytesperline = bytesperline;
 	pix_fmt->plane_fmt[0].sizeimage = sizeimage;
 	pix_fmt->num_planes = 1;
-	return 0;
 }
 
 static int rpivid_querycap(struct file *file, void *priv,
@@ -260,14 +267,13 @@ static u32 pixelformat_from_sps(const st
 {
 	u32 pf = 0;
 
-	// Use width 0 as a signifier of unsetness
-	if (!is_sps_set(sps)) {
+	if (!is_sps_set(sps) || !rpivid_hevc_validate_sps(sps)) {
 		/* Treat this as an error? For now return both */
 		if (index == 0)
 			pf = V4L2_PIX_FMT_NV12_COL128;
 		else if (index == 1)
 			pf = V4L2_PIX_FMT_NV12_10_COL128;
-	} else if (index == 0 && rpivid_hevc_validate_sps(sps)) {
+	} else if (index == 0) {
 		if (sps->bit_depth_luma_minus8 == 0)
 			pf = V4L2_PIX_FMT_NV12_COL128;
 		else if (sps->bit_depth_luma_minus8 == 2)
@@ -282,11 +288,14 @@ rpivid_hevc_default_dst_fmt(struct rpivi
 {
 	const struct v4l2_ctrl_hevc_sps * const sps =
 		rpivid_find_control_data(ctx, V4L2_CID_MPEG_VIDEO_HEVC_SPS);
-	struct v4l2_pix_format_mplane pix_fmt = {
-		.width = sps->pic_width_in_luma_samples,
-		.height = sps->pic_height_in_luma_samples,
-		.pixelformat = pixelformat_from_sps(sps, 0)
-	};
+	struct v4l2_pix_format_mplane pix_fmt;
+
+	memset(&pix_fmt, 0, sizeof(pix_fmt));
+	if (is_sps_set(sps)) {
+		pix_fmt.width = sps->pic_width_in_luma_samples;
+		pix_fmt.height = sps->pic_height_in_luma_samples;
+		pix_fmt.pixelformat = pixelformat_from_sps(sps, 0);
+	}
 
 	rpivid_prepare_dst_format(&pix_fmt);
 	return pix_fmt;
@@ -315,14 +324,23 @@ static int rpivid_enum_fmt_vid_cap(struc
 	return 0;
 }
 
+/*
+ * get dst format - sets it to default if otherwise unset
+ * returns a pointer to the struct as a convienience
+ */
+static struct v4l2_pix_format_mplane *get_dst_fmt(struct rpivid_ctx *const ctx)
+{
+	if (!ctx->dst_fmt_set)
+		ctx->dst_fmt = rpivid_hevc_default_dst_fmt(ctx);
+	return &ctx->dst_fmt;
+}
+
 static int rpivid_g_fmt_vid_cap(struct file *file, void *priv,
 				struct v4l2_format *f)
 {
 	struct rpivid_ctx *ctx = rpivid_file2ctx(file);
 
-	if (!ctx->dst_fmt_set)
-		ctx->dst_fmt = rpivid_hevc_default_dst_fmt(ctx);
-	f->fmt.pix_mp = ctx->dst_fmt;
+	f->fmt.pix_mp = *get_dst_fmt(ctx);
 	return 0;
 }
 
@@ -358,31 +376,20 @@ static int rpivid_try_fmt_vid_cap(struct
 			break;
 	}
 
-	// If we can't use requested fmt then set to default
-	if (pixelformat == 0) {
-		pixelformat = pixelformat_from_sps(sps, 0);
-		// If we don't have a default then give up
-		if (pixelformat == 0)
-			return -EINVAL;
-	}
-
 	// We don't have any way of finding out colourspace so believe
 	// anything we are told - take anything set in src as a default
 	if (f->fmt.pix_mp.colorspace == V4L2_COLORSPACE_DEFAULT)
 		copy_color(&f->fmt.pix_mp, &ctx->src_fmt);
 
 	f->fmt.pix_mp.pixelformat = pixelformat;
-	return rpivid_prepare_dst_format(&f->fmt.pix_mp);
+	rpivid_prepare_dst_format(&f->fmt.pix_mp);
+	return 0;
 }
 
 static int rpivid_try_fmt_vid_out(struct file *file, void *priv,
 				  struct v4l2_format *f)
 {
-	if (rpivid_prepare_src_format(&f->fmt.pix_mp)) {
-		// Set default src format
-		f->fmt.pix_mp.pixelformat = RPIVID_SRC_PIXELFORMAT_DEFAULT;
-		rpivid_prepare_src_format(&f->fmt.pix_mp);
-	}
+	rpivid_prepare_src_format(&f->fmt.pix_mp);
 	return 0;
 }
 
@@ -474,7 +481,7 @@ static int rpivid_queue_setup(struct vb2
 	if (V4L2_TYPE_IS_OUTPUT(vq->type))
 		pix_fmt = &ctx->src_fmt;
 	else
-		pix_fmt = &ctx->dst_fmt;
+		pix_fmt = get_dst_fmt(ctx);
 
 	if (*nplanes) {
 		if (sizes[0] < pix_fmt->plane_fmt[0].sizeimage)
--- a/drivers/staging/media/rpivid/rpivid_video.h
+++ b/drivers/staging/media/rpivid/rpivid_video.h
@@ -28,7 +28,6 @@ int rpivid_queue_init(void *priv, struct
 size_t rpivid_bit_buf_size(unsigned int w, unsigned int h, unsigned int bits_minus8);
 size_t rpivid_round_up_size(const size_t x);
 
-int rpivid_prepare_src_format(struct v4l2_pix_format_mplane *pix_fmt);
-int rpivid_prepare_dst_format(struct v4l2_pix_format_mplane *pix_fmt);
+void rpivid_prepare_src_format(struct v4l2_pix_format_mplane *pix_fmt);
 
 #endif
