[PATCH v5 39/39] media: imx: propagate sink pad formats to source pads

[Steve Longerbeam <slongerbeam@gmail.com>]

As part of this, separate format try code from *_set_fmt() into
*_try_fmt(), so that the latter function can be used to propagate
a legal format from sink to source. This also reduces subsequent
bloat in *_set_fmt().

imx-ic-prp never needed separate formats for sink and source pads,
so propagation in this case was easy, just have only a single
format shared by both pads.

Signed-off-by: Steve Longerbeam <steve_longerbeam@mentor.com>
---
 drivers/staging/media/imx/imx-ic-prp.c        |  31 +++---
 drivers/staging/media/imx/imx-ic-prpencvf.c   |  86 ++++++++++-----
 drivers/staging/media/imx/imx-media-capture.c |  12 ++
 drivers/staging/media/imx/imx-media-csi.c     | 152 ++++++++++++++++----------
 drivers/staging/media/imx/imx-media-vdic.c    |  72 +++++++-----
 5 files changed, 224 insertions(+), 129 deletions(-)

diff --git a/drivers/staging/media/imx/imx-ic-prp.c b/drivers/staging/media/imx/imx-ic-prp.c
index 83cd2b4..ec742e6 100644
--- a/drivers/staging/media/imx/imx-ic-prp.c
+++ b/drivers/staging/media/imx/imx-ic-prp.c
@@ -56,8 +56,7 @@ struct prp_priv {
 	/* the CSI id at link validate */
 	int csi_id;
 
-	struct v4l2_mbus_framefmt format_mbus[PRP_NUM_PADS];
-	const struct imx_media_pixfmt *cc[PRP_NUM_PADS];
+	struct v4l2_mbus_framefmt format_mbus;
 	struct v4l2_fract frame_interval;
 
 	bool stream_on; /* streaming is on */
@@ -98,7 +97,7 @@ __prp_get_fmt(struct prp_priv *priv, struct v4l2_subdev_pad_config *cfg,
 	if (which == V4L2_SUBDEV_FORMAT_TRY)
 		return v4l2_subdev_get_try_format(&ic_priv->sd, cfg, pad);
 	else
-		return &priv->format_mbus[pad];
+		return &priv->format_mbus;
 }
 
 /*
@@ -167,7 +166,7 @@ static int prp_set_fmt(struct v4l2_subdev *sd,
 		       struct v4l2_subdev_format *sdformat)
 {
 	struct prp_priv *priv = sd_to_priv(sd);
-	const struct imx_media_pixfmt *cc = NULL;
+	const struct imx_media_pixfmt *cc;
 	struct v4l2_mbus_framefmt *infmt;
 	int ret = 0;
 	u32 code;
@@ -201,17 +200,14 @@ static int prp_set_fmt(struct v4l2_subdev *sd,
 		/* Output pads mirror input pad */
 		infmt = __prp_get_fmt(priv, cfg, PRP_SINK_PAD,
 				      sdformat->which);
-		cc = imx_media_find_ipu_format(infmt->code, CS_SEL_ANY);
 		sdformat->format = *infmt;
 		break;
 	}
 
-	if (sdformat->which == V4L2_SUBDEV_FORMAT_TRY) {
+	if (sdformat->which == V4L2_SUBDEV_FORMAT_TRY)
 		cfg->try_fmt = sdformat->format;
-	} else {
-		priv->format_mbus[sdformat->pad] = sdformat->format;
-		priv->cc[sdformat->pad] = cc;
-	}
+	else
+		priv->format_mbus = sdformat->format;
 
 out:
 	mutex_unlock(&priv->lock);
@@ -427,20 +423,19 @@ static int prp_registered(struct v4l2_subdev *sd)
 	for (i = 0; i < PRP_NUM_PADS; i++) {
 		priv->pad[i].flags = (i == PRP_SINK_PAD) ?
 			MEDIA_PAD_FL_SINK : MEDIA_PAD_FL_SOURCE;
-
-		/* set a default mbus format  */
-		imx_media_enum_ipu_format(&code, 0, CS_SEL_YUV);
-		ret = imx_media_init_mbus_fmt(&priv->format_mbus[i],
-					      640, 480, code, V4L2_FIELD_NONE,
-					      &priv->cc[i]);
-		if (ret)
-			return ret;
 	}
 
 	/* init default frame interval */
 	priv->frame_interval.numerator = 1;
 	priv->frame_interval.denominator = 30;
 
