---

static void mxc_isi_video_free_discard_buffers(struct mxc_isi_video *video)
{
	unsigned int i;

	for (i = 0; i < video->pix.num_planes; i++) {
		struct mxc_isi_dma_buffer *buf = &video->discard_buffer[i];

		if (!buf->addr)
			continue;

		dma_free_coherent(video->pipe->isi->dev, buf->size, buf->addr,
				  buf->dma);
		buf->addr = NULL;
	}
}

static int mxc_isi_video_alloc_discard_buffers(struct mxc_isi_video *video)
{
	unsigned int i, j;

	/* Allocate memory for each plane. */
	for (i = 0; i < video->pix.num_planes; i++) {
		struct mxc_isi_dma_buffer *buf = &video->discard_buffer[i];

		buf->size = PAGE_ALIGN(video->pix.plane_fmt[i].sizeimage);
		buf->addr = dma_alloc_coherent(video->pipe->isi->dev, buf->size,
					       &buf->dma, GFP_DMA | GFP_KERNEL);
		if (!buf->addr) {
			mxc_isi_video_free_discard_buffers(video);
			return -ENOMEM;
		}

		dev_dbg(video->pipe->isi->dev,
			"discard buffer plane %u: %zu bytes @%pad (CPU address %p)\n",
			i, buf->size, &buf->dma, buf->addr);
	}

	/* Fill the DMA addresses in the discard buffers. */
	for (i = 0; i < ARRAY_SIZE(video->buf_discard); ++i) {
		struct mxc_isi_buffer *buf = &video->buf_discard[i];

		buf->discard = true;

		for (j = 0; j < video->pix.num_planes; ++j)
			buf->dma_addrs[j] = video->discard_buffer[j].dma;
	}

	return 0;
}

static int mxc_isi_video_validate_format(struct mxc_isi_video *video)
{
	const struct v4l2_mbus_framefmt *format;
	const struct mxc_isi_format_info *info;
	struct v4l2_subdev_state *state;
	struct v4l2_subdev *sd = &video->pipe->sd;
	int ret = 0;

	state = v4l2_subdev_lock_and_get_active_state(sd);

	info = mxc_isi_format_by_fourcc(video->pix.pixelformat,
					MXC_ISI_VIDEO_CAP);
	format = v4l2_subdev_state_get_format(state, MXC_ISI_PIPE_PAD_SOURCE);
#ifdef MY_CODE
	if (format->code != info->mbus_code ||
	    format->width != video->pix.width ||
	    format->height != video->pix.height) {
		dev_info(video->pipe->isi->dev,
			"%s: configuration mismatch, 0x%04x/%ux%u != 0x%04x/%ux%u\n",
			__func__, format->code, format->width, format->height,
			info->mbus_code, video->pix.width, video->pix.height);
		ret = -EINVAL;
	}
#endif
	v4l2_subdev_unlock_state(state);

	return ret;
}

static void mxc_isi_video_return_buffers(struct mxc_isi_video *video,
					 enum vb2_buffer_state state)
{
	struct mxc_isi_buffer *buf;

	spin_lock_irq(&video->buf_lock);

	while (!list_empty(&video->out_active)) {
		buf = list_first_entry(&video->out_active,
				       struct mxc_isi_buffer, list);
		list_del_init(&buf->list);
		if (buf->discard)
			continue;

		vb2_buffer_done(&buf->v4l2_buf.vb2_buf, state);
	}

	while (!list_empty(&video->out_pending)) {
		buf = list_first_entry(&video->out_pending,
				       struct mxc_isi_buffer, list);
		list_del_init(&buf->list);
		vb2_buffer_done(&buf->v4l2_buf.vb2_buf, state);
	}

	while (!list_empty(&video->out_discard)) {
		buf = list_first_entry(&video->out_discard,
				       struct mxc_isi_buffer, list);
		list_del_init(&buf->list);
	}

	INIT_LIST_HEAD(&video->out_active);
	INIT_LIST_HEAD(&video->out_pending);
	INIT_LIST_HEAD(&video->out_discard);

	spin_unlock_irq(&video->buf_lock);
}

