ipu3-v4l2.c

   1// SPDX-License-Identifier: GPL-2.0
   2// Copyright (C) 2018 Intel Corporation
   3
   4#include <linux/module.h>
   5#include <linux/pm_runtime.h>
   6#include <linux/string_choices.h>
   7
   8#include <media/v4l2-event.h>
   9#include <media/v4l2-ioctl.h>
  10
  11#include "ipu3.h"
  12#include "ipu3-dmamap.h"
  13
  14/******************** v4l2_subdev_ops ********************/
  15
  16#define IPU3_RUNNING_MODE_VIDEO		0
  17#define IPU3_RUNNING_MODE_STILL		1
  18
  19static int imgu_subdev_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
  20{
  21	struct imgu_v4l2_subdev *imgu_sd = container_of(sd,
  22							struct imgu_v4l2_subdev,
  23							subdev);
  24	struct imgu_device *imgu = v4l2_get_subdevdata(sd);
  25	struct imgu_media_pipe *imgu_pipe = &imgu->imgu_pipe[imgu_sd->pipe];
  26	struct v4l2_rect try_crop = {
  27		.top = 0,
  28		.left = 0,
  29	};
  30	unsigned int i;
  31
  32	try_crop.width =
  33		imgu_pipe->nodes[IMGU_NODE_IN].vdev_fmt.fmt.pix_mp.width;
  34	try_crop.height =
  35		imgu_pipe->nodes[IMGU_NODE_IN].vdev_fmt.fmt.pix_mp.height;
  36
  37	/* Initialize try_fmt */
  38	for (i = 0; i < IMGU_NODE_NUM; i++) {
  39		struct v4l2_mbus_framefmt *try_fmt =
  40			v4l2_subdev_state_get_format(fh->state, i);
  41
  42		try_fmt->width = try_crop.width;
  43		try_fmt->height = try_crop.height;
  44		try_fmt->code = imgu_pipe->nodes[i].pad_fmt.code;
  45		try_fmt->field = V4L2_FIELD_NONE;
  46	}
  47
  48	*v4l2_subdev_state_get_crop(fh->state, IMGU_NODE_IN) = try_crop;
  49	*v4l2_subdev_state_get_compose(fh->state, IMGU_NODE_IN) = try_crop;
  50
  51	return 0;
  52}
  53
  54static int imgu_subdev_s_stream(struct v4l2_subdev *sd, int enable)
  55{
  56	int i;
  57	unsigned int node;
  58	int r = 0;
  59	struct imgu_device *imgu = v4l2_get_subdevdata(sd);
  60	struct imgu_v4l2_subdev *imgu_sd = container_of(sd,
  61							struct imgu_v4l2_subdev,
  62							subdev);
  63	unsigned int pipe = imgu_sd->pipe;
  64	struct device *dev = &imgu->pci_dev->dev;
  65	struct v4l2_pix_format_mplane *fmts[IPU3_CSS_QUEUES] = { NULL };
  66	struct v4l2_rect *rects[IPU3_CSS_RECTS] = { NULL };
  67	struct imgu_css_pipe *css_pipe = &imgu->css.pipes[pipe];
  68	struct imgu_media_pipe *imgu_pipe = &imgu->imgu_pipe[pipe];
  69
  70	dev_dbg(dev, "%s %d for pipe %u", __func__, enable, pipe);
  71	/* grab ctrl after streamon and return after off */
  72	v4l2_ctrl_grab(imgu_sd->ctrl, enable);
  73
  74	if (!enable) {
  75		imgu_sd->active = false;
  76		return 0;
  77	}
  78
  79	for (i = 0; i < IMGU_NODE_NUM; i++)
  80		imgu_pipe->queue_enabled[i] = imgu_pipe->nodes[i].enabled;
  81
  82	/* This is handled specially */
  83	imgu_pipe->queue_enabled[IPU3_CSS_QUEUE_PARAMS] = false;
  84
  85	/* Initialize CSS formats */
  86	for (i = 0; i < IPU3_CSS_QUEUES; i++) {
  87		node = imgu_map_node(imgu, i);
  88		/* No need to reconfig meta nodes */
  89		if (node == IMGU_NODE_STAT_3A || node == IMGU_NODE_PARAMS)
  90			continue;
  91		fmts[i] = imgu_pipe->queue_enabled[node] ?
  92			&imgu_pipe->nodes[node].vdev_fmt.fmt.pix_mp : NULL;
  93	}
  94
  95	/* Enable VF output only when VF queue requested by user */
  96	css_pipe->vf_output_en = false;
  97	if (imgu_pipe->nodes[IMGU_NODE_VF].enabled)
  98		css_pipe->vf_output_en = true;
  99
 100	if (atomic_read(&imgu_sd->running_mode) == IPU3_RUNNING_MODE_VIDEO)
 101		css_pipe->pipe_id = IPU3_CSS_PIPE_ID_VIDEO;
 102	else
 103		css_pipe->pipe_id = IPU3_CSS_PIPE_ID_CAPTURE;
 104
 105	dev_dbg(dev, "IPU3 pipe %u pipe_id %u", pipe, css_pipe->pipe_id);
 106
 107	rects[IPU3_CSS_RECT_EFFECTIVE] = &imgu_sd->rect.eff;
 108	rects[IPU3_CSS_RECT_BDS] = &imgu_sd->rect.bds;
 109	rects[IPU3_CSS_RECT_GDC] = &imgu_sd->rect.gdc;
 110
 111	r = imgu_css_fmt_set(&imgu->css, fmts, rects, pipe);
 112	if (r) {
 113		dev_err(dev, "failed to set initial formats pipe %u with (%d)",
 114			pipe, r);
 115		return r;
 116	}
 117
 118	imgu_sd->active = true;
 119
 120	return 0;
 121}
 122
 123static int imgu_subdev_get_fmt(struct v4l2_subdev *sd,
 124			       struct v4l2_subdev_state *sd_state,
 125			       struct v4l2_subdev_format *fmt)
 126{
 127	struct imgu_device *imgu = v4l2_get_subdevdata(sd);
 128	struct v4l2_mbus_framefmt *mf;
 129	struct imgu_media_pipe *imgu_pipe;
 130	u32 pad = fmt->pad;
 131	struct imgu_v4l2_subdev *imgu_sd = container_of(sd,
 132							struct imgu_v4l2_subdev,
 133							subdev);
 134	unsigned int pipe = imgu_sd->pipe;
 135
 136	imgu_pipe = &imgu->imgu_pipe[pipe];
 137	if (fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
 138		fmt->format = imgu_pipe->nodes[pad].pad_fmt;
 139	} else {
 140		mf = v4l2_subdev_state_get_format(sd_state, pad);
 141		fmt->format = *mf;
 142	}
 143
 144	return 0;
 145}
 146
 147static int imgu_subdev_set_fmt(struct v4l2_subdev *sd,
 148			       struct v4l2_subdev_state *sd_state,
 149			       struct v4l2_subdev_format *fmt)
 150{
 151	struct imgu_media_pipe *imgu_pipe;
 152	struct imgu_device *imgu = v4l2_get_subdevdata(sd);
 153	struct imgu_v4l2_subdev *imgu_sd = container_of(sd,
 154							struct imgu_v4l2_subdev,
 155							subdev);
 156	struct v4l2_mbus_framefmt *mf;
 157	u32 pad = fmt->pad;
 158	unsigned int pipe = imgu_sd->pipe;
 159
 160	dev_dbg(&imgu->pci_dev->dev, "set subdev %u pad %u fmt to [%ux%u]",
 161		pipe, pad, fmt->format.width, fmt->format.height);
 162
 163	imgu_pipe = &imgu->imgu_pipe[pipe];
 164	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY)
 165		mf = v4l2_subdev_state_get_format(sd_state, pad);
 166	else
 167		mf = &imgu_pipe->nodes[pad].pad_fmt;
 168
 169	fmt->format.code = mf->code;
 170	/* Clamp the w and h based on the hardware capabilities */
 171	if (imgu_sd->subdev_pads[pad].flags & MEDIA_PAD_FL_SOURCE) {
 172		fmt->format.width = clamp(fmt->format.width,
 173					  IPU3_OUTPUT_MIN_WIDTH,
 174					  IPU3_OUTPUT_MAX_WIDTH);
 175		fmt->format.height = clamp(fmt->format.height,
 176					   IPU3_OUTPUT_MIN_HEIGHT,
 177					   IPU3_OUTPUT_MAX_HEIGHT);
 178	} else {
 179		fmt->format.width = clamp(fmt->format.width,
 180					  IPU3_INPUT_MIN_WIDTH,
 181					  IPU3_INPUT_MAX_WIDTH);
 182		fmt->format.height = clamp(fmt->format.height,
 183					   IPU3_INPUT_MIN_HEIGHT,
 184					   IPU3_INPUT_MAX_HEIGHT);
 185	}
 186
 187	*mf = fmt->format;
 188
 189	return 0;
 190}
 191
 192static struct v4l2_rect *
 193imgu_subdev_get_crop(struct imgu_v4l2_subdev *sd,
 194		     struct v4l2_subdev_state *sd_state, unsigned int pad,
 195		     enum v4l2_subdev_format_whence which)
 196{
 197	if (which == V4L2_SUBDEV_FORMAT_TRY)
 198		return v4l2_subdev_state_get_crop(sd_state, pad);
 199	else
 200		return &sd->rect.eff;
 201}
 202
 203static struct v4l2_rect *
 204imgu_subdev_get_compose(struct imgu_v4l2_subdev *sd,
 205			struct v4l2_subdev_state *sd_state, unsigned int pad,
 206			enum v4l2_subdev_format_whence which)
 207{
 208	if (which == V4L2_SUBDEV_FORMAT_TRY)
 209		return v4l2_subdev_state_get_compose(sd_state, pad);
 210	else
 211		return &sd->rect.bds;
 212}
 213
 214static int imgu_subdev_get_selection(struct v4l2_subdev *sd,
 215				     struct v4l2_subdev_state *sd_state,
 216				     struct v4l2_subdev_selection *sel)
 217{
 218	struct imgu_v4l2_subdev *imgu_sd =
 219		container_of(sd, struct imgu_v4l2_subdev, subdev);
 220
 221	if (sel->pad != IMGU_NODE_IN)
 222		return -EINVAL;
 223
 224	switch (sel->target) {
 225	case V4L2_SEL_TGT_CROP:
 226		sel->r = *imgu_subdev_get_crop(imgu_sd, sd_state, sel->pad,
 227					       sel->which);
 228		return 0;
 229	case V4L2_SEL_TGT_COMPOSE:
 230		sel->r = *imgu_subdev_get_compose(imgu_sd, sd_state, sel->pad,
 231						  sel->which);
 232		return 0;
 233	default:
 234		return -EINVAL;
 235	}
 236}
 237
 238static int imgu_subdev_set_selection(struct v4l2_subdev *sd,
 239				     struct v4l2_subdev_state *sd_state,
 240				     struct v4l2_subdev_selection *sel)
 241{
 242	struct imgu_device *imgu = v4l2_get_subdevdata(sd);
 243	struct imgu_v4l2_subdev *imgu_sd =
 244		container_of(sd, struct imgu_v4l2_subdev, subdev);
 245	struct v4l2_rect *rect;
 246
 247	dev_dbg(&imgu->pci_dev->dev,
 248		 "set subdev %u sel which %u target 0x%4x rect [%ux%u]",
 249		 imgu_sd->pipe, sel->which, sel->target,
 250		 sel->r.width, sel->r.height);
 251
 252	if (sel->pad != IMGU_NODE_IN)
 253		return -EINVAL;
 254
 255	switch (sel->target) {
 256	case V4L2_SEL_TGT_CROP:
 257		rect = imgu_subdev_get_crop(imgu_sd, sd_state, sel->pad,
 258					    sel->which);
 259		break;
 260	case V4L2_SEL_TGT_COMPOSE:
 261		rect = imgu_subdev_get_compose(imgu_sd, sd_state, sel->pad,
 262					       sel->which);
 263		break;
 264	default:
 265		return -EINVAL;
 266	}
 267
 268	*rect = sel->r;
 269	return 0;
 270}
 271
 272/******************** media_entity_operations ********************/
 273
 274static int imgu_link_setup(struct media_entity *entity,
 275			   const struct media_pad *local,
 276			   const struct media_pad *remote, u32 flags)
 277{
 278	struct imgu_media_pipe *imgu_pipe;
 279	struct v4l2_subdev *sd = container_of(entity, struct v4l2_subdev,
 280					      entity);
 281	struct imgu_device *imgu = v4l2_get_subdevdata(sd);
 282	struct imgu_v4l2_subdev *imgu_sd = container_of(sd,
 283							struct imgu_v4l2_subdev,
 284							subdev);
 285	unsigned int pipe = imgu_sd->pipe;
 286	u32 pad = local->index;
 287
 288	WARN_ON(pad >= IMGU_NODE_NUM);
 289
 290	dev_dbg(&imgu->pci_dev->dev, "pipe %u pad %u is %s", pipe, pad,
 291		 str_enabled_disabled(flags & MEDIA_LNK_FL_ENABLED));
 292
 293	imgu_pipe = &imgu->imgu_pipe[pipe];
 294	imgu_pipe->nodes[pad].enabled = flags & MEDIA_LNK_FL_ENABLED;
 295
 296	/* enable input node to enable the pipe */
 297	if (pad != IMGU_NODE_IN)
 298		return 0;
 299
 300	if (flags & MEDIA_LNK_FL_ENABLED)
 301		__set_bit(pipe, imgu->css.enabled_pipes);
 302	else
 303		__clear_bit(pipe, imgu->css.enabled_pipes);
 304
 305	dev_dbg(&imgu->pci_dev->dev, "pipe %u is %s", pipe,
 306		 str_enabled_disabled(flags & MEDIA_LNK_FL_ENABLED));
 307
 308	return 0;
 309}
 310
 311/******************** vb2_ops ********************/
 312
 313static int imgu_vb2_buf_init(struct vb2_buffer *vb)
 314{
 315	struct sg_table *sg = vb2_dma_sg_plane_desc(vb, 0);
 316	struct imgu_device *imgu = vb2_get_drv_priv(vb->vb2_queue);
 317	struct imgu_buffer *buf = container_of(vb,
 318		struct imgu_buffer, vid_buf.vbb.vb2_buf);
 319	struct imgu_video_device *node =
 320		container_of(vb->vb2_queue, struct imgu_video_device, vbq);
 321	unsigned int queue = imgu_node_to_queue(node->id);
 322
 323	if (queue == IPU3_CSS_QUEUE_PARAMS)
 324		return 0;
 325
 326	return imgu_dmamap_map_sg(imgu, sg->sgl, sg->nents, &buf->map);
 327}
 328
 329/* Called when each buffer is freed */
 330static void imgu_vb2_buf_cleanup(struct vb2_buffer *vb)
 331{
 332	struct imgu_device *imgu = vb2_get_drv_priv(vb->vb2_queue);
 333	struct imgu_buffer *buf = container_of(vb,
 334		struct imgu_buffer, vid_buf.vbb.vb2_buf);
 335	struct imgu_video_device *node =
 336		container_of(vb->vb2_queue, struct imgu_video_device, vbq);
 337	unsigned int queue = imgu_node_to_queue(node->id);
 338
 339	if (queue == IPU3_CSS_QUEUE_PARAMS)
 340		return;
 341
 342	imgu_dmamap_unmap(imgu, &buf->map);
 343}
 344
 345/* Transfer buffer ownership to me */
 346static void imgu_vb2_buf_queue(struct vb2_buffer *vb)
 347{
 348	struct imgu_device *imgu = vb2_get_drv_priv(vb->vb2_queue);
 349	struct imgu_video_device *node =
 350		container_of(vb->vb2_queue, struct imgu_video_device, vbq);
 351	unsigned int queue = imgu_node_to_queue(node->id);
 352	struct imgu_buffer *buf = container_of(vb, struct imgu_buffer,
 353					       vid_buf.vbb.vb2_buf);
 354	unsigned long need_bytes;
 355	unsigned long payload = vb2_get_plane_payload(vb, 0);
 356
 357	if (vb->vb2_queue->type == V4L2_BUF_TYPE_META_CAPTURE ||
 358	    vb->vb2_queue->type == V4L2_BUF_TYPE_META_OUTPUT)
 359		need_bytes = node->vdev_fmt.fmt.meta.buffersize;
 360	else
 361		need_bytes = node->vdev_fmt.fmt.pix_mp.plane_fmt[0].sizeimage;
 362
 363	if (queue == IPU3_CSS_QUEUE_PARAMS && payload && payload < need_bytes) {
 364		dev_err(&imgu->pci_dev->dev, "invalid data size for params.");
 365		vb2_buffer_done(vb, VB2_BUF_STATE_ERROR);
 366		return;
 367	}
 368
 369	mutex_lock(&imgu->lock);
 370	if (queue != IPU3_CSS_QUEUE_PARAMS)
 371		imgu_css_buf_init(&buf->css_buf, queue, buf->map.daddr);
 372
 373	list_add_tail(&buf->vid_buf.list, &node->buffers);
 374	mutex_unlock(&imgu->lock);
 375
 376	vb2_set_plane_payload(vb, 0, need_bytes);
 377
 378	mutex_lock(&imgu->streaming_lock);
 379	if (imgu->streaming)
 380		imgu_queue_buffers(imgu, false, node->pipe);
 381	mutex_unlock(&imgu->streaming_lock);
 382
 383	dev_dbg(&imgu->pci_dev->dev, "%s for pipe %u node %u", __func__,
 384		node->pipe, node->id);
 385}
 386
 387static int imgu_vb2_queue_setup(struct vb2_queue *vq,
 388				unsigned int *num_buffers,
 389				unsigned int *num_planes,
 390				unsigned int sizes[],
 391				struct device *alloc_devs[])
 392{
 393	struct imgu_device *imgu = vb2_get_drv_priv(vq);
 394	struct imgu_video_device *node =
 395		container_of(vq, struct imgu_video_device, vbq);
 396	const struct v4l2_format *fmt = &node->vdev_fmt;
 397	unsigned int size;
 398
 399	*num_buffers = clamp_val(*num_buffers, 1, VB2_MAX_FRAME);
 400	alloc_devs[0] = &imgu->pci_dev->dev;
 401
 402	if (vq->type == V4L2_BUF_TYPE_META_CAPTURE ||
 403	    vq->type == V4L2_BUF_TYPE_META_OUTPUT)
 404		size = fmt->fmt.meta.buffersize;
 405	else
 406		size = fmt->fmt.pix_mp.plane_fmt[0].sizeimage;
 407
 408	if (*num_planes) {
 409		if (sizes[0] < size)
 410			return -EINVAL;
 411		size = sizes[0];
 412	}
 413
 414	*num_planes = 1;
 415	sizes[0] = size;
 416
 417	/* Initialize buffer queue */
 418	INIT_LIST_HEAD(&node->buffers);
 419
 420	return 0;
 421}
 422
 423/* Check if all enabled video nodes are streaming, exception ignored */
 424static bool imgu_all_nodes_streaming(struct imgu_device *imgu,
 425				     struct imgu_video_device *except)
 426{
 427	unsigned int i, pipe, p;
 428	struct imgu_video_device *node;
 429	struct device *dev = &imgu->pci_dev->dev;
 430
 431	pipe = except->pipe;
 432	if (!test_bit(pipe, imgu->css.enabled_pipes)) {
 433		dev_warn(&imgu->pci_dev->dev,
 434			 "pipe %u link is not ready yet", pipe);
 435		return false;
 436	}
 437
 438	for_each_set_bit(p, imgu->css.enabled_pipes, IMGU_MAX_PIPE_NUM) {
 439		for (i = 0; i < IMGU_NODE_NUM; i++) {
 440			node = &imgu->imgu_pipe[p].nodes[i];
 441			dev_dbg(dev, "%s pipe %u queue %u name %s enabled = %u",
 442				__func__, p, i, node->name, node->enabled);
 443			if (node == except)
 444				continue;
 445			if (node->enabled && !vb2_start_streaming_called(&node->vbq))
 446				return false;
 447		}
 448	}
 449
 450	return true;
 451}
 452
 453static void imgu_return_all_buffers(struct imgu_device *imgu,
 454				    struct imgu_video_device *node,
 455				    enum vb2_buffer_state state)
 456{
 457	struct imgu_vb2_buffer *b, *b0;
 458
 459	/* Return all buffers */
 460	mutex_lock(&imgu->lock);
 461	list_for_each_entry_safe(b, b0, &node->buffers, list) {
 462		list_del(&b->list);
 463		vb2_buffer_done(&b->vbb.vb2_buf, state);
 464	}
 465	mutex_unlock(&imgu->lock);
 466}
 467
 468static int imgu_vb2_start_streaming(struct vb2_queue *vq, unsigned int count)
 469{
 470	struct imgu_media_pipe *imgu_pipe;
 471	struct imgu_device *imgu = vb2_get_drv_priv(vq);
 472	struct device *dev = &imgu->pci_dev->dev;
 473	struct imgu_video_device *node =
 474		container_of(vq, struct imgu_video_device, vbq);
 475	int r;
 476	unsigned int pipe;
 477
 478	dev_dbg(dev, "%s node name %s pipe %u id %u", __func__,
 479		node->name, node->pipe, node->id);
 480
 481	mutex_lock(&imgu->streaming_lock);
 482	if (imgu->streaming) {
 483		r = -EBUSY;
 484		mutex_unlock(&imgu->streaming_lock);
 485		goto fail_return_bufs;
 486	}
 487	mutex_unlock(&imgu->streaming_lock);
 488
 489	if (!node->enabled) {
 490		dev_err(dev, "IMGU node is not enabled");
 491		r = -EINVAL;
 492		goto fail_return_bufs;
 493	}
 494
 495	pipe = node->pipe;
 496	imgu_pipe = &imgu->imgu_pipe[pipe];
 497	atomic_set(&node->sequence, 0);
 498	r = video_device_pipeline_start(&node->vdev, &imgu_pipe->pipeline);
 499	if (r < 0)
 500		goto fail_return_bufs;
 501
 502	if (!imgu_all_nodes_streaming(imgu, node))
 503		return 0;
 504
 505	for_each_set_bit(pipe, imgu->css.enabled_pipes, IMGU_MAX_PIPE_NUM) {
 506		r = v4l2_subdev_call(&imgu->imgu_pipe[pipe].imgu_sd.subdev,
 507				     video, s_stream, 1);
 508		if (r < 0)
 509			goto fail_stop_pipeline;
 510	}
 511
 512	/* Start streaming of the whole pipeline now */
 513	dev_dbg(dev, "IMGU streaming is ready to start");
 514	mutex_lock(&imgu->streaming_lock);
 515	r = imgu_s_stream(imgu, true);
 516	if (!r)
 517		imgu->streaming = true;
 518	mutex_unlock(&imgu->streaming_lock);
 519
 520	return 0;
 521
 522fail_stop_pipeline:
 523	video_device_pipeline_stop(&node->vdev);
 524fail_return_bufs:
 525	imgu_return_all_buffers(imgu, node, VB2_BUF_STATE_QUEUED);
 526
 527	return r;
 528}
 529
 530static void imgu_vb2_stop_streaming(struct vb2_queue *vq)
 531{
 532	struct imgu_media_pipe *imgu_pipe;
 533	struct imgu_device *imgu = vb2_get_drv_priv(vq);
 534	struct device *dev = &imgu->pci_dev->dev;
 535	struct imgu_video_device *node =
 536		container_of(vq, struct imgu_video_device, vbq);
 537	int r;
 538	unsigned int pipe;
 539	bool stop_streaming = false;
 540
 541	/* Verify that the node had been setup with imgu_v4l2_node_setup() */
 542	WARN_ON(!node->enabled);
 543
 544	pipe = node->pipe;
 545	dev_dbg(dev, "Try to stream off node [%u][%u]", pipe, node->id);
 546
 547	/*
 548	 * When the first node of a streaming setup is stopped, the entire
 549	 * pipeline needs to stop before individual nodes are disabled.
 550	 * Perform the inverse of the initial setup.
 551	 *
 552	 * Part 1 - s_stream on the entire pipeline
 553	 */
 554	mutex_lock(&imgu->streaming_lock);
 555	if (imgu->streaming) {
 556		/* Yes, really stop streaming now */
 557		dev_dbg(dev, "IMGU streaming is ready to stop");
 558		r = imgu_s_stream(imgu, false);
 559		if (!r)
 560			imgu->streaming = false;
 561		stop_streaming = true;
 562	}
 563	mutex_unlock(&imgu->streaming_lock);
 564
 565	/* Part 2 - s_stream on subdevs
 566	 *
 567	 * If we call s_stream multiple times, Linux v6.7's call_s_stream()
 568	 * WARNs and aborts. Thus, disable all pipes at once, and only once.
 569	 */
 570	if (stop_streaming) {
 571		for_each_set_bit(pipe, imgu->css.enabled_pipes,
 572				 IMGU_MAX_PIPE_NUM) {
 573			imgu_pipe = &imgu->imgu_pipe[pipe];
 574
 575			r = v4l2_subdev_call(&imgu_pipe->imgu_sd.subdev,
 576					     video, s_stream, 0);
 577			if (r)
 578				dev_err(&imgu->pci_dev->dev,
 579					"failed to stop subdev streaming\n");
 580		}
 581	}
 582
 583	/* Part 3 - individual node teardown */
 584	video_device_pipeline_stop(&node->vdev);
 585	imgu_return_all_buffers(imgu, node, VB2_BUF_STATE_ERROR);
 586}
 587
 588/******************** v4l2_ioctl_ops ********************/
 589
 590#define VID_CAPTURE	0
 591#define VID_OUTPUT	1
 592#define DEF_VID_CAPTURE	0
 593#define DEF_VID_OUTPUT	1
 594
 595struct imgu_fmt {
 596	u32	fourcc;
 597	u16	type; /* VID_CAPTURE or VID_OUTPUT not both */
 598};
 599
 600/* format descriptions for capture and preview */
 601static const struct imgu_fmt formats[] = {
 602	{ V4L2_PIX_FMT_NV12, VID_CAPTURE },
 603	{ V4L2_PIX_FMT_IPU3_SGRBG10, VID_OUTPUT },
 604	{ V4L2_PIX_FMT_IPU3_SBGGR10, VID_OUTPUT },
 605	{ V4L2_PIX_FMT_IPU3_SGBRG10, VID_OUTPUT },
 606	{ V4L2_PIX_FMT_IPU3_SRGGB10, VID_OUTPUT },
 607};
 608
 609/* Find the first matched format, return default if not found */
 610static const struct imgu_fmt *find_format(struct v4l2_format *f, u32 type)
 611{
 612	unsigned int i;
 613
 614	for (i = 0; i < ARRAY_SIZE(formats); i++) {
 615		if (formats[i].fourcc == f->fmt.pix_mp.pixelformat &&
 616		    formats[i].type == type)
 617			return &formats[i];
 618	}
 619
 620	return type == VID_CAPTURE ? &formats[DEF_VID_CAPTURE] :
 621				     &formats[DEF_VID_OUTPUT];
 622}
 623
 624static int imgu_vidioc_querycap(struct file *file, void *fh,
 625				struct v4l2_capability *cap)
 626{
 627	struct imgu_device *imgu = video_drvdata(file);
 628
 629	strscpy(cap->driver, IMGU_NAME, sizeof(cap->driver));
 630	strscpy(cap->card, IMGU_NAME, sizeof(cap->card));
 631	snprintf(cap->bus_info, sizeof(cap->bus_info), "PCI:%s",
 632		 pci_name(imgu->pci_dev));
 633
 634	return 0;
 635}
 636
 637static int enum_fmts(struct v4l2_fmtdesc *f, u32 type)
 638{
 639	unsigned int i, j;
 640
 641	if (f->mbus_code != 0 && f->mbus_code != MEDIA_BUS_FMT_FIXED)
 642		return -EINVAL;
 643
 644	for (i = j = 0; i < ARRAY_SIZE(formats); ++i) {
 645		if (formats[i].type == type) {
 646			if (j == f->index)
 647				break;
 648			++j;
 649		}
 650	}
 651
 652	if (i < ARRAY_SIZE(formats)) {
 653		f->pixelformat = formats[i].fourcc;
 654		return 0;
 655	}
 656
 657	return -EINVAL;
 658}
 659
 660static int vidioc_enum_fmt_vid_cap(struct file *file, void *priv,
 661				   struct v4l2_fmtdesc *f)
 662{
 663	if (f->type != V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
 664		return -EINVAL;
 665
 666	return enum_fmts(f, VID_CAPTURE);
 667}
 668
 669static int vidioc_enum_fmt_vid_out(struct file *file, void *priv,
 670				   struct v4l2_fmtdesc *f)
 671{
 672	if (f->type != V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
 673		return -EINVAL;
 674
 675	return enum_fmts(f, VID_OUTPUT);
 676}
 677
 678/* Propagate forward always the format from the CIO2 subdev */
 679static int imgu_vidioc_g_fmt(struct file *file, void *fh,
 680			     struct v4l2_format *f)
 681{
 682	struct imgu_video_device *node = file_to_intel_imgu_node(file);
 683
 684	f->fmt = node->vdev_fmt.fmt;
 685
 686	return 0;
 687}
 688
 689/*
 690 * Set input/output format. Unless it is just a try, this also resets
 691 * selections (ie. effective and BDS resolutions) to defaults.
 692 */
 693static int imgu_fmt(struct imgu_device *imgu, unsigned int pipe, int node,
 694		    struct v4l2_format *f, bool try)
 695{
 696	struct device *dev = &imgu->pci_dev->dev;
 697	struct v4l2_pix_format_mplane *fmts[IPU3_CSS_QUEUES] = { NULL };
 698	struct v4l2_rect *rects[IPU3_CSS_RECTS] = { NULL };
 699	struct v4l2_mbus_framefmt pad_fmt;
 700	unsigned int i, css_q;
 701	int ret;
 702	struct imgu_css_pipe *css_pipe = &imgu->css.pipes[pipe];
 703	struct imgu_media_pipe *imgu_pipe = &imgu->imgu_pipe[pipe];
 704	struct imgu_v4l2_subdev *imgu_sd = &imgu_pipe->imgu_sd;
 705
 706	dev_dbg(dev, "set fmt node [%u][%u](try = %u)", pipe, node, try);
 707
 708	for (i = 0; i < IMGU_NODE_NUM; i++)
 709		dev_dbg(dev, "IMGU pipe %u node %u enabled = %u",
 710			pipe, i, imgu_pipe->nodes[i].enabled);
 711
 712	if (imgu_pipe->nodes[IMGU_NODE_VF].enabled)
 713		css_pipe->vf_output_en = true;
 714
 715	if (atomic_read(&imgu_sd->running_mode) == IPU3_RUNNING_MODE_VIDEO)
 716		css_pipe->pipe_id = IPU3_CSS_PIPE_ID_VIDEO;
 717	else
 718		css_pipe->pipe_id = IPU3_CSS_PIPE_ID_CAPTURE;
 719
 720	dev_dbg(dev, "IPU3 pipe %u pipe_id = %u", pipe, css_pipe->pipe_id);
 721
 722	css_q = imgu_node_to_queue(node);
 723	for (i = 0; i < IPU3_CSS_QUEUES; i++) {
 724		unsigned int inode = imgu_map_node(imgu, i);
 725
 726		/* Skip the meta node */
 727		if (inode == IMGU_NODE_STAT_3A || inode == IMGU_NODE_PARAMS)
 728			continue;
 729
 730		/* CSS expects some format on OUT queue */
 731		if (i != IPU3_CSS_QUEUE_OUT &&
 732		    !imgu_pipe->nodes[inode].enabled && !try) {
 733			fmts[i] = NULL;
 734			continue;
 735		}
 736
 737		if (i == css_q) {
 738			fmts[i] = &f->fmt.pix_mp;
 739			continue;
 740		}
 741
 742		if (try) {
 743			fmts[i] = kmemdup(&imgu_pipe->nodes[inode].vdev_fmt.fmt.pix_mp,
 744					  sizeof(struct v4l2_pix_format_mplane),
 745					  GFP_KERNEL);
 746			if (!fmts[i]) {
 747				ret = -ENOMEM;
 748				goto out;
 749			}
 750		} else {
 751			fmts[i] = &imgu_pipe->nodes[inode].vdev_fmt.fmt.pix_mp;
 752		}
 753
 754	}
 755
 756	if (!try) {
 757		/* eff and bds res got by imgu_s_sel */
 758		struct imgu_v4l2_subdev *imgu_sd = &imgu_pipe->imgu_sd;
 759
 760		rects[IPU3_CSS_RECT_EFFECTIVE] = &imgu_sd->rect.eff;
 761		rects[IPU3_CSS_RECT_BDS] = &imgu_sd->rect.bds;
 762		rects[IPU3_CSS_RECT_GDC] = &imgu_sd->rect.gdc;
 763
 764		/* suppose that pad fmt was set by subdev s_fmt before */
 765		pad_fmt = imgu_pipe->nodes[IMGU_NODE_IN].pad_fmt;
 766		rects[IPU3_CSS_RECT_GDC]->width = pad_fmt.width;
 767		rects[IPU3_CSS_RECT_GDC]->height = pad_fmt.height;
 768	}
 769
 770	if (!fmts[css_q]) {
 771		ret = -EINVAL;
 772		goto out;
 773	}
 774
 775	if (try)
 776		ret = imgu_css_fmt_try(&imgu->css, fmts, rects, pipe);
 777	else
 778		ret = imgu_css_fmt_set(&imgu->css, fmts, rects, pipe);
 779
 780	/* ret is the binary number in the firmware blob */
 781	if (ret < 0)
 782		goto out;
 783
 784	/*
 785	 * imgu doesn't set the node to the value given by user
 786	 * before we return success from this function, so set it here.
 787	 */
 788	if (!try)
 789		imgu_pipe->nodes[node].vdev_fmt.fmt.pix_mp = f->fmt.pix_mp;
 790
 791out:
 792	if (try) {
 793		for (i = 0; i < IPU3_CSS_QUEUES; i++)
 794			if (i != css_q)
 795				kfree(fmts[i]);
 796	}
 797
 798	return ret;
 799}
 800
 801static int imgu_try_fmt(struct file *file, void *fh, struct v4l2_format *f)
 802{
 803	struct v4l2_pix_format_mplane *pixm = &f->fmt.pix_mp;
 804	const struct imgu_fmt *fmt;
 805
 806	if (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
 807		fmt = find_format(f, VID_CAPTURE);
 808	else if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
 809		fmt = find_format(f, VID_OUTPUT);
 810	else
 811		return -EINVAL;
 812
 813	pixm->pixelformat = fmt->fourcc;
 814
 815	return 0;
 816}
 817
 818static int imgu_vidioc_try_fmt(struct file *file, void *fh,
 819			       struct v4l2_format *f)
 820{
 821	struct imgu_device *imgu = video_drvdata(file);
 822	struct device *dev = &imgu->pci_dev->dev;
 823	struct imgu_video_device *node = file_to_intel_imgu_node(file);
 824	struct v4l2_pix_format_mplane *pix_mp = &f->fmt.pix_mp;
 825	int r;
 826
 827	dev_dbg(dev, "%s [%ux%u] for node %u\n", __func__,
 828		pix_mp->width, pix_mp->height, node->id);
 829
 830	r = imgu_try_fmt(file, fh, f);
 831	if (r)
 832		return r;
 833
 834	return imgu_fmt(imgu, node->pipe, node->id, f, true);
 835}
 836
 837static int imgu_vidioc_s_fmt(struct file *file, void *fh, struct v4l2_format *f)
 838{
 839	struct imgu_device *imgu = video_drvdata(file);
 840	struct device *dev = &imgu->pci_dev->dev;
 841	struct imgu_video_device *node = file_to_intel_imgu_node(file);
 842	struct v4l2_pix_format_mplane *pix_mp = &f->fmt.pix_mp;
 843	int r;
 844
 845	dev_dbg(dev, "%s [%ux%u] for node %u\n", __func__,
 846		pix_mp->width, pix_mp->height, node->id);
 847
 848	r = imgu_try_fmt(file, fh, f);
 849	if (r)
 850		return r;
 851
 852	return imgu_fmt(imgu, node->pipe, node->id, f, false);
 853}
 854
 855struct imgu_meta_fmt {
 856	__u32 fourcc;
 857	char *name;
 858};
 859
 860/* From drivers/media/v4l2-core/v4l2-ioctl.c */
 861static const struct imgu_meta_fmt meta_fmts[] = {
 862	{ V4L2_META_FMT_IPU3_PARAMS, "IPU3 processing parameters" },
 863	{ V4L2_META_FMT_IPU3_STAT_3A, "IPU3 3A statistics" },
 864};
 865
 866static int imgu_meta_enum_format(struct file *file, void *fh,
 867				 struct v4l2_fmtdesc *fmt)
 868{
 869	struct imgu_video_device *node = file_to_intel_imgu_node(file);
 870	unsigned int i = fmt->type == V4L2_BUF_TYPE_META_OUTPUT ? 0 : 1;
 871
 872	/* Each node is dedicated to only one meta format */
 873	if (fmt->index > 0 || fmt->type != node->vbq.type)
 874		return -EINVAL;
 875
 876	if (fmt->mbus_code != 0 && fmt->mbus_code != MEDIA_BUS_FMT_FIXED)
 877		return -EINVAL;
 878
 879	strscpy(fmt->description, meta_fmts[i].name, sizeof(fmt->description));
 880	fmt->pixelformat = meta_fmts[i].fourcc;
 881
 882	return 0;
 883}
 884
 885static int imgu_vidioc_g_meta_fmt(struct file *file, void *fh,
 886				  struct v4l2_format *f)
 887{
 888	struct imgu_video_device *node = file_to_intel_imgu_node(file);
 889
 890	if (f->type != node->vbq.type)
 891		return -EINVAL;
 892
 893	f->fmt = node->vdev_fmt.fmt;
 894
 895	return 0;
 896}
 897
 898/******************** function pointers ********************/
 899
 900static const struct v4l2_subdev_internal_ops imgu_subdev_internal_ops = {
 901	.open = imgu_subdev_open,
 902};
 903
 904static const struct v4l2_subdev_core_ops imgu_subdev_core_ops = {
 905	.subscribe_event = v4l2_ctrl_subdev_subscribe_event,
 906	.unsubscribe_event = v4l2_event_subdev_unsubscribe,
 907};
 908
 909static const struct v4l2_subdev_video_ops imgu_subdev_video_ops = {
 910	.s_stream = imgu_subdev_s_stream,
 911};
 912
 913static const struct v4l2_subdev_pad_ops imgu_subdev_pad_ops = {
 914	.link_validate = v4l2_subdev_link_validate_default,
 915	.get_fmt = imgu_subdev_get_fmt,
 916	.set_fmt = imgu_subdev_set_fmt,
 917	.get_selection = imgu_subdev_get_selection,
 918	.set_selection = imgu_subdev_set_selection,
 919};
 920
 921static const struct v4l2_subdev_ops imgu_subdev_ops = {
 922	.core = &imgu_subdev_core_ops,
 923	.video = &imgu_subdev_video_ops,
 924	.pad = &imgu_subdev_pad_ops,
 925};
 926
 927static const struct media_entity_operations imgu_media_ops = {
 928	.link_setup = imgu_link_setup,
 929	.link_validate = v4l2_subdev_link_validate,
 930};
 931
 932/****************** vb2_ops of the Q ********************/
 933
 934static const struct vb2_ops imgu_vb2_ops = {
 935	.buf_init = imgu_vb2_buf_init,
 936	.buf_cleanup = imgu_vb2_buf_cleanup,
 937	.buf_queue = imgu_vb2_buf_queue,
 938	.queue_setup = imgu_vb2_queue_setup,
 939	.start_streaming = imgu_vb2_start_streaming,
 940	.stop_streaming = imgu_vb2_stop_streaming,
 941};
 942
 943/****************** v4l2_file_operations *****************/
 944
 945static const struct v4l2_file_operations imgu_v4l2_fops = {
 946	.unlocked_ioctl = video_ioctl2,
 947	.open = v4l2_fh_open,
 948	.release = vb2_fop_release,
 949	.poll = vb2_fop_poll,
 950	.mmap = vb2_fop_mmap,
 951};
 952
 953/******************** v4l2_ioctl_ops ********************/
 954
 955static const struct v4l2_ioctl_ops imgu_v4l2_ioctl_ops = {
 956	.vidioc_querycap = imgu_vidioc_querycap,
 957
 958	.vidioc_enum_fmt_vid_cap = vidioc_enum_fmt_vid_cap,
 959	.vidioc_g_fmt_vid_cap_mplane = imgu_vidioc_g_fmt,
 960	.vidioc_s_fmt_vid_cap_mplane = imgu_vidioc_s_fmt,
 961	.vidioc_try_fmt_vid_cap_mplane = imgu_vidioc_try_fmt,
 962
 963	.vidioc_enum_fmt_vid_out = vidioc_enum_fmt_vid_out,
 964	.vidioc_g_fmt_vid_out_mplane = imgu_vidioc_g_fmt,
 965	.vidioc_s_fmt_vid_out_mplane = imgu_vidioc_s_fmt,
 966	.vidioc_try_fmt_vid_out_mplane = imgu_vidioc_try_fmt,
 967
 968	/* buffer queue management */
 969	.vidioc_reqbufs = vb2_ioctl_reqbufs,
 970	.vidioc_create_bufs = vb2_ioctl_create_bufs,
 971	.vidioc_prepare_buf = vb2_ioctl_prepare_buf,
 972	.vidioc_querybuf = vb2_ioctl_querybuf,
 973	.vidioc_qbuf = vb2_ioctl_qbuf,
 974	.vidioc_dqbuf = vb2_ioctl_dqbuf,
 975	.vidioc_streamon = vb2_ioctl_streamon,
 976	.vidioc_streamoff = vb2_ioctl_streamoff,
 977	.vidioc_expbuf = vb2_ioctl_expbuf,
 978};
 979
 980static const struct v4l2_ioctl_ops imgu_v4l2_meta_ioctl_ops = {
 981	.vidioc_querycap = imgu_vidioc_querycap,
 982
 983	/* meta capture */
 984	.vidioc_enum_fmt_meta_cap = imgu_meta_enum_format,
 985	.vidioc_g_fmt_meta_cap = imgu_vidioc_g_meta_fmt,
 986	.vidioc_s_fmt_meta_cap = imgu_vidioc_g_meta_fmt,
 987	.vidioc_try_fmt_meta_cap = imgu_vidioc_g_meta_fmt,
 988
 989	/* meta output */
 990	.vidioc_enum_fmt_meta_out = imgu_meta_enum_format,
 991	.vidioc_g_fmt_meta_out = imgu_vidioc_g_meta_fmt,
 992	.vidioc_s_fmt_meta_out = imgu_vidioc_g_meta_fmt,
 993	.vidioc_try_fmt_meta_out = imgu_vidioc_g_meta_fmt,
 994
 995	.vidioc_reqbufs = vb2_ioctl_reqbufs,
 996	.vidioc_create_bufs = vb2_ioctl_create_bufs,
 997	.vidioc_prepare_buf = vb2_ioctl_prepare_buf,
 998	.vidioc_querybuf = vb2_ioctl_querybuf,
 999	.vidioc_qbuf = vb2_ioctl_qbuf,
1000	.vidioc_dqbuf = vb2_ioctl_dqbuf,
1001	.vidioc_streamon = vb2_ioctl_streamon,
1002	.vidioc_streamoff = vb2_ioctl_streamoff,
1003	.vidioc_expbuf = vb2_ioctl_expbuf,
1004};
1005
1006static int imgu_sd_s_ctrl(struct v4l2_ctrl *ctrl)
1007{
1008	struct imgu_v4l2_subdev *imgu_sd =
1009		container_of(ctrl->handler, struct imgu_v4l2_subdev, ctrl_handler);
1010	struct imgu_device *imgu = v4l2_get_subdevdata(&imgu_sd->subdev);
1011	struct device *dev = &imgu->pci_dev->dev;
1012
1013	dev_dbg(dev, "set val %d to ctrl 0x%8x for subdev %u",
1014		ctrl->val, ctrl->id, imgu_sd->pipe);
1015
1016	switch (ctrl->id) {
1017	case V4L2_CID_INTEL_IPU3_MODE:
1018		atomic_set(&imgu_sd->running_mode, ctrl->val);
1019		return 0;
1020	default:
1021		return -EINVAL;
1022	}
1023}
1024
1025static const struct v4l2_ctrl_ops imgu_subdev_ctrl_ops = {
1026	.s_ctrl = imgu_sd_s_ctrl,
1027};
1028
1029static const char * const imgu_ctrl_mode_strings[] = {
1030	"Video mode",
1031	"Still mode",
1032};
1033
1034static const struct v4l2_ctrl_config imgu_subdev_ctrl_mode = {
1035	.ops = &imgu_subdev_ctrl_ops,
1036	.id = V4L2_CID_INTEL_IPU3_MODE,
1037	.name = "IPU3 Pipe Mode",
1038	.type = V4L2_CTRL_TYPE_MENU,
1039	.max = ARRAY_SIZE(imgu_ctrl_mode_strings) - 1,
1040	.def = IPU3_RUNNING_MODE_VIDEO,
1041	.qmenu = imgu_ctrl_mode_strings,
1042};
1043
1044/******************** Framework registration ********************/
1045
1046/* helper function to config node's video properties */
1047static void imgu_node_to_v4l2(u32 node, struct video_device *vdev,
1048			      struct v4l2_format *f)
1049{
1050	u32 cap;
1051
1052	/* Should not happen */
1053	WARN_ON(node >= IMGU_NODE_NUM);
1054
1055	switch (node) {
1056	case IMGU_NODE_IN:
1057		cap = V4L2_CAP_VIDEO_OUTPUT_MPLANE;
1058		f->type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
1059		vdev->ioctl_ops = &imgu_v4l2_ioctl_ops;
1060		break;
1061	case IMGU_NODE_PARAMS:
1062		cap = V4L2_CAP_META_OUTPUT;
1063		f->type = V4L2_BUF_TYPE_META_OUTPUT;
1064		f->fmt.meta.dataformat = V4L2_META_FMT_IPU3_PARAMS;
1065		vdev->ioctl_ops = &imgu_v4l2_meta_ioctl_ops;
1066		imgu_css_meta_fmt_set(&f->fmt.meta);
1067		break;
1068	case IMGU_NODE_STAT_3A:
1069		cap = V4L2_CAP_META_CAPTURE;
1070		f->type = V4L2_BUF_TYPE_META_CAPTURE;
1071		f->fmt.meta.dataformat = V4L2_META_FMT_IPU3_STAT_3A;
1072		vdev->ioctl_ops = &imgu_v4l2_meta_ioctl_ops;
1073		imgu_css_meta_fmt_set(&f->fmt.meta);
1074		break;
1075	default:
1076		cap = V4L2_CAP_VIDEO_CAPTURE_MPLANE;
1077		f->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
1078		vdev->ioctl_ops = &imgu_v4l2_ioctl_ops;
1079	}
1080
1081	vdev->device_caps = V4L2_CAP_STREAMING | V4L2_CAP_IO_MC | cap;
1082}
1083
1084static int imgu_v4l2_subdev_register(struct imgu_device *imgu,
1085				     struct imgu_v4l2_subdev *imgu_sd,
1086				     unsigned int pipe)
1087{
1088	int i, r;
1089	struct v4l2_ctrl_handler *hdl = &imgu_sd->ctrl_handler;
1090	struct imgu_media_pipe *imgu_pipe = &imgu->imgu_pipe[pipe];
1091
1092	/* Initialize subdev media entity */
1093	imgu_sd->subdev.entity.ops = &imgu_media_ops;
1094	for (i = 0; i < IMGU_NODE_NUM; i++) {
1095		imgu_sd->subdev_pads[i].flags = imgu_pipe->nodes[i].output ?
1096			MEDIA_PAD_FL_SINK : MEDIA_PAD_FL_SOURCE;
1097	}
1098	r = media_entity_pads_init(&imgu_sd->subdev.entity, IMGU_NODE_NUM,
1099				   imgu_sd->subdev_pads);
1100	if (r) {
1101		dev_err(&imgu->pci_dev->dev,
1102			"failed initialize subdev media entity (%d)\n", r);
1103		return r;
1104	}
1105
1106	/* Initialize subdev */
1107	v4l2_subdev_init(&imgu_sd->subdev, &imgu_subdev_ops);
1108	imgu_sd->subdev.entity.function = MEDIA_ENT_F_PROC_VIDEO_STATISTICS;
1109	imgu_sd->subdev.internal_ops = &imgu_subdev_internal_ops;
1110	imgu_sd->subdev.flags = V4L2_SUBDEV_FL_HAS_DEVNODE |
1111				V4L2_SUBDEV_FL_HAS_EVENTS;
1112	snprintf(imgu_sd->subdev.name, sizeof(imgu_sd->subdev.name),
1113		 "%s %u", IMGU_NAME, pipe);
1114	v4l2_set_subdevdata(&imgu_sd->subdev, imgu);
1115	atomic_set(&imgu_sd->running_mode, IPU3_RUNNING_MODE_VIDEO);
1116	v4l2_ctrl_handler_init(hdl, 1);
1117	imgu_sd->subdev.ctrl_handler = hdl;
1118	imgu_sd->ctrl = v4l2_ctrl_new_custom(hdl, &imgu_subdev_ctrl_mode, NULL);
1119	if (hdl->error) {
1120		r = hdl->error;
1121		dev_err(&imgu->pci_dev->dev,
1122			"failed to create subdev v4l2 ctrl with err %d", r);
1123		goto fail_subdev;
1124	}
1125	r = v4l2_device_register_subdev(&imgu->v4l2_dev, &imgu_sd->subdev);
1126	if (r) {
1127		dev_err(&imgu->pci_dev->dev,
1128			"failed initialize subdev (%d)\n", r);
1129		goto fail_subdev;
1130	}
1131
1132	imgu_sd->pipe = pipe;
1133	return 0;
1134
1135fail_subdev:
1136	v4l2_ctrl_handler_free(imgu_sd->subdev.ctrl_handler);
1137	media_entity_cleanup(&imgu_sd->subdev.entity);
1138
1139	return r;
1140}
1141
1142static int imgu_v4l2_node_setup(struct imgu_device *imgu, unsigned int pipe,
1143				int node_num)
1144{
1145	int r;
1146	u32 flags;
1147	struct v4l2_mbus_framefmt def_bus_fmt = { 0 };
1148	struct v4l2_pix_format_mplane def_pix_fmt = { 0 };
1149	struct device *dev = &imgu->pci_dev->dev;
1150	struct imgu_media_pipe *imgu_pipe = &imgu->imgu_pipe[pipe];
1151	struct v4l2_subdev *sd = &imgu_pipe->imgu_sd.subdev;
1152	struct imgu_video_device *node = &imgu_pipe->nodes[node_num];
1153	struct video_device *vdev = &node->vdev;
1154	struct vb2_queue *vbq = &node->vbq;
1155
1156	/* Initialize formats to default values */
1157	def_bus_fmt.width = 1920;
1158	def_bus_fmt.height = 1080;
1159	def_bus_fmt.code = MEDIA_BUS_FMT_FIXED;
1160	def_bus_fmt.field = V4L2_FIELD_NONE;
1161	def_bus_fmt.colorspace = V4L2_COLORSPACE_RAW;
1162	def_bus_fmt.ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT;
1163	def_bus_fmt.quantization = V4L2_QUANTIZATION_DEFAULT;
1164	def_bus_fmt.xfer_func = V4L2_XFER_FUNC_DEFAULT;
1165
1166	def_pix_fmt.width = def_bus_fmt.width;
1167	def_pix_fmt.height = def_bus_fmt.height;
1168	def_pix_fmt.field = def_bus_fmt.field;
1169	def_pix_fmt.num_planes = 1;
1170	def_pix_fmt.plane_fmt[0].bytesperline =
1171		imgu_bytesperline(def_pix_fmt.width,
1172				  IMGU_ABI_FRAME_FORMAT_RAW_PACKED);
1173	def_pix_fmt.plane_fmt[0].sizeimage =
1174		def_pix_fmt.height * def_pix_fmt.plane_fmt[0].bytesperline;
1175	def_pix_fmt.flags = 0;
1176	def_pix_fmt.colorspace = def_bus_fmt.colorspace;
1177	def_pix_fmt.ycbcr_enc = def_bus_fmt.ycbcr_enc;
1178	def_pix_fmt.quantization = def_bus_fmt.quantization;
1179	def_pix_fmt.xfer_func = def_bus_fmt.xfer_func;
1180
1181	/* Initialize miscellaneous variables */
1182	mutex_init(&node->lock);
1183	INIT_LIST_HEAD(&node->buffers);
1184
1185	/* Initialize formats to default values */
1186	node->pad_fmt = def_bus_fmt;
1187	node->id = node_num;
1188	node->pipe = pipe;
1189	imgu_node_to_v4l2(node_num, vdev, &node->vdev_fmt);
1190	if (node->vdev_fmt.type ==
1191	    V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE ||
1192	    node->vdev_fmt.type ==
1193	    V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
1194		def_pix_fmt.pixelformat = node->output ?
1195			V4L2_PIX_FMT_IPU3_SGRBG10 :
1196			V4L2_PIX_FMT_NV12;
1197		node->vdev_fmt.fmt.pix_mp = def_pix_fmt;
1198	}
1199
1200	/* Initialize media entities */
1201	node->vdev_pad.flags = node->output ?
1202		MEDIA_PAD_FL_SOURCE : MEDIA_PAD_FL_SINK;
1203	vdev->entity.ops = NULL;
1204	r = media_entity_pads_init(&vdev->entity, 1, &node->vdev_pad);
1205	if (r) {
1206		dev_err(dev, "failed initialize media entity (%d)\n", r);
1207		mutex_destroy(&node->lock);
1208		return r;
1209	}
1210
1211	/* Initialize vbq */
1212	vbq->type = node->vdev_fmt.type;
1213	vbq->io_modes = VB2_USERPTR | VB2_MMAP | VB2_DMABUF;
1214	vbq->ops = &imgu_vb2_ops;
1215	vbq->mem_ops = &vb2_dma_sg_memops;
1216	if (imgu->buf_struct_size <= 0)
1217		imgu->buf_struct_size =
1218			sizeof(struct imgu_vb2_buffer);
1219	vbq->buf_struct_size = imgu->buf_struct_size;
1220	vbq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
1221	/* can streamon w/o buffers */
1222	vbq->min_queued_buffers = 0;
1223	vbq->drv_priv = imgu;
1224	vbq->lock = &node->lock;
1225	r = vb2_queue_init(vbq);
1226	if (r) {
1227		dev_err(dev, "failed to initialize video queue (%d)", r);
1228		media_entity_cleanup(&vdev->entity);
1229		return r;
1230	}
1231
1232	/* Initialize vdev */
1233	snprintf(vdev->name, sizeof(vdev->name), "%s %u %s",
1234		 IMGU_NAME, pipe, node->name);
1235	vdev->release = video_device_release_empty;
1236	vdev->fops = &imgu_v4l2_fops;
1237	vdev->lock = &node->lock;
1238	vdev->v4l2_dev = &imgu->v4l2_dev;
1239	vdev->queue = &node->vbq;
1240	vdev->vfl_dir = node->output ? VFL_DIR_TX : VFL_DIR_RX;
1241	video_set_drvdata(vdev, imgu);
1242	r = video_register_device(vdev, VFL_TYPE_VIDEO, -1);
1243	if (r) {
1244		dev_err(dev, "failed to register video device (%d)", r);
1245		media_entity_cleanup(&vdev->entity);
1246		return r;
1247	}
1248
1249	/* Create link between video node and the subdev pad */
1250	flags = 0;
1251	if (node->enabled)
1252		flags |= MEDIA_LNK_FL_ENABLED;
1253	if (node->output) {
1254		r = media_create_pad_link(&vdev->entity, 0, &sd->entity,
1255					  node_num, flags);
1256	} else {
1257		if (node->id == IMGU_NODE_OUT) {
1258			flags |= MEDIA_LNK_FL_ENABLED | MEDIA_LNK_FL_IMMUTABLE;
1259			node->enabled = true;
1260		}
1261
1262		r = media_create_pad_link(&sd->entity, node_num, &vdev->entity,
1263					  0, flags);
1264	}
1265	if (r) {
1266		dev_err(dev, "failed to create pad link (%d)", r);
1267		video_unregister_device(vdev);
1268		return r;
1269	}
1270
1271	return 0;
1272}
1273
1274static void imgu_v4l2_nodes_cleanup_pipe(struct imgu_device *imgu,
1275					 unsigned int pipe, int node)
1276{
1277	int i;
1278	struct imgu_media_pipe *imgu_pipe = &imgu->imgu_pipe[pipe];
1279
1280	for (i = 0; i < node; i++) {
1281		video_unregister_device(&imgu_pipe->nodes[i].vdev);
1282		media_entity_cleanup(&imgu_pipe->nodes[i].vdev.entity);
1283		mutex_destroy(&imgu_pipe->nodes[i].lock);
1284	}
1285}
1286
1287static int imgu_v4l2_nodes_setup_pipe(struct imgu_device *imgu, int pipe)
1288{
1289	int i;
1290
1291	for (i = 0; i < IMGU_NODE_NUM; i++) {
1292		int r = imgu_v4l2_node_setup(imgu, pipe, i);
1293
1294		if (r) {
1295			imgu_v4l2_nodes_cleanup_pipe(imgu, pipe, i);
1296			return r;
1297		}
1298	}
1299	return 0;
1300}
1301
1302static void imgu_v4l2_subdev_cleanup(struct imgu_device *imgu, unsigned int i)
1303{
1304	struct imgu_media_pipe *imgu_pipe = &imgu->imgu_pipe[i];
1305
1306	v4l2_device_unregister_subdev(&imgu_pipe->imgu_sd.subdev);
1307	v4l2_ctrl_handler_free(imgu_pipe->imgu_sd.subdev.ctrl_handler);
1308	media_entity_cleanup(&imgu_pipe->imgu_sd.subdev.entity);
1309}
1310
1311static void imgu_v4l2_cleanup_pipes(struct imgu_device *imgu, unsigned int pipe)
1312{
1313	int i;
1314
1315	for (i = 0; i < pipe; i++) {
1316		imgu_v4l2_nodes_cleanup_pipe(imgu, i, IMGU_NODE_NUM);
1317		imgu_v4l2_subdev_cleanup(imgu, i);
1318	}
1319}
1320
1321static int imgu_v4l2_register_pipes(struct imgu_device *imgu)
1322{
1323	struct imgu_media_pipe *imgu_pipe;
1324	int i, r;
1325
1326	for (i = 0; i < IMGU_MAX_PIPE_NUM; i++) {
1327		imgu_pipe = &imgu->imgu_pipe[i];
1328		r = imgu_v4l2_subdev_register(imgu, &imgu_pipe->imgu_sd, i);
1329		if (r) {
1330			dev_err(&imgu->pci_dev->dev,
1331				"failed to register subdev%u ret (%d)\n", i, r);
1332			goto pipes_cleanup;
1333		}
1334		r = imgu_v4l2_nodes_setup_pipe(imgu, i);
1335		if (r) {
1336			imgu_v4l2_subdev_cleanup(imgu, i);
1337			goto pipes_cleanup;
1338		}
1339	}
1340
1341	return 0;
1342
1343pipes_cleanup:
1344	imgu_v4l2_cleanup_pipes(imgu, i);
1345	return r;
1346}
1347
1348int imgu_v4l2_register(struct imgu_device *imgu)
1349{
1350	int r;
1351
1352	/* Initialize miscellaneous variables */
1353	imgu->streaming = false;
1354
1355	/* Set up media device */
1356	media_device_pci_init(&imgu->media_dev, imgu->pci_dev, IMGU_NAME);
1357
1358	/* Set up v4l2 device */
1359	imgu->v4l2_dev.mdev = &imgu->media_dev;
1360	imgu->v4l2_dev.ctrl_handler = NULL;
1361	r = v4l2_device_register(&imgu->pci_dev->dev, &imgu->v4l2_dev);
1362	if (r) {
1363		dev_err(&imgu->pci_dev->dev,
1364			"failed to register V4L2 device (%d)\n", r);
1365		goto fail_v4l2_dev;
1366	}
1367
1368	r = imgu_v4l2_register_pipes(imgu);
1369	if (r) {
1370		dev_err(&imgu->pci_dev->dev,
1371			"failed to register pipes (%d)\n", r);
1372		goto fail_v4l2_pipes;
1373	}
1374
1375	r = v4l2_device_register_subdev_nodes(&imgu->v4l2_dev);
1376	if (r) {
1377		dev_err(&imgu->pci_dev->dev,
1378			"failed to register subdevs (%d)\n", r);
1379		goto fail_subdevs;
1380	}
1381
1382	r = media_device_register(&imgu->media_dev);
1383	if (r) {
1384		dev_err(&imgu->pci_dev->dev,
1385			"failed to register media device (%d)\n", r);
1386		goto fail_subdevs;
1387	}
1388
1389	return 0;
1390
1391fail_subdevs:
1392	imgu_v4l2_cleanup_pipes(imgu, IMGU_MAX_PIPE_NUM);
1393fail_v4l2_pipes:
1394	v4l2_device_unregister(&imgu->v4l2_dev);
1395fail_v4l2_dev:
1396	media_device_cleanup(&imgu->media_dev);
1397
1398	return r;
1399}
1400
1401int imgu_v4l2_unregister(struct imgu_device *imgu)
1402{
1403	media_device_unregister(&imgu->media_dev);
1404	imgu_v4l2_cleanup_pipes(imgu, IMGU_MAX_PIPE_NUM);
1405	v4l2_device_unregister(&imgu->v4l2_dev);
1406	media_device_cleanup(&imgu->media_dev);
1407
1408	return 0;
1409}
1410
1411void imgu_v4l2_buffer_done(struct vb2_buffer *vb,
1412			   enum vb2_buffer_state state)
1413{
1414	struct imgu_vb2_buffer *b =
1415		container_of(vb, struct imgu_vb2_buffer, vbb.vb2_buf);
1416
1417	list_del(&b->list);
1418	vb2_buffer_done(&b->vbb.vb2_buf, state);
1419}