rga-buf.c

  1// SPDX-License-Identifier: GPL-2.0-only
  2/*
  3 * Copyright (C) 2017 Rockchip Electronics Co., Ltd.
  4 * Author: Jacob Chen <jacob-chen@iotwrt.com>
  5 */
  6
  7#include <linux/pm_runtime.h>
  8#include <linux/scatterlist.h>
  9
 10#include <media/v4l2-common.h>
 11#include <media/v4l2-device.h>
 12#include <media/v4l2-ioctl.h>
 13#include <media/v4l2-mem2mem.h>
 14#include <media/videobuf2-dma-sg.h>
 15#include <media/videobuf2-v4l2.h>
 16
 17#include "rga-hw.h"
 18#include "rga.h"
 19
 20static ssize_t fill_descriptors(struct rga_dma_desc *desc, size_t max_desc,
 21				struct sg_table *sgt)
 22{
 23	struct sg_dma_page_iter iter;
 24	struct rga_dma_desc *tmp = desc;
 25	size_t n_desc = 0;
 26	dma_addr_t addr;
 27
 28	for_each_sgtable_dma_page(sgt, &iter, 0) {
 29		if (n_desc > max_desc)
 30			return -EINVAL;
 31		addr = sg_page_iter_dma_address(&iter);
 32		tmp->addr = lower_32_bits(addr);
 33		tmp++;
 34		n_desc++;
 35	}
 36
 37	return n_desc;
 38}
 39
 40static int
 41rga_queue_setup(struct vb2_queue *vq,
 42		unsigned int *nbuffers, unsigned int *nplanes,
 43		unsigned int sizes[], struct device *alloc_devs[])
 44{
 45	struct rga_ctx *ctx = vb2_get_drv_priv(vq);
 46	struct rga_frame *f = rga_get_frame(ctx, vq->type);
 47	const struct v4l2_pix_format_mplane *pix_fmt;
 48	int i;
 49
 50	if (IS_ERR(f))
 51		return PTR_ERR(f);
 52
 53	pix_fmt = &f->pix;
 54
 55	if (*nplanes) {
 56		if (*nplanes != pix_fmt->num_planes)
 57			return -EINVAL;
 58
 59		for (i = 0; i < pix_fmt->num_planes; i++)
 60			if (sizes[i] < pix_fmt->plane_fmt[i].sizeimage)
 61				return -EINVAL;
 62
 63		return 0;
 64	}
 65
 66	*nplanes = pix_fmt->num_planes;
 67
 68	for (i = 0; i < pix_fmt->num_planes; i++)
 69		sizes[i] = pix_fmt->plane_fmt[i].sizeimage;
 70
 71	return 0;
 72}
 73
 74static int rga_buf_init(struct vb2_buffer *vb)
 75{
 76	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
 77	struct rga_vb_buffer *rbuf = vb_to_rga(vbuf);
 78	struct rga_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
 79	struct rockchip_rga *rga = ctx->rga;
 80	struct rga_frame *f = rga_get_frame(ctx, vb->vb2_queue->type);
 81	size_t n_desc = 0;
 82
 83	if (IS_ERR(f))
 84		return PTR_ERR(f);
 85
 86	n_desc = DIV_ROUND_UP(f->size, PAGE_SIZE);
 87
 88	rbuf->n_desc = n_desc;
 89	rbuf->dma_desc = dma_alloc_coherent(rga->dev,
 90					    rbuf->n_desc * sizeof(*rbuf->dma_desc),
 91					    &rbuf->dma_desc_pa, GFP_KERNEL);
 92	if (!rbuf->dma_desc)
 93		return -ENOMEM;
 94
 95	return 0;
 96}
 97
 98static int get_plane_offset(struct rga_frame *f, int plane)
 99{
100	if (plane == 0)
101		return 0;
102	if (plane == 1)
103		return f->width * f->height;
104	if (plane == 2)
105		return f->width * f->height + (f->width * f->height / f->fmt->uv_factor);
106
107	return -EINVAL;
108}
109
110static int rga_buf_prepare(struct vb2_buffer *vb)
111{
112	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
113	struct rga_vb_buffer *rbuf = vb_to_rga(vbuf);
114	struct rga_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
115	struct rga_frame *f = rga_get_frame(ctx, vb->vb2_queue->type);
116	ssize_t n_desc = 0;
117	size_t curr_desc = 0;
118	int i;
119	const struct v4l2_format_info *info;
120	unsigned int offsets[VIDEO_MAX_PLANES];
121
122	if (IS_ERR(f))
123		return PTR_ERR(f);
124
125	if (V4L2_TYPE_IS_OUTPUT(vb->vb2_queue->type)) {
126		if (vbuf->field == V4L2_FIELD_ANY)
127			vbuf->field = V4L2_FIELD_NONE;
128		if (vbuf->field != V4L2_FIELD_NONE)
129			return -EINVAL;
130	}
131
132	for (i = 0; i < vb->num_planes; i++) {
133		vb2_set_plane_payload(vb, i, f->pix.plane_fmt[i].sizeimage);
134
135		/* Create local MMU table for RGA */
136		n_desc = fill_descriptors(&rbuf->dma_desc[curr_desc],
137					  rbuf->n_desc - curr_desc,
138					  vb2_dma_sg_plane_desc(vb, i));
139		if (n_desc < 0) {
140			v4l2_err(&ctx->rga->v4l2_dev,
141				 "Failed to map video buffer to RGA\n");
142			return n_desc;
143		}
144		offsets[i] = curr_desc << PAGE_SHIFT;
145		curr_desc += n_desc;
146	}
147
148	/* Fill the remaining planes */
149	info = v4l2_format_info(f->fmt->fourcc);
150	for (i = info->mem_planes; i < info->comp_planes; i++)
151		offsets[i] = get_plane_offset(f, i);
152
153	rbuf->offset.y_off = offsets[0];
154	rbuf->offset.u_off = offsets[1];
155	rbuf->offset.v_off = offsets[2];
156
157	return 0;
158}
159
160static void rga_buf_queue(struct vb2_buffer *vb)
161{
162	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
163	struct rga_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
164
165	v4l2_m2m_buf_queue(ctx->fh.m2m_ctx, vbuf);
166}
167
168static void rga_buf_cleanup(struct vb2_buffer *vb)
169{
170	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
171	struct rga_vb_buffer *rbuf = vb_to_rga(vbuf);
172	struct rga_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
173	struct rockchip_rga *rga = ctx->rga;
174
175	dma_free_coherent(rga->dev, rbuf->n_desc * sizeof(*rbuf->dma_desc),
176			  rbuf->dma_desc, rbuf->dma_desc_pa);
177}
178
179static void rga_buf_return_buffers(struct vb2_queue *q,
180				   enum vb2_buffer_state state)
181{
182	struct rga_ctx *ctx = vb2_get_drv_priv(q);
183	struct vb2_v4l2_buffer *vbuf;
184
185	for (;;) {
186		if (V4L2_TYPE_IS_OUTPUT(q->type))
187			vbuf = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
188		else
189			vbuf = v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
190		if (!vbuf)
191			break;
192		v4l2_m2m_buf_done(vbuf, state);
193	}
194}
195
196static int rga_buf_start_streaming(struct vb2_queue *q, unsigned int count)
197{
198	struct rga_ctx *ctx = vb2_get_drv_priv(q);
199	struct rockchip_rga *rga = ctx->rga;
200	int ret;
201
202	ret = pm_runtime_resume_and_get(rga->dev);
203	if (ret < 0) {
204		rga_buf_return_buffers(q, VB2_BUF_STATE_QUEUED);
205		return ret;
206	}
207
208	if (V4L2_TYPE_IS_OUTPUT(q->type))
209		ctx->osequence = 0;
210	else
211		ctx->csequence = 0;
212
213	return 0;
214}
215
216static void rga_buf_stop_streaming(struct vb2_queue *q)
217{
218	struct rga_ctx *ctx = vb2_get_drv_priv(q);
219	struct rockchip_rga *rga = ctx->rga;
220
221	rga_buf_return_buffers(q, VB2_BUF_STATE_ERROR);
222	pm_runtime_put(rga->dev);
223}
224
225const struct vb2_ops rga_qops = {
226	.queue_setup = rga_queue_setup,
227	.buf_init = rga_buf_init,
228	.buf_prepare = rga_buf_prepare,
229	.buf_queue = rga_buf_queue,
230	.buf_cleanup = rga_buf_cleanup,
231	.start_streaming = rga_buf_start_streaming,
232	.stop_streaming = rga_buf_stop_streaming,
233};