static void mxc_isi_video_queue_first_buffers(struct mxc_isi_video *video)
{
	const struct mxc_isi_plat_data *pdata = video->pipe->isi->pdata;
	unsigned int discard;
	unsigned int i;

	lockdep_assert_held(&video->buf_lock);

	/*
	 * Queue two ISI channel output buffers. We are not guaranteed to have
	 * any buffer in the pending list when this function is called from the
	 * system resume handler. Use pending buffers as much as possible, and
	 * use discard buffers to fill the remaining slots.
	 */

	/* How many discard buffers do we need to queue first ? */
	discard = list_empty(&video->out_pending) ? 2
		: list_is_singular(&video->out_pending) ? 1
		: 0;

	for (i = 0; i < 2; ++i) {
		enum mxc_isi_buf_id buf_id = i == 0 ? MXC_ISI_BUF1
					   : MXC_ISI_BUF2;
		struct mxc_isi_buffer *buf;
		struct list_head *list;

		list = i < discard ? &video->out_discard : &video->out_pending;
		buf = list_first_entry(list, struct mxc_isi_buffer, list);

		mxc_isi_channel_set_outbuf(video->pipe, buf->dma_addrs, buf_id);
		mxc_isi_channel_set_max_size(video->pipe, &video->pix, pdata->buf_max_size);
		buf->id = buf_id;
		list_move_tail(&buf->list, &video->out_active);
	}
}

static inline struct mxc_isi_buffer *to_isi_buffer(struct vb2_v4l2_buffer *v4l2_buf)
{
	return container_of(v4l2_buf, struct mxc_isi_buffer, v4l2_buf);
}

int mxc_isi_video_queue_setup(const struct v4l2_pix_format_mplane *format,
			      const struct mxc_isi_format_info *info,
			      unsigned int *num_buffers,
			      unsigned int *num_planes, unsigned int sizes[])
{
	unsigned int i;

	if (*num_planes) {
		if (*num_planes != info->mem_planes)
			return -EINVAL;

		for (i = 0; i < info->mem_planes; ++i) {
			if (sizes[i] < format->plane_fmt[i].sizeimage)
				return -EINVAL;
		}

		return 0;
	}

	*num_planes = info->mem_planes;

	for (i = 0; i < info->mem_planes; ++i)
		sizes[i] = format->plane_fmt[i].sizeimage;

	return 0;
}

void mxc_isi_video_buffer_init(struct vb2_buffer *vb2, dma_addr_t dma_addrs[3],
			       const struct mxc_isi_format_info *info,
			       const struct v4l2_pix_format_mplane *pix)
{
	unsigned int i;

	for (i = 0; i < info->mem_planes; ++i)
		dma_addrs[i] = vb2_dma_contig_plane_dma_addr(vb2, i);

	/*
	 * For single-planar pixel formats with multiple color planes, split
	 * the buffer into color planes.
	 */
	if (info->color_planes != info->mem_planes) {
		unsigned int size = pix->plane_fmt[0].bytesperline * pix->height;

		for (i = 1; i < info->color_planes; ++i) {
			unsigned int vsub = i > 1 ? info->vsub : 1;

			dma_addrs[i] = dma_addrs[i - 1] + size / vsub;
		}
	}
}

int mxc_isi_video_buffer_prepare(struct mxc_isi_dev *isi, struct vb2_buffer *vb2,
				 const struct mxc_isi_format_info *info,
				 const struct v4l2_pix_format_mplane *pix)
{
	struct vb2_v4l2_buffer *v4l2_buf = to_vb2_v4l2_buffer(vb2);
	unsigned int i;

	for (i = 0; i < info->mem_planes; i++) {
		unsigned long size = pix->plane_fmt[i].sizeimage;

		if (vb2_plane_size(vb2, i) < size) {
			dev_err(isi->dev, "User buffer too small (%ld < %ld)\n",
				vb2_plane_size(vb2, i), size);
			return -EINVAL;
		}

		vb2_set_plane_payload(vb2, i, size);
	}

	v4l2_buf->field = pix->field;

	return 0;
}

static int mxc_isi_vb2_queue_setup(struct vb2_queue *q,
				   unsigned int *num_buffers,
				   unsigned int *num_planes,
				   unsigned int sizes[],
				   struct device *alloc_devs[])
{
	struct mxc_isi_video *video = vb2_get_drv_priv(q);

	return mxc_isi_video_queue_setup(&video->pix, video->fmtinfo,
					 num_buffers, num_planes, sizes);
}

static int mxc_isi_vb2_buffer_init(struct vb2_buffer *vb2)
{
	struct mxc_isi_buffer *buf = to_isi_buffer(to_vb2_v4l2_buffer(vb2));
	struct mxc_isi_video *video = vb2_get_drv_priv(vb2->vb2_queue);

	mxc_isi_video_buffer_init(vb2, buf->dma_addrs, video->fmtinfo,
				  &video->pix);

	return 0;
}

static int mxc_isi_vb2_buffer_prepare(struct vb2_buffer *vb2)
{
	struct mxc_isi_video *video = vb2_get_drv_priv(vb2->vb2_queue);

	return mxc_isi_video_buffer_prepare(video->pipe->isi, vb2,
					    video->fmtinfo, &video->pix);
}