1 files changed, 268 insertions, 64 deletions

diff --git a/drivers/media/platform/ti-vpe/cal-video.c b/drivers/media/platform/ti-vpe/cal-video.c
index 779f1e1bc529..7b7436a355ee 100644
--- a/drivers/media/platform/ti-vpe/cal-video.c
+++ b/drivers/media/platform/ti-vpe/cal-video.c
@@ -40,7 +40,35 @@ static char *fourcc_to_str(u32 fmt)
 }
 
 /* ------------------------------------------------------------------
- *	V4L2 Video IOCTLs
+ *	V4L2 Common IOCTLs
+ * ------------------------------------------------------------------
+ */
+
+static int cal_querycap(struct file *file, void *priv,
+			struct v4l2_capability *cap)
+{
+	struct cal_ctx *ctx = video_drvdata(file);
+
+	strscpy(cap->driver, CAL_MODULE_NAME, sizeof(cap->driver));
+	strscpy(cap->card, CAL_MODULE_NAME, sizeof(cap->card));
+
+	snprintf(cap->bus_info, sizeof(cap->bus_info),
+		 "platform:%s", dev_name(ctx->cal->dev));
+	return 0;
+}
+
+static int cal_g_fmt_vid_cap(struct file *file, void *priv,
+			     struct v4l2_format *f)
+{
+	struct cal_ctx *ctx = video_drvdata(file);
+
+	*f = ctx->v_fmt;
+
+	return 0;
+}
+
+/* ------------------------------------------------------------------
+ *	V4L2 Video Node Centric IOCTLs
  * ------------------------------------------------------------------
  */
 
@@ -74,19 +102,6 @@ static const struct cal_format_info *find_format_by_code(struct cal_ctx *ctx,
 	return NULL;
 }
 
-static int cal_querycap(struct file *file, void *priv,
-			struct v4l2_capability *cap)
-{
-	struct cal_ctx *ctx = video_drvdata(file);
-
-	strscpy(cap->driver, CAL_MODULE_NAME, sizeof(cap->driver));
-	strscpy(cap->card, CAL_MODULE_NAME, sizeof(cap->card));
-
-	snprintf(cap->bus_info, sizeof(cap->bus_info),
-		 "platform:%s", dev_name(ctx->cal->dev));
-	return 0;
-}
-
 static int cal_enum_fmt_vid_cap(struct file *file, void  *priv,
 				struct v4l2_fmtdesc *f)
 {
@@ -174,16 +189,6 @@ static void cal_calc_format_size(struct cal_ctx *ctx,
 		f->fmt.pix.bytesperline, f->fmt.pix.sizeimage);
 }
 
