1 // SPDX-License-Identifier: GPL-2.0-or-later
2 /*
3  * Samsung S5P/EXYNOS4 SoC series FIMC (video postprocessor) driver
4  *
5  * Copyright (C) 2012 - 2013 Samsung Electronics Co., Ltd.
6  * Sylwester Nawrocki <s.nawrocki@samsung.com>
7  */
8 
9 #include <linux/module.h>
10 #include <linux/kernel.h>
11 #include <linux/types.h>
12 #include <linux/errno.h>
13 #include <linux/bug.h>
14 #include <linux/interrupt.h>
15 #include <linux/device.h>
16 #include <linux/platform_device.h>
17 #include <linux/pm_runtime.h>
18 #include <linux/list.h>
19 #include <linux/io.h>
20 #include <linux/slab.h>
21 #include <linux/clk.h>
22 #include <media/v4l2-ioctl.h>
23 #include <media/videobuf2-v4l2.h>
24 #include <media/videobuf2-dma-contig.h>
25 
26 #include "common.h"
27 #include "fimc-core.h"
28 #include "fimc-reg.h"
29 #include "media-dev.h"
30 
get_m2m_fmt_flags(unsigned int stream_type)31 static unsigned int get_m2m_fmt_flags(unsigned int stream_type)
32 {
33 	if (stream_type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
34 		return FMT_FLAGS_M2M_IN;
35 	else
36 		return FMT_FLAGS_M2M_OUT;
37 }
38 
fimc_m2m_job_finish(struct fimc_ctx * ctx,int vb_state)39 void fimc_m2m_job_finish(struct fimc_ctx *ctx, int vb_state)
40 {
41 	struct vb2_v4l2_buffer *src_vb, *dst_vb;
42 
43 	if (!ctx || !ctx->fh.m2m_ctx)
44 		return;
45 
46 	src_vb = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
47 	dst_vb = v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
48 
49 	if (src_vb)
50 		v4l2_m2m_buf_done(src_vb, vb_state);
51 	if (dst_vb)
52 		v4l2_m2m_buf_done(dst_vb, vb_state);
53 	if (src_vb && dst_vb)
54 		v4l2_m2m_job_finish(ctx->fimc_dev->m2m.m2m_dev,
55 				    ctx->fh.m2m_ctx);
56 }
57 
58 /* Complete the transaction which has been scheduled for execution. */
fimc_m2m_shutdown(struct fimc_ctx * ctx)59 static void fimc_m2m_shutdown(struct fimc_ctx *ctx)
60 {
61 	struct fimc_dev *fimc = ctx->fimc_dev;
62 
63 	if (!fimc_m2m_pending(fimc))
64 		return;
65 
66 	fimc_ctx_state_set(FIMC_CTX_SHUT, ctx);
67 
68 	wait_event_timeout(fimc->irq_queue,
69 			!fimc_ctx_state_is_set(FIMC_CTX_SHUT, ctx),
70 			FIMC_SHUTDOWN_TIMEOUT);
71 }
72 
start_streaming(struct vb2_queue * q,unsigned int count)73 static int start_streaming(struct vb2_queue *q, unsigned int count)
74 {
75 	struct fimc_ctx *ctx = q->drv_priv;
76 
77 	return pm_runtime_resume_and_get(&ctx->fimc_dev->pdev->dev);
78 }
79 
stop_streaming(struct vb2_queue * q)80 static void stop_streaming(struct vb2_queue *q)
81 {
82 	struct fimc_ctx *ctx = q->drv_priv;
83 
84 	fimc_m2m_shutdown(ctx);
85 	fimc_m2m_job_finish(ctx, VB2_BUF_STATE_ERROR);
86 	pm_runtime_put(&ctx->fimc_dev->pdev->dev);
87 }
88 
fimc_device_run(void * priv)89 static void fimc_device_run(void *priv)
90 {
91 	struct vb2_v4l2_buffer *src_vb, *dst_vb;
92 	struct fimc_ctx *ctx = priv;
93 	struct fimc_frame *sf, *df;
94 	struct fimc_dev *fimc;
95 	unsigned long flags;
96 	int ret;
97 
98 	if (WARN(!ctx, "Null context\n"))
99 		return;
100 
101 	fimc = ctx->fimc_dev;
102 	spin_lock_irqsave(&fimc->slock, flags);
103 
104 	set_bit(ST_M2M_PEND, &fimc->state);
105 	sf = &ctx->s_frame;
106 	df = &ctx->d_frame;
107 
108 	if (ctx->state & FIMC_PARAMS) {
109 		/* Prepare the DMA offsets for scaler */
110 		fimc_prepare_dma_offset(ctx, sf);
111 		fimc_prepare_dma_offset(ctx, df);
112 	}
113 
114 	src_vb = v4l2_m2m_next_src_buf(ctx->fh.m2m_ctx);
115 	ret = fimc_prepare_addr(ctx, &src_vb->vb2_buf, sf, &sf->addr);
116 	if (ret)
117 		goto dma_unlock;
118 
119 	dst_vb = v4l2_m2m_next_dst_buf(ctx->fh.m2m_ctx);
120 	ret = fimc_prepare_addr(ctx, &dst_vb->vb2_buf, df, &df->addr);
121 	if (ret)
122 		goto dma_unlock;
123 
124 	dst_vb->vb2_buf.timestamp = src_vb->vb2_buf.timestamp;
125 	dst_vb->flags &= ~V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
126 	dst_vb->flags |=
127 		src_vb->flags & V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
128 
129 	/* Reconfigure hardware if the context has changed. */
130 	if (fimc->m2m.ctx != ctx) {
131 		ctx->state |= FIMC_PARAMS;
132 		fimc->m2m.ctx = ctx;
133 	}
134 
135 	if (ctx->state & FIMC_PARAMS) {
136 		fimc_set_yuv_order(ctx);
137 		fimc_hw_set_input_path(ctx);
138 		fimc_hw_set_in_dma(ctx);
139 		ret = fimc_set_scaler_info(ctx);
140 		if (ret)
141 			goto dma_unlock;
142 		fimc_hw_set_prescaler(ctx);
143 		fimc_hw_set_mainscaler(ctx);
144 		fimc_hw_set_target_format(ctx);
145 		fimc_hw_set_rotation(ctx);
146 		fimc_hw_set_effect(ctx);
147 		fimc_hw_set_out_dma(ctx);
148 		if (fimc->drv_data->alpha_color)
149 			fimc_hw_set_rgb_alpha(ctx);
150 		fimc_hw_set_output_path(ctx);
151 	}
152 	fimc_hw_set_input_addr(fimc, &sf->addr);
153 	fimc_hw_set_output_addr(fimc, &df->addr, -1);
154 
155 	fimc_activate_capture(ctx);
156 	ctx->state &= (FIMC_CTX_M2M | FIMC_CTX_CAP);
157 	fimc_hw_activate_input_dma(fimc, true);
158 
159 dma_unlock:
160 	spin_unlock_irqrestore(&fimc->slock, flags);
161 }
162 
fimc_job_abort(void * priv)163 static void fimc_job_abort(void *priv)
164 {
165 	fimc_m2m_shutdown(priv);
166 }
167 
fimc_queue_setup(struct vb2_queue * vq,unsigned int * num_buffers,unsigned int * num_planes,unsigned int sizes[],struct device * alloc_devs[])168 static int fimc_queue_setup(struct vb2_queue *vq,
169 			    unsigned int *num_buffers, unsigned int *num_planes,
170 			    unsigned int sizes[], struct device *alloc_devs[])
171 {
172 	struct fimc_ctx *ctx = vb2_get_drv_priv(vq);
173 	struct fimc_frame *f;
174 	int i;
175 
176 	f = ctx_get_frame(ctx, vq->type);
177 	if (IS_ERR(f))
178 		return PTR_ERR(f);
179 	/*
180 	 * Return number of non-contiguous planes (plane buffers)
181 	 * depending on the configured color format.
182 	 */
183 	if (!f->fmt)
184 		return -EINVAL;
185 
186 	*num_planes = f->fmt->memplanes;
187 	for (i = 0; i < f->fmt->memplanes; i++)
188 		sizes[i] = f->payload[i];
189 	return 0;
190 }
191 
fimc_buf_prepare(struct vb2_buffer * vb)192 static int fimc_buf_prepare(struct vb2_buffer *vb)
193 {
194 	struct fimc_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
195 	struct fimc_frame *frame;
196 	int i;
197 
198 	frame = ctx_get_frame(ctx, vb->vb2_queue->type);
199 	if (IS_ERR(frame))
200 		return PTR_ERR(frame);
201 
202 	for (i = 0; i < frame->fmt->memplanes; i++)
203 		vb2_set_plane_payload(vb, i, frame->payload[i]);
204 
205 	return 0;
206 }
207 
fimc_buf_queue(struct vb2_buffer * vb)208 static void fimc_buf_queue(struct vb2_buffer *vb)
209 {
210 	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
211 	struct fimc_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
212 	v4l2_m2m_buf_queue(ctx->fh.m2m_ctx, vbuf);
213 }
214 
215 static const struct vb2_ops fimc_qops = {
216 	.queue_setup	 = fimc_queue_setup,
217 	.buf_prepare	 = fimc_buf_prepare,
218 	.buf_queue	 = fimc_buf_queue,
219 	.wait_prepare	 = vb2_ops_wait_prepare,
220 	.wait_finish	 = vb2_ops_wait_finish,
221 	.stop_streaming	 = stop_streaming,
222 	.start_streaming = start_streaming,
223 };
224 
225 /*
226  * V4L2 ioctl handlers
227  */
fimc_m2m_querycap(struct file * file,void * fh,struct v4l2_capability * cap)228 static int fimc_m2m_querycap(struct file *file, void *fh,
229 				     struct v4l2_capability *cap)
230 {
231 	struct fimc_dev *fimc = video_drvdata(file);
232 
233 	__fimc_vidioc_querycap(&fimc->pdev->dev, cap);
234 	return 0;
235 }
236 
fimc_m2m_enum_fmt(struct file * file,void * priv,struct v4l2_fmtdesc * f)237 static int fimc_m2m_enum_fmt(struct file *file, void *priv,
238 			     struct v4l2_fmtdesc *f)
239 {
240 	struct fimc_fmt *fmt;
241 
242 	fmt = fimc_find_format(NULL, NULL, get_m2m_fmt_flags(f->type),
243 			       f->index);
244 	if (!fmt)
245 		return -EINVAL;
246 
247 	f->pixelformat = fmt->fourcc;
248 	return 0;
249 }
250 
fimc_m2m_g_fmt_mplane(struct file * file,void * fh,struct v4l2_format * f)251 static int fimc_m2m_g_fmt_mplane(struct file *file, void *fh,
252 				 struct v4l2_format *f)
253 {
254 	struct fimc_ctx *ctx = fh_to_ctx(fh);
255 	struct fimc_frame *frame = ctx_get_frame(ctx, f->type);
256 
257 	if (IS_ERR(frame))
258 		return PTR_ERR(frame);
259 
260 	__fimc_get_format(frame, f);
261 	return 0;
262 }
263 
fimc_try_fmt_mplane(struct fimc_ctx * ctx,struct v4l2_format * f)264 static int fimc_try_fmt_mplane(struct fimc_ctx *ctx, struct v4l2_format *f)
265 {
266 	struct fimc_dev *fimc = ctx->fimc_dev;
267 	const struct fimc_variant *variant = fimc->variant;
268 	struct v4l2_pix_format_mplane *pix = &f->fmt.pix_mp;
269 	struct fimc_fmt *fmt;
270 	u32 max_w, mod_x, mod_y;
271 
272 	if (!IS_M2M(f->type))
273 		return -EINVAL;
274 
275 	fmt = fimc_find_format(&pix->pixelformat, NULL,
276 			       get_m2m_fmt_flags(f->type), 0);
277 	if (WARN(fmt == NULL, "Pixel format lookup failed"))
278 		return -EINVAL;
279 
280 	if (pix->field == V4L2_FIELD_ANY)
281 		pix->field = V4L2_FIELD_NONE;
282 	else if (pix->field != V4L2_FIELD_NONE)
283 		return -EINVAL;
284 
285 	if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
286 		max_w = variant->pix_limit->scaler_dis_w;
287 		mod_x = ffs(variant->min_inp_pixsize) - 1;
288 	} else {
289 		max_w = variant->pix_limit->out_rot_dis_w;
290 		mod_x = ffs(variant->min_out_pixsize) - 1;
291 	}
292 
293 	if (tiled_fmt(fmt)) {
294 		mod_x = 6; /* 64 x 32 pixels tile */
295 		mod_y = 5;
296 	} else {
297 		if (variant->min_vsize_align == 1)
298 			mod_y = fimc_fmt_is_rgb(fmt->color) ? 0 : 1;
299 		else
300 			mod_y = ffs(variant->min_vsize_align) - 1;
301 	}
302 
303 	v4l_bound_align_image(&pix->width, 16, max_w, mod_x,
304 		&pix->height, 8, variant->pix_limit->scaler_dis_w, mod_y, 0);
305 
306 	fimc_adjust_mplane_format(fmt, pix->width, pix->height, &f->fmt.pix_mp);
307 	return 0;
308 }
309 
fimc_m2m_try_fmt_mplane(struct file * file,void * fh,struct v4l2_format * f)310 static int fimc_m2m_try_fmt_mplane(struct file *file, void *fh,
311 				   struct v4l2_format *f)
312 {
313 	struct fimc_ctx *ctx = fh_to_ctx(fh);
314 	return fimc_try_fmt_mplane(ctx, f);
315 }
316 
__set_frame_format(struct fimc_frame * frame,struct fimc_fmt * fmt,struct v4l2_pix_format_mplane * pixm)317 static void __set_frame_format(struct fimc_frame *frame, struct fimc_fmt *fmt,
318 			       struct v4l2_pix_format_mplane *pixm)
319 {
320 	int i;
321 
322 	for (i = 0; i < fmt->memplanes; i++) {
323 		frame->bytesperline[i] = pixm->plane_fmt[i].bytesperline;
324 		frame->payload[i] = pixm->plane_fmt[i].sizeimage;
325 	}
326 
327 	frame->f_width = pixm->width;
328 	frame->f_height	= pixm->height;
329 	frame->o_width = pixm->width;
330 	frame->o_height = pixm->height;
331 	frame->width = pixm->width;
332 	frame->height = pixm->height;
333 	frame->offs_h = 0;
334 	frame->offs_v = 0;
335 	frame->fmt = fmt;
336 }
337 
fimc_m2m_s_fmt_mplane(struct file * file,void * fh,struct v4l2_format * f)338 static int fimc_m2m_s_fmt_mplane(struct file *file, void *fh,
339 				 struct v4l2_format *f)
340 {
341 	struct fimc_ctx *ctx = fh_to_ctx(fh);
342 	struct fimc_dev *fimc = ctx->fimc_dev;
343 	struct fimc_fmt *fmt;
344 	struct vb2_queue *vq;
345 	struct fimc_frame *frame;
346 	int ret;
347 
348 	ret = fimc_try_fmt_mplane(ctx, f);
349 	if (ret)
350 		return ret;
351 
352 	vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);
353 
354 	if (vb2_is_busy(vq)) {
355 		v4l2_err(&fimc->m2m.vfd, "queue (%d) busy\n", f->type);
356 		return -EBUSY;
357 	}
358 
359 	if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
360 		frame = &ctx->s_frame;
361 	else
362 		frame = &ctx->d_frame;
363 
364 	fmt = fimc_find_format(&f->fmt.pix_mp.pixelformat, NULL,
365 			       get_m2m_fmt_flags(f->type), 0);
366 	if (!fmt)
367 		return -EINVAL;
368 
369 	__set_frame_format(frame, fmt, &f->fmt.pix_mp);
370 
371 	/* Update RGB Alpha control state and value range */
372 	fimc_alpha_ctrl_update(ctx);
373 
374 	return 0;
375 }
376 
fimc_m2m_g_selection(struct file * file,void * fh,struct v4l2_selection * s)377 static int fimc_m2m_g_selection(struct file *file, void *fh,
378 				struct v4l2_selection *s)
379 {
380 	struct fimc_ctx *ctx = fh_to_ctx(fh);
381 	struct fimc_frame *frame;
382 
383 	frame = ctx_get_frame(ctx, s->type);
384 	if (IS_ERR(frame))
385 		return PTR_ERR(frame);
386 
387 	switch (s->target) {
388 	case V4L2_SEL_TGT_CROP:
389 	case V4L2_SEL_TGT_CROP_DEFAULT:
390 	case V4L2_SEL_TGT_CROP_BOUNDS:
391 		if (s->type != V4L2_BUF_TYPE_VIDEO_OUTPUT)
392 			return -EINVAL;
393 		break;
394 	case V4L2_SEL_TGT_COMPOSE:
395 	case V4L2_SEL_TGT_COMPOSE_DEFAULT:
396 	case V4L2_SEL_TGT_COMPOSE_BOUNDS:
397 		if (s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
398 			return -EINVAL;
399 		break;
400 	default:
401 		return -EINVAL;
402 	}
403 
404 	switch (s->target) {
405 	case V4L2_SEL_TGT_CROP:
406 	case V4L2_SEL_TGT_COMPOSE:
407 		s->r.left = frame->offs_h;
408 		s->r.top = frame->offs_v;
409 		s->r.width = frame->width;
410 		s->r.height = frame->height;
411 		break;
412 	case V4L2_SEL_TGT_CROP_DEFAULT:
413 	case V4L2_SEL_TGT_CROP_BOUNDS:
414 	case V4L2_SEL_TGT_COMPOSE_DEFAULT:
415 	case V4L2_SEL_TGT_COMPOSE_BOUNDS:
416 		s->r.left = 0;
417 		s->r.top = 0;
418 		s->r.width = frame->o_width;
419 		s->r.height = frame->o_height;
420 		break;
421 	default:
422 		return -EINVAL;
423 	}
424 	return 0;
425 }
426 
fimc_m2m_try_selection(struct fimc_ctx * ctx,struct v4l2_selection * s)427 static int fimc_m2m_try_selection(struct fimc_ctx *ctx,
428 				  struct v4l2_selection *s)
429 {
430 	struct fimc_dev *fimc = ctx->fimc_dev;
431 	struct fimc_frame *f;
432 	u32 min_size, halign, depth = 0;
433 	int i;
434 
435 	if (s->r.top < 0 || s->r.left < 0) {
436 		v4l2_err(&fimc->m2m.vfd,
437 			"doesn't support negative values for top & left\n");
438 		return -EINVAL;
439 	}
440 	if (s->type == V4L2_BUF_TYPE_VIDEO_CAPTURE) {
441 		f = &ctx->d_frame;
442 		if (s->target != V4L2_SEL_TGT_COMPOSE)
443 			return -EINVAL;
444 	} else if (s->type == V4L2_BUF_TYPE_VIDEO_OUTPUT) {
445 		f = &ctx->s_frame;
446 		if (s->target != V4L2_SEL_TGT_CROP)
447 			return -EINVAL;
448 	} else {
449 		return -EINVAL;
450 	}
451 
452 	min_size = (f == &ctx->s_frame) ?
453 		fimc->variant->min_inp_pixsize : fimc->variant->min_out_pixsize;
454 
455 	/* Get pixel alignment constraints. */
456 	if (fimc->variant->min_vsize_align == 1)
457 		halign = fimc_fmt_is_rgb(f->fmt->color) ? 0 : 1;
458 	else
459 		halign = ffs(fimc->variant->min_vsize_align) - 1;
460 
461 	for (i = 0; i < f->fmt->memplanes; i++)
462 		depth += f->fmt->depth[i];
463 
464 	v4l_bound_align_image(&s->r.width, min_size, f->o_width,
465 			      ffs(min_size) - 1,
466 			      &s->r.height, min_size, f->o_height,
467 			      halign, 64/(ALIGN(depth, 8)));
468 
469 	/* adjust left/top if cropping rectangle is out of bounds */
470 	if (s->r.left + s->r.width > f->o_width)
471 		s->r.left = f->o_width - s->r.width;
472 	if (s->r.top + s->r.height > f->o_height)
473 		s->r.top = f->o_height - s->r.height;
474 
475 	s->r.left = round_down(s->r.left, min_size);
476 	s->r.top  = round_down(s->r.top, fimc->variant->hor_offs_align);
477 
478 	dbg("l:%d, t:%d, w:%d, h:%d, f_w: %d, f_h: %d",
479 	    s->r.left, s->r.top, s->r.width, s->r.height,
480 	    f->f_width, f->f_height);
481 
482 	return 0;
483 }
484 
fimc_m2m_s_selection(struct file * file,void * fh,struct v4l2_selection * s)485 static int fimc_m2m_s_selection(struct file *file, void *fh,
486 				struct v4l2_selection *s)
487 {
488 	struct fimc_ctx *ctx = fh_to_ctx(fh);
489 	struct fimc_dev *fimc = ctx->fimc_dev;
490 	struct fimc_frame *f;
491 	int ret;
492 
493 	ret = fimc_m2m_try_selection(ctx, s);
494 	if (ret)
495 		return ret;
496 
497 	f = (s->type == V4L2_BUF_TYPE_VIDEO_OUTPUT) ?
498 		&ctx->s_frame : &ctx->d_frame;
499 
500 	/* Check to see if scaling ratio is within supported range */
501 	if (s->type == V4L2_BUF_TYPE_VIDEO_OUTPUT) {
502 		ret = fimc_check_scaler_ratio(ctx, s->r.width,
503 				s->r.height, ctx->d_frame.width,
504 				ctx->d_frame.height, ctx->rotation);
505 	} else {
506 		ret = fimc_check_scaler_ratio(ctx, ctx->s_frame.width,
507 				ctx->s_frame.height, s->r.width,
508 				s->r.height, ctx->rotation);
509 	}
510 	if (ret) {
511 		v4l2_err(&fimc->m2m.vfd, "Out of scaler range\n");
512 		return -EINVAL;
513 	}
514 
515 	f->offs_h = s->r.left;
516 	f->offs_v = s->r.top;
517 	f->width  = s->r.width;
518 	f->height = s->r.height;
519 
520 	fimc_ctx_state_set(FIMC_PARAMS, ctx);
521 
522 	return 0;
523 }
524 
525 static const struct v4l2_ioctl_ops fimc_m2m_ioctl_ops = {
526 	.vidioc_querycap		= fimc_m2m_querycap,
527 	.vidioc_enum_fmt_vid_cap	= fimc_m2m_enum_fmt,
528 	.vidioc_enum_fmt_vid_out	= fimc_m2m_enum_fmt,
529 	.vidioc_g_fmt_vid_cap_mplane	= fimc_m2m_g_fmt_mplane,
530 	.vidioc_g_fmt_vid_out_mplane	= fimc_m2m_g_fmt_mplane,
531 	.vidioc_try_fmt_vid_cap_mplane	= fimc_m2m_try_fmt_mplane,
532 	.vidioc_try_fmt_vid_out_mplane	= fimc_m2m_try_fmt_mplane,
533 	.vidioc_s_fmt_vid_cap_mplane	= fimc_m2m_s_fmt_mplane,
534 	.vidioc_s_fmt_vid_out_mplane	= fimc_m2m_s_fmt_mplane,
535 	.vidioc_reqbufs			= v4l2_m2m_ioctl_reqbufs,
536 	.vidioc_querybuf		= v4l2_m2m_ioctl_querybuf,
537 	.vidioc_qbuf			= v4l2_m2m_ioctl_qbuf,
538 	.vidioc_dqbuf			= v4l2_m2m_ioctl_dqbuf,
539 	.vidioc_expbuf			= v4l2_m2m_ioctl_expbuf,
540 	.vidioc_streamon		= v4l2_m2m_ioctl_streamon,
541 	.vidioc_streamoff		= v4l2_m2m_ioctl_streamoff,
542 	.vidioc_g_selection		= fimc_m2m_g_selection,
543 	.vidioc_s_selection		= fimc_m2m_s_selection,
544 
545 };
546 
queue_init(void * priv,struct vb2_queue * src_vq,struct vb2_queue * dst_vq)547 static int queue_init(void *priv, struct vb2_queue *src_vq,
548 		      struct vb2_queue *dst_vq)
549 {
550 	struct fimc_ctx *ctx = priv;
551 	int ret;
552 
553 	src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
554 	src_vq->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF;
555 	src_vq->drv_priv = ctx;
556 	src_vq->ops = &fimc_qops;
557 	src_vq->mem_ops = &vb2_dma_contig_memops;
558 	src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
559 	src_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
560 	src_vq->lock = &ctx->fimc_dev->lock;
561 	src_vq->dev = &ctx->fimc_dev->pdev->dev;
562 
563 	ret = vb2_queue_init(src_vq);
564 	if (ret)
565 		return ret;
566 
567 	dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
568 	dst_vq->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF;
569 	dst_vq->drv_priv = ctx;
570 	dst_vq->ops = &fimc_qops;
571 	dst_vq->mem_ops = &vb2_dma_contig_memops;
572 	dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
573 	dst_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
574 	dst_vq->lock = &ctx->fimc_dev->lock;
575 	dst_vq->dev = &ctx->fimc_dev->pdev->dev;
576 
577 	return vb2_queue_init(dst_vq);
578 }
579 
fimc_m2m_set_default_format(struct fimc_ctx * ctx)580 static int fimc_m2m_set_default_format(struct fimc_ctx *ctx)
581 {
582 	struct v4l2_pix_format_mplane pixm = {
583 		.pixelformat	= V4L2_PIX_FMT_RGB32,
584 		.width		= 800,
585 		.height		= 600,
586 		.plane_fmt[0]	= {
587 			.bytesperline = 800 * 4,
588 			.sizeimage = 800 * 4 * 600,
589 		},
590 	};
591 	struct fimc_fmt *fmt;
592 
593 	fmt = fimc_find_format(&pixm.pixelformat, NULL, FMT_FLAGS_M2M, 0);
594 	if (!fmt)
595 		return -EINVAL;
596 
597 	__set_frame_format(&ctx->s_frame, fmt, &pixm);
598 	__set_frame_format(&ctx->d_frame, fmt, &pixm);
599 
600 	return 0;
601 }
602 
fimc_m2m_open(struct file * file)603 static int fimc_m2m_open(struct file *file)
604 {
605 	struct fimc_dev *fimc = video_drvdata(file);
606 	struct fimc_ctx *ctx;
607 	int ret = -EBUSY;
608 
609 	pr_debug("pid: %d, state: %#lx\n", task_pid_nr(current), fimc->state);
610 
611 	if (mutex_lock_interruptible(&fimc->lock))
612 		return -ERESTARTSYS;
613 	/*
614 	 * Don't allow simultaneous open() of the mem-to-mem and the
615 	 * capture video node that belong to same FIMC IP instance.
616 	 */
617 	if (test_bit(ST_CAPT_BUSY, &fimc->state))
618 		goto unlock;
619 
620 	ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
621 	if (!ctx) {
622 		ret = -ENOMEM;
623 		goto unlock;
624 	}
625 	v4l2_fh_init(&ctx->fh, &fimc->m2m.vfd);
626 	ctx->fimc_dev = fimc;
627 
628 	/* Default color format */
629 	ctx->s_frame.fmt = fimc_get_format(0);
630 	ctx->d_frame.fmt = fimc_get_format(0);
631 
632 	ret = fimc_ctrls_create(ctx);
633 	if (ret)
634 		goto error_fh;
635 
636 	/* Use separate control handler per file handle */
637 	ctx->fh.ctrl_handler = &ctx->ctrls.handler;
638 	file->private_data = &ctx->fh;
639 	v4l2_fh_add(&ctx->fh);
640 
641 	/* Setup the device context for memory-to-memory mode */
642 	ctx->state = FIMC_CTX_M2M;
643 	ctx->flags = 0;
644 	ctx->in_path = FIMC_IO_DMA;
645 	ctx->out_path = FIMC_IO_DMA;
646 	ctx->scaler.enabled = 1;
647 
648 	ctx->fh.m2m_ctx = v4l2_m2m_ctx_init(fimc->m2m.m2m_dev, ctx, queue_init);
649 	if (IS_ERR(ctx->fh.m2m_ctx)) {
650 		ret = PTR_ERR(ctx->fh.m2m_ctx);
651 		goto error_c;
652 	}
653 
654 	if (fimc->m2m.refcnt++ == 0)
655 		set_bit(ST_M2M_RUN, &fimc->state);
656 
657 	ret = fimc_m2m_set_default_format(ctx);
658 	if (ret < 0)
659 		goto error_m2m_ctx;
660 
661 	mutex_unlock(&fimc->lock);
662 	return 0;
663 
664 error_m2m_ctx:
665 	v4l2_m2m_ctx_release(ctx->fh.m2m_ctx);
666 error_c:
667 	fimc_ctrls_delete(ctx);
668 	v4l2_fh_del(&ctx->fh);
669 error_fh:
670 	v4l2_fh_exit(&ctx->fh);
671 	kfree(ctx);
672 unlock:
673 	mutex_unlock(&fimc->lock);
674 	return ret;
675 }
676 
fimc_m2m_release(struct file * file)677 static int fimc_m2m_release(struct file *file)
678 {
679 	struct fimc_ctx *ctx = fh_to_ctx(file->private_data);
680 	struct fimc_dev *fimc = ctx->fimc_dev;
681 
682 	dbg("pid: %d, state: 0x%lx, refcnt= %d",
683 		task_pid_nr(current), fimc->state, fimc->m2m.refcnt);
684 
685 	mutex_lock(&fimc->lock);
686 
687 	v4l2_m2m_ctx_release(ctx->fh.m2m_ctx);
688 	fimc_ctrls_delete(ctx);
689 	v4l2_fh_del(&ctx->fh);
690 	v4l2_fh_exit(&ctx->fh);
691 
692 	if (--fimc->m2m.refcnt <= 0)
693 		clear_bit(ST_M2M_RUN, &fimc->state);
694 	kfree(ctx);
695 
696 	mutex_unlock(&fimc->lock);
697 	return 0;
698 }
699 
700 static const struct v4l2_file_operations fimc_m2m_fops = {
701 	.owner		= THIS_MODULE,
702 	.open		= fimc_m2m_open,
703 	.release	= fimc_m2m_release,
704 	.poll		= v4l2_m2m_fop_poll,
705 	.unlocked_ioctl	= video_ioctl2,
706 	.mmap		= v4l2_m2m_fop_mmap,
707 };
708 
709 static const struct v4l2_m2m_ops m2m_ops = {
710 	.device_run	= fimc_device_run,
711 	.job_abort	= fimc_job_abort,
712 };
713 
fimc_register_m2m_device(struct fimc_dev * fimc,struct v4l2_device * v4l2_dev)714 int fimc_register_m2m_device(struct fimc_dev *fimc,
715 			     struct v4l2_device *v4l2_dev)
716 {
717 	struct video_device *vfd = &fimc->m2m.vfd;
718 	int ret;
719 
720 	fimc->v4l2_dev = v4l2_dev;
721 
722 	memset(vfd, 0, sizeof(*vfd));
723 	vfd->fops = &fimc_m2m_fops;
724 	vfd->ioctl_ops = &fimc_m2m_ioctl_ops;
725 	vfd->v4l2_dev = v4l2_dev;
726 	vfd->minor = -1;
727 	vfd->release = video_device_release_empty;
728 	vfd->lock = &fimc->lock;
729 	vfd->vfl_dir = VFL_DIR_M2M;
730 	vfd->device_caps = V4L2_CAP_STREAMING | V4L2_CAP_VIDEO_M2M_MPLANE;
731 	set_bit(V4L2_FL_QUIRK_INVERTED_CROP, &vfd->flags);
732 
733 	snprintf(vfd->name, sizeof(vfd->name), "fimc.%d.m2m", fimc->id);
734 	video_set_drvdata(vfd, fimc);
735 
736 	fimc->m2m.m2m_dev = v4l2_m2m_init(&m2m_ops);
737 	if (IS_ERR(fimc->m2m.m2m_dev)) {
738 		v4l2_err(v4l2_dev, "failed to initialize v4l2-m2m device\n");
739 		return PTR_ERR(fimc->m2m.m2m_dev);
740 	}
741 
742 	ret = media_entity_pads_init(&vfd->entity, 0, NULL);
743 	if (ret)
744 		goto err_me;
745 
746 	ret = video_register_device(vfd, VFL_TYPE_VIDEO, -1);
747 	if (ret)
748 		goto err_vd;
749 
750 	v4l2_info(v4l2_dev, "Registered %s as /dev/%s\n",
751 		  vfd->name, video_device_node_name(vfd));
752 	return 0;
753 
754 err_vd:
755 	media_entity_cleanup(&vfd->entity);
756 err_me:
757 	v4l2_m2m_release(fimc->m2m.m2m_dev);
758 	return ret;
759 }
760 
fimc_unregister_m2m_device(struct fimc_dev * fimc)761 void fimc_unregister_m2m_device(struct fimc_dev *fimc)
762 {
763 	if (!fimc)
764 		return;
765 
766 	if (fimc->m2m.m2m_dev)
767 		v4l2_m2m_release(fimc->m2m.m2m_dev);
768 
769 	if (video_is_registered(&fimc->m2m.vfd)) {
770 		video_unregister_device(&fimc->m2m.vfd);
771 		media_entity_cleanup(&fimc->m2m.vfd.entity);
772 	}
773 }
774