drivers/staging/media/hantro/hantro_drv.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /*
   3  * Hantro VPU codec driver
   4  *
   5  * Copyright (C) 2018 Collabora, Ltd.
   6  * Copyright 2018 Google LLC.
   7  *      Tomasz Figa <tfiga@chromium.org>
   8  *
   9  * Based on s5p-mfc driver by Samsung Electronics Co., Ltd.
  10  * Copyright (C) 2011 Samsung Electronics Co., Ltd.
  11  */
  12
  13 #include <linux/clk.h>
  14 #include <linux/module.h>
  15 #include <linux/of.h>
  16 #include <linux/platform_device.h>
  17 #include <linux/pm.h>
  18 #include <linux/pm_runtime.h>
  19 #include <linux/slab.h>
  20 #include <linux/videodev2.h>
  21 #include <linux/workqueue.h>
  22 #include <media/v4l2-event.h>
  23 #include <media/v4l2-mem2mem.h>
  24 #include <media/videobuf2-core.h>
  25 #include <media/videobuf2-vmalloc.h>
  26
  27 #include "hantro_v4l2.h"
  28 #include "hantro.h"
  29 #include "hantro_hw.h"
  30
  31 #define DRIVER_NAME "hantro-vpu"
  32
  33 int hantro_debug;
  34 module_param_named(debug, hantro_debug, int, 0644);
  35 MODULE_PARM_DESC(debug,
  36                  "Debug level - higher value produces more verbose messages");
  37
  38 void *hantro_get_ctrl(struct hantro_ctx *ctx, u32 id)
  39 {
  40         struct v4l2_ctrl *ctrl;
  41
  42         ctrl = v4l2_ctrl_find(&ctx->ctrl_handler, id);
  43         return ctrl ? ctrl->p_cur.p : NULL;
  44 }
  45
  46 dma_addr_t hantro_get_ref(struct vb2_queue *q, u64 ts)
  47 {
  48         struct vb2_buffer *buf;
  49         int index;
  50
  51         index = vb2_find_timestamp(q, ts, 0);
  52         if (index < 0)
  53                 return 0;
  54         buf = vb2_get_buffer(q, index);
  55         return vb2_dma_contig_plane_dma_addr(buf, 0);
  56 }
  57
  58 static int
  59 hantro_enc_buf_finish(struct hantro_ctx *ctx, struct vb2_buffer *buf,
  60                       unsigned int bytesused)
  61 {
  62         size_t avail_size;
  63
  64         avail_size = vb2_plane_size(buf, 0) - ctx->vpu_dst_fmt->header_size;
  65         if (bytesused > avail_size)
  66                 return -EINVAL;
  67         /*
  68          * The bounce buffer is only for the JPEG encoder.
  69          * TODO: Rework the JPEG encoder to eliminate the need
  70          * for a bounce buffer.
  71          */
  72         if (ctx->jpeg_enc.bounce_buffer.cpu) {
  73                 memcpy(vb2_plane_vaddr(buf, 0) +
  74                        ctx->vpu_dst_fmt->header_size,
  75                        ctx->jpeg_enc.bounce_buffer.cpu, bytesused);
  76         }
  77         buf->planes[0].bytesused =
  78                 ctx->vpu_dst_fmt->header_size + bytesused;
  79         return 0;
  80 }
  81
  82 static int
  83 hantro_dec_buf_finish(struct hantro_ctx *ctx, struct vb2_buffer *buf,
  84                       unsigned int bytesused)
  85 {
  86         /* For decoders set bytesused as per the output picture. */
  87         buf->planes[0].bytesused = ctx->dst_fmt.plane_fmt[0].sizeimage;
  88         return 0;
  89 }
  90
  91 static void hantro_job_finish(struct hantro_dev *vpu,
  92                               struct hantro_ctx *ctx,
  93                               unsigned int bytesused,
  94                               enum vb2_buffer_state result)
  95 {
  96         struct vb2_v4l2_buffer *src, *dst;
  97         int ret;
  98
  99         pm_runtime_mark_last_busy(vpu->dev);
 100         pm_runtime_put_autosuspend(vpu->dev);
 101         clk_bulk_disable(vpu->variant->num_clocks, vpu->clocks);
 102
 103         src = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
 104         dst = v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
 105
 106         if (WARN_ON(!src))
 107                 return;
 108         if (WARN_ON(!dst))
 109                 return;
 110
 111         src->sequence = ctx->sequence_out++;
 112         dst->sequence = ctx->sequence_cap++;
 113
 114         ret = ctx->buf_finish(ctx, &dst->vb2_buf, bytesused);
 115         if (ret)
 116                 result = VB2_BUF_STATE_ERROR;
 117
 118         v4l2_m2m_buf_done(src, result);
 119         v4l2_m2m_buf_done(dst, result);
 120
 121         v4l2_m2m_job_finish(vpu->m2m_dev, ctx->fh.m2m_ctx);
 122 }
 123
 124 void hantro_irq_done(struct hantro_dev *vpu, unsigned int bytesused,
 125                      enum vb2_buffer_state result)
 126 {
 127         struct hantro_ctx *ctx =
 128                 v4l2_m2m_get_curr_priv(vpu->m2m_dev);
 129
 130         /*
 131          * If cancel_delayed_work returns false
 132          * the timeout expired. The watchdog is running,
 133          * and will take care of finishing the job.
 134          */
 135         if (cancel_delayed_work(&vpu->watchdog_work))
 136                 hantro_job_finish(vpu, ctx, bytesused, result);
 137 }
 138
 139 void hantro_watchdog(struct work_struct *work)
 140 {
 141         struct hantro_dev *vpu;
 142         struct hantro_ctx *ctx;
 143
 144         vpu = container_of(to_delayed_work(work),
 145                            struct hantro_dev, watchdog_work);
 146         ctx = v4l2_m2m_get_curr_priv(vpu->m2m_dev);
 147         if (ctx) {
 148                 vpu_err("frame processing timed out!\n");
 149                 ctx->codec_ops->reset(ctx);
 150                 hantro_job_finish(vpu, ctx, 0, VB2_BUF_STATE_ERROR);
 151         }
 152 }
 153
 154 void hantro_prepare_run(struct hantro_ctx *ctx)
 155 {
 156         struct vb2_v4l2_buffer *src_buf;
 157
 158         src_buf = hantro_get_src_buf(ctx);
 159         v4l2_ctrl_request_setup(src_buf->vb2_buf.req_obj.req,
 160                                 &ctx->ctrl_handler);
 161 }
 162
 163 void hantro_finish_run(struct hantro_ctx *ctx)
 164 {
 165         struct vb2_v4l2_buffer *src_buf;
 166
 167         src_buf = hantro_get_src_buf(ctx);
 168         v4l2_ctrl_request_complete(src_buf->vb2_buf.req_obj.req,
 169                                    &ctx->ctrl_handler);
 170
 171         /* Kick the watchdog. */
 172         schedule_delayed_work(&ctx->dev->watchdog_work,
 173                               msecs_to_jiffies(2000));
 174 }
 175
 176 static void device_run(void *priv)
 177 {
 178         struct hantro_ctx *ctx = priv;
 179         struct vb2_v4l2_buffer *src, *dst;
 180         int ret;
 181
 182         src = hantro_get_src_buf(ctx);
 183         dst = hantro_get_dst_buf(ctx);
 184
 185         ret = clk_bulk_enable(ctx->dev->variant->num_clocks, ctx->dev->clocks);
 186         if (ret)
 187                 goto err_cancel_job;
 188         ret = pm_runtime_get_sync(ctx->dev->dev);
 189         if (ret < 0)
 190                 goto err_cancel_job;
 191
 192         v4l2_m2m_buf_copy_metadata(src, dst, true);
 193
 194         ctx->codec_ops->run(ctx);
 195         return;
 196
 197 err_cancel_job:
 198         hantro_job_finish(ctx->dev, ctx, 0, VB2_BUF_STATE_ERROR);
 199 }
 200
 201 bool hantro_is_encoder_ctx(const struct hantro_ctx *ctx)
 202 {
 203         return ctx->buf_finish == hantro_enc_buf_finish;
 204 }
 205
 206 static struct v4l2_m2m_ops vpu_m2m_ops = {
 207         .device_run = device_run,
 208 };
 209
 210 static int
 211 queue_init(void *priv, struct vb2_queue *src_vq, struct vb2_queue *dst_vq)
 212 {
 213         struct hantro_ctx *ctx = priv;
 214         int ret;
 215
 216         src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
 217         src_vq->io_modes = VB2_MMAP | VB2_DMABUF;
 218         src_vq->drv_priv = ctx;
 219         src_vq->ops = &hantro_queue_ops;
 220         src_vq->mem_ops = &vb2_dma_contig_memops;
 221
 222         /*
 223          * Driver does mostly sequential access, so sacrifice TLB efficiency
 224          * for faster allocation. Also, no CPU access on the source queue,
 225          * so no kernel mapping needed.
 226          */
 227         src_vq->dma_attrs = DMA_ATTR_ALLOC_SINGLE_PAGES |
 228                             DMA_ATTR_NO_KERNEL_MAPPING;
 229         src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
 230         src_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
 231         src_vq->lock = &ctx->dev->vpu_mutex;
 232         src_vq->dev = ctx->dev->v4l2_dev.dev;
 233         src_vq->supports_requests = true;
 234
 235         ret = vb2_queue_init(src_vq);
 236         if (ret)
 237                 return ret;
 238
 239         /*
 240          * When encoding, the CAPTURE queue doesn't need dma memory,
 241          * as the CPU needs to create the JPEG frames, from the
 242          * hardware-produced JPEG payload.
 243          *
 244          * For the DMA destination buffer, we use a bounce buffer.
 245          */
 246         if (hantro_is_encoder_ctx(ctx)) {
 247                 dst_vq->mem_ops = &vb2_vmalloc_memops;
 248         } else {
 249                 dst_vq->bidirectional = true;
 250                 dst_vq->mem_ops = &vb2_dma_contig_memops;
 251                 dst_vq->dma_attrs = DMA_ATTR_ALLOC_SINGLE_PAGES |
 252                                     DMA_ATTR_NO_KERNEL_MAPPING;
 253         }
 254
 255         dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
 256         dst_vq->io_modes = VB2_MMAP | VB2_DMABUF;
 257         dst_vq->drv_priv = ctx;
 258         dst_vq->ops = &hantro_queue_ops;
 259         dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
 260         dst_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
 261         dst_vq->lock = &ctx->dev->vpu_mutex;
 262         dst_vq->dev = ctx->dev->v4l2_dev.dev;
 263
 264         return vb2_queue_init(dst_vq);
 265 }
 266
 267 static int hantro_s_ctrl(struct v4l2_ctrl *ctrl)
 268 {
 269         struct hantro_ctx *ctx;
 270
 271         ctx = container_of(ctrl->handler,
 272                            struct hantro_ctx, ctrl_handler);
 273
 274         vpu_debug(1, "s_ctrl: id = %d, val = %d\n", ctrl->id, ctrl->val);
 275
 276         switch (ctrl->id) {
 277         case V4L2_CID_JPEG_COMPRESSION_QUALITY:
 278                 ctx->jpeg_quality = ctrl->val;
 279                 break;
 280         default:
 281                 return -EINVAL;
 282         }
 283
 284         return 0;
 285 }
 286
 287 static const struct v4l2_ctrl_ops hantro_ctrl_ops = {
 288         .s_ctrl = hantro_s_ctrl,
 289 };
 290
 291 static const struct hantro_ctrl controls[] = {
 292         {
 293                 .codec = HANTRO_JPEG_ENCODER,
 294                 .cfg = {
 295                         .id = V4L2_CID_JPEG_COMPRESSION_QUALITY,
 296                         .min = 5,
 297                         .max = 100,
 298                         .step = 1,
 299                         .def = 50,
 300                         .ops = &hantro_ctrl_ops,
 301                 },
 302         }, {
 303                 .codec = HANTRO_MPEG2_DECODER,
 304                 .cfg = {
 305                         .id = V4L2_CID_MPEG_VIDEO_MPEG2_SLICE_PARAMS,
 306                 },
 307         }, {
 308                 .codec = HANTRO_MPEG2_DECODER,
 309                 .cfg = {
 310                         .id = V4L2_CID_MPEG_VIDEO_MPEG2_QUANTIZATION,
 311                 },
 312         }, {
 313                 .codec = HANTRO_VP8_DECODER,
 314                 .cfg = {
 315                         .id = V4L2_CID_MPEG_VIDEO_VP8_FRAME_HEADER,
 316                 },
 317         }, {
 318                 .codec = HANTRO_H264_DECODER,
 319                 .cfg = {
 320                         .id = V4L2_CID_MPEG_VIDEO_H264_DECODE_PARAMS,
 321                 },
 322         }, {
 323                 .codec = HANTRO_H264_DECODER,
 324                 .cfg = {
 325                         .id = V4L2_CID_MPEG_VIDEO_H264_SLICE_PARAMS,
 326                 },
 327         }, {
 328                 .codec = HANTRO_H264_DECODER,
 329                 .cfg = {
 330                         .id = V4L2_CID_MPEG_VIDEO_H264_SPS,
 331                 },
 332         }, {
 333                 .codec = HANTRO_H264_DECODER,
 334                 .cfg = {
 335                         .id = V4L2_CID_MPEG_VIDEO_H264_PPS,
 336                 },
 337         }, {
 338                 .codec = HANTRO_H264_DECODER,
 339                 .cfg = {
 340                         .id = V4L2_CID_MPEG_VIDEO_H264_SCALING_MATRIX,
 341                 },
 342         }, {
 343                 .codec = HANTRO_H264_DECODER,
 344                 .cfg = {
 345                         .id = V4L2_CID_MPEG_VIDEO_H264_DECODE_MODE,
 346                         .min = V4L2_MPEG_VIDEO_H264_DECODE_MODE_FRAME_BASED,
 347                         .def = V4L2_MPEG_VIDEO_H264_DECODE_MODE_FRAME_BASED,
 348                         .max = V4L2_MPEG_VIDEO_H264_DECODE_MODE_FRAME_BASED,
 349                 },
 350         }, {
 351                 .codec = HANTRO_H264_DECODER,
 352                 .cfg = {
 353                         .id = V4L2_CID_MPEG_VIDEO_H264_START_CODE,
 354                         .min = V4L2_MPEG_VIDEO_H264_START_CODE_ANNEX_B,
 355                         .def = V4L2_MPEG_VIDEO_H264_START_CODE_ANNEX_B,
 356                         .max = V4L2_MPEG_VIDEO_H264_START_CODE_ANNEX_B,
 357                 },
 358         }, {
 359         },
 360 };
 361
 362 static int hantro_ctrls_setup(struct hantro_dev *vpu,
 363                               struct hantro_ctx *ctx,
 364                               int allowed_codecs)
 365 {
 366         int i, num_ctrls = ARRAY_SIZE(controls);
 367
 368         v4l2_ctrl_handler_init(&ctx->ctrl_handler, num_ctrls);
 369
 370         for (i = 0; i < num_ctrls; i++) {
 371                 if (!(allowed_codecs & controls[i].codec))
 372                         continue;
 373
 374                 v4l2_ctrl_new_custom(&ctx->ctrl_handler,
 375                                      &controls[i].cfg, NULL);
 376                 if (ctx->ctrl_handler.error) {
 377                         vpu_err("Adding control (%d) failed %d\n",
 378                                 controls[i].cfg.id,
 379                                 ctx->ctrl_handler.error);
 380                         v4l2_ctrl_handler_free(&ctx->ctrl_handler);
 381                         return ctx->ctrl_handler.error;
 382                 }
 383         }
 384         return v4l2_ctrl_handler_setup(&ctx->ctrl_handler);
 385 }
 386
 387 /*
 388  * V4L2 file operations.
 389  */
 390
 391 static int hantro_open(struct file *filp)
 392 {
 393         struct hantro_dev *vpu = video_drvdata(filp);
 394         struct video_device *vdev = video_devdata(filp);
 395         struct hantro_func *func = hantro_vdev_to_func(vdev);
 396         struct hantro_ctx *ctx;
 397         int allowed_codecs, ret;
 398
 399         /*
 400          * We do not need any extra locking here, because we operate only
 401          * on local data here, except reading few fields from dev, which
 402          * do not change through device's lifetime (which is guaranteed by
 403          * reference on module from open()) and V4L2 internal objects (such
 404          * as vdev and ctx->fh), which have proper locking done in respective
 405          * helper functions used here.
 406          */
 407
 408         ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
 409         if (!ctx)
 410                 return -ENOMEM;
 411
 412         ctx->dev = vpu;
 413         if (func->id == MEDIA_ENT_F_PROC_VIDEO_ENCODER) {
 414                 allowed_codecs = vpu->variant->codec & HANTRO_ENCODERS;
 415                 ctx->buf_finish = hantro_enc_buf_finish;
 416                 ctx->fh.m2m_ctx = v4l2_m2m_ctx_init(vpu->m2m_dev, ctx,
 417                                                     queue_init);
 418         } else if (func->id == MEDIA_ENT_F_PROC_VIDEO_DECODER) {
 419                 allowed_codecs = vpu->variant->codec & HANTRO_DECODERS;
 420                 ctx->buf_finish = hantro_dec_buf_finish;
 421                 ctx->fh.m2m_ctx = v4l2_m2m_ctx_init(vpu->m2m_dev, ctx,
 422                                                     queue_init);
 423         } else {
 424                 ctx->fh.m2m_ctx = ERR_PTR(-ENODEV);
 425         }
 426         if (IS_ERR(ctx->fh.m2m_ctx)) {
 427                 ret = PTR_ERR(ctx->fh.m2m_ctx);
 428                 kfree(ctx);
 429                 return ret;
 430         }
 431
 432         v4l2_fh_init(&ctx->fh, vdev);
 433         filp->private_data = &ctx->fh;
 434         v4l2_fh_add(&ctx->fh);
 435
 436         hantro_reset_fmts(ctx);
 437
 438         ret = hantro_ctrls_setup(vpu, ctx, allowed_codecs);
 439         if (ret) {
 440                 vpu_err("Failed to set up controls\n");
 441                 goto err_fh_free;
 442         }
 443         ctx->fh.ctrl_handler = &ctx->ctrl_handler;
 444
 445         return 0;
 446
 447 err_fh_free:
 448         v4l2_fh_del(&ctx->fh);
 449         v4l2_fh_exit(&ctx->fh);
 450         kfree(ctx);
 451         return ret;
 452 }
 453
 454 static int hantro_release(struct file *filp)
 455 {
 456         struct hantro_ctx *ctx =
 457                 container_of(filp->private_data, struct hantro_ctx, fh);
 458
 459         /*
 460          * No need for extra locking because this was the last reference
 461          * to this file.
 462          */
 463         v4l2_m2m_ctx_release(ctx->fh.m2m_ctx);
 464         v4l2_fh_del(&ctx->fh);
 465         v4l2_fh_exit(&ctx->fh);
 466         v4l2_ctrl_handler_free(&ctx->ctrl_handler);
 467         kfree(ctx);
 468
 469         return 0;
 470 }
 471
 472 static const struct v4l2_file_operations hantro_fops = {
 473         .owner = THIS_MODULE,
 474         .open = hantro_open,
 475         .release = hantro_release,
 476         .poll = v4l2_m2m_fop_poll,
 477         .unlocked_ioctl = video_ioctl2,
 478         .mmap = v4l2_m2m_fop_mmap,
 479 };
 480
 481 static const struct of_device_id of_hantro_match[] = {
 482 #ifdef CONFIG_VIDEO_HANTRO_ROCKCHIP
 483         { .compatible = "rockchip,rk3399-vpu", .data = &rk3399_vpu_variant, },
 484         { .compatible = "rockchip,rk3328-vpu", .data = &rk3328_vpu_variant, },
 485         { .compatible = "rockchip,rk3288-vpu", .data = &rk3288_vpu_variant, },
 486 #endif
 487         { /* sentinel */ }
 488 };
 489 MODULE_DEVICE_TABLE(of, of_hantro_match);
 490
 491 static int hantro_register_entity(struct media_device *mdev,
 492                                   struct media_entity *entity,
 493                                   const char *entity_name,
 494                                   struct media_pad *pads, int num_pads,
 495                                   int function, struct video_device *vdev)
 496 {
 497         char *name;
 498         int ret;
 499
 500         entity->obj_type = MEDIA_ENTITY_TYPE_BASE;
 501         if (function == MEDIA_ENT_F_IO_V4L) {
 502                 entity->info.dev.major = VIDEO_MAJOR;
 503                 entity->info.dev.minor = vdev->minor;
 504         }
 505
 506         name = devm_kasprintf(mdev->dev, GFP_KERNEL, "%s-%s", vdev->name,
 507                               entity_name);
 508         if (!name)
 509                 return -ENOMEM;
 510
 511         entity->name = name;
 512         entity->function = function;
 513
 514         ret = media_entity_pads_init(entity, num_pads, pads);
 515         if (ret)
 516                 return ret;
 517
 518         ret = media_device_register_entity(mdev, entity);
 519         if (ret)
 520                 return ret;
 521
 522         return 0;
 523 }
 524
 525 static int hantro_attach_func(struct hantro_dev *vpu,
 526                               struct hantro_func *func)
 527 {
 528         struct media_device *mdev = &vpu->mdev;
 529         struct media_link *link;
 530         int ret;
 531
 532         /* Create the three encoder entities with their pads */
 533         func->source_pad.flags = MEDIA_PAD_FL_SOURCE;
 534         ret = hantro_register_entity(mdev, &func->vdev.entity, "source",
 535                                      &func->source_pad, 1, MEDIA_ENT_F_IO_V4L,
 536                                      &func->vdev);
 537         if (ret)
 538                 return ret;
 539
 540         func->proc_pads[0].flags = MEDIA_PAD_FL_SINK;
 541         func->proc_pads[1].flags = MEDIA_PAD_FL_SOURCE;
 542         ret = hantro_register_entity(mdev, &func->proc, "proc",
 543                                      func->proc_pads, 2, func->id,
 544                                      &func->vdev);
 545         if (ret)
 546                 goto err_rel_entity0;
 547
 548         func->sink_pad.flags = MEDIA_PAD_FL_SINK;
 549         ret = hantro_register_entity(mdev, &func->sink, "sink",
 550                                      &func->sink_pad, 1, MEDIA_ENT_F_IO_V4L,
 551                                      &func->vdev);
 552         if (ret)
 553                 goto err_rel_entity1;
 554
 555         /* Connect the three entities */
 556         ret = media_create_pad_link(&func->vdev.entity, 0, &func->proc, 0,
 557                                     MEDIA_LNK_FL_IMMUTABLE |
 558                                     MEDIA_LNK_FL_ENABLED);
 559         if (ret)
 560                 goto err_rel_entity2;
 561
 562         ret = media_create_pad_link(&func->proc, 1, &func->sink, 0,
 563                                     MEDIA_LNK_FL_IMMUTABLE |
 564                                     MEDIA_LNK_FL_ENABLED);
 565         if (ret)
 566                 goto err_rm_links0;
 567
 568         /* Create video interface */
 569         func->intf_devnode = media_devnode_create(mdev, MEDIA_INTF_T_V4L_VIDEO,
 570                                                   0, VIDEO_MAJOR,
 571                                                   func->vdev.minor);
 572         if (!func->intf_devnode) {
 573                 ret = -ENOMEM;
 574                 goto err_rm_links1;
 575         }
 576
 577         /* Connect the two DMA engines to the interface */
 578         link = media_create_intf_link(&func->vdev.entity,
 579                                       &func->intf_devnode->intf,
 580                                       MEDIA_LNK_FL_IMMUTABLE |
 581                                       MEDIA_LNK_FL_ENABLED);
 582         if (!link) {
 583                 ret = -ENOMEM;
 584                 goto err_rm_devnode;
 585         }
 586
 587         link = media_create_intf_link(&func->sink, &func->intf_devnode->intf,
 588                                       MEDIA_LNK_FL_IMMUTABLE |
 589                                       MEDIA_LNK_FL_ENABLED);
 590         if (!link) {
 591                 ret = -ENOMEM;
 592                 goto err_rm_devnode;
 593         }
 594         return 0;
 595
 596 err_rm_devnode:
 597         media_devnode_remove(func->intf_devnode);
 598
 599 err_rm_links1:
 600         media_entity_remove_links(&func->sink);
 601
 602 err_rm_links0:
 603         media_entity_remove_links(&func->proc);
 604         media_entity_remove_links(&func->vdev.entity);
 605
 606 err_rel_entity2:
 607         media_device_unregister_entity(&func->sink);
 608
 609 err_rel_entity1:
 610         media_device_unregister_entity(&func->proc);
 611
 612 err_rel_entity0:
 613         media_device_unregister_entity(&func->vdev.entity);
 614         return ret;
 615 }
 616
 617 static void hantro_detach_func(struct hantro_func *func)
 618 {
 619         media_devnode_remove(func->intf_devnode);
 620         media_entity_remove_links(&func->sink);
 621         media_entity_remove_links(&func->proc);
 622         media_entity_remove_links(&func->vdev.entity);
 623         media_device_unregister_entity(&func->sink);
 624         media_device_unregister_entity(&func->proc);
 625         media_device_unregister_entity(&func->vdev.entity);
 626 }
 627
 628 static int hantro_add_func(struct hantro_dev *vpu, unsigned int funcid)
 629 {
 630         const struct of_device_id *match;
 631         struct hantro_func *func;
 632         struct video_device *vfd;
 633         int ret;
 634
 635         match = of_match_node(of_hantro_match, vpu->dev->of_node);
 636         func = devm_kzalloc(vpu->dev, sizeof(*func), GFP_KERNEL);
 637         if (!func) {
 638                 v4l2_err(&vpu->v4l2_dev, "Failed to allocate video device\n");
 639                 return -ENOMEM;
 640         }
 641
 642         func->id = funcid;
 643
 644         vfd = &func->vdev;
 645         vfd->fops = &hantro_fops;
 646         vfd->release = video_device_release_empty;
 647         vfd->lock = &vpu->vpu_mutex;
 648         vfd->v4l2_dev = &vpu->v4l2_dev;
 649         vfd->vfl_dir = VFL_DIR_M2M;
 650         vfd->device_caps = V4L2_CAP_STREAMING | V4L2_CAP_VIDEO_M2M_MPLANE;
 651         vfd->ioctl_ops = &hantro_ioctl_ops;
 652         snprintf(vfd->name, sizeof(vfd->name), "%s-%s", match->compatible,
 653                  funcid == MEDIA_ENT_F_PROC_VIDEO_ENCODER ? "enc" : "dec");
 654
 655         if (funcid == MEDIA_ENT_F_PROC_VIDEO_ENCODER)
 656                 vpu->encoder = func;
 657         else
 658                 vpu->decoder = func;
 659
 660         video_set_drvdata(vfd, vpu);
 661
 662         ret = video_register_device(vfd, VFL_TYPE_GRABBER, -1);
 663         if (ret) {
 664                 v4l2_err(&vpu->v4l2_dev, "Failed to register video device\n");
 665                 return ret;
 666         }
 667
 668         ret = hantro_attach_func(vpu, func);
 669         if (ret) {
 670                 v4l2_err(&vpu->v4l2_dev,
 671                          "Failed to attach functionality to the media device\n");
 672                 goto err_unreg_dev;
 673         }
 674
 675         v4l2_info(&vpu->v4l2_dev, "registered %s as /dev/video%d\n", vfd->name,
 676                   vfd->num);
 677
 678         return 0;
 679
 680 err_unreg_dev:
 681         video_unregister_device(vfd);
 682         return ret;
 683 }
 684
 685 static int hantro_add_enc_func(struct hantro_dev *vpu)
 686 {
 687         if (!vpu->variant->enc_fmts)
 688                 return 0;
 689
 690         return hantro_add_func(vpu, MEDIA_ENT_F_PROC_VIDEO_ENCODER);
 691 }
 692
 693 static int hantro_add_dec_func(struct hantro_dev *vpu)
 694 {
 695         if (!vpu->variant->dec_fmts)
 696                 return 0;
 697
 698         return hantro_add_func(vpu, MEDIA_ENT_F_PROC_VIDEO_DECODER);
 699 }
 700
 701 static void hantro_remove_func(struct hantro_dev *vpu,
 702                                unsigned int funcid)
 703 {
 704         struct hantro_func *func;
 705
 706         if (funcid == MEDIA_ENT_F_PROC_VIDEO_ENCODER)
 707                 func = vpu->encoder;
 708         else
 709                 func = vpu->decoder;
 710
 711         if (!func)
 712                 return;
 713
 714         hantro_detach_func(func);
 715         video_unregister_device(&func->vdev);
 716 }
 717
 718 static void hantro_remove_enc_func(struct hantro_dev *vpu)
 719 {
 720         hantro_remove_func(vpu, MEDIA_ENT_F_PROC_VIDEO_ENCODER);
 721 }
 722
 723 static void hantro_remove_dec_func(struct hantro_dev *vpu)
 724 {
 725         hantro_remove_func(vpu, MEDIA_ENT_F_PROC_VIDEO_DECODER);
 726 }
 727
 728 static const struct media_device_ops hantro_m2m_media_ops = {
 729         .req_validate = vb2_request_validate,
 730         .req_queue = v4l2_m2m_request_queue,
 731 };
 732
 733 static int hantro_probe(struct platform_device *pdev)
 734 {
 735         const struct of_device_id *match;
 736         struct hantro_dev *vpu;
 737         struct resource *res;
 738         int num_bases;
 739         int i, ret;
 740
 741         vpu = devm_kzalloc(&pdev->dev, sizeof(*vpu), GFP_KERNEL);
 742         if (!vpu)
 743                 return -ENOMEM;
 744
 745         vpu->dev = &pdev->dev;
 746         vpu->pdev = pdev;
 747         mutex_init(&vpu->vpu_mutex);
 748         spin_lock_init(&vpu->irqlock);
 749
 750         match = of_match_node(of_hantro_match, pdev->dev.of_node);
 751         vpu->variant = match->data;
 752
 753         INIT_DELAYED_WORK(&vpu->watchdog_work, hantro_watchdog);
 754
 755         vpu->clocks = devm_kcalloc(&pdev->dev, vpu->variant->num_clocks,
 756                                    sizeof(*vpu->clocks), GFP_KERNEL);
 757         if (!vpu->clocks)
 758                 return -ENOMEM;
 759
 760         for (i = 0; i < vpu->variant->num_clocks; i++)
 761                 vpu->clocks[i].id = vpu->variant->clk_names[i];
 762         ret = devm_clk_bulk_get(&pdev->dev, vpu->variant->num_clocks,
 763                                 vpu->clocks);
 764         if (ret)
 765                 return ret;
 766
 767         num_bases = vpu->variant->num_regs ?: 1;
 768         vpu->reg_bases = devm_kcalloc(&pdev->dev, num_bases,
 769                                       sizeof(*vpu->reg_bases), GFP_KERNEL);
 770         if (!vpu->reg_bases)
 771                 return -ENOMEM;
 772
 773         for (i = 0; i < num_bases; i++) {
 774                 res = vpu->variant->reg_names ?
 775                       platform_get_resource_byname(vpu->pdev, IORESOURCE_MEM,
 776                                                    vpu->variant->reg_names[i]) :
 777                       platform_get_resource(vpu->pdev, IORESOURCE_MEM, 0);
 778                 vpu->reg_bases[i] = devm_ioremap_resource(vpu->dev, res);
 779                 if (IS_ERR(vpu->reg_bases[i]))
 780                         return PTR_ERR(vpu->reg_bases[i]);
 781         }
 782         vpu->enc_base = vpu->reg_bases[0] + vpu->variant->enc_offset;
 783         vpu->dec_base = vpu->reg_bases[0] + vpu->variant->dec_offset;
 784
 785         ret = dma_set_coherent_mask(vpu->dev, DMA_BIT_MASK(32));
 786         if (ret) {
 787                 dev_err(vpu->dev, "Could not set DMA coherent mask.\n");
 788                 return ret;
 789         }
 790         vb2_dma_contig_set_max_seg_size(&pdev->dev, DMA_BIT_MASK(32));
 791
 792         for (i = 0; i < vpu->variant->num_irqs; i++) {
 793                 const char *irq_name = vpu->variant->irqs[i].name;
 794                 int irq;
 795
 796                 if (!vpu->variant->irqs[i].handler)
 797                         continue;
 798
 799                 irq = platform_get_irq_byname(vpu->pdev, irq_name);
 800                 if (irq <= 0)
 801                         return -ENXIO;
 802
 803                 ret = devm_request_irq(vpu->dev, irq,
 804                                        vpu->variant->irqs[i].handler, 0,
 805                                        dev_name(vpu->dev), vpu);
 806                 if (ret) {
 807                         dev_err(vpu->dev, "Could not request %s IRQ.\n",
 808                                 irq_name);
 809                         return ret;
 810                 }
 811         }
 812
 813         ret = vpu->variant->init(vpu);
 814         if (ret) {
 815                 dev_err(&pdev->dev, "Failed to init VPU hardware\n");
 816                 return ret;
 817         }
 818
 819         pm_runtime_set_autosuspend_delay(vpu->dev, 100);
 820         pm_runtime_use_autosuspend(vpu->dev);
 821         pm_runtime_enable(vpu->dev);
 822
 823         ret = clk_bulk_prepare(vpu->variant->num_clocks, vpu->clocks);
 824         if (ret) {
 825                 dev_err(&pdev->dev, "Failed to prepare clocks\n");
 826                 return ret;
 827         }
 828
 829         ret = v4l2_device_register(&pdev->dev, &vpu->v4l2_dev);
 830         if (ret) {
 831                 dev_err(&pdev->dev, "Failed to register v4l2 device\n");
 832                 goto err_clk_unprepare;
 833         }
 834         platform_set_drvdata(pdev, vpu);
 835
 836         vpu->m2m_dev = v4l2_m2m_init(&vpu_m2m_ops);
 837         if (IS_ERR(vpu->m2m_dev)) {
 838                 v4l2_err(&vpu->v4l2_dev, "Failed to init mem2mem device\n");
 839                 ret = PTR_ERR(vpu->m2m_dev);
 840                 goto err_v4l2_unreg;
 841         }
 842
 843         vpu->mdev.dev = vpu->dev;
 844         strscpy(vpu->mdev.model, DRIVER_NAME, sizeof(vpu->mdev.model));
 845         strscpy(vpu->mdev.bus_info, "platform: " DRIVER_NAME,
 846                 sizeof(vpu->mdev.model));
 847         media_device_init(&vpu->mdev);
 848         vpu->mdev.ops = &hantro_m2m_media_ops;
 849         vpu->v4l2_dev.mdev = &vpu->mdev;
 850
 851         ret = hantro_add_enc_func(vpu);
 852         if (ret) {
 853                 dev_err(&pdev->dev, "Failed to register encoder\n");
 854                 goto err_m2m_rel;
 855         }
 856
 857         ret = hantro_add_dec_func(vpu);
 858         if (ret) {
 859                 dev_err(&pdev->dev, "Failed to register decoder\n");
 860                 goto err_rm_enc_func;
 861         }
 862
 863         ret = media_device_register(&vpu->mdev);
 864         if (ret) {
 865                 v4l2_err(&vpu->v4l2_dev, "Failed to register mem2mem media device\n");
 866                 goto err_rm_dec_func;
 867         }
 868
 869         return 0;
 870
 871 err_rm_dec_func:
 872         hantro_remove_dec_func(vpu);
 873 err_rm_enc_func:
 874         hantro_remove_enc_func(vpu);
 875 err_m2m_rel:
 876         media_device_cleanup(&vpu->mdev);
 877         v4l2_m2m_release(vpu->m2m_dev);
 878 err_v4l2_unreg:
 879         v4l2_device_unregister(&vpu->v4l2_dev);
 880 err_clk_unprepare:
 881         clk_bulk_unprepare(vpu->variant->num_clocks, vpu->clocks);
 882         pm_runtime_dont_use_autosuspend(vpu->dev);
 883         pm_runtime_disable(vpu->dev);
 884         return ret;
 885 }
 886
 887 static int hantro_remove(struct platform_device *pdev)
 888 {
 889         struct hantro_dev *vpu = platform_get_drvdata(pdev);
 890
 891         v4l2_info(&vpu->v4l2_dev, "Removing %s\n", pdev->name);
 892
 893         media_device_unregister(&vpu->mdev);
 894         hantro_remove_dec_func(vpu);
 895         hantro_remove_enc_func(vpu);
 896         media_device_cleanup(&vpu->mdev);
 897         v4l2_m2m_release(vpu->m2m_dev);
 898         v4l2_device_unregister(&vpu->v4l2_dev);
 899         clk_bulk_unprepare(vpu->variant->num_clocks, vpu->clocks);
 900         pm_runtime_dont_use_autosuspend(vpu->dev);
 901         pm_runtime_disable(vpu->dev);
 902         return 0;
 903 }
 904
 905 #ifdef CONFIG_PM
 906 static int hantro_runtime_resume(struct device *dev)
 907 {
 908         struct hantro_dev *vpu = dev_get_drvdata(dev);
 909
 910         if (vpu->variant->runtime_resume)
 911                 return vpu->variant->runtime_resume(vpu);
 912
 913         return 0;
 914 }
 915 #endif
 916
 917 static const struct dev_pm_ops hantro_pm_ops = {
 918         SET_SYSTEM_SLEEP_PM_OPS(pm_runtime_force_suspend,
 919                                 pm_runtime_force_resume)
 920         SET_RUNTIME_PM_OPS(NULL, hantro_runtime_resume, NULL)
 921 };
 922
 923 static struct platform_driver hantro_driver = {
 924         .probe = hantro_probe,
 925         .remove = hantro_remove,
 926         .driver = {
 927                    .name = DRIVER_NAME,
 928                    .of_match_table = of_match_ptr(of_hantro_match),
 929                    .pm = &hantro_pm_ops,
 930         },
 931 };
 932 module_platform_driver(hantro_driver);
 933
 934 MODULE_LICENSE("GPL v2");
 935 MODULE_AUTHOR("Alpha Lin <Alpha.Lin@Rock-Chips.com>");
 936 MODULE_AUTHOR("Tomasz Figa <tfiga@chromium.org>");
 937 MODULE_AUTHOR("Ezequiel Garcia <ezequiel@collabora.com>");
 938 MODULE_DESCRIPTION("Hantro VPU codec driver");