-static int cal_g_fmt_vid_cap(struct file *file, void *priv,
-			     struct v4l2_format *f)
-{
-	struct cal_ctx *ctx = video_drvdata(file);
-
-	*f = ctx->v_fmt;
-
-	return 0;
-}
-
 static int cal_try_fmt_vid_cap(struct file *file, void *priv,
 			       struct v4l2_format *f)
 {
@@ -383,16 +388,7 @@ static int cal_enum_frameintervals(struct file *file, void *priv,
 	return 0;
 }
 
-static const struct v4l2_file_operations cal_fops = {
-	.owner		= THIS_MODULE,
-	.open           = v4l2_fh_open,
-	.release        = vb2_fop_release,
-	.poll		= vb2_fop_poll,
-	.unlocked_ioctl = video_ioctl2, /* V4L2 ioctl handler */
-	.mmap           = vb2_fop_mmap,
-};
-
-static const struct v4l2_ioctl_ops cal_ioctl_ops = {
+static const struct v4l2_ioctl_ops cal_ioctl_video_ops = {
 	.vidioc_querycap      = cal_querycap,
 	.vidioc_enum_fmt_vid_cap  = cal_enum_fmt_vid_cap,
 	.vidioc_g_fmt_vid_cap     = cal_g_fmt_vid_cap,
@@ -418,7 +414,155 @@ static const struct v4l2_ioctl_ops cal_ioctl_ops = {
 };
 
 /* ------------------------------------------------------------------
- *	videobuf2 Operations
+ *	V4L2 Media Controller Centric IOCTLs
+ * ------------------------------------------------------------------
+ */
+
+static int cal_mc_enum_fmt_vid_cap(struct file *file, void  *priv,
+				   struct v4l2_fmtdesc *f)
+{
+	if (f->index >= cal_num_formats)
+		return -EINVAL;
+
+	f->pixelformat = cal_formats[f->index].fourcc;
+	f->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+
+	return 0;
+}
+
+static void cal_mc_try_fmt(struct cal_ctx *ctx, struct v4l2_format *f,
+			   const struct cal_format_info **info)
+{
+	struct v4l2_pix_format *format = &f->fmt.pix;
+	const struct cal_format_info *fmtinfo;
+	unsigned int bpp;
+
+	/*
+	 * Default to the first format if the requested pixel format code isn't
+	 * supported.
+	 */
+	fmtinfo = cal_format_by_fourcc(f->fmt.pix.pixelformat);
+	if (!fmtinfo)
+		fmtinfo = &cal_formats[0];
+
+	/*
+	 * Clamp the size, update the pixel format. The field and colorspace are
+	 * accepted as-is, except for V4L2_FIELD_ANY that is turned into
+	 * V4L2_FIELD_NONE.
+	 */
+	bpp = ALIGN(fmtinfo->bpp, 8);
+
+	format->width = clamp_t(unsigned int, format->width,
+				CAL_MIN_WIDTH_BYTES * 8 / bpp,
+				CAL_MAX_WIDTH_BYTES * 8 / bpp);
+	format->height = clamp_t(unsigned int, format->height,
+				 CAL_MIN_HEIGHT_LINES, CAL_MAX_HEIGHT_LINES);
+	format->pixelformat = fmtinfo->fourcc;
+
+	if (format->field == V4L2_FIELD_ANY)
+		format->field = V4L2_FIELD_NONE;
+
+	/*
+	 * Calculate the number of bytes per line and the image size. The
+	 * hardware stores the stride as a number of 16 bytes words, in a
+	 * signed 15-bit value. Only 14 bits are thus usable.
+	 */
+	format->bytesperline = ALIGN(clamp(format->bytesperline,
+					   format->width * bpp / 8,
+					   ((1U << 14) - 1) * 16), 16);
+
+	format->sizeimage = format->height * format->bytesperline;
+
+	format->colorspace = ctx->v_fmt.fmt.pix.colorspace;
+
+	if (info)
+		*info = fmtinfo;
+
+	ctx_dbg(3, ctx, "%s: %s %ux%u (bytesperline %u sizeimage %u)\n",
+		__func__, fourcc_to_str(format->pixelformat),
+		format->width, format->height,
+		format->bytesperline, format->sizeimage);
+}
+
+static int cal_mc_try_fmt_vid_cap(struct file *file, void *priv,
+				  struct v4l2_format *f)
+{
+	struct cal_ctx *ctx = video_drvdata(file);
+
+	cal_mc_try_fmt(ctx, f, NULL);
+	return 0;
+}
+
+static int cal_mc_s_fmt_vid_cap(struct file *file, void *priv,
+				struct v4l2_format *f)
+{
+	struct cal_ctx *ctx = video_drvdata(file);
+	const struct cal_format_info *fmtinfo;
+
+	if (vb2_is_busy(&ctx->vb_vidq)) {
+		ctx_dbg(3, ctx, "%s device busy\n", __func__);
+		return -EBUSY;
+	}
+
+	cal_mc_try_fmt(ctx, f, &fmtinfo);
+
+	ctx->v_fmt = *f;
+	ctx->fmtinfo = fmtinfo;
+
+	return 0;
+}
+
+static int cal_mc_enum_framesizes(struct file *file, void *fh,
+				  struct v4l2_frmsizeenum *fsize)
+{
+	struct cal_ctx *ctx = video_drvdata(file);
+	const struct cal_format_info *fmtinfo;
+	unsigned int bpp;
+
+	if (fsize->index > 0)
+		return -EINVAL;
+
+	fmtinfo = cal_format_by_fourcc(fsize->pixel_format);
+	if (!fmtinfo) {
+		ctx_dbg(3, ctx, "Invalid pixel format 0x%08x\n",
+			fsize->pixel_format);
+		return -EINVAL;
+	}
+
+	bpp = ALIGN(fmtinfo->bpp, 8);
+
+	fsize->type = V4L2_FRMSIZE_TYPE_STEPWISE;
+	fsize->stepwise.min_width = CAL_MIN_WIDTH_BYTES * 8 / bpp;
+	fsize->stepwise.max_width = CAL_MAX_WIDTH_BYTES * 8 / bpp;
+	fsize->stepwise.step_width = 64 / bpp;
+	fsize->stepwise.min_height = CAL_MIN_HEIGHT_LINES;
+	fsize->stepwise.max_height = CAL_MAX_HEIGHT_LINES;
+	fsize->stepwise.step_height = 1;
+
+	return 0;
+}
+
+static const struct v4l2_ioctl_ops cal_ioctl_mc_ops = {
+	.vidioc_querycap      = cal_querycap,
+	.vidioc_enum_fmt_vid_cap  = cal_mc_enum_fmt_vid_cap,
+	.vidioc_g_fmt_vid_cap     = cal_g_fmt_vid_cap,
+	.vidioc_try_fmt_vid_cap   = cal_mc_try_fmt_vid_cap,
+	.vidioc_s_fmt_vid_cap     = cal_mc_s_fmt_vid_cap,
+	.vidioc_enum_framesizes   = cal_mc_enum_framesizes,
+	.vidioc_reqbufs       = vb2_ioctl_reqbufs,
+	.vidioc_create_bufs   = vb2_ioctl_create_bufs,
+	.vidioc_prepare_buf   = vb2_ioctl_prepare_buf,
+	.vidioc_querybuf      = vb2_ioctl_querybuf,
+	.vidioc_qbuf          = vb2_ioctl_qbuf,
+	.vidioc_dqbuf         = vb2_ioctl_dqbuf,
+	.vidioc_expbuf        = vb2_ioctl_expbuf,
+	.vidioc_streamon      = vb2_ioctl_streamon,
+	.vidioc_streamoff     = vb2_ioctl_streamoff,
+	.vidioc_log_status    = v4l2_ctrl_log_status,
+};
+
+/* ------------------------------------------------------------------
+ *	videobuf2 Common Operations
  * ------------------------------------------------------------------
  */
 
@@ -504,6 +648,26 @@ static void cal_release_buffers(struct cal_ctx *ctx,
 	spin_unlock_irq(&ctx->dma.lock);
 }
 
+/* ------------------------------------------------------------------
+ *	videobuf2 Operations
+ * ------------------------------------------------------------------
+ */
+
+static int cal_video_check_format(struct cal_ctx *ctx)
+{
+	const struct v4l2_mbus_framefmt *format;
+
+	format = &ctx->phy->formats[CAL_CAMERARX_PAD_SOURCE];
+
+	if (ctx->fmtinfo->code != format->code ||
+	    ctx->v_fmt.fmt.pix.height != format->height ||
+	    ctx->v_fmt.fmt.pix.width != format->width ||
+	    ctx->v_fmt.fmt.pix.field != format->field)
+		return -EPIPE;
+
+	return 0;
+}
+
 static int cal_start_streaming(struct vb2_queue *vq, unsigned int count)
 {
 	struct cal_ctx *ctx = vb2_get_drv_priv(vq);
@@ -511,6 +675,23 @@ static int cal_start_streaming(struct vb2_queue *vq, unsigned int count)
 	dma_addr_t addr;
 	int ret;
 
+	ret = media_pipeline_start(&ctx->vdev.entity, &ctx->phy->pipe);
+	if (ret < 0) {
+		ctx_err(ctx, "Failed to start media pipeline: %d\n", ret);
+		goto error_release_buffers;
+	}
+
+	/*
+	 * Verify that the currently configured format matches the output of
+	 * the connected CAMERARX.
+	 */
+	ret = cal_video_check_format(ctx);
+	if (ret < 0) {
+		ctx_dbg(3, ctx,
+			"Format mismatch between CAMERARX and video node\n");
+		goto error_pipeline;
+	}
+
 	spin_lock_irq(&ctx->dma.lock);
 	buf = list_first_entry(&ctx->dma.queue, struct cal_buffer, list);
 	ctx->dma.pending = buf;
@@ -526,18 +707,22 @@ static int cal_start_streaming(struct vb2_queue *vq, unsigned int count)
 
 	ret = v4l2_subdev_call(&ctx->phy->subdev, video, s_stream, 1);
 	if (ret)
-		goto err;
+		goto error_stop;
 
 	if (cal_debug >= 4)
 		cal_quickdump_regs(ctx->cal);
 
 	return 0;
 
-err:
+error_stop:
 	cal_ctx_stop(ctx);
 	pm_runtime_put_sync(ctx->cal->dev);
 
+error_pipeline:
+	media_pipeline_stop(&ctx->vdev.entity);
+error_release_buffers:
 	cal_release_buffers(ctx, VB2_BUF_STATE_QUEUED);
+
 	return ret;
 }
 
@@ -552,6 +737,8 @@ static void cal_stop_streaming(struct vb2_queue *vq)
 	pm_runtime_put_sync(ctx->cal->dev);
 
 	cal_release_buffers(ctx, VB2_BUF_STATE_ERROR);
+
+	media_pipeline_stop(&ctx->vdev.entity);
 }
 
 static const struct vb2_ops cal_video_qops = {
@@ -569,13 +756,13 @@ static const struct vb2_ops cal_video_qops = {
  * ------------------------------------------------------------------
  */
 
-static const struct video_device cal_videodev = {
-	.name		= CAL_MODULE_NAME,
-	.fops		= &cal_fops,
-	.ioctl_ops	= &cal_ioctl_ops,
-	.minor		= -1,
-	.release	= video_device_release_empty,
-	.device_caps	= V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING,
+static const struct v4l2_file_operations cal_fops = {
+	.owner		= THIS_MODULE,
+	.open           = v4l2_fh_open,
+	.release        = vb2_fop_release,
+	.poll		= vb2_fop_poll,
+	.unlocked_ioctl = video_ioctl2, /* V4L2 ioctl handler */
+	.mmap           = vb2_fop_mmap,
 };
 
 static int cal_ctx_v4l2_init_formats(struct cal_ctx *ctx)
@@ -591,15 +778,21 @@ static int cal_ctx_v4l2_init_formats(struct cal_ctx *ctx)
 				       sizeof(*ctx->active_fmt), GFP_KERNEL);
 	ctx->num_active_fmt = 0;
 
-	for (j = 0, i = 0; ret != -EINVAL; ++j) {
+	for (j = 0, i = 0; ; ++j) {
 
 		memset(&mbus_code, 0, sizeof(mbus_code));
 		mbus_code.index = j;
 		mbus_code.which = V4L2_SUBDEV_FORMAT_ACTIVE;
 		ret = v4l2_subdev_call(ctx->phy->sensor, pad, enum_mbus_code,
 				       NULL, &mbus_code);
-		if (ret)
-			continue;
+		if (ret == -EINVAL)
+			break;
+
+		if (ret) {
+			ctx_err(ctx, "Error enumerating mbus codes in subdev %s: %d\n",
+				ctx->phy->sensor->name, ret);
+			return ret;
+		}
 
 		ctx_dbg(2, ctx,
 			"subdev %s: code: %04x idx: %u\n",
@@ -640,7 +833,6 @@ static int cal_ctx_v4l2_init_formats(struct cal_ctx *ctx)
 	v4l2_fill_pix_format(&ctx->v_fmt.fmt.pix, &mbus_fmt);
 	ctx->v_fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 	ctx->v_fmt.fmt.pix.pixelformat = fmtinfo->fourcc;
-	ctx->v_fmt.fmt.pix.field = mbus_fmt.field;
 	cal_calc_format_size(ctx, fmtinfo, &ctx->v_fmt);
 	ctx->fmtinfo = fmtinfo;
 
@@ -649,7 +841,6 @@ static int cal_ctx_v4l2_init_formats(struct cal_ctx *ctx)
 
 int cal_ctx_v4l2_register(struct cal_ctx *ctx)
 {
-	struct v4l2_ctrl_handler *hdl = &ctx->ctrl_handler;
 	struct video_device *vfd = &ctx->vdev;
 	int ret;
 
@@ -657,11 +848,15 @@ int cal_ctx_v4l2_register(struct cal_ctx *ctx)
 	if (ret)
 		return ret;
 
-	ret = v4l2_ctrl_add_handler(hdl, ctx->phy->sensor->ctrl_handler, NULL,
-				    true);
-	if (ret < 0) {
-		ctx_err(ctx, "Failed to add sensor ctrl handler\n");
-		return ret;
+	if (!cal_mc_api) {
+		struct v4l2_ctrl_handler *hdl = &ctx->ctrl_handler;
+
+		ret = v4l2_ctrl_add_handler(hdl, ctx->phy->sensor->ctrl_handler,
+					    NULL, true);
+		if (ret < 0) {
+			ctx_err(ctx, "Failed to add sensor ctrl handler\n");
+			return ret;
+		}
 	}
 
 	ret = video_register_device(vfd, VFL_TYPE_VIDEO, cal_video_nr);
@@ -698,7 +893,6 @@ void cal_ctx_v4l2_unregister(struct cal_ctx *ctx)
 
 int cal_ctx_v4l2_init(struct cal_ctx *ctx)
 {
-	struct v4l2_ctrl_handler *hdl = &ctx->ctrl_handler;
 	struct video_device *vfd = &ctx->vdev;
 	struct vb2_queue *q = &ctx->vb_vidq;
 	int ret;
@@ -725,10 +919,14 @@ int cal_ctx_v4l2_init(struct cal_ctx *ctx)
 		return ret;
 
 	/* Initialize the video device and media entity. */
-	*vfd = cal_videodev;
+	vfd->fops = &cal_fops;
+	vfd->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING
+			 | (cal_mc_api ? V4L2_CAP_IO_MC : 0);
 	vfd->v4l2_dev = &ctx->cal->v4l2_dev;
 	vfd->queue = q;
 	snprintf(vfd->name, sizeof(vfd->name), "CAL output %u", ctx->index);
+	vfd->release = video_device_release_empty;
+	vfd->ioctl_ops = cal_mc_api ? &cal_ioctl_mc_ops : &cal_ioctl_video_ops;
 	vfd->lock = &ctx->mutex;
 	video_set_drvdata(vfd, ctx);
 
@@ -737,14 +935,18 @@ int cal_ctx_v4l2_init(struct cal_ctx *ctx)
 	if (ret < 0)
 		return ret;
 
-	/* Initialize the control handler. */
-	ret = v4l2_ctrl_handler_init(hdl, 11);
-	if (ret < 0) {
-		ctx_err(ctx, "Failed to init ctrl handler\n");
-		goto error;
-	}
+	if (!cal_mc_api) {
+		/* Initialize the control handler. */
+		struct v4l2_ctrl_handler *hdl = &ctx->ctrl_handler;
 
-	vfd->ctrl_handler = hdl;
+		ret = v4l2_ctrl_handler_init(hdl, 11);
+		if (ret < 0) {
+			ctx_err(ctx, "Failed to init ctrl handler\n");
+			goto error;
+		}
+
+		vfd->ctrl_handler = hdl;
+	}
 
 	return 0;
 
@@ -755,6 +957,8 @@ error:
 
 void cal_ctx_v4l2_cleanup(struct cal_ctx *ctx)
 {
-	v4l2_ctrl_handler_free(&ctx->ctrl_handler);
+	if (!cal_mc_api)
+		v4l2_ctrl_handler_free(&ctx->ctrl_handler);
+
 	media_entity_cleanup(&ctx->vdev.entity);
 }