bdisp-v4l2.c

   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
  36static inline struct bdisp_ctx *file_to_ctx(struct file *filp)
  37{
  38	return container_of(file_to_v4l2_fh(filp), struct bdisp_ctx, fh);
  39}
  40
  41enum bdisp_dev_flags {
  42	ST_M2M_OPEN,            /* Driver opened */
  43	ST_M2M_RUNNING,         /* HW device running */
  44	ST_M2M_SUSPENDED,       /* Driver suspended */
  45	ST_M2M_SUSPENDING,      /* Driver being suspended */
  46};
  47
  48static const struct bdisp_fmt bdisp_formats[] = {
  49	/* ARGB888. [31:0] A:R:G:B 8:8:8:8 little endian */
  50	{
  51		.pixelformat    = V4L2_PIX_FMT_ABGR32, /* is actually ARGB */
  52		.nb_planes      = 1,
  53		.bpp            = 32,
  54		.bpp_plane0     = 32,
  55		.w_align        = 1,
  56		.h_align        = 1
  57	},
  58	/* XRGB888. [31:0] x:R:G:B 8:8:8:8 little endian */
  59	{
  60		.pixelformat    = V4L2_PIX_FMT_XBGR32, /* is actually xRGB */
  61		.nb_planes      = 1,
  62		.bpp            = 32,
  63		.bpp_plane0     = 32,
  64		.w_align        = 1,
  65		.h_align        = 1
  66	},
  67	/* RGB565. [15:0] R:G:B 5:6:5 little endian */
  68	{
  69		.pixelformat    = V4L2_PIX_FMT_RGB565,
  70		.nb_planes      = 1,
  71		.bpp            = 16,
  72		.bpp_plane0     = 16,
  73		.w_align        = 1,
  74		.h_align        = 1
  75	},
  76	/* NV12. YUV420SP - 1 plane for Y + 1 plane for (CbCr) */
  77	{
  78		.pixelformat    = V4L2_PIX_FMT_NV12,
  79		.nb_planes      = 2,
  80		.bpp            = 12,
  81		.bpp_plane0     = 8,
  82		.w_align        = 2,
  83		.h_align        = 2
  84	},
  85	/* RGB888. [23:0] B:G:R 8:8:8 little endian */
  86	{
  87		.pixelformat    = V4L2_PIX_FMT_RGB24,
  88		.nb_planes      = 1,
  89		.bpp            = 24,
  90		.bpp_plane0     = 24,
  91		.w_align        = 1,
  92		.h_align        = 1
  93	},
  94	/* YU12. YUV420P - 1 plane for Y + 1 plane for Cb + 1 plane for Cr
  95	 * To keep as the LAST element of this table (no support on capture)
  96	 */
  97	{
  98		.pixelformat    = V4L2_PIX_FMT_YUV420,
  99		.nb_planes      = 3,
 100		.bpp            = 12,
 101		.bpp_plane0     = 8,
 102		.w_align        = 2,
 103		.h_align        = 2
 104	}
 105};
 106
 107/* Default format : HD ARGB32*/
 108#define BDISP_DEF_WIDTH         1920
 109#define BDISP_DEF_HEIGHT        1080
 110
 111static const struct bdisp_frame bdisp_dflt_fmt = {
 112		.width          = BDISP_DEF_WIDTH,
 113		.height         = BDISP_DEF_HEIGHT,
 114		.fmt            = &bdisp_formats[0],
 115		.field          = V4L2_FIELD_NONE,
 116		.bytesperline   = BDISP_DEF_WIDTH * 4,
 117		.sizeimage      = BDISP_DEF_WIDTH * BDISP_DEF_HEIGHT * 4,
 118		.colorspace     = V4L2_COLORSPACE_REC709,
 119		.crop           = {0, 0, BDISP_DEF_WIDTH, BDISP_DEF_HEIGHT},
 120		.paddr          = {0, 0, 0, 0}
 121};
 122
 123static inline void bdisp_ctx_state_lock_set(u32 state, struct bdisp_ctx *ctx)
 124{
 125	unsigned long flags;
 126
 127	spin_lock_irqsave(&ctx->bdisp_dev->slock, flags);
 128	ctx->state |= state;
 129	spin_unlock_irqrestore(&ctx->bdisp_dev->slock, flags);
 130}
 131
 132static inline void bdisp_ctx_state_lock_clear(u32 state, struct bdisp_ctx *ctx)
 133{
 134	unsigned long flags;
 135
 136	spin_lock_irqsave(&ctx->bdisp_dev->slock, flags);
 137	ctx->state &= ~state;
 138	spin_unlock_irqrestore(&ctx->bdisp_dev->slock, flags);
 139}
 140
 141static inline bool bdisp_ctx_state_is_set(u32 mask, struct bdisp_ctx *ctx)
 142{
 143	unsigned long flags;
 144	bool ret;
 145
 146	spin_lock_irqsave(&ctx->bdisp_dev->slock, flags);
 147	ret = (ctx->state & mask) == mask;
 148	spin_unlock_irqrestore(&ctx->bdisp_dev->slock, flags);
 149
 150	return ret;
 151}
 152
 153static const struct bdisp_fmt *bdisp_find_fmt(u32 pixelformat)
 154{
 155	const struct bdisp_fmt *fmt;
 156	unsigned int i;
 157
 158	for (i = 0; i < ARRAY_SIZE(bdisp_formats); i++) {
 159		fmt = &bdisp_formats[i];
 160		if (fmt->pixelformat == pixelformat)
 161			return fmt;
 162	}
 163
 164	return NULL;
 165}
 166
 167static struct bdisp_frame *ctx_get_frame(struct bdisp_ctx *ctx,
 168					 enum v4l2_buf_type type)
 169{
 170	switch (type) {
 171	case V4L2_BUF_TYPE_VIDEO_OUTPUT:
 172		return &ctx->src;
 173	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
 174		return &ctx->dst;
 175	default:
 176		dev_err(ctx->bdisp_dev->dev,
 177			"Wrong buffer/video queue type (%d)\n", type);
 178		break;
 179	}
 180
 181	return ERR_PTR(-EINVAL);
 182}
 183
 184static void bdisp_job_finish(struct bdisp_ctx *ctx, int vb_state)
 185{
 186	struct vb2_v4l2_buffer *src_vb, *dst_vb;
 187
 188	if (WARN(!ctx || !ctx->fh.m2m_ctx, "Null hardware context\n"))
 189		return;
 190
 191	dev_dbg(ctx->bdisp_dev->dev, "%s\n", __func__);
 192
 193	src_vb = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
 194	dst_vb = v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
 195
 196	if (src_vb && dst_vb) {
 197		dst_vb->vb2_buf.timestamp = src_vb->vb2_buf.timestamp;
 198		dst_vb->timecode = src_vb->timecode;
 199		dst_vb->flags &= ~V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
 200		dst_vb->flags |= src_vb->flags &
 201					  V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
 202
 203		v4l2_m2m_buf_done(src_vb, vb_state);
 204		v4l2_m2m_buf_done(dst_vb, vb_state);
 205
 206		v4l2_m2m_job_finish(ctx->bdisp_dev->m2m.m2m_dev,
 207				    ctx->fh.m2m_ctx);
 208	}
 209}
 210
 211static int bdisp_ctx_stop_req(struct bdisp_ctx *ctx)
 212{
 213	struct bdisp_ctx *curr_ctx;
 214	struct bdisp_dev *bdisp = ctx->bdisp_dev;
 215	int ret;
 216
 217	dev_dbg(ctx->bdisp_dev->dev, "%s\n", __func__);
 218
 219	cancel_delayed_work(&bdisp->timeout_work);
 220
 221	curr_ctx = v4l2_m2m_get_curr_priv(bdisp->m2m.m2m_dev);
 222	if (!test_bit(ST_M2M_RUNNING, &bdisp->state) || (curr_ctx != ctx))
 223		return 0;
 224
 225	bdisp_ctx_state_lock_set(BDISP_CTX_STOP_REQ, ctx);
 226
 227	ret = wait_event_timeout(bdisp->irq_queue,
 228			!bdisp_ctx_state_is_set(BDISP_CTX_STOP_REQ, ctx),
 229			BDISP_WORK_TIMEOUT);
 230
 231	if (!ret) {
 232		dev_err(ctx->bdisp_dev->dev, "%s IRQ timeout\n", __func__);
 233		return -ETIMEDOUT;
 234	}
 235
 236	return 0;
 237}
 238
 239static void __bdisp_job_abort(struct bdisp_ctx *ctx)
 240{
 241	int ret;
 242
 243	ret = bdisp_ctx_stop_req(ctx);
 244	if ((ret == -ETIMEDOUT) || (ctx->state & BDISP_CTX_ABORT)) {
 245		bdisp_ctx_state_lock_clear(BDISP_CTX_STOP_REQ | BDISP_CTX_ABORT,
 246					   ctx);
 247		bdisp_job_finish(ctx, VB2_BUF_STATE_ERROR);
 248	}
 249}
 250
 251static void bdisp_job_abort(void *priv)
 252{
 253	__bdisp_job_abort((struct bdisp_ctx *)priv);
 254}
 255
 256static int bdisp_get_addr(struct bdisp_ctx *ctx, struct vb2_buffer *vb,
 257			  struct bdisp_frame *frame, dma_addr_t *paddr)
 258{
 259	if (!vb || !frame)
 260		return -EINVAL;
 261
 262	paddr[0] = vb2_dma_contig_plane_dma_addr(vb, 0);
 263
 264	if (frame->fmt->nb_planes > 1)
 265		/* UV (NV12) or U (420P) */
 266		paddr[1] = (dma_addr_t)(paddr[0] +
 267				frame->bytesperline * frame->height);
 268
 269	if (frame->fmt->nb_planes > 2)
 270		/* V (420P) */
 271		paddr[2] = (dma_addr_t)(paddr[1] +
 272				(frame->bytesperline * frame->height) / 4);
 273
 274	if (frame->fmt->nb_planes > 3)
 275		dev_dbg(ctx->bdisp_dev->dev, "ignoring some planes\n");
 276
 277	dev_dbg(ctx->bdisp_dev->dev,
 278		"%s plane[0]=%pad plane[1]=%pad plane[2]=%pad\n",
 279		__func__, &paddr[0], &paddr[1], &paddr[2]);
 280
 281	return 0;
 282}
 283
 284static int bdisp_get_bufs(struct bdisp_ctx *ctx)
 285{
 286	struct bdisp_frame *src, *dst;
 287	struct vb2_v4l2_buffer *src_vb, *dst_vb;
 288	int ret;
 289
 290	src = &ctx->src;
 291	dst = &ctx->dst;
 292
 293	src_vb = v4l2_m2m_next_src_buf(ctx->fh.m2m_ctx);
 294	ret = bdisp_get_addr(ctx, &src_vb->vb2_buf, src, src->paddr);
 295	if (ret)
 296		return ret;
 297
 298	dst_vb = v4l2_m2m_next_dst_buf(ctx->fh.m2m_ctx);
 299	ret = bdisp_get_addr(ctx, &dst_vb->vb2_buf, dst, dst->paddr);
 300	if (ret)
 301		return ret;
 302
 303	dst_vb->vb2_buf.timestamp = src_vb->vb2_buf.timestamp;
 304
 305	return 0;
 306}
 307
 308static void bdisp_device_run(void *priv)
 309{
 310	struct bdisp_ctx *ctx = priv;
 311	struct bdisp_dev *bdisp;
 312	unsigned long flags;
 313	int err = 0;
 314
 315	if (WARN(!ctx, "Null hardware context\n"))
 316		return;
 317
 318	bdisp = ctx->bdisp_dev;
 319	dev_dbg(bdisp->dev, "%s\n", __func__);
 320	spin_lock_irqsave(&bdisp->slock, flags);
 321
 322	if (bdisp->m2m.ctx != ctx) {
 323		dev_dbg(bdisp->dev, "ctx updated: %p -> %p\n",
 324			bdisp->m2m.ctx, ctx);
 325		ctx->state |= BDISP_PARAMS;
 326		bdisp->m2m.ctx = ctx;
 327	}
 328
 329	if (ctx->state & BDISP_CTX_STOP_REQ) {
 330		ctx->state &= ~BDISP_CTX_STOP_REQ;
 331		ctx->state |= BDISP_CTX_ABORT;
 332		wake_up(&bdisp->irq_queue);
 333		goto out;
 334	}
 335
 336	err = bdisp_get_bufs(ctx);
 337	if (err) {
 338		dev_err(bdisp->dev, "cannot get address\n");
 339		goto out;
 340	}
 341
 342	bdisp_dbg_perf_begin(bdisp);
 343
 344	err = bdisp_hw_reset(bdisp);
 345	if (err) {
 346		dev_err(bdisp->dev, "could not get HW ready\n");
 347		goto out;
 348	}
 349
 350	err = bdisp_hw_update(ctx);
 351	if (err) {
 352		dev_err(bdisp->dev, "could not send HW request\n");
 353		goto out;
 354	}
 355
 356	queue_delayed_work(bdisp->work_queue, &bdisp->timeout_work,
 357			   BDISP_WORK_TIMEOUT);
 358	set_bit(ST_M2M_RUNNING, &bdisp->state);
 359out:
 360	ctx->state &= ~BDISP_PARAMS;
 361	spin_unlock_irqrestore(&bdisp->slock, flags);
 362	if (err)
 363		bdisp_job_finish(ctx, VB2_BUF_STATE_ERROR);
 364}
 365
 366static const struct v4l2_m2m_ops bdisp_m2m_ops = {
 367	.device_run     = bdisp_device_run,
 368	.job_abort      = bdisp_job_abort,
 369};
 370
 371static int __bdisp_s_ctrl(struct bdisp_ctx *ctx, struct v4l2_ctrl *ctrl)
 372{
 373	if (ctrl->flags & V4L2_CTRL_FLAG_INACTIVE)
 374		return 0;
 375
 376	switch (ctrl->id) {
 377	case V4L2_CID_HFLIP:
 378		ctx->hflip = ctrl->val;
 379		break;
 380	case V4L2_CID_VFLIP:
 381		ctx->vflip = ctrl->val;
 382		break;
 383	default:
 384		dev_err(ctx->bdisp_dev->dev, "unknown control %d\n", ctrl->id);
 385		return -EINVAL;
 386	}
 387
 388	ctx->state |= BDISP_PARAMS;
 389
 390	return 0;
 391}
 392
 393static int bdisp_s_ctrl(struct v4l2_ctrl *ctrl)
 394{
 395	struct bdisp_ctx *ctx = container_of(ctrl->handler, struct bdisp_ctx,
 396						ctrl_handler);
 397	unsigned long flags;
 398	int ret;
 399
 400	spin_lock_irqsave(&ctx->bdisp_dev->slock, flags);
 401	ret = __bdisp_s_ctrl(ctx, ctrl);
 402	spin_unlock_irqrestore(&ctx->bdisp_dev->slock, flags);
 403
 404	return ret;
 405}
 406
 407static const struct v4l2_ctrl_ops bdisp_c_ops = {
 408	.s_ctrl = bdisp_s_ctrl,
 409};
 410
 411static int bdisp_ctrls_create(struct bdisp_ctx *ctx)
 412{
 413	if (ctx->ctrls_rdy)
 414		return 0;
 415
 416	v4l2_ctrl_handler_init(&ctx->ctrl_handler, BDISP_MAX_CTRL_NUM);
 417
 418	ctx->bdisp_ctrls.hflip = v4l2_ctrl_new_std(&ctx->ctrl_handler,
 419				&bdisp_c_ops, V4L2_CID_HFLIP, 0, 1, 1, 0);
 420	ctx->bdisp_ctrls.vflip = v4l2_ctrl_new_std(&ctx->ctrl_handler,
 421				&bdisp_c_ops, V4L2_CID_VFLIP, 0, 1, 1, 0);
 422
 423	if (ctx->ctrl_handler.error) {
 424		int err = ctx->ctrl_handler.error;
 425
 426		v4l2_ctrl_handler_free(&ctx->ctrl_handler);
 427		return err;
 428	}
 429
 430	ctx->ctrls_rdy = true;
 431
 432	return 0;
 433}
 434
 435static void bdisp_ctrls_delete(struct bdisp_ctx *ctx)
 436{
 437	if (ctx->ctrls_rdy) {
 438		v4l2_ctrl_handler_free(&ctx->ctrl_handler);
 439		ctx->ctrls_rdy = false;
 440	}
 441}
 442
 443static int bdisp_queue_setup(struct vb2_queue *vq,
 444			     unsigned int *nb_buf, unsigned int *nb_planes,
 445			     unsigned int sizes[], struct device *alloc_devs[])
 446{
 447	struct bdisp_ctx *ctx = vb2_get_drv_priv(vq);
 448	struct bdisp_frame *frame = ctx_get_frame(ctx, vq->type);
 449
 450	if (IS_ERR(frame)) {
 451		dev_err(ctx->bdisp_dev->dev, "Invalid frame (%p)\n", frame);
 452		return PTR_ERR(frame);
 453	}
 454
 455	if (!frame->fmt) {
 456		dev_err(ctx->bdisp_dev->dev, "Invalid format\n");
 457		return -EINVAL;
 458	}
 459
 460	if (*nb_planes)
 461		return sizes[0] < frame->sizeimage ? -EINVAL : 0;
 462
 463	*nb_planes = 1;
 464	sizes[0] = frame->sizeimage;
 465
 466	return 0;
 467}
 468
 469static int bdisp_buf_prepare(struct vb2_buffer *vb)
 470{
 471	struct bdisp_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
 472	struct bdisp_frame *frame = ctx_get_frame(ctx, vb->vb2_queue->type);
 473
 474	if (IS_ERR(frame)) {
 475		dev_err(ctx->bdisp_dev->dev, "Invalid frame (%p)\n", frame);
 476		return PTR_ERR(frame);
 477	}
 478
 479	if (vb->vb2_queue->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
 480		vb2_set_plane_payload(vb, 0, frame->sizeimage);
 481
 482	return 0;
 483}
 484
 485static void bdisp_buf_queue(struct vb2_buffer *vb)
 486{
 487	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
 488	struct bdisp_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
 489
 490	/* return to V4L2 any 0-size buffer so it can be dequeued by user */
 491	if (!vb2_get_plane_payload(vb, 0)) {
 492		dev_dbg(ctx->bdisp_dev->dev, "0 data buffer, skip it\n");
 493		vb2_buffer_done(vb, VB2_BUF_STATE_DONE);
 494		return;
 495	}
 496
 497	if (ctx->fh.m2m_ctx)
 498		v4l2_m2m_buf_queue(ctx->fh.m2m_ctx, vbuf);
 499}
 500
 501static int bdisp_start_streaming(struct vb2_queue *q, unsigned int count)
 502{
 503	struct bdisp_ctx *ctx = q->drv_priv;
 504	struct vb2_v4l2_buffer *buf;
 505	int ret = pm_runtime_resume_and_get(ctx->bdisp_dev->dev);
 506
 507	if (ret < 0) {
 508		dev_err(ctx->bdisp_dev->dev, "failed to set runtime PM\n");
 509
 510		if (q->type == V4L2_BUF_TYPE_VIDEO_OUTPUT) {
 511			while ((buf = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx)))
 512				v4l2_m2m_buf_done(buf, VB2_BUF_STATE_QUEUED);
 513		} else {
 514			while ((buf = v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx)))
 515				v4l2_m2m_buf_done(buf, VB2_BUF_STATE_QUEUED);
 516		}
 517
 518		return ret;
 519	}
 520
 521	return 0;
 522}
 523
 524static void bdisp_stop_streaming(struct vb2_queue *q)
 525{
 526	struct bdisp_ctx *ctx = q->drv_priv;
 527
 528	__bdisp_job_abort(ctx);
 529
 530	pm_runtime_put(ctx->bdisp_dev->dev);
 531}
 532
 533static const struct vb2_ops bdisp_qops = {
 534	.queue_setup     = bdisp_queue_setup,
 535	.buf_prepare     = bdisp_buf_prepare,
 536	.buf_queue       = bdisp_buf_queue,
 537	.stop_streaming  = bdisp_stop_streaming,
 538	.start_streaming = bdisp_start_streaming,
 539};
 540
 541static int queue_init(void *priv,
 542		      struct vb2_queue *src_vq, struct vb2_queue *dst_vq)
 543{
 544	struct bdisp_ctx *ctx = priv;
 545	int ret;
 546
 547	memset(src_vq, 0, sizeof(*src_vq));
 548	src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
 549	src_vq->io_modes = VB2_MMAP | VB2_DMABUF;
 550	src_vq->drv_priv = ctx;
 551	src_vq->ops = &bdisp_qops;
 552	src_vq->mem_ops = &vb2_dma_contig_memops;
 553	src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
 554	src_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
 555	src_vq->lock = &ctx->bdisp_dev->lock;
 556	src_vq->dev = ctx->bdisp_dev->v4l2_dev.dev;
 557
 558	ret = vb2_queue_init(src_vq);
 559	if (ret)
 560		return ret;
 561
 562	memset(dst_vq, 0, sizeof(*dst_vq));
 563	dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 564	dst_vq->io_modes = VB2_MMAP | VB2_DMABUF;
 565	dst_vq->drv_priv = ctx;
 566	dst_vq->ops = &bdisp_qops;
 567	dst_vq->mem_ops = &vb2_dma_contig_memops;
 568	dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
 569	dst_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
 570	dst_vq->lock = &ctx->bdisp_dev->lock;
 571	dst_vq->dev = ctx->bdisp_dev->v4l2_dev.dev;
 572
 573	return vb2_queue_init(dst_vq);
 574}
 575
 576static int bdisp_open(struct file *file)
 577{
 578	struct bdisp_dev *bdisp = video_drvdata(file);
 579	struct bdisp_ctx *ctx = NULL;
 580	int ret;
 581
 582	if (mutex_lock_interruptible(&bdisp->lock))
 583		return -ERESTARTSYS;
 584
 585	/* Allocate memory for both context and node */
 586	ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
 587	if (!ctx) {
 588		ret = -ENOMEM;
 589		goto unlock;
 590	}
 591	ctx->bdisp_dev = bdisp;
 592
 593	if (bdisp_hw_alloc_nodes(ctx)) {
 594		dev_err(bdisp->dev, "no memory for nodes\n");
 595		ret = -ENOMEM;
 596		goto mem_ctx;
 597	}
 598
 599	v4l2_fh_init(&ctx->fh, bdisp->m2m.vdev);
 600
 601	ret = bdisp_ctrls_create(ctx);
 602	if (ret) {
 603		dev_err(bdisp->dev, "Failed to create control\n");
 604		goto error_fh;
 605	}
 606
 607	/* Use separate control handler per file handle */
 608	ctx->fh.ctrl_handler = &ctx->ctrl_handler;
 609	v4l2_fh_add(&ctx->fh, file);
 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
 633error_ctrls:
 634	bdisp_ctrls_delete(ctx);
 635	v4l2_fh_del(&ctx->fh, file);
 636error_fh:
 637	v4l2_fh_exit(&ctx->fh);
 638	bdisp_hw_free_nodes(ctx);
 639mem_ctx:
 640	kfree(ctx);
 641unlock:
 642	mutex_unlock(&bdisp->lock);
 643
 644	return ret;
 645}
 646
 647static int bdisp_release(struct file *file)
 648{
 649	struct bdisp_ctx *ctx = file_to_ctx(file);
 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, file);
 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
 675static 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
 684static int bdisp_querycap(struct file *file, void *fh,
 685			  struct v4l2_capability *cap)
 686{
 687	struct bdisp_ctx *ctx = file_to_ctx(file);
 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
 697static int bdisp_enum_fmt(struct file *file, void *fh, struct v4l2_fmtdesc *f)
 698{
 699	struct bdisp_ctx *ctx = file_to_ctx(file);
 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
 717static int bdisp_g_fmt(struct file *file, void *fh, struct v4l2_format *f)
 718{
 719	struct bdisp_ctx *ctx = file_to_ctx(file);
 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
 741static int bdisp_try_fmt(struct file *file, void *fh, struct v4l2_format *f)
 742{
 743	struct bdisp_ctx *ctx = file_to_ctx(file);
 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
 791static int bdisp_s_fmt(struct file *file, void *fh, struct v4l2_format *f)
 792{
 793	struct bdisp_ctx *ctx = file_to_ctx(file);
 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
 843static int bdisp_g_selection(struct file *file, void *fh,
 844			     struct v4l2_selection *s)
 845{
 846	struct bdisp_ctx *ctx = file_to_ctx(file);
 847	struct bdisp_frame *frame;
 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
 905static 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
 921static int bdisp_s_selection(struct file *file, void *fh,
 922			     struct v4l2_selection *s)
 923{
 924	struct bdisp_ctx *ctx = file_to_ctx(file);
 925	struct bdisp_frame *frame;
 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: (%d,%d)/%ux%u vs frame: %dx%d\n",
 959			out.left, out.top, out.width, out.height,
 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: (%d,%d)/%ux%u vs frame: %dx%d\n",
 972			out.left, out.top, out.width, out.height,
 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: (%d,%d)/%ux%u -> (%d,%d)/%ux%u\n",
 988			__func__, in->left, in->top, in->width, in->height,
 989			out.left, out.top, out.width, out.height);
 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
1000static int bdisp_streamon(struct file *file, void *fh, enum v4l2_buf_type type)
1001{
1002	struct bdisp_ctx *ctx = file_to_ctx(file);
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
1019static 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
1043static 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
1080static 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
1091static 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
1126isr_unlock:
1127	spin_unlock(&bdisp->slock);
1128
1129	return IRQ_HANDLED;
1130}
1131
1132static 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
1140static 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
1160static int bdisp_m2m_suspend(struct bdisp_dev *bdisp)
1161{
1162	unsigned long flags;
1163	long time_left;
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	time_left = 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 (!time_left) {
1181		dev_err(bdisp->dev, "%s IRQ timeout\n", __func__);
1182		return -EAGAIN;
1183	}
1184
1185	return 0;
1186}
1187
1188static 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
1204static 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
1215static 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
1226static 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
1245static 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
1253static 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
1260static void 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
1282static int bdisp_probe(struct platform_device *pdev)
1283{
1284	struct bdisp_dev *bdisp;
1285	struct device *dev = &pdev->dev;
1286	int ret;
1287
1288	dev_dbg(dev, "%s\n", __func__);
1289
1290	bdisp = devm_kzalloc(dev, sizeof(struct bdisp_dev), GFP_KERNEL);
1291	if (!bdisp)
1292		return -ENOMEM;
1293
1294	ret = dma_coerce_mask_and_coherent(dev, DMA_BIT_MASK(32));
1295	if (ret)
1296		return ret;
1297
1298	bdisp->pdev = pdev;
1299	bdisp->dev = dev;
1300	platform_set_drvdata(pdev, bdisp);
1301
1302	if (dev->of_node)
1303		bdisp->id = of_alias_get_id(pdev->dev.of_node, BDISP_NAME);
1304	else
1305		bdisp->id = pdev->id;
1306
1307	init_waitqueue_head(&bdisp->irq_queue);
1308	INIT_DELAYED_WORK(&bdisp->timeout_work, bdisp_irq_timeout);
1309	bdisp->work_queue = create_workqueue(BDISP_NAME);
1310	if (!bdisp->work_queue)
1311		return -ENOMEM;
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
1388err_filter:
1389	bdisp_hw_free_filters(bdisp->dev);
1390err_pm:
1391	pm_runtime_put(dev);
1392err_remove:
1393	pm_runtime_disable(dev);
1394	bdisp_debugfs_remove(bdisp);
1395	v4l2_device_unregister(&bdisp->v4l2_dev);
1396err_clk:
1397	clk_unprepare(bdisp->clock);
1398err_wq:
1399	destroy_workqueue(bdisp->work_queue);
1400	return ret;
1401}
1402
1403static const struct of_device_id bdisp_match_types[] = {
1404	{
1405		.compatible = "st,stih407-bdisp",
1406	},
1407	{ /* end node */ }
1408};
1409
1410MODULE_DEVICE_TABLE(of, bdisp_match_types);
1411
1412static struct platform_driver bdisp_driver = {
1413	.probe          = bdisp_probe,
1414	.remove         = bdisp_remove,
1415	.driver         = {
1416		.name           = BDISP_NAME,
1417		.of_match_table = bdisp_match_types,
1418		.pm             = &bdisp_pm_ops,
1419	},
1420};
1421
1422module_platform_driver(bdisp_driver);
1423
1424MODULE_DESCRIPTION("2D blitter for STMicroelectronics SoC");
1425MODULE_AUTHOR("Fabien Dessenne <fabien.dessenne@st.com>");
1426MODULE_LICENSE("GPL");