1 // SPDX-License-Identifier: GPL-2.0
2 /*
3  * Copyright (C) STMicroelectronics SA 2014
4  * Authors: Fabien Dessenne <fabien.dessenne@st.com> for STMicroelectronics.
5  */
6 
7 #include <linux/errno.h>
8 #include <linux/interrupt.h>
9 #include <linux/kernel.h>
10 #include <linux/module.h>
11 #include <linux/of.h>
12 #include <linux/pm_runtime.h>
13 #include <linux/slab.h>
14 
15 #include <media/v4l2-event.h>
16 #include <media/v4l2-ioctl.h>
17 
18 #include "bdisp.h"
19 
20 #define BDISP_MAX_CTRL_NUM      10
21 
22 #define BDISP_WORK_TIMEOUT      ((100 * HZ) / 1000)
23 
24 /* User configuration change */
25 #define BDISP_PARAMS            BIT(0) /* Config updated */
26 #define BDISP_SRC_FMT           BIT(1) /* Source set */
27 #define BDISP_DST_FMT           BIT(2) /* Destination set */
28 #define BDISP_CTX_STOP_REQ      BIT(3) /* Stop request */
29 #define BDISP_CTX_ABORT         BIT(4) /* Abort while device run */
30 
31 #define BDISP_MIN_W             1
32 #define BDISP_MAX_W             8191
33 #define BDISP_MIN_H             1
34 #define BDISP_MAX_H             8191
35 
36 #define fh_to_ctx(__fh) container_of(__fh, struct bdisp_ctx, fh)
37 
38 enum bdisp_dev_flags {
39 	ST_M2M_OPEN,            /* Driver opened */
40 	ST_M2M_RUNNING,         /* HW device running */
41 	ST_M2M_SUSPENDED,       /* Driver suspended */
42 	ST_M2M_SUSPENDING,      /* Driver being suspended */
43 };
44 
45 static const struct bdisp_fmt bdisp_formats[] = {
46 	/* ARGB888. [31:0] A:R:G:B 8:8:8:8 little endian */
47 	{
48 		.pixelformat    = V4L2_PIX_FMT_ABGR32, /* is actually ARGB */
49 		.nb_planes      = 1,
50 		.bpp            = 32,
51 		.bpp_plane0     = 32,
52 		.w_align        = 1,
53 		.h_align        = 1
54 	},
55 	/* XRGB888. [31:0] x:R:G:B 8:8:8:8 little endian */
56 	{
57 		.pixelformat    = V4L2_PIX_FMT_XBGR32, /* is actually xRGB */
58 		.nb_planes      = 1,
59 		.bpp            = 32,
60 		.bpp_plane0     = 32,
61 		.w_align        = 1,
62 		.h_align        = 1
63 	},
64 	/* RGB565. [15:0] R:G:B 5:6:5 little endian */
65 	{
66 		.pixelformat    = V4L2_PIX_FMT_RGB565,
67 		.nb_planes      = 1,
68 		.bpp            = 16,
69 		.bpp_plane0     = 16,
70 		.w_align        = 1,
71 		.h_align        = 1
72 	},
73 	/* NV12. YUV420SP - 1 plane for Y + 1 plane for (CbCr) */
74 	{
75 		.pixelformat    = V4L2_PIX_FMT_NV12,
76 		.nb_planes      = 2,
77 		.bpp            = 12,
78 		.bpp_plane0     = 8,
79 		.w_align        = 2,
80 		.h_align        = 2
81 	},
82 	/* RGB888. [23:0] B:G:R 8:8:8 little endian */
83 	{
84 		.pixelformat    = V4L2_PIX_FMT_RGB24,
85 		.nb_planes      = 1,
86 		.bpp            = 24,
87 		.bpp_plane0     = 24,
88 		.w_align        = 1,
89 		.h_align        = 1
90 	},
91 	/* YU12. YUV420P - 1 plane for Y + 1 plane for Cb + 1 plane for Cr
92 	 * To keep as the LAST element of this table (no support on capture)
93 	 */
94 	{
95 		.pixelformat    = V4L2_PIX_FMT_YUV420,
96 		.nb_planes      = 3,
97 		.bpp            = 12,
98 		.bpp_plane0     = 8,
99 		.w_align        = 2,
100 		.h_align        = 2
101 	}
102 };
103 
104 /* Default format : HD ARGB32*/
105 #define BDISP_DEF_WIDTH         1920
106 #define BDISP_DEF_HEIGHT        1080
107 
108 static const struct bdisp_frame bdisp_dflt_fmt = {
109 		.width          = BDISP_DEF_WIDTH,
110 		.height         = BDISP_DEF_HEIGHT,
111 		.fmt            = &bdisp_formats[0],
112 		.field          = V4L2_FIELD_NONE,
113 		.bytesperline   = BDISP_DEF_WIDTH * 4,
114 		.sizeimage      = BDISP_DEF_WIDTH * BDISP_DEF_HEIGHT * 4,
115 		.colorspace     = V4L2_COLORSPACE_REC709,
116 		.crop           = {0, 0, BDISP_DEF_WIDTH, BDISP_DEF_HEIGHT},
117 		.paddr          = {0, 0, 0, 0}
118 };
119 
120 static inline void bdisp_ctx_state_lock_set(u32 state, struct bdisp_ctx *ctx)
121 {
122 	unsigned long flags;
123 
124 	spin_lock_irqsave(&ctx->bdisp_dev->slock, flags);
125 	ctx->state |= state;
126 	spin_unlock_irqrestore(&ctx->bdisp_dev->slock, flags);
127 }
128 
129 static inline void bdisp_ctx_state_lock_clear(u32 state, struct bdisp_ctx *ctx)
130 {
131 	unsigned long flags;
132 
133 	spin_lock_irqsave(&ctx->bdisp_dev->slock, flags);
134 	ctx->state &= ~state;
135 	spin_unlock_irqrestore(&ctx->bdisp_dev->slock, flags);
136 }
137 
138 static inline bool bdisp_ctx_state_is_set(u32 mask, struct bdisp_ctx *ctx)
139 {
140 	unsigned long flags;
141 	bool ret;
142 
143 	spin_lock_irqsave(&ctx->bdisp_dev->slock, flags);
144 	ret = (ctx->state & mask) == mask;
145 	spin_unlock_irqrestore(&ctx->bdisp_dev->slock, flags);
146 
147 	return ret;
148 }
149 
150 static const struct bdisp_fmt *bdisp_find_fmt(u32 pixelformat)
151 {
152 	const struct bdisp_fmt *fmt;
153 	unsigned int i;
154 
155 	for (i = 0; i < ARRAY_SIZE(bdisp_formats); i++) {
156 		fmt = &bdisp_formats[i];
157 		if (fmt->pixelformat == pixelformat)
158 			return fmt;
159 	}
160 
161 	return NULL;
162 }
163 
164 static struct bdisp_frame *ctx_get_frame(struct bdisp_ctx *ctx,
165 					 enum v4l2_buf_type type)
166 {
167 	switch (type) {
168 	case V4L2_BUF_TYPE_VIDEO_OUTPUT:
169 		return &ctx->src;
170 	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
171 		return &ctx->dst;
172 	default:
173 		dev_err(ctx->bdisp_dev->dev,
174 			"Wrong buffer/video queue type (%d)\n", type);
175 		break;
176 	}
177 
178 	return ERR_PTR(-EINVAL);
179 }
180 
181 static void bdisp_job_finish(struct bdisp_ctx *ctx, int vb_state)
182 {
183 	struct vb2_v4l2_buffer *src_vb, *dst_vb;
184 
185 	if (WARN(!ctx || !ctx->fh.m2m_ctx, "Null hardware context\n"))
186 		return;
187 
188 	dev_dbg(ctx->bdisp_dev->dev, "%s\n", __func__);
189 
190 	src_vb = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
191 	dst_vb = v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
192 
193 	if (src_vb && dst_vb) {
194 		dst_vb->vb2_buf.timestamp = src_vb->vb2_buf.timestamp;
195 		dst_vb->timecode = src_vb->timecode;
196 		dst_vb->flags &= ~V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
197 		dst_vb->flags |= src_vb->flags &
198 					  V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
199 
200 		v4l2_m2m_buf_done(src_vb, vb_state);
201 		v4l2_m2m_buf_done(dst_vb, vb_state);
202 
203 		v4l2_m2m_job_finish(ctx->bdisp_dev->m2m.m2m_dev,
204 				    ctx->fh.m2m_ctx);
205 	}
206 }
207 
208 static int bdisp_ctx_stop_req(struct bdisp_ctx *ctx)
209 {
210 	struct bdisp_ctx *curr_ctx;
211 	struct bdisp_dev *bdisp = ctx->bdisp_dev;
212 	int ret;
213 
214 	dev_dbg(ctx->bdisp_dev->dev, "%s\n", __func__);
215 
216 	cancel_delayed_work(&bdisp->timeout_work);
217 
218 	curr_ctx = v4l2_m2m_get_curr_priv(bdisp->m2m.m2m_dev);
219 	if (!test_bit(ST_M2M_RUNNING, &bdisp->state) || (curr_ctx != ctx))
220 		return 0;
221 
222 	bdisp_ctx_state_lock_set(BDISP_CTX_STOP_REQ, ctx);
223 
224 	ret = wait_event_timeout(bdisp->irq_queue,
225 			!bdisp_ctx_state_is_set(BDISP_CTX_STOP_REQ, ctx),
226 			BDISP_WORK_TIMEOUT);
227 
228 	if (!ret) {
229 		dev_err(ctx->bdisp_dev->dev, "%s IRQ timeout\n", __func__);
230 		return -ETIMEDOUT;
231 	}
232 
233 	return 0;
234 }
235 
236 static void __bdisp_job_abort(struct bdisp_ctx *ctx)
237 {
238 	int ret;
239 
240 	ret = bdisp_ctx_stop_req(ctx);
241 	if ((ret == -ETIMEDOUT) || (ctx->state & BDISP_CTX_ABORT)) {
242 		bdisp_ctx_state_lock_clear(BDISP_CTX_STOP_REQ | BDISP_CTX_ABORT,
243 					   ctx);
244 		bdisp_job_finish(ctx, VB2_BUF_STATE_ERROR);
245 	}
246 }
247 
248 static void bdisp_job_abort(void *priv)
249 {
250 	__bdisp_job_abort((struct bdisp_ctx *)priv);
251 }
252 
253 static int bdisp_get_addr(struct bdisp_ctx *ctx, struct vb2_buffer *vb,
254 			  struct bdisp_frame *frame, dma_addr_t *paddr)
255 {
256 	if (!vb || !frame)
257 		return -EINVAL;
258 
259 	paddr[0] = vb2_dma_contig_plane_dma_addr(vb, 0);
260 
261 	if (frame->fmt->nb_planes > 1)
262 		/* UV (NV12) or U (420P) */
263 		paddr[1] = (dma_addr_t)(paddr[0] +
264 				frame->bytesperline * frame->height);
265 
266 	if (frame->fmt->nb_planes > 2)
267 		/* V (420P) */
268 		paddr[2] = (dma_addr_t)(paddr[1] +
269 				(frame->bytesperline * frame->height) / 4);
270 
271 	if (frame->fmt->nb_planes > 3)
272 		dev_dbg(ctx->bdisp_dev->dev, "ignoring some planes\n");
273 
274 	dev_dbg(ctx->bdisp_dev->dev,
275 		"%s plane[0]=%pad plane[1]=%pad plane[2]=%pad\n",
276 		__func__, &paddr[0], &paddr[1], &paddr[2]);
277 
278 	return 0;
279 }
280 
281 static int bdisp_get_bufs(struct bdisp_ctx *ctx)
282 {
283 	struct bdisp_frame *src, *dst;
284 	struct vb2_v4l2_buffer *src_vb, *dst_vb;
285 	int ret;
286 
287 	src = &ctx->src;
288 	dst = &ctx->dst;
289 
290 	src_vb = v4l2_m2m_next_src_buf(ctx->fh.m2m_ctx);
291 	ret = bdisp_get_addr(ctx, &src_vb->vb2_buf, src, src->paddr);
292 	if (ret)
293 		return ret;
294 
295 	dst_vb = v4l2_m2m_next_dst_buf(ctx->fh.m2m_ctx);
296 	ret = bdisp_get_addr(ctx, &dst_vb->vb2_buf, dst, dst->paddr);
297 	if (ret)
298 		return ret;
299 
300 	dst_vb->vb2_buf.timestamp = src_vb->vb2_buf.timestamp;
301 
302 	return 0;
303 }
304 
305 static void bdisp_device_run(void *priv)
306 {
307 	struct bdisp_ctx *ctx = priv;
308 	struct bdisp_dev *bdisp;
309 	unsigned long flags;
310 	int err = 0;
311 
312 	if (WARN(!ctx, "Null hardware context\n"))
313 		return;
314 
315 	bdisp = ctx->bdisp_dev;
316 	dev_dbg(bdisp->dev, "%s\n", __func__);
317 	spin_lock_irqsave(&bdisp->slock, flags);
318 
319 	if (bdisp->m2m.ctx != ctx) {
320 		dev_dbg(bdisp->dev, "ctx updated: %p -> %p\n",
321 			bdisp->m2m.ctx, ctx);
322 		ctx->state |= BDISP_PARAMS;
323 		bdisp->m2m.ctx = ctx;
324 	}
325 
326 	if (ctx->state & BDISP_CTX_STOP_REQ) {
327 		ctx->state &= ~BDISP_CTX_STOP_REQ;
328 		ctx->state |= BDISP_CTX_ABORT;
329 		wake_up(&bdisp->irq_queue);
330 		goto out;
331 	}
332 
333 	err = bdisp_get_bufs(ctx);
334 	if (err) {
335 		dev_err(bdisp->dev, "cannot get address\n");
336 		goto out;
337 	}
338 
339 	bdisp_dbg_perf_begin(bdisp);
340 
341 	err = bdisp_hw_reset(bdisp);
342 	if (err) {
343 		dev_err(bdisp->dev, "could not get HW ready\n");
344 		goto out;
345 	}
346 
347 	err = bdisp_hw_update(ctx);
348 	if (err) {
349 		dev_err(bdisp->dev, "could not send HW request\n");
350 		goto out;
351 	}
352 
353 	queue_delayed_work(bdisp->work_queue, &bdisp->timeout_work,
354 			   BDISP_WORK_TIMEOUT);
355 	set_bit(ST_M2M_RUNNING, &bdisp->state);
356 out:
357 	ctx->state &= ~BDISP_PARAMS;
358 	spin_unlock_irqrestore(&bdisp->slock, flags);
359 	if (err)
360 		bdisp_job_finish(ctx, VB2_BUF_STATE_ERROR);
361 }
362 
363 static const struct v4l2_m2m_ops bdisp_m2m_ops = {
364 	.device_run     = bdisp_device_run,
365 	.job_abort      = bdisp_job_abort,
366 };
367 
368 static int __bdisp_s_ctrl(struct bdisp_ctx *ctx, struct v4l2_ctrl *ctrl)
369 {
370 	if (ctrl->flags & V4L2_CTRL_FLAG_INACTIVE)
371 		return 0;
372 
373 	switch (ctrl->id) {
374 	case V4L2_CID_HFLIP:
375 		ctx->hflip = ctrl->val;
376 		break;
377 	case V4L2_CID_VFLIP:
378 		ctx->vflip = ctrl->val;
379 		break;
380 	default:
381 		dev_err(ctx->bdisp_dev->dev, "unknown control %d\n", ctrl->id);
382 		return -EINVAL;
383 	}
384 
385 	ctx->state |= BDISP_PARAMS;
386 
387 	return 0;
388 }
389 
390 static int bdisp_s_ctrl(struct v4l2_ctrl *ctrl)
391 {
392 	struct bdisp_ctx *ctx = container_of(ctrl->handler, struct bdisp_ctx,
393 						ctrl_handler);
394 	unsigned long flags;
395 	int ret;
396 
397 	spin_lock_irqsave(&ctx->bdisp_dev->slock, flags);
398 	ret = __bdisp_s_ctrl(ctx, ctrl);
399 	spin_unlock_irqrestore(&ctx->bdisp_dev->slock, flags);
400 
401 	return ret;
402 }
403 
404 static const struct v4l2_ctrl_ops bdisp_c_ops = {
405 	.s_ctrl = bdisp_s_ctrl,
406 };
407 
408 static int bdisp_ctrls_create(struct bdisp_ctx *ctx)
409 {
410 	if (ctx->ctrls_rdy)
411 		return 0;
412 
413 	v4l2_ctrl_handler_init(&ctx->ctrl_handler, BDISP_MAX_CTRL_NUM);
414 
415 	ctx->bdisp_ctrls.hflip = v4l2_ctrl_new_std(&ctx->ctrl_handler,
416 				&bdisp_c_ops, V4L2_CID_HFLIP, 0, 1, 1, 0);
417 	ctx->bdisp_ctrls.vflip = v4l2_ctrl_new_std(&ctx->ctrl_handler,
418 				&bdisp_c_ops, V4L2_CID_VFLIP, 0, 1, 1, 0);
419 
420 	if (ctx->ctrl_handler.error) {
421 		int err = ctx->ctrl_handler.error;
422 
423 		v4l2_ctrl_handler_free(&ctx->ctrl_handler);
424 		return err;
425 	}
426 
427 	ctx->ctrls_rdy = true;
428 
429 	return 0;
430 }
431 
432 static void bdisp_ctrls_delete(struct bdisp_ctx *ctx)
433 {
434 	if (ctx->ctrls_rdy) {
435 		v4l2_ctrl_handler_free(&ctx->ctrl_handler);
436 		ctx->ctrls_rdy = false;
437 	}
438 }
439 
440 static int bdisp_queue_setup(struct vb2_queue *vq,
441 			     unsigned int *nb_buf, unsigned int *nb_planes,
442 			     unsigned int sizes[], struct device *alloc_devs[])
443 {
444 	struct bdisp_ctx *ctx = vb2_get_drv_priv(vq);
445 	struct bdisp_frame *frame = ctx_get_frame(ctx, vq->type);
446 
447 	if (IS_ERR(frame)) {
448 		dev_err(ctx->bdisp_dev->dev, "Invalid frame (%p)\n", frame);
449 		return PTR_ERR(frame);
450 	}
451 
452 	if (!frame->fmt) {
453 		dev_err(ctx->bdisp_dev->dev, "Invalid format\n");
454 		return -EINVAL;
455 	}
456 
457 	if (*nb_planes)
458 		return sizes[0] < frame->sizeimage ? -EINVAL : 0;
459 
460 	*nb_planes = 1;
461 	sizes[0] = frame->sizeimage;
462 
463 	return 0;
464 }
465 
466 static int bdisp_buf_prepare(struct vb2_buffer *vb)
467 {
468 	struct bdisp_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
469 	struct bdisp_frame *frame = ctx_get_frame(ctx, vb->vb2_queue->type);
470 
471 	if (IS_ERR(frame)) {
472 		dev_err(ctx->bdisp_dev->dev, "Invalid frame (%p)\n", frame);
473 		return PTR_ERR(frame);
474 	}
475 
476 	if (vb->vb2_queue->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
477 		vb2_set_plane_payload(vb, 0, frame->sizeimage);
478 
479 	return 0;
480 }
481 
482 static void bdisp_buf_queue(struct vb2_buffer *vb)
483 {
484 	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
485 	struct bdisp_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
486 
487 	/* return to V4L2 any 0-size buffer so it can be dequeued by user */
488 	if (!vb2_get_plane_payload(vb, 0)) {
489 		dev_dbg(ctx->bdisp_dev->dev, "0 data buffer, skip it\n");
490 		vb2_buffer_done(vb, VB2_BUF_STATE_DONE);
491 		return;
492 	}
493 
494 	if (ctx->fh.m2m_ctx)
495 		v4l2_m2m_buf_queue(ctx->fh.m2m_ctx, vbuf);
496 }
497 
498 static int bdisp_start_streaming(struct vb2_queue *q, unsigned int count)
499 {
500 	struct bdisp_ctx *ctx = q->drv_priv;
501 	struct vb2_v4l2_buffer *buf;
502 	int ret = pm_runtime_resume_and_get(ctx->bdisp_dev->dev);
503 
504 	if (ret < 0) {
505 		dev_err(ctx->bdisp_dev->dev, "failed to set runtime PM\n");
506 
507 		if (q->type == V4L2_BUF_TYPE_VIDEO_OUTPUT) {
508 			while ((buf = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx)))
509 				v4l2_m2m_buf_done(buf, VB2_BUF_STATE_QUEUED);
510 		} else {
511 			while ((buf = v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx)))
512 				v4l2_m2m_buf_done(buf, VB2_BUF_STATE_QUEUED);
513 		}
514 
515 		return ret;
516 	}
517 
518 	return 0;
519 }
520 
521 static void bdisp_stop_streaming(struct vb2_queue *q)
522 {
523 	struct bdisp_ctx *ctx = q->drv_priv;
524 
525 	__bdisp_job_abort(ctx);
526 
527 	pm_runtime_put(ctx->bdisp_dev->dev);
528 }
529 
530 static const struct vb2_ops bdisp_qops = {
531 	.queue_setup     = bdisp_queue_setup,
532 	.buf_prepare     = bdisp_buf_prepare,
533 	.buf_queue       = bdisp_buf_queue,
534 	.wait_prepare    = vb2_ops_wait_prepare,
535 	.wait_finish     = vb2_ops_wait_finish,
536 	.stop_streaming  = bdisp_stop_streaming,
537 	.start_streaming = bdisp_start_streaming,
538 };
539 
540 static int queue_init(void *priv,
541 		      struct vb2_queue *src_vq, struct vb2_queue *dst_vq)
542 {
543 	struct bdisp_ctx *ctx = priv;
544 	int ret;
545 
546 	memset(src_vq, 0, sizeof(*src_vq));
547 	src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
548 	src_vq->io_modes = VB2_MMAP | VB2_DMABUF;
549 	src_vq->drv_priv = ctx;
550 	src_vq->ops = &bdisp_qops;
551 	src_vq->mem_ops = &vb2_dma_contig_memops;
552 	src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
553 	src_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
554 	src_vq->lock = &ctx->bdisp_dev->lock;
555 	src_vq->dev = ctx->bdisp_dev->v4l2_dev.dev;
556 
557 	ret = vb2_queue_init(src_vq);
558 	if (ret)
559 		return ret;
560 
561 	memset(dst_vq, 0, sizeof(*dst_vq));
562 	dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
563 	dst_vq->io_modes = VB2_MMAP | VB2_DMABUF;
564 	dst_vq->drv_priv = ctx;
565 	dst_vq->ops = &bdisp_qops;
566 	dst_vq->mem_ops = &vb2_dma_contig_memops;
567 	dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
568 	dst_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
569 	dst_vq->lock = &ctx->bdisp_dev->lock;
570 	dst_vq->dev = ctx->bdisp_dev->v4l2_dev.dev;
571 
572 	return vb2_queue_init(dst_vq);
573 }
574 
575 static int bdisp_open(struct file *file)
576 {
577 	struct bdisp_dev *bdisp = video_drvdata(file);
578 	struct bdisp_ctx *ctx = NULL;
579 	int ret;
580 
581 	if (mutex_lock_interruptible(&bdisp->lock))
582 		return -ERESTARTSYS;
583 
584 	/* Allocate memory for both context and node */
585 	ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
586 	if (!ctx) {
587 		ret = -ENOMEM;
588 		goto unlock;
589 	}
590 	ctx->bdisp_dev = bdisp;
591 
592 	if (bdisp_hw_alloc_nodes(ctx)) {
593 		dev_err(bdisp->dev, "no memory for nodes\n");
594 		ret = -ENOMEM;
595 		goto mem_ctx;
596 	}
597 
598 	v4l2_fh_init(&ctx->fh, bdisp->m2m.vdev);
599 
600 	ret = bdisp_ctrls_create(ctx);
601 	if (ret) {
602 		dev_err(bdisp->dev, "Failed to create control\n");
603 		goto error_fh;
604 	}
605 
606 	/* Use separate control handler per file handle */
607 	ctx->fh.ctrl_handler = &ctx->ctrl_handler;
608 	file->private_data = &ctx->fh;
609 	v4l2_fh_add(&ctx->fh);
610 
611 	/* Default format */
612 	ctx->src = bdisp_dflt_fmt;
613 	ctx->dst = bdisp_dflt_fmt;
614 
615 	/* Setup the device context for mem2mem mode. */
616 	ctx->fh.m2m_ctx = v4l2_m2m_ctx_init(bdisp->m2m.m2m_dev, ctx,
617 					    queue_init);
618 	if (IS_ERR(ctx->fh.m2m_ctx)) {
619 		dev_err(bdisp->dev, "Failed to initialize m2m context\n");
620 		ret = PTR_ERR(ctx->fh.m2m_ctx);
621 		goto error_ctrls;
622 	}
623 
624 	bdisp->m2m.refcnt++;
625 	set_bit(ST_M2M_OPEN, &bdisp->state);
626 
627 	dev_dbg(bdisp->dev, "driver opened, ctx = 0x%p\n", ctx);
628 
629 	mutex_unlock(&bdisp->lock);
630 
631 	return 0;
632 
633 error_ctrls:
634 	bdisp_ctrls_delete(ctx);
635 	v4l2_fh_del(&ctx->fh);
636 error_fh:
637 	v4l2_fh_exit(&ctx->fh);
638 	bdisp_hw_free_nodes(ctx);
639 mem_ctx:
640 	kfree(ctx);
641 unlock:
642 	mutex_unlock(&bdisp->lock);
643 
644 	return ret;
645 }
646 
647 static int bdisp_release(struct file *file)
648 {
649 	struct bdisp_ctx *ctx = fh_to_ctx(file->private_data);
650 	struct bdisp_dev *bdisp = ctx->bdisp_dev;
651 
652 	dev_dbg(bdisp->dev, "%s\n", __func__);
653 
654 	mutex_lock(&bdisp->lock);
655 
656 	v4l2_m2m_ctx_release(ctx->fh.m2m_ctx);
657 
658 	bdisp_ctrls_delete(ctx);
659 
660 	v4l2_fh_del(&ctx->fh);
661 	v4l2_fh_exit(&ctx->fh);
662 
663 	if (--bdisp->m2m.refcnt <= 0)
664 		clear_bit(ST_M2M_OPEN, &bdisp->state);
665 
666 	bdisp_hw_free_nodes(ctx);
667 
668 	kfree(ctx);
669 
670 	mutex_unlock(&bdisp->lock);
671 
672 	return 0;
673 }
674 
675 static const struct v4l2_file_operations bdisp_fops = {
676 	.owner          = THIS_MODULE,
677 	.open           = bdisp_open,
678 	.release        = bdisp_release,
679 	.poll           = v4l2_m2m_fop_poll,
680 	.unlocked_ioctl = video_ioctl2,
681 	.mmap           = v4l2_m2m_fop_mmap,
682 };
683 
684 static int bdisp_querycap(struct file *file, void *fh,
685 			  struct v4l2_capability *cap)
686 {
687 	struct bdisp_ctx *ctx = fh_to_ctx(fh);
688 	struct bdisp_dev *bdisp = ctx->bdisp_dev;
689 
690 	strscpy(cap->driver, bdisp->pdev->name, sizeof(cap->driver));
691 	strscpy(cap->card, bdisp->pdev->name, sizeof(cap->card));
692 	snprintf(cap->bus_info, sizeof(cap->bus_info), "platform:%s%d",
693 		 BDISP_NAME, bdisp->id);
694 	return 0;
695 }
696 
697 static int bdisp_enum_fmt(struct file *file, void *fh, struct v4l2_fmtdesc *f)
698 {
699 	struct bdisp_ctx *ctx = fh_to_ctx(fh);
700 	const struct bdisp_fmt *fmt;
701 
702 	if (f->index >= ARRAY_SIZE(bdisp_formats))
703 		return -EINVAL;
704 
705 	fmt = &bdisp_formats[f->index];
706 
707 	if ((fmt->pixelformat == V4L2_PIX_FMT_YUV420) &&
708 	    (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)) {
709 		dev_dbg(ctx->bdisp_dev->dev, "No YU12 on capture\n");
710 		return -EINVAL;
711 	}
712 	f->pixelformat = fmt->pixelformat;
713 
714 	return 0;
715 }
716 
717 static int bdisp_g_fmt(struct file *file, void *fh, struct v4l2_format *f)
718 {
719 	struct bdisp_ctx *ctx = fh_to_ctx(fh);
720 	struct v4l2_pix_format *pix;
721 	struct bdisp_frame *frame  = ctx_get_frame(ctx, f->type);
722 
723 	if (IS_ERR(frame)) {
724 		dev_err(ctx->bdisp_dev->dev, "Invalid frame (%p)\n", frame);
725 		return PTR_ERR(frame);
726 	}
727 
728 	pix = &f->fmt.pix;
729 	pix->width = frame->width;
730 	pix->height = frame->height;
731 	pix->pixelformat = frame->fmt->pixelformat;
732 	pix->field = frame->field;
733 	pix->bytesperline = frame->bytesperline;
734 	pix->sizeimage = frame->sizeimage;
735 	pix->colorspace = (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT) ?
736 				frame->colorspace : bdisp_dflt_fmt.colorspace;
737 
738 	return 0;
739 }
740 
741 static int bdisp_try_fmt(struct file *file, void *fh, struct v4l2_format *f)
742 {
743 	struct bdisp_ctx *ctx = fh_to_ctx(fh);
744 	struct v4l2_pix_format *pix = &f->fmt.pix;
745 	const struct bdisp_fmt *format;
746 	u32 in_w, in_h;
747 
748 	format = bdisp_find_fmt(pix->pixelformat);
749 	if (!format) {
750 		dev_dbg(ctx->bdisp_dev->dev, "Unknown format 0x%x\n",
751 			pix->pixelformat);
752 		return -EINVAL;
753 	}
754 
755 	/* YUV420P only supported for VIDEO_OUTPUT */
756 	if ((format->pixelformat == V4L2_PIX_FMT_YUV420) &&
757 	    (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)) {
758 		dev_dbg(ctx->bdisp_dev->dev, "No YU12 on capture\n");
759 		return -EINVAL;
760 	}
761 
762 	/* Field (interlaced only supported on OUTPUT) */
763 	if ((f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE) ||
764 	    (pix->field != V4L2_FIELD_INTERLACED))
765 		pix->field = V4L2_FIELD_NONE;
766 
767 	/* Adjust width & height */
768 	in_w = pix->width;
769 	in_h = pix->height;
770 	v4l_bound_align_image(&pix->width,
771 			      BDISP_MIN_W, BDISP_MAX_W,
772 			      ffs(format->w_align) - 1,
773 			      &pix->height,
774 			      BDISP_MIN_H, BDISP_MAX_H,
775 			      ffs(format->h_align) - 1,
776 			      0);
777 	if ((pix->width != in_w) || (pix->height != in_h))
778 		dev_dbg(ctx->bdisp_dev->dev,
779 			"%s size updated: %dx%d -> %dx%d\n", __func__,
780 			in_w, in_h, pix->width, pix->height);
781 
782 	pix->bytesperline = (pix->width * format->bpp_plane0) / 8;
783 	pix->sizeimage = (pix->width * pix->height * format->bpp) / 8;
784 
785 	if (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
786 		pix->colorspace = bdisp_dflt_fmt.colorspace;
787 
788 	return 0;
789 }
790 
791 static int bdisp_s_fmt(struct file *file, void *fh, struct v4l2_format *f)
792 {
793 	struct bdisp_ctx *ctx = fh_to_ctx(fh);
794 	struct vb2_queue *vq;
795 	struct bdisp_frame *frame;
796 	struct v4l2_pix_format *pix;
797 	int ret;
798 	u32 state;
799 
800 	ret = bdisp_try_fmt(file, fh, f);
801 	if (ret) {
802 		dev_err(ctx->bdisp_dev->dev, "Cannot set format\n");
803 		return ret;
804 	}
805 
806 	vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);
807 	if (vb2_is_streaming(vq)) {
808 		dev_err(ctx->bdisp_dev->dev, "queue (%d) busy\n", f->type);
809 		return -EBUSY;
810 	}
811 
812 	frame = (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT) ?
813 			&ctx->src : &ctx->dst;
814 	pix = &f->fmt.pix;
815 	frame->fmt = bdisp_find_fmt(pix->pixelformat);
816 	if (!frame->fmt) {
817 		dev_err(ctx->bdisp_dev->dev, "Unknown format 0x%x\n",
818 			pix->pixelformat);
819 		return -EINVAL;
820 	}
821 
822 	frame->width = pix->width;
823 	frame->height = pix->height;
824 	frame->bytesperline = pix->bytesperline;
825 	frame->sizeimage = pix->sizeimage;
826 	frame->field = pix->field;
827 	if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT)
828 		frame->colorspace = pix->colorspace;
829 
830 	frame->crop.width = frame->width;
831 	frame->crop.height = frame->height;
832 	frame->crop.left = 0;
833 	frame->crop.top = 0;
834 
835 	state = BDISP_PARAMS;
836 	state |= (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE) ?
837 			BDISP_DST_FMT : BDISP_SRC_FMT;
838 	bdisp_ctx_state_lock_set(state, ctx);
839 
840 	return 0;
841 }
842 
843 static int bdisp_g_selection(struct file *file, void *fh,
844 			     struct v4l2_selection *s)
845 {
846 	struct bdisp_frame *frame;
847 	struct bdisp_ctx *ctx = fh_to_ctx(fh);
848 
849 	frame = ctx_get_frame(ctx, s->type);
850 	if (IS_ERR(frame)) {
851 		dev_err(ctx->bdisp_dev->dev, "Invalid frame (%p)\n", frame);
852 		return PTR_ERR(frame);
853 	}
854 
855 	switch (s->type) {
856 	case V4L2_BUF_TYPE_VIDEO_OUTPUT:
857 		switch (s->target) {
858 		case V4L2_SEL_TGT_CROP:
859 			/* cropped frame */
860 			s->r = frame->crop;
861 			break;
862 		case V4L2_SEL_TGT_CROP_DEFAULT:
863 		case V4L2_SEL_TGT_CROP_BOUNDS:
864 			/* complete frame */
865 			s->r.left = 0;
866 			s->r.top = 0;
867 			s->r.width = frame->width;
868 			s->r.height = frame->height;
869 			break;
870 		default:
871 			dev_err(ctx->bdisp_dev->dev, "Invalid target\n");
872 			return -EINVAL;
873 		}
874 		break;
875 
876 	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
877 		switch (s->target) {
878 		case V4L2_SEL_TGT_COMPOSE:
879 		case V4L2_SEL_TGT_COMPOSE_PADDED:
880 			/* composed (cropped) frame */
881 			s->r = frame->crop;
882 			break;
883 		case V4L2_SEL_TGT_COMPOSE_DEFAULT:
884 		case V4L2_SEL_TGT_COMPOSE_BOUNDS:
885 			/* complete frame */
886 			s->r.left = 0;
887 			s->r.top = 0;
888 			s->r.width = frame->width;
889 			s->r.height = frame->height;
890 			break;
891 		default:
892 			dev_err(ctx->bdisp_dev->dev, "Invalid target\n");
893 			return -EINVAL;
894 		}
895 		break;
896 
897 	default:
898 		dev_err(ctx->bdisp_dev->dev, "Invalid type\n");
899 		return -EINVAL;
900 	}
901 
902 	return 0;
903 }
904 
905 static int is_rect_enclosed(struct v4l2_rect *a, struct v4l2_rect *b)
906 {
907 	/* Return 1 if a is enclosed in b, or 0 otherwise. */
908 
909 	if (a->left < b->left || a->top < b->top)
910 		return 0;
911 
912 	if (a->left + a->width > b->left + b->width)
913 		return 0;
914 
915 	if (a->top + a->height > b->top + b->height)
916 		return 0;
917 
918 	return 1;
919 }
920 
921 static int bdisp_s_selection(struct file *file, void *fh,
922 			     struct v4l2_selection *s)
923 {
924 	struct bdisp_frame *frame;
925 	struct bdisp_ctx *ctx = fh_to_ctx(fh);
926 	struct v4l2_rect *in, out;
927 	bool valid = false;
928 
929 	if ((s->type == V4L2_BUF_TYPE_VIDEO_OUTPUT) &&
930 	    (s->target == V4L2_SEL_TGT_CROP))
931 		valid = true;
932 
933 	if ((s->type == V4L2_BUF_TYPE_VIDEO_CAPTURE) &&
934 	    (s->target == V4L2_SEL_TGT_COMPOSE))
935 		valid = true;
936 
937 	if (!valid) {
938 		dev_err(ctx->bdisp_dev->dev, "Invalid type / target\n");
939 		return -EINVAL;
940 	}
941 
942 	frame = ctx_get_frame(ctx, s->type);
943 	if (IS_ERR(frame)) {
944 		dev_err(ctx->bdisp_dev->dev, "Invalid frame (%p)\n", frame);
945 		return PTR_ERR(frame);
946 	}
947 
948 	in = &s->r;
949 	out = *in;
950 
951 	/* Align and check origin */
952 	out.left = ALIGN(in->left, frame->fmt->w_align);
953 	out.top = ALIGN(in->top, frame->fmt->h_align);
954 
955 	if ((out.left < 0) || (out.left >= frame->width) ||
956 	    (out.top < 0) || (out.top >= frame->height)) {
957 		dev_err(ctx->bdisp_dev->dev,
958 			"Invalid crop: %dx%d@(%d,%d) vs frame: %dx%d\n",
959 			out.width, out.height, out.left, out.top,
960 			frame->width, frame->height);
961 		return -EINVAL;
962 	}
963 
964 	/* Align and check size */
965 	out.width = ALIGN(in->width, frame->fmt->w_align);
966 	out.height = ALIGN(in->height, frame->fmt->w_align);
967 
968 	if (((out.left + out.width) > frame->width) ||
969 	    ((out.top + out.height) > frame->height)) {
970 		dev_err(ctx->bdisp_dev->dev,
971 			"Invalid crop: %dx%d@(%d,%d) vs frame: %dx%d\n",
972 			out.width, out.height, out.left, out.top,
973 			frame->width, frame->height);
974 		return -EINVAL;
975 	}
976 
977 	/* Checks adjust constraints flags */
978 	if (s->flags & V4L2_SEL_FLAG_LE && !is_rect_enclosed(&out, in))
979 		return -ERANGE;
980 
981 	if (s->flags & V4L2_SEL_FLAG_GE && !is_rect_enclosed(in, &out))
982 		return -ERANGE;
983 
984 	if ((out.left != in->left) || (out.top != in->top) ||
985 	    (out.width != in->width) || (out.height != in->height)) {
986 		dev_dbg(ctx->bdisp_dev->dev,
987 			"%s crop updated: %dx%d@(%d,%d) -> %dx%d@(%d,%d)\n",
988 			__func__, in->width, in->height, in->left, in->top,
989 			out.width, out.height, out.left, out.top);
990 		*in = out;
991 	}
992 
993 	frame->crop = out;
994 
995 	bdisp_ctx_state_lock_set(BDISP_PARAMS, ctx);
996 
997 	return 0;
998 }
999 
1000 static int bdisp_streamon(struct file *file, void *fh, enum v4l2_buf_type type)
1001 {
1002 	struct bdisp_ctx *ctx = fh_to_ctx(fh);
1003 
1004 	if ((type == V4L2_BUF_TYPE_VIDEO_OUTPUT) &&
1005 	    !bdisp_ctx_state_is_set(BDISP_SRC_FMT, ctx)) {
1006 		dev_err(ctx->bdisp_dev->dev, "src not defined\n");
1007 		return -EINVAL;
1008 	}
1009 
1010 	if ((type == V4L2_BUF_TYPE_VIDEO_CAPTURE) &&
1011 	    !bdisp_ctx_state_is_set(BDISP_DST_FMT, ctx)) {
1012 		dev_err(ctx->bdisp_dev->dev, "dst not defined\n");
1013 		return -EINVAL;
1014 	}
1015 
1016 	return v4l2_m2m_streamon(file, ctx->fh.m2m_ctx, type);
1017 }
1018 
1019 static const struct v4l2_ioctl_ops bdisp_ioctl_ops = {
1020 	.vidioc_querycap                = bdisp_querycap,
1021 	.vidioc_enum_fmt_vid_cap        = bdisp_enum_fmt,
1022 	.vidioc_enum_fmt_vid_out        = bdisp_enum_fmt,
1023 	.vidioc_g_fmt_vid_cap           = bdisp_g_fmt,
1024 	.vidioc_g_fmt_vid_out           = bdisp_g_fmt,
1025 	.vidioc_try_fmt_vid_cap         = bdisp_try_fmt,
1026 	.vidioc_try_fmt_vid_out         = bdisp_try_fmt,
1027 	.vidioc_s_fmt_vid_cap           = bdisp_s_fmt,
1028 	.vidioc_s_fmt_vid_out           = bdisp_s_fmt,
1029 	.vidioc_g_selection		= bdisp_g_selection,
1030 	.vidioc_s_selection		= bdisp_s_selection,
1031 	.vidioc_reqbufs                 = v4l2_m2m_ioctl_reqbufs,
1032 	.vidioc_create_bufs             = v4l2_m2m_ioctl_create_bufs,
1033 	.vidioc_expbuf                  = v4l2_m2m_ioctl_expbuf,
1034 	.vidioc_querybuf                = v4l2_m2m_ioctl_querybuf,
1035 	.vidioc_qbuf                    = v4l2_m2m_ioctl_qbuf,
1036 	.vidioc_dqbuf                   = v4l2_m2m_ioctl_dqbuf,
1037 	.vidioc_streamon                = bdisp_streamon,
1038 	.vidioc_streamoff               = v4l2_m2m_ioctl_streamoff,
1039 	.vidioc_subscribe_event         = v4l2_ctrl_subscribe_event,
1040 	.vidioc_unsubscribe_event       = v4l2_event_unsubscribe,
1041 };
1042 
1043 static int bdisp_register_device(struct bdisp_dev *bdisp)
1044 {
1045 	int ret;
1046 
1047 	if (!bdisp)
1048 		return -ENODEV;
1049 
1050 	bdisp->vdev.fops        = &bdisp_fops;
1051 	bdisp->vdev.ioctl_ops   = &bdisp_ioctl_ops;
1052 	bdisp->vdev.release     = video_device_release_empty;
1053 	bdisp->vdev.lock        = &bdisp->lock;
1054 	bdisp->vdev.vfl_dir     = VFL_DIR_M2M;
1055 	bdisp->vdev.v4l2_dev    = &bdisp->v4l2_dev;
1056 	bdisp->vdev.device_caps = V4L2_CAP_STREAMING | V4L2_CAP_VIDEO_M2M;
1057 	snprintf(bdisp->vdev.name, sizeof(bdisp->vdev.name), "%s.%d",
1058 		 BDISP_NAME, bdisp->id);
1059 
1060 	video_set_drvdata(&bdisp->vdev, bdisp);
1061 
1062 	bdisp->m2m.vdev = &bdisp->vdev;
1063 	bdisp->m2m.m2m_dev = v4l2_m2m_init(&bdisp_m2m_ops);
1064 	if (IS_ERR(bdisp->m2m.m2m_dev)) {
1065 		dev_err(bdisp->dev, "failed to initialize v4l2-m2m device\n");
1066 		return PTR_ERR(bdisp->m2m.m2m_dev);
1067 	}
1068 
1069 	ret = video_register_device(&bdisp->vdev, VFL_TYPE_VIDEO, -1);
1070 	if (ret) {
1071 		dev_err(bdisp->dev,
1072 			"%s(): failed to register video device\n", __func__);
1073 		v4l2_m2m_release(bdisp->m2m.m2m_dev);
1074 		return ret;
1075 	}
1076 
1077 	return 0;
1078 }
1079 
1080 static void bdisp_unregister_device(struct bdisp_dev *bdisp)
1081 {
1082 	if (!bdisp)
1083 		return;
1084 
1085 	if (bdisp->m2m.m2m_dev)
1086 		v4l2_m2m_release(bdisp->m2m.m2m_dev);
1087 
1088 	video_unregister_device(bdisp->m2m.vdev);
1089 }
1090 
1091 static irqreturn_t bdisp_irq_thread(int irq, void *priv)
1092 {
1093 	struct bdisp_dev *bdisp = priv;
1094 	struct bdisp_ctx *ctx;
1095 
1096 	spin_lock(&bdisp->slock);
1097 
1098 	bdisp_dbg_perf_end(bdisp);
1099 
1100 	cancel_delayed_work(&bdisp->timeout_work);
1101 
1102 	if (!test_and_clear_bit(ST_M2M_RUNNING, &bdisp->state))
1103 		goto isr_unlock;
1104 
1105 	if (test_and_clear_bit(ST_M2M_SUSPENDING, &bdisp->state)) {
1106 		set_bit(ST_M2M_SUSPENDED, &bdisp->state);
1107 		wake_up(&bdisp->irq_queue);
1108 		goto isr_unlock;
1109 	}
1110 
1111 	ctx = v4l2_m2m_get_curr_priv(bdisp->m2m.m2m_dev);
1112 	if (!ctx || !ctx->fh.m2m_ctx)
1113 		goto isr_unlock;
1114 
1115 	spin_unlock(&bdisp->slock);
1116 
1117 	bdisp_job_finish(ctx, VB2_BUF_STATE_DONE);
1118 
1119 	if (bdisp_ctx_state_is_set(BDISP_CTX_STOP_REQ, ctx)) {
1120 		bdisp_ctx_state_lock_clear(BDISP_CTX_STOP_REQ, ctx);
1121 		wake_up(&bdisp->irq_queue);
1122 	}
1123 
1124 	return IRQ_HANDLED;
1125 
1126 isr_unlock:
1127 	spin_unlock(&bdisp->slock);
1128 
1129 	return IRQ_HANDLED;
1130 }
1131 
1132 static irqreturn_t bdisp_irq_handler(int irq, void *priv)
1133 {
1134 	if (bdisp_hw_get_and_clear_irq((struct bdisp_dev *)priv))
1135 		return IRQ_NONE;
1136 	else
1137 		return IRQ_WAKE_THREAD;
1138 }
1139 
1140 static void bdisp_irq_timeout(struct work_struct *ptr)
1141 {
1142 	struct delayed_work *twork = to_delayed_work(ptr);
1143 	struct bdisp_dev *bdisp = container_of(twork, struct bdisp_dev,
1144 			timeout_work);
1145 	struct bdisp_ctx *ctx;
1146 
1147 	ctx = v4l2_m2m_get_curr_priv(bdisp->m2m.m2m_dev);
1148 
1149 	dev_err(ctx->bdisp_dev->dev, "Device work timeout\n");
1150 
1151 	spin_lock(&bdisp->slock);
1152 	clear_bit(ST_M2M_RUNNING, &bdisp->state);
1153 	spin_unlock(&bdisp->slock);
1154 
1155 	bdisp_hw_reset(bdisp);
1156 
1157 	bdisp_job_finish(ctx, VB2_BUF_STATE_ERROR);
1158 }
1159 
1160 static int bdisp_m2m_suspend(struct bdisp_dev *bdisp)
1161 {
1162 	unsigned long flags;
1163 	int timeout;
1164 
1165 	spin_lock_irqsave(&bdisp->slock, flags);
1166 	if (!test_bit(ST_M2M_RUNNING, &bdisp->state)) {
1167 		spin_unlock_irqrestore(&bdisp->slock, flags);
1168 		return 0;
1169 	}
1170 	clear_bit(ST_M2M_SUSPENDED, &bdisp->state);
1171 	set_bit(ST_M2M_SUSPENDING, &bdisp->state);
1172 	spin_unlock_irqrestore(&bdisp->slock, flags);
1173 
1174 	timeout = wait_event_timeout(bdisp->irq_queue,
1175 				     test_bit(ST_M2M_SUSPENDED, &bdisp->state),
1176 				     BDISP_WORK_TIMEOUT);
1177 
1178 	clear_bit(ST_M2M_SUSPENDING, &bdisp->state);
1179 
1180 	if (!timeout) {
1181 		dev_err(bdisp->dev, "%s IRQ timeout\n", __func__);
1182 		return -EAGAIN;
1183 	}
1184 
1185 	return 0;
1186 }
1187 
1188 static int bdisp_m2m_resume(struct bdisp_dev *bdisp)
1189 {
1190 	struct bdisp_ctx *ctx;
1191 	unsigned long flags;
1192 
1193 	spin_lock_irqsave(&bdisp->slock, flags);
1194 	ctx = bdisp->m2m.ctx;
1195 	bdisp->m2m.ctx = NULL;
1196 	spin_unlock_irqrestore(&bdisp->slock, flags);
1197 
1198 	if (test_and_clear_bit(ST_M2M_SUSPENDED, &bdisp->state))
1199 		bdisp_job_finish(ctx, VB2_BUF_STATE_ERROR);
1200 
1201 	return 0;
1202 }
1203 
1204 static int bdisp_runtime_resume(struct device *dev)
1205 {
1206 	struct bdisp_dev *bdisp = dev_get_drvdata(dev);
1207 	int ret = clk_enable(bdisp->clock);
1208 
1209 	if (ret)
1210 		return ret;
1211 
1212 	return bdisp_m2m_resume(bdisp);
1213 }
1214 
1215 static int bdisp_runtime_suspend(struct device *dev)
1216 {
1217 	struct bdisp_dev *bdisp = dev_get_drvdata(dev);
1218 	int ret = bdisp_m2m_suspend(bdisp);
1219 
1220 	if (!ret)
1221 		clk_disable(bdisp->clock);
1222 
1223 	return ret;
1224 }
1225 
1226 static int bdisp_resume(struct device *dev)
1227 {
1228 	struct bdisp_dev *bdisp = dev_get_drvdata(dev);
1229 	unsigned long flags;
1230 	int opened;
1231 
1232 	spin_lock_irqsave(&bdisp->slock, flags);
1233 	opened = test_bit(ST_M2M_OPEN, &bdisp->state);
1234 	spin_unlock_irqrestore(&bdisp->slock, flags);
1235 
1236 	if (!opened)
1237 		return 0;
1238 
1239 	if (!pm_runtime_suspended(dev))
1240 		return bdisp_runtime_resume(dev);
1241 
1242 	return 0;
1243 }
1244 
1245 static int bdisp_suspend(struct device *dev)
1246 {
1247 	if (!pm_runtime_suspended(dev))
1248 		return bdisp_runtime_suspend(dev);
1249 
1250 	return 0;
1251 }
1252 
1253 static const struct dev_pm_ops bdisp_pm_ops = {
1254 	.suspend                = bdisp_suspend,
1255 	.resume                 = bdisp_resume,
1256 	.runtime_suspend        = bdisp_runtime_suspend,
1257 	.runtime_resume         = bdisp_runtime_resume,
1258 };
1259 
1260 static int bdisp_remove(struct platform_device *pdev)
1261 {
1262 	struct bdisp_dev *bdisp = platform_get_drvdata(pdev);
1263 
1264 	bdisp_unregister_device(bdisp);
1265 
1266 	bdisp_hw_free_filters(bdisp->dev);
1267 
1268 	pm_runtime_disable(&pdev->dev);
1269 
1270 	bdisp_debugfs_remove(bdisp);
1271 
1272 	v4l2_device_unregister(&bdisp->v4l2_dev);
1273 
1274 	if (!IS_ERR(bdisp->clock))
1275 		clk_unprepare(bdisp->clock);
1276 
1277 	destroy_workqueue(bdisp->work_queue);
1278 
1279 	dev_dbg(&pdev->dev, "%s driver unloaded\n", pdev->name);
1280 
1281 	return 0;
1282 }
1283 
1284 static int bdisp_probe(struct platform_device *pdev)
1285 {
1286 	struct bdisp_dev *bdisp;
1287 	struct device *dev = &pdev->dev;
1288 	int ret;
1289 
1290 	dev_dbg(dev, "%s\n", __func__);
1291 
1292 	bdisp = devm_kzalloc(dev, sizeof(struct bdisp_dev), GFP_KERNEL);
1293 	if (!bdisp)
1294 		return -ENOMEM;
1295 
1296 	ret = dma_coerce_mask_and_coherent(dev, DMA_BIT_MASK(32));
1297 	if (ret)
1298 		return ret;
1299 
1300 	bdisp->pdev = pdev;
1301 	bdisp->dev = dev;
1302 	platform_set_drvdata(pdev, bdisp);
1303 
1304 	if (dev->of_node)
1305 		bdisp->id = of_alias_get_id(pdev->dev.of_node, BDISP_NAME);
1306 	else
1307 		bdisp->id = pdev->id;
1308 
1309 	init_waitqueue_head(&bdisp->irq_queue);
1310 	INIT_DELAYED_WORK(&bdisp->timeout_work, bdisp_irq_timeout);
1311 	bdisp->work_queue = create_workqueue(BDISP_NAME);
1312 
1313 	spin_lock_init(&bdisp->slock);
1314 	mutex_init(&bdisp->lock);
1315 
1316 	/* get resources */
1317 	bdisp->regs = devm_platform_ioremap_resource(pdev, 0);
1318 	if (IS_ERR(bdisp->regs)) {
1319 		ret = PTR_ERR(bdisp->regs);
1320 		goto err_wq;
1321 	}
1322 
1323 	bdisp->clock = devm_clk_get(dev, BDISP_NAME);
1324 	if (IS_ERR(bdisp->clock)) {
1325 		dev_err(dev, "failed to get clock\n");
1326 		ret = PTR_ERR(bdisp->clock);
1327 		goto err_wq;
1328 	}
1329 
1330 	ret = clk_prepare(bdisp->clock);
1331 	if (ret < 0) {
1332 		dev_err(dev, "clock prepare failed\n");
1333 		bdisp->clock = ERR_PTR(-EINVAL);
1334 		goto err_wq;
1335 	}
1336 
1337 	ret = platform_get_irq(pdev, 0);
1338 	if (ret < 0)
1339 		goto err_clk;
1340 
1341 	ret = devm_request_threaded_irq(dev, ret, bdisp_irq_handler,
1342 					bdisp_irq_thread, IRQF_ONESHOT,
1343 					pdev->name, bdisp);
1344 	if (ret) {
1345 		dev_err(dev, "failed to install irq\n");
1346 		goto err_clk;
1347 	}
1348 
1349 	/* v4l2 register */
1350 	ret = v4l2_device_register(dev, &bdisp->v4l2_dev);
1351 	if (ret) {
1352 		dev_err(dev, "failed to register\n");
1353 		goto err_clk;
1354 	}
1355 
1356 	/* Debug */
1357 	bdisp_debugfs_create(bdisp);
1358 
1359 	/* Power management */
1360 	pm_runtime_enable(dev);
1361 	ret = pm_runtime_resume_and_get(dev);
1362 	if (ret < 0) {
1363 		dev_err(dev, "failed to set PM\n");
1364 		goto err_remove;
1365 	}
1366 
1367 	/* Filters */
1368 	if (bdisp_hw_alloc_filters(bdisp->dev)) {
1369 		dev_err(bdisp->dev, "no memory for filters\n");
1370 		ret = -ENOMEM;
1371 		goto err_pm;
1372 	}
1373 
1374 	/* Register */
1375 	ret = bdisp_register_device(bdisp);
1376 	if (ret) {
1377 		dev_err(dev, "failed to register\n");
1378 		goto err_filter;
1379 	}
1380 
1381 	dev_info(dev, "%s%d registered as /dev/video%d\n", BDISP_NAME,
1382 		 bdisp->id, bdisp->vdev.num);
1383 
1384 	pm_runtime_put(dev);
1385 
1386 	return 0;
1387 
1388 err_filter:
1389 	bdisp_hw_free_filters(bdisp->dev);
1390 err_pm:
1391 	pm_runtime_put(dev);
1392 err_remove:
1393 	pm_runtime_disable(dev);
1394 	bdisp_debugfs_remove(bdisp);
1395 	v4l2_device_unregister(&bdisp->v4l2_dev);
1396 err_clk:
1397 	if (!IS_ERR(bdisp->clock))
1398 		clk_unprepare(bdisp->clock);
1399 err_wq:
1400 	destroy_workqueue(bdisp->work_queue);
1401 	return ret;
1402 }
1403 
1404 static const struct of_device_id bdisp_match_types[] = {
1405 	{
1406 		.compatible = "st,stih407-bdisp",
1407 	},
1408 	{ /* end node */ }
1409 };
1410 
1411 MODULE_DEVICE_TABLE(of, bdisp_match_types);
1412 
1413 static struct platform_driver bdisp_driver = {
1414 	.probe          = bdisp_probe,
1415 	.remove         = bdisp_remove,
1416 	.driver         = {
1417 		.name           = BDISP_NAME,
1418 		.of_match_table = bdisp_match_types,
1419 		.pm             = &bdisp_pm_ops,
1420 	},
1421 };
1422 
1423 module_platform_driver(bdisp_driver);
1424 
1425 MODULE_DESCRIPTION("2D blitter for STMicroelectronics SoC");
1426 MODULE_AUTHOR("Fabien Dessenne <fabien.dessenne@st.com>");
1427 MODULE_LICENSE("GPL");
1428