+	/* set a default mbus format  */
+	imx_media_enum_ipu_format(&code, 0, CS_SEL_YUV);
+	ret = imx_media_init_mbus_fmt(&priv->format_mbus, 640, 480, code,
+				      V4L2_FIELD_NONE, NULL);
+	if (ret)
+		return ret;
+
 	return media_entity_pads_init(&sd->entity, PRP_NUM_PADS, priv->pad);
 }
 
diff --git a/drivers/staging/media/imx/imx-ic-prpencvf.c b/drivers/staging/media/imx/imx-ic-prpencvf.c
index b42103c..644dd33 100644
--- a/drivers/staging/media/imx/imx-ic-prpencvf.c
+++ b/drivers/staging/media/imx/imx-ic-prpencvf.c
@@ -753,35 +753,23 @@ static int prp_get_fmt(struct v4l2_subdev *sd,
 	return ret;
 }
 
-static int prp_set_fmt(struct v4l2_subdev *sd,
-		       struct v4l2_subdev_pad_config *cfg,
-		       struct v4l2_subdev_format *sdformat)
+static void prp_try_fmt(struct prp_priv *priv,
+			struct v4l2_subdev_pad_config *cfg,
+			struct v4l2_subdev_format *sdformat,
+			const struct imx_media_pixfmt **cc)
 {
-	struct prp_priv *priv = sd_to_priv(sd);
-	const struct imx_media_pixfmt *cc;
-	struct v4l2_mbus_framefmt *infmt;
-	int ret = 0;
-	u32 code;
-
-	if (sdformat->pad >= PRPENCVF_NUM_PADS)
-		return -EINVAL;
-
-	mutex_lock(&priv->lock);
+	*cc = imx_media_find_ipu_format(sdformat->format.code, CS_SEL_ANY);
+	if (!*cc) {
+		u32 code;
 
-	if (priv->stream_on) {
-		ret = -EBUSY;
-		goto out;
-	}
-
-	cc = imx_media_find_ipu_format(sdformat->format.code, CS_SEL_ANY);
-	if (!cc) {
 		imx_media_enum_ipu_format(&code, 0, CS_SEL_ANY);
-		cc = imx_media_find_ipu_format(code, CS_SEL_ANY);
-		sdformat->format.code = cc->codes[0];
+		*cc = imx_media_find_ipu_format(code, CS_SEL_ANY);
+		sdformat->format.code = (*cc)->codes[0];
 	}
 
 	if (sdformat->pad == PRPENCVF_SRC_PAD) {
-		infmt = __prp_get_fmt(priv, cfg, PRPENCVF_SINK_PAD,
+		struct v4l2_mbus_framefmt *infmt =
+			__prp_get_fmt(priv, cfg, PRPENCVF_SINK_PAD,
 				      sdformat->which);
 
 		if (sdformat->format.field != V4L2_FIELD_NONE)
@@ -809,14 +797,60 @@ static int prp_set_fmt(struct v4l2_subdev *sd,
 				      MIN_H_SINK, MAX_H_SINK, H_ALIGN_SINK,
 				      S_ALIGN);
 	}
+}
+
+static int prp_set_fmt(struct v4l2_subdev *sd,
+		       struct v4l2_subdev_pad_config *cfg,
+		       struct v4l2_subdev_format *sdformat)
+{
+	struct prp_priv *priv = sd_to_priv(sd);
+	struct imx_media_video_dev *vdev = priv->vdev;
+	const struct imx_media_pixfmt *cc;
+	struct v4l2_pix_format vdev_fmt;
+	int ret = 0;
+
+	if (sdformat->pad >= PRPENCVF_NUM_PADS)
+		return -EINVAL;
+
+	mutex_lock(&priv->lock);
+
+	if (priv->stream_on) {
+		ret = -EBUSY;
+		goto out;
+	}
+
+	prp_try_fmt(priv, cfg, sdformat, &cc);
 
 	if (sdformat->which == V4L2_SUBDEV_FORMAT_TRY) {
 		cfg->try_fmt = sdformat->format;
-	} else {
-		priv->format_mbus[sdformat->pad] = sdformat->format;
-		priv->cc[sdformat->pad] = cc;
+		goto out;
+	}
+
+	priv->format_mbus[sdformat->pad] = sdformat->format;
+	priv->cc[sdformat->pad] = cc;
+
+	/* propagate a default format to source pad */
+	if (sdformat->pad == PRPENCVF_SINK_PAD) {
+		const struct imx_media_pixfmt *outcc;
+		struct v4l2_subdev_format format;
+
+		format.pad = PRPENCVF_SRC_PAD;
+		format.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+		format.format = sdformat->format;
+		prp_try_fmt(priv, cfg, &format, &outcc);
+
+		priv->format_mbus[PRPENCVF_SRC_PAD] = format.format;
+		priv->cc[PRPENCVF_SRC_PAD] = outcc;
 	}
 
+	/* propagate output pad format to capture device */
+	imx_media_mbus_fmt_to_pix_fmt(&vdev_fmt,
+				      &priv->format_mbus[PRPENCVF_SRC_PAD],
+				      priv->cc[PRPENCVF_SRC_PAD]);
+	mutex_unlock(&priv->lock);
+	imx_media_capture_device_set_format(vdev, &vdev_fmt);
+
+	return 0;
 out:
 	mutex_unlock(&priv->lock);
 	return ret;
diff --git a/drivers/staging/media/imx/imx-media-capture.c b/drivers/staging/media/imx/imx-media-capture.c
index a757e05..ee91439 100644
--- a/drivers/staging/media/imx/imx-media-capture.c
+++ b/drivers/staging/media/imx/imx-media-capture.c
@@ -498,6 +498,18 @@ static struct video_device capture_videodev = {
 	.device_caps	= V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING,
 };
 
+void imx_media_capture_device_set_format(struct imx_media_video_dev *vdev,
+					 struct v4l2_pix_format *pix)
+{
+	struct capture_priv *priv = to_capture_priv(vdev);
+
+	mutex_lock(&priv->mutex);
+	priv->vdev.fmt.fmt.pix = *pix;
+	priv->vdev.cc = imx_media_find_format(pix->pixelformat, CS_SEL_ANY);
+	mutex_unlock(&priv->mutex);
+}
+EXPORT_SYMBOL_GPL(imx_media_capture_device_set_format);
+
 struct imx_media_buffer *
 imx_media_capture_device_next_buf(struct imx_media_video_dev *vdev)
 {
diff --git a/drivers/staging/media/imx/imx-media-csi.c b/drivers/staging/media/imx/imx-media-csi.c
index cf070be..fc0036a 100644
--- a/drivers/staging/media/imx/imx-media-csi.c
+++ b/drivers/staging/media/imx/imx-media-csi.c
@@ -986,11 +986,11 @@ __csi_get_fmt(struct csi_priv *priv, struct v4l2_subdev_pad_config *cfg,
 		return &priv->format_mbus[pad];
 }
 
-static int csi_try_crop(struct csi_priv *priv,
-			struct v4l2_rect *crop,
-			struct v4l2_subdev_pad_config *cfg,
-			struct v4l2_mbus_framefmt *infmt,
-			struct imx_media_subdev *sensor)
+static void csi_try_crop(struct csi_priv *priv,
+			 struct v4l2_rect *crop,
+			 struct v4l2_subdev_pad_config *cfg,
+			 struct v4l2_mbus_framefmt *infmt,
+			 struct imx_media_subdev *sensor)
 {
 	struct v4l2_of_endpoint *sensor_ep;
 
@@ -1019,8 +1019,6 @@ static int csi_try_crop(struct csi_priv *priv,
 		if (crop->top + crop->height > infmt->height)
 			crop->top = infmt->height - crop->height;
 	}
-
-	return 0;
 }
 
 static int csi_enum_mbus_code(struct v4l2_subdev *sd,
@@ -1092,34 +1090,17 @@ static int csi_get_fmt(struct v4l2_subdev *sd,
 	return ret;
 }
 
-static int csi_set_fmt(struct v4l2_subdev *sd,
-		       struct v4l2_subdev_pad_config *cfg,
-		       struct v4l2_subdev_format *sdformat)
+static void csi_try_fmt(struct csi_priv *priv,
+			struct imx_media_subdev *sensor,
+			struct v4l2_subdev_pad_config *cfg,
+			struct v4l2_subdev_format *sdformat,
+			struct v4l2_rect *crop,
+			const struct imx_media_pixfmt **cc)
 {
-	struct csi_priv *priv = v4l2_get_subdevdata(sd);
-	const struct imx_media_pixfmt *cc, *incc;
+	const struct imx_media_pixfmt *incc;
 	struct v4l2_mbus_framefmt *infmt;
-	struct imx_media_subdev *sensor;
-	struct v4l2_rect crop;
-	int ret = 0;
 	u32 code;
 
-	if (sdformat->pad >= CSI_NUM_PADS)
-		return -EINVAL;
-
-	sensor = imx_media_find_sensor(priv->md, &priv->sd.entity);
-	if (IS_ERR(sensor)) {
-		v4l2_err(&priv->sd, "no sensor attached\n");
-		return PTR_ERR(sensor);
-	}
-
-	mutex_lock(&priv->lock);
-
-	if (priv->stream_on) {
-		ret = -EBUSY;
-		goto out;
-	}
-
 	switch (sdformat->pad) {
 	case CSI_SRC_PAD_DIRECT:
 	case CSI_SRC_PAD_IDMAC:
@@ -1140,17 +1121,17 @@ static int csi_set_fmt(struct v4l2_subdev *sd,
 
 		if (incc->bayer) {
 			sdformat->format.code = infmt->code;
-			cc = incc;
+			*cc = incc;
 		} else {
 			u32 cs_sel = (incc->cs == IPUV3_COLORSPACE_YUV) ?
 				CS_SEL_YUV : CS_SEL_RGB;
 
-			cc = imx_media_find_ipu_format(sdformat->format.code,
-						       cs_sel);
-			if (!cc) {
+			*cc = imx_media_find_ipu_format(sdformat->format.code,
+							cs_sel);
+			if (!*cc) {
 				imx_media_enum_ipu_format(&code, 0, cs_sel);
-				cc = imx_media_find_ipu_format(code, cs_sel);
-				sdformat->format.code = cc->codes[0];
+				*cc = imx_media_find_ipu_format(code, cs_sel);
+				sdformat->format.code = (*cc)->codes[0];
 			}
 		}
 
@@ -1172,39 +1153,92 @@ static int csi_set_fmt(struct v4l2_subdev *sd,
 		v4l_bound_align_image(&sdformat->format.width, MIN_W, MAX_W,
 				      W_ALIGN, &sdformat->format.height,
 				      MIN_H, MAX_H, H_ALIGN, S_ALIGN);
-		crop.left = 0;
-		crop.top = 0;
-		crop.width = sdformat->format.width;
-		crop.height = sdformat->format.height;
-		ret = csi_try_crop(priv, &crop, cfg, &sdformat->format, sensor);
-		if (ret)
-			goto out;
+		crop->left = 0;
+		crop->top = 0;
+		crop->width = sdformat->format.width;
+		crop->height = sdformat->format.height;
+		csi_try_crop(priv, crop, cfg, &sdformat->format, sensor);
 
-		cc = imx_media_find_mbus_format(sdformat->format.code,
-						CS_SEL_ANY, true);
-		if (!cc) {
+		*cc = imx_media_find_mbus_format(sdformat->format.code,
+						 CS_SEL_ANY, true);
+		if (!*cc) {
 			imx_media_enum_mbus_format(&code, 0,
 						   CS_SEL_ANY, false);
-			cc = imx_media_find_mbus_format(code,
+			*cc = imx_media_find_mbus_format(code,
 							CS_SEL_ANY, false);
-			sdformat->format.code = cc->codes[0];
+			sdformat->format.code = (*cc)->codes[0];
 		}
 		break;
-	default:
-		ret = -EINVAL;
+	}
+}
+
+static int csi_set_fmt(struct v4l2_subdev *sd,
+		       struct v4l2_subdev_pad_config *cfg,
+		       struct v4l2_subdev_format *sdformat)
+{
+	struct csi_priv *priv = v4l2_get_subdevdata(sd);
+	struct imx_media_video_dev *vdev = priv->vdev;
+	const struct imx_media_pixfmt *cc;
+	struct imx_media_subdev *sensor;
+	struct v4l2_pix_format vdev_fmt;
+	struct v4l2_rect crop;
+	int ret = 0;
+
+	if (sdformat->pad >= CSI_NUM_PADS)
+		return -EINVAL;
+
+	sensor = imx_media_find_sensor(priv->md, &priv->sd.entity);
+	if (IS_ERR(sensor)) {
+		v4l2_err(&priv->sd, "no sensor attached\n");
+		return PTR_ERR(sensor);
+	}
+
+	mutex_lock(&priv->lock);
+
+	if (priv->stream_on) {
+		ret = -EBUSY;
 		goto out;
 	}
 
+	csi_try_fmt(priv, sensor, cfg, sdformat, &crop, &cc);
+
 	if (sdformat->which == V4L2_SUBDEV_FORMAT_TRY) {
 		cfg->try_fmt = sdformat->format;
-	} else {
-		priv->format_mbus[sdformat->pad] = sdformat->format;
-		priv->cc[sdformat->pad] = cc;
-		/* Reset the crop window if this is the input pad */
-		if (sdformat->pad == CSI_SINK_PAD)
-			priv->crop = crop;
+		goto out;
 	}
 
+	priv->format_mbus[sdformat->pad] = sdformat->format;
+	priv->cc[sdformat->pad] = cc;
+
+	if (sdformat->pad == CSI_SINK_PAD) {
+		int pad;
+
+		/* reset the crop window */
+		priv->crop = crop;
+
+		/* propagate format to source pads */
+		for (pad = CSI_SINK_PAD + 1; pad < CSI_NUM_PADS; pad++) {
+			const struct imx_media_pixfmt *outcc;
+			struct v4l2_subdev_format format;
+
+			format.pad = pad;
+			format.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+			format.format = sdformat->format;
+			csi_try_fmt(priv, sensor, cfg, &format, &crop, &outcc);
+
+			priv->format_mbus[pad] = format.format;
+			priv->cc[pad] = outcc;
+		}
+	}
+
+	/* propagate IDMAC output pad format to capture device */
+	imx_media_mbus_fmt_to_pix_fmt(&vdev_fmt,
+				      &priv->format_mbus[CSI_SRC_PAD_IDMAC],
+				      priv->cc[CSI_SRC_PAD_IDMAC]);
+	mutex_unlock(&priv->lock);
+	imx_media_capture_device_set_format(vdev, &vdev_fmt);
+
+	return 0;
 out:
 	mutex_unlock(&priv->lock);
 	return ret;
@@ -1295,9 +1329,7 @@ static int csi_set_selection(struct v4l2_subdev *sd,
 	}
 
 	infmt = __csi_get_fmt(priv, cfg, CSI_SINK_PAD, sel->which);
-	ret = csi_try_crop(priv, &sel->r, cfg, infmt, sensor);
-	if (ret)
-		goto out;
+	csi_try_crop(priv, &sel->r, cfg, infmt, sensor);
 
 	if (sel->which == V4L2_SUBDEV_FORMAT_TRY) {
 		cfg->try_crop = sel->r;
diff --git a/drivers/staging/media/imx/imx-media-vdic.c b/drivers/staging/media/imx/imx-media-vdic.c
index 070f9da..58eda18 100644
--- a/drivers/staging/media/imx/imx-media-vdic.c
+++ b/drivers/staging/media/imx/imx-media-vdic.c
@@ -562,31 +562,20 @@ static int vdic_get_fmt(struct v4l2_subdev *sd,
 	return ret;
 }
 
-static int vdic_set_fmt(struct v4l2_subdev *sd,
-			struct v4l2_subdev_pad_config *cfg,
-			struct v4l2_subdev_format *sdformat)
+static void vdic_try_fmt(struct vdic_priv *priv,
+			 struct v4l2_subdev_pad_config *cfg,
+			 struct v4l2_subdev_format *sdformat,
+			 const struct imx_media_pixfmt **cc)
 {
-	struct vdic_priv *priv = v4l2_get_subdevdata(sd);
-	const struct imx_media_pixfmt *cc;
 	struct v4l2_mbus_framefmt *infmt;
-	int ret = 0;
-	u32 code;
-
-	if (sdformat->pad >= VDIC_NUM_PADS)
-		return -EINVAL;
-
-	mutex_lock(&priv->lock);
 
-	if (priv->stream_on) {
-		ret = -EBUSY;
-		goto out;
-	}
+	*cc = imx_media_find_ipu_format(sdformat->format.code, CS_SEL_YUV);
+	if (!*cc) {
+		u32 code;
 
-	cc = imx_media_find_ipu_format(sdformat->format.code, CS_SEL_YUV);
-	if (!cc) {
 		imx_media_enum_ipu_format(&code, 0, CS_SEL_YUV);
-		cc = imx_media_find_ipu_format(code, CS_SEL_YUV);
-		sdformat->format.code = cc->codes[0];
+		*cc = imx_media_find_ipu_format(code, CS_SEL_YUV);
+		sdformat->format.code = (*cc)->codes[0];
 	}
 
 	switch (sdformat->pad) {
@@ -609,18 +598,51 @@ static int vdic_set_fmt(struct v4l2_subdev *sd,
 		if (!V4L2_FIELD_HAS_BOTH(sdformat->format.field))
 			sdformat->format.field = V4L2_FIELD_SEQ_TB;
 		break;
-	default:
-		ret = -EINVAL;
+	}
+}
+
+static int vdic_set_fmt(struct v4l2_subdev *sd,
+			struct v4l2_subdev_pad_config *cfg,
+			struct v4l2_subdev_format *sdformat)
+{
+	struct vdic_priv *priv = v4l2_get_subdevdata(sd);
+	const struct imx_media_pixfmt *cc;
+	int ret = 0;
+
+	if (sdformat->pad >= VDIC_NUM_PADS)
+		return -EINVAL;
+
+	mutex_lock(&priv->lock);
+
+	if (priv->stream_on) {
+		ret = -EBUSY;
 		goto out;
 	}
 
+	vdic_try_fmt(priv, cfg, sdformat, &cc);
+
 	if (sdformat->which == V4L2_SUBDEV_FORMAT_TRY) {
 		cfg->try_fmt = sdformat->format;
-	} else {
-		priv->format_mbus[sdformat->pad] = sdformat->format;
-		priv->cc[sdformat->pad] = cc;
+		goto out;
 	}
 
+	priv->format_mbus[sdformat->pad] = sdformat->format;
+	priv->cc[sdformat->pad] = cc;
+
+	/* propagate format to source pad */
+	if (sdformat->pad == VDIC_SINK_PAD_DIRECT ||
+	    sdformat->pad == VDIC_SINK_PAD_IDMAC) {
+		const struct imx_media_pixfmt *outcc;
+		struct v4l2_subdev_format format;
+
+		format.pad = VDIC_SRC_PAD_DIRECT;
+		format.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+		format.format = sdformat->format;
+		vdic_try_fmt(priv, cfg, &format, &outcc);
+
+		priv->format_mbus[VDIC_SRC_PAD_DIRECT] = format.format;
+		priv->cc[VDIC_SRC_PAD_DIRECT] = outcc;
+	}
 out:
 	mutex_unlock(&priv->lock);
 	return ret;
-- 
2.7.4