linux/drivers/media/platform/sti/bdisp/bdisp-v4l2.c
<<
>>
Prefs
   1/*
   2 * Copyright (C) STMicroelectronics SA 2014
   3 * Authors: Fabien Dessenne <fabien.dessenne@st.com> for STMicroelectronics.
   4 * License terms:  GNU General Public License (GPL), version 2
   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
  38enum 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
  45static 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
 108static 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
 120static 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
 129static 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
 138static 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
 150static 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
 164static 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
 181static 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
 208static 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
 236static 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
 248static void bdisp_job_abort(void *priv)
 249{
 250        __bdisp_job_abort((struct bdisp_ctx *)priv);
 251}
 252
 253static 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
 281static 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
 305static 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);
 356out:
 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
 363static struct v4l2_m2m_ops bdisp_m2m_ops = {
 364        .device_run     = bdisp_device_run,
 365        .job_abort      = bdisp_job_abort,
 366};
 367
 368static 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
 390static 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
 404static const struct v4l2_ctrl_ops bdisp_c_ops = {
 405        .s_ctrl = bdisp_s_ctrl,
 406};
 407
 408static 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
 432static 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
 440static 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
 466static 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
 482static 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
 498static 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_get_sync(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
 521static 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
 530static 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
 540static 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
 575static 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
 633error_ctrls:
 634        bdisp_ctrls_delete(ctx);
 635error_fh:
 636        v4l2_fh_del(&ctx->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 = 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        if (mutex_lock_interruptible(&bdisp->lock))
 655                return -ERESTARTSYS;
 656
 657        v4l2_m2m_ctx_release(ctx->fh.m2m_ctx);
 658
 659        bdisp_ctrls_delete(ctx);
 660
 661        v4l2_fh_del(&ctx->fh);
 662        v4l2_fh_exit(&ctx->fh);
 663
 664        if (--bdisp->m2m.refcnt <= 0)
 665                clear_bit(ST_M2M_OPEN, &bdisp->state);
 666
 667        bdisp_hw_free_nodes(ctx);
 668
 669        kfree(ctx);
 670
 671        mutex_unlock(&bdisp->lock);
 672
 673        return 0;
 674}
 675
 676static const struct v4l2_file_operations bdisp_fops = {
 677        .owner          = THIS_MODULE,
 678        .open           = bdisp_open,
 679        .release        = bdisp_release,
 680        .poll           = v4l2_m2m_fop_poll,
 681        .unlocked_ioctl = video_ioctl2,
 682        .mmap           = v4l2_m2m_fop_mmap,
 683};
 684
 685static int bdisp_querycap(struct file *file, void *fh,
 686                          struct v4l2_capability *cap)
 687{
 688        struct bdisp_ctx *ctx = fh_to_ctx(fh);
 689        struct bdisp_dev *bdisp = ctx->bdisp_dev;
 690
 691        strlcpy(cap->driver, bdisp->pdev->name, sizeof(cap->driver));
 692        strlcpy(cap->card, bdisp->pdev->name, sizeof(cap->card));
 693        snprintf(cap->bus_info, sizeof(cap->bus_info), "platform:%s%d",
 694                 BDISP_NAME, bdisp->id);
 695
 696        cap->device_caps = V4L2_CAP_STREAMING | V4L2_CAP_VIDEO_M2M;
 697
 698        cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
 699
 700        return 0;
 701}
 702
 703static int bdisp_enum_fmt(struct file *file, void *fh, struct v4l2_fmtdesc *f)
 704{
 705        struct bdisp_ctx *ctx = fh_to_ctx(fh);
 706        const struct bdisp_fmt *fmt;
 707
 708        if (f->index >= ARRAY_SIZE(bdisp_formats))
 709                return -EINVAL;
 710
 711        fmt = &bdisp_formats[f->index];
 712
 713        if ((fmt->pixelformat == V4L2_PIX_FMT_YUV420) &&
 714            (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)) {
 715                dev_dbg(ctx->bdisp_dev->dev, "No YU12 on capture\n");
 716                return -EINVAL;
 717        }
 718        f->pixelformat = fmt->pixelformat;
 719
 720        return 0;
 721}
 722
 723static int bdisp_g_fmt(struct file *file, void *fh, struct v4l2_format *f)
 724{
 725        struct bdisp_ctx *ctx = fh_to_ctx(fh);
 726        struct v4l2_pix_format *pix = &f->fmt.pix;
 727        struct bdisp_frame *frame  = ctx_get_frame(ctx, f->type);
 728
 729        if (IS_ERR(frame)) {
 730                dev_err(ctx->bdisp_dev->dev, "Invalid frame (%p)\n", frame);
 731                return PTR_ERR(frame);
 732        }
 733
 734        pix = &f->fmt.pix;
 735        pix->width = frame->width;
 736        pix->height = frame->height;
 737        pix->pixelformat = frame->fmt->pixelformat;
 738        pix->field = frame->field;
 739        pix->bytesperline = frame->bytesperline;
 740        pix->sizeimage = frame->sizeimage;
 741        pix->colorspace = (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT) ?
 742                                frame->colorspace : bdisp_dflt_fmt.colorspace;
 743
 744        return 0;
 745}
 746
 747static int bdisp_try_fmt(struct file *file, void *fh, struct v4l2_format *f)
 748{
 749        struct bdisp_ctx *ctx = fh_to_ctx(fh);
 750        struct v4l2_pix_format *pix = &f->fmt.pix;
 751        const struct bdisp_fmt *format;
 752        u32 in_w, in_h;
 753
 754        format = bdisp_find_fmt(pix->pixelformat);
 755        if (!format) {
 756                dev_dbg(ctx->bdisp_dev->dev, "Unknown format 0x%x\n",
 757                        pix->pixelformat);
 758                return -EINVAL;
 759        }
 760
 761        /* YUV420P only supported for VIDEO_OUTPUT */
 762        if ((format->pixelformat == V4L2_PIX_FMT_YUV420) &&
 763            (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)) {
 764                dev_dbg(ctx->bdisp_dev->dev, "No YU12 on capture\n");
 765                return -EINVAL;
 766        }
 767
 768        /* Field (interlaced only supported on OUTPUT) */
 769        if ((f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE) ||
 770            (pix->field != V4L2_FIELD_INTERLACED))
 771                pix->field = V4L2_FIELD_NONE;
 772
 773        /* Adjust width & height */
 774        in_w = pix->width;
 775        in_h = pix->height;
 776        v4l_bound_align_image(&pix->width,
 777                              BDISP_MIN_W, BDISP_MAX_W,
 778                              ffs(format->w_align) - 1,
 779                              &pix->height,
 780                              BDISP_MIN_H, BDISP_MAX_H,
 781                              ffs(format->h_align) - 1,
 782                              0);
 783        if ((pix->width != in_w) || (pix->height != in_h))
 784                dev_dbg(ctx->bdisp_dev->dev,
 785                        "%s size updated: %dx%d -> %dx%d\n", __func__,
 786                        in_w, in_h, pix->width, pix->height);
 787
 788        pix->bytesperline = (pix->width * format->bpp_plane0) / 8;
 789        pix->sizeimage = (pix->width * pix->height * format->bpp) / 8;
 790
 791        if (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
 792                pix->colorspace = bdisp_dflt_fmt.colorspace;
 793
 794        return 0;
 795}
 796
 797static int bdisp_s_fmt(struct file *file, void *fh, struct v4l2_format *f)
 798{
 799        struct bdisp_ctx *ctx = fh_to_ctx(fh);
 800        struct vb2_queue *vq;
 801        struct bdisp_frame *frame;
 802        struct v4l2_pix_format *pix;
 803        int ret;
 804        u32 state;
 805
 806        ret = bdisp_try_fmt(file, fh, f);
 807        if (ret) {
 808                dev_err(ctx->bdisp_dev->dev, "Cannot set format\n");
 809                return ret;
 810        }
 811
 812        vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);
 813        if (vb2_is_streaming(vq)) {
 814                dev_err(ctx->bdisp_dev->dev, "queue (%d) busy\n", f->type);
 815                return -EBUSY;
 816        }
 817
 818        frame = (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT) ?
 819                        &ctx->src : &ctx->dst;
 820        pix = &f->fmt.pix;
 821        frame->fmt = bdisp_find_fmt(pix->pixelformat);
 822        if (!frame->fmt) {
 823                dev_err(ctx->bdisp_dev->dev, "Unknown format 0x%x\n",
 824                        pix->pixelformat);
 825                return -EINVAL;
 826        }
 827
 828        frame->width = pix->width;
 829        frame->height = pix->height;
 830        frame->bytesperline = pix->bytesperline;
 831        frame->sizeimage = pix->sizeimage;
 832        frame->field = pix->field;
 833        if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT)
 834                frame->colorspace = pix->colorspace;
 835
 836        frame->crop.width = frame->width;
 837        frame->crop.height = frame->height;
 838        frame->crop.left = 0;
 839        frame->crop.top = 0;
 840
 841        state = BDISP_PARAMS;
 842        state |= (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE) ?
 843                        BDISP_DST_FMT : BDISP_SRC_FMT;
 844        bdisp_ctx_state_lock_set(state, ctx);
 845
 846        return 0;
 847}
 848
 849static int bdisp_g_selection(struct file *file, void *fh,
 850                             struct v4l2_selection *s)
 851{
 852        struct bdisp_frame *frame;
 853        struct bdisp_ctx *ctx = fh_to_ctx(fh);
 854
 855        frame = ctx_get_frame(ctx, s->type);
 856        if (IS_ERR(frame)) {
 857                dev_err(ctx->bdisp_dev->dev, "Invalid frame (%p)\n", frame);
 858                return PTR_ERR(frame);
 859        }
 860
 861        switch (s->type) {
 862        case V4L2_BUF_TYPE_VIDEO_OUTPUT:
 863                switch (s->target) {
 864                case V4L2_SEL_TGT_CROP:
 865                        /* cropped frame */
 866                        s->r = frame->crop;
 867                        break;
 868                case V4L2_SEL_TGT_CROP_DEFAULT:
 869                case V4L2_SEL_TGT_CROP_BOUNDS:
 870                        /* complete frame */
 871                        s->r.left = 0;
 872                        s->r.top = 0;
 873                        s->r.width = frame->width;
 874                        s->r.height = frame->height;
 875                        break;
 876                default:
 877                        dev_err(ctx->bdisp_dev->dev, "Invalid target\n");
 878                        return -EINVAL;
 879                }
 880                break;
 881
 882        case V4L2_BUF_TYPE_VIDEO_CAPTURE:
 883                switch (s->target) {
 884                case V4L2_SEL_TGT_COMPOSE:
 885                case V4L2_SEL_TGT_COMPOSE_PADDED:
 886                        /* composed (cropped) frame */
 887                        s->r = frame->crop;
 888                        break;
 889                case V4L2_SEL_TGT_COMPOSE_DEFAULT:
 890                case V4L2_SEL_TGT_COMPOSE_BOUNDS:
 891                        /* complete frame */
 892                        s->r.left = 0;
 893                        s->r.top = 0;
 894                        s->r.width = frame->width;
 895                        s->r.height = frame->height;
 896                        break;
 897                default:
 898                        dev_err(ctx->bdisp_dev->dev, "Invalid target\n");
 899                        return -EINVAL;
 900                }
 901                break;
 902
 903        default:
 904                dev_err(ctx->bdisp_dev->dev, "Invalid type\n");
 905                return -EINVAL;
 906        }
 907
 908        return 0;
 909}
 910
 911static int is_rect_enclosed(struct v4l2_rect *a, struct v4l2_rect *b)
 912{
 913        /* Return 1 if a is enclosed in b, or 0 otherwise. */
 914
 915        if (a->left < b->left || a->top < b->top)
 916                return 0;
 917
 918        if (a->left + a->width > b->left + b->width)
 919                return 0;
 920
 921        if (a->top + a->height > b->top + b->height)
 922                return 0;
 923
 924        return 1;
 925}
 926
 927static int bdisp_s_selection(struct file *file, void *fh,
 928                             struct v4l2_selection *s)
 929{
 930        struct bdisp_frame *frame;
 931        struct bdisp_ctx *ctx = fh_to_ctx(fh);
 932        struct v4l2_rect *in, out;
 933        bool valid = false;
 934
 935        if ((s->type == V4L2_BUF_TYPE_VIDEO_OUTPUT) &&
 936            (s->target == V4L2_SEL_TGT_CROP))
 937                valid = true;
 938
 939        if ((s->type == V4L2_BUF_TYPE_VIDEO_CAPTURE) &&
 940            (s->target == V4L2_SEL_TGT_COMPOSE))
 941                valid = true;
 942
 943        if (!valid) {
 944                dev_err(ctx->bdisp_dev->dev, "Invalid type / target\n");
 945                return -EINVAL;
 946        }
 947
 948        frame = ctx_get_frame(ctx, s->type);
 949        if (IS_ERR(frame)) {
 950                dev_err(ctx->bdisp_dev->dev, "Invalid frame (%p)\n", frame);
 951                return PTR_ERR(frame);
 952        }
 953
 954        in = &s->r;
 955        out = *in;
 956
 957        /* Align and check origin */
 958        out.left = ALIGN(in->left, frame->fmt->w_align);
 959        out.top = ALIGN(in->top, frame->fmt->h_align);
 960
 961        if ((out.left < 0) || (out.left >= frame->width) ||
 962            (out.top < 0) || (out.top >= frame->height)) {
 963                dev_err(ctx->bdisp_dev->dev,
 964                        "Invalid crop: %dx%d@(%d,%d) vs frame: %dx%d\n",
 965                        out.width, out.height, out.left, out.top,
 966                        frame->width, frame->height);
 967                return -EINVAL;
 968        }
 969
 970        /* Align and check size */
 971        out.width = ALIGN(in->width, frame->fmt->w_align);
 972        out.height = ALIGN(in->height, frame->fmt->w_align);
 973
 974        if (((out.left + out.width) > frame->width) ||
 975            ((out.top + out.height) > frame->height)) {
 976                dev_err(ctx->bdisp_dev->dev,
 977                        "Invalid crop: %dx%d@(%d,%d) vs frame: %dx%d\n",
 978                        out.width, out.height, out.left, out.top,
 979                        frame->width, frame->height);
 980                return -EINVAL;
 981        }
 982
 983        /* Checks adjust constraints flags */
 984        if (s->flags & V4L2_SEL_FLAG_LE && !is_rect_enclosed(&out, in))
 985                return -ERANGE;
 986
 987        if (s->flags & V4L2_SEL_FLAG_GE && !is_rect_enclosed(in, &out))
 988                return -ERANGE;
 989
 990        if ((out.left != in->left) || (out.top != in->top) ||
 991            (out.width != in->width) || (out.height != in->height)) {
 992                dev_dbg(ctx->bdisp_dev->dev,
 993                        "%s crop updated: %dx%d@(%d,%d) -> %dx%d@(%d,%d)\n",
 994                        __func__, in->width, in->height, in->left, in->top,
 995                        out.width, out.height, out.left, out.top);
 996                *in = out;
 997        }
 998
 999        frame->crop = out;
1000
1001        bdisp_ctx_state_lock_set(BDISP_PARAMS, ctx);
1002
1003        return 0;
1004}
1005
1006static int bdisp_streamon(struct file *file, void *fh, enum v4l2_buf_type type)
1007{
1008        struct bdisp_ctx *ctx = fh_to_ctx(fh);
1009
1010        if ((type == V4L2_BUF_TYPE_VIDEO_OUTPUT) &&
1011            !bdisp_ctx_state_is_set(BDISP_SRC_FMT, ctx)) {
1012                dev_err(ctx->bdisp_dev->dev, "src not defined\n");
1013                return -EINVAL;
1014        }
1015
1016        if ((type == V4L2_BUF_TYPE_VIDEO_CAPTURE) &&
1017            !bdisp_ctx_state_is_set(BDISP_DST_FMT, ctx)) {
1018                dev_err(ctx->bdisp_dev->dev, "dst not defined\n");
1019                return -EINVAL;
1020        }
1021
1022        return v4l2_m2m_streamon(file, ctx->fh.m2m_ctx, type);
1023}
1024
1025static const struct v4l2_ioctl_ops bdisp_ioctl_ops = {
1026        .vidioc_querycap                = bdisp_querycap,
1027        .vidioc_enum_fmt_vid_cap        = bdisp_enum_fmt,
1028        .vidioc_enum_fmt_vid_out        = bdisp_enum_fmt,
1029        .vidioc_g_fmt_vid_cap           = bdisp_g_fmt,
1030        .vidioc_g_fmt_vid_out           = bdisp_g_fmt,
1031        .vidioc_try_fmt_vid_cap         = bdisp_try_fmt,
1032        .vidioc_try_fmt_vid_out         = bdisp_try_fmt,
1033        .vidioc_s_fmt_vid_cap           = bdisp_s_fmt,
1034        .vidioc_s_fmt_vid_out           = bdisp_s_fmt,
1035        .vidioc_g_selection             = bdisp_g_selection,
1036        .vidioc_s_selection             = bdisp_s_selection,
1037        .vidioc_reqbufs                 = v4l2_m2m_ioctl_reqbufs,
1038        .vidioc_create_bufs             = v4l2_m2m_ioctl_create_bufs,
1039        .vidioc_expbuf                  = v4l2_m2m_ioctl_expbuf,
1040        .vidioc_querybuf                = v4l2_m2m_ioctl_querybuf,
1041        .vidioc_qbuf                    = v4l2_m2m_ioctl_qbuf,
1042        .vidioc_dqbuf                   = v4l2_m2m_ioctl_dqbuf,
1043        .vidioc_streamon                = bdisp_streamon,
1044        .vidioc_streamoff               = v4l2_m2m_ioctl_streamoff,
1045        .vidioc_subscribe_event         = v4l2_ctrl_subscribe_event,
1046        .vidioc_unsubscribe_event       = v4l2_event_unsubscribe,
1047};
1048
1049static int bdisp_register_device(struct bdisp_dev *bdisp)
1050{
1051        int ret;
1052
1053        if (!bdisp)
1054                return -ENODEV;
1055
1056        bdisp->vdev.fops        = &bdisp_fops;
1057        bdisp->vdev.ioctl_ops   = &bdisp_ioctl_ops;
1058        bdisp->vdev.release     = video_device_release_empty;
1059        bdisp->vdev.lock        = &bdisp->lock;
1060        bdisp->vdev.vfl_dir     = VFL_DIR_M2M;
1061        bdisp->vdev.v4l2_dev    = &bdisp->v4l2_dev;
1062        snprintf(bdisp->vdev.name, sizeof(bdisp->vdev.name), "%s.%d",
1063                 BDISP_NAME, bdisp->id);
1064
1065        video_set_drvdata(&bdisp->vdev, bdisp);
1066
1067        bdisp->m2m.vdev = &bdisp->vdev;
1068        bdisp->m2m.m2m_dev = v4l2_m2m_init(&bdisp_m2m_ops);
1069        if (IS_ERR(bdisp->m2m.m2m_dev)) {
1070                dev_err(bdisp->dev, "failed to initialize v4l2-m2m device\n");
1071                return PTR_ERR(bdisp->m2m.m2m_dev);
1072        }
1073
1074        ret = video_register_device(&bdisp->vdev, VFL_TYPE_GRABBER, -1);
1075        if (ret) {
1076                dev_err(bdisp->dev,
1077                        "%s(): failed to register video device\n", __func__);
1078                v4l2_m2m_release(bdisp->m2m.m2m_dev);
1079                return ret;
1080        }
1081
1082        return 0;
1083}
1084
1085static void bdisp_unregister_device(struct bdisp_dev *bdisp)
1086{
1087        if (!bdisp)
1088                return;
1089
1090        if (bdisp->m2m.m2m_dev)
1091                v4l2_m2m_release(bdisp->m2m.m2m_dev);
1092
1093        video_unregister_device(bdisp->m2m.vdev);
1094}
1095
1096static irqreturn_t bdisp_irq_thread(int irq, void *priv)
1097{
1098        struct bdisp_dev *bdisp = priv;
1099        struct bdisp_ctx *ctx;
1100
1101        spin_lock(&bdisp->slock);
1102
1103        bdisp_dbg_perf_end(bdisp);
1104
1105        cancel_delayed_work(&bdisp->timeout_work);
1106
1107        if (!test_and_clear_bit(ST_M2M_RUNNING, &bdisp->state))
1108                goto isr_unlock;
1109
1110        if (test_and_clear_bit(ST_M2M_SUSPENDING, &bdisp->state)) {
1111                set_bit(ST_M2M_SUSPENDED, &bdisp->state);
1112                wake_up(&bdisp->irq_queue);
1113                goto isr_unlock;
1114        }
1115
1116        ctx = v4l2_m2m_get_curr_priv(bdisp->m2m.m2m_dev);
1117        if (!ctx || !ctx->fh.m2m_ctx)
1118                goto isr_unlock;
1119
1120        spin_unlock(&bdisp->slock);
1121
1122        bdisp_job_finish(ctx, VB2_BUF_STATE_DONE);
1123
1124        if (bdisp_ctx_state_is_set(BDISP_CTX_STOP_REQ, ctx)) {
1125                bdisp_ctx_state_lock_clear(BDISP_CTX_STOP_REQ, ctx);
1126                wake_up(&bdisp->irq_queue);
1127        }
1128
1129        return IRQ_HANDLED;
1130
1131isr_unlock:
1132        spin_unlock(&bdisp->slock);
1133
1134        return IRQ_HANDLED;
1135}
1136
1137static irqreturn_t bdisp_irq_handler(int irq, void *priv)
1138{
1139        if (bdisp_hw_get_and_clear_irq((struct bdisp_dev *)priv))
1140                return IRQ_NONE;
1141        else
1142                return IRQ_WAKE_THREAD;
1143}
1144
1145static void bdisp_irq_timeout(struct work_struct *ptr)
1146{
1147        struct delayed_work *twork = to_delayed_work(ptr);
1148        struct bdisp_dev *bdisp = container_of(twork, struct bdisp_dev,
1149                        timeout_work);
1150        struct bdisp_ctx *ctx;
1151
1152        ctx = v4l2_m2m_get_curr_priv(bdisp->m2m.m2m_dev);
1153
1154        dev_err(ctx->bdisp_dev->dev, "Device work timeout\n");
1155
1156        spin_lock(&bdisp->slock);
1157        clear_bit(ST_M2M_RUNNING, &bdisp->state);
1158        spin_unlock(&bdisp->slock);
1159
1160        bdisp_hw_reset(bdisp);
1161
1162        bdisp_job_finish(ctx, VB2_BUF_STATE_ERROR);
1163}
1164
1165static int bdisp_m2m_suspend(struct bdisp_dev *bdisp)
1166{
1167        unsigned long flags;
1168        int timeout;
1169
1170        spin_lock_irqsave(&bdisp->slock, flags);
1171        if (!test_bit(ST_M2M_RUNNING, &bdisp->state)) {
1172                spin_unlock_irqrestore(&bdisp->slock, flags);
1173                return 0;
1174        }
1175        clear_bit(ST_M2M_SUSPENDED, &bdisp->state);
1176        set_bit(ST_M2M_SUSPENDING, &bdisp->state);
1177        spin_unlock_irqrestore(&bdisp->slock, flags);
1178
1179        timeout = wait_event_timeout(bdisp->irq_queue,
1180                                     test_bit(ST_M2M_SUSPENDED, &bdisp->state),
1181                                     BDISP_WORK_TIMEOUT);
1182
1183        clear_bit(ST_M2M_SUSPENDING, &bdisp->state);
1184
1185        if (!timeout) {
1186                dev_err(bdisp->dev, "%s IRQ timeout\n", __func__);
1187                return -EAGAIN;
1188        }
1189
1190        return 0;
1191}
1192
1193static int bdisp_m2m_resume(struct bdisp_dev *bdisp)
1194{
1195        struct bdisp_ctx *ctx;
1196        unsigned long flags;
1197
1198        spin_lock_irqsave(&bdisp->slock, flags);
1199        ctx = bdisp->m2m.ctx;
1200        bdisp->m2m.ctx = NULL;
1201        spin_unlock_irqrestore(&bdisp->slock, flags);
1202
1203        if (test_and_clear_bit(ST_M2M_SUSPENDED, &bdisp->state))
1204                bdisp_job_finish(ctx, VB2_BUF_STATE_ERROR);
1205
1206        return 0;
1207}
1208
1209static int bdisp_runtime_resume(struct device *dev)
1210{
1211        struct bdisp_dev *bdisp = dev_get_drvdata(dev);
1212        int ret = clk_enable(bdisp->clock);
1213
1214        if (ret)
1215                return ret;
1216
1217        return bdisp_m2m_resume(bdisp);
1218}
1219
1220static int bdisp_runtime_suspend(struct device *dev)
1221{
1222        struct bdisp_dev *bdisp = dev_get_drvdata(dev);
1223        int ret = bdisp_m2m_suspend(bdisp);
1224
1225        if (!ret)
1226                clk_disable(bdisp->clock);
1227
1228        return ret;
1229}
1230
1231static int bdisp_resume(struct device *dev)
1232{
1233        struct bdisp_dev *bdisp = dev_get_drvdata(dev);
1234        unsigned long flags;
1235        int opened;
1236
1237        spin_lock_irqsave(&bdisp->slock, flags);
1238        opened = test_bit(ST_M2M_OPEN, &bdisp->state);
1239        spin_unlock_irqrestore(&bdisp->slock, flags);
1240
1241        if (!opened)
1242                return 0;
1243
1244        if (!pm_runtime_suspended(dev))
1245                return bdisp_runtime_resume(dev);
1246
1247        return 0;
1248}
1249
1250static int bdisp_suspend(struct device *dev)
1251{
1252        if (!pm_runtime_suspended(dev))
1253                return bdisp_runtime_suspend(dev);
1254
1255        return 0;
1256}
1257
1258static const struct dev_pm_ops bdisp_pm_ops = {
1259        .suspend                = bdisp_suspend,
1260        .resume                 = bdisp_resume,
1261        .runtime_suspend        = bdisp_runtime_suspend,
1262        .runtime_resume         = bdisp_runtime_resume,
1263};
1264
1265static int bdisp_remove(struct platform_device *pdev)
1266{
1267        struct bdisp_dev *bdisp = platform_get_drvdata(pdev);
1268
1269        bdisp_unregister_device(bdisp);
1270
1271        bdisp_hw_free_filters(bdisp->dev);
1272
1273        pm_runtime_disable(&pdev->dev);
1274
1275        bdisp_debugfs_remove(bdisp);
1276
1277        v4l2_device_unregister(&bdisp->v4l2_dev);
1278
1279        if (!IS_ERR(bdisp->clock))
1280                clk_unprepare(bdisp->clock);
1281
1282        dev_dbg(&pdev->dev, "%s driver unloaded\n", pdev->name);
1283
1284        return 0;
1285}
1286
1287static int bdisp_probe(struct platform_device *pdev)
1288{
1289        struct bdisp_dev *bdisp;
1290        struct resource *res;
1291        struct device *dev = &pdev->dev;
1292        int ret;
1293
1294        dev_dbg(dev, "%s\n", __func__);
1295
1296        bdisp = devm_kzalloc(dev, sizeof(struct bdisp_dev), GFP_KERNEL);
1297        if (!bdisp)
1298                return -ENOMEM;
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        res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
1318        bdisp->regs = devm_ioremap_resource(dev, res);
1319        if (IS_ERR(bdisp->regs)) {
1320                dev_err(dev, "failed to get regs\n");
1321                return PTR_ERR(bdisp->regs);
1322        }
1323
1324        bdisp->clock = devm_clk_get(dev, BDISP_NAME);
1325        if (IS_ERR(bdisp->clock)) {
1326                dev_err(dev, "failed to get clock\n");
1327                return PTR_ERR(bdisp->clock);
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                return ret;
1335        }
1336
1337        res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
1338        if (!res) {
1339                dev_err(dev, "failed to get IRQ resource\n");
1340                goto err_clk;
1341        }
1342
1343        ret = devm_request_threaded_irq(dev, res->start, bdisp_irq_handler,
1344                                        bdisp_irq_thread, IRQF_ONESHOT,
1345                                        pdev->name, bdisp);
1346        if (ret) {
1347                dev_err(dev, "failed to install irq\n");
1348                goto err_clk;
1349        }
1350
1351        /* v4l2 register */
1352        ret = v4l2_device_register(dev, &bdisp->v4l2_dev);
1353        if (ret) {
1354                dev_err(dev, "failed to register\n");
1355                goto err_clk;
1356        }
1357
1358        /* Debug */
1359        ret = bdisp_debugfs_create(bdisp);
1360        if (ret) {
1361                dev_err(dev, "failed to create debugfs\n");
1362                goto err_v4l2;
1363        }
1364
1365        /* Power management */
1366        pm_runtime_enable(dev);
1367        ret = pm_runtime_get_sync(dev);
1368        if (ret < 0) {
1369                dev_err(dev, "failed to set PM\n");
1370                goto err_dbg;
1371        }
1372
1373        /* Filters */
1374        if (bdisp_hw_alloc_filters(bdisp->dev)) {
1375                dev_err(bdisp->dev, "no memory for filters\n");
1376                ret = -ENOMEM;
1377                goto err_pm;
1378        }
1379
1380        /* Register */
1381        ret = bdisp_register_device(bdisp);
1382        if (ret) {
1383                dev_err(dev, "failed to register\n");
1384                goto err_filter;
1385        }
1386
1387        dev_info(dev, "%s%d registered as /dev/video%d\n", BDISP_NAME,
1388                 bdisp->id, bdisp->vdev.num);
1389
1390        pm_runtime_put(dev);
1391
1392        return 0;
1393
1394err_filter:
1395        bdisp_hw_free_filters(bdisp->dev);
1396err_pm:
1397        pm_runtime_put(dev);
1398err_dbg:
1399        bdisp_debugfs_remove(bdisp);
1400err_v4l2:
1401        v4l2_device_unregister(&bdisp->v4l2_dev);
1402err_clk:
1403        if (!IS_ERR(bdisp->clock))
1404                clk_unprepare(bdisp->clock);
1405
1406        return ret;
1407}
1408
1409static const struct of_device_id bdisp_match_types[] = {
1410        {
1411                .compatible = "st,stih407-bdisp",
1412        },
1413        { /* end node */ }
1414};
1415
1416MODULE_DEVICE_TABLE(of, bdisp_match_types);
1417
1418static struct platform_driver bdisp_driver = {
1419        .probe          = bdisp_probe,
1420        .remove         = bdisp_remove,
1421        .driver         = {
1422                .name           = BDISP_NAME,
1423                .of_match_table = bdisp_match_types,
1424                .pm             = &bdisp_pm_ops,
1425        },
1426};
1427
1428module_platform_driver(bdisp_driver);
1429
1430MODULE_DESCRIPTION("2D blitter for STMicroelectronics SoC");
1431MODULE_AUTHOR("Fabien Dessenne <fabien.dessenne@st.com>");
1432MODULE_LICENSE("GPL");
1433