drivers/media/platform/vicodec/vicodec-core.c

   1 // SPDX-License-Identifier: GPL-2.0-only
   2 /*
   3  * A virtual codec example device.
   4  *
   5  * Copyright 2018 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
   6  *
   7  * This is a virtual codec device driver for testing the codec framework.
   8  * It simulates a device that uses memory buffers for both source and
   9  * destination and encodes or decodes the data.
  10  */
  11
  12 #include <linux/module.h>
  13 #include <linux/delay.h>
  14 #include <linux/fs.h>
  15 #include <linux/sched.h>
  16 #include <linux/slab.h>
  17
  18 #include <linux/platform_device.h>
  19 #include <media/v4l2-mem2mem.h>
  20 #include <media/v4l2-device.h>
  21 #include <media/v4l2-ioctl.h>
  22 #include <media/v4l2-ctrls.h>
  23 #include <media/v4l2-event.h>
  24 #include <media/videobuf2-vmalloc.h>
  25
  26 #include "vicodec-codec.h"
  27
  28 MODULE_DESCRIPTION("Virtual codec device");
  29 MODULE_AUTHOR("Hans Verkuil <hans.verkuil@cisco.com>");
  30 MODULE_LICENSE("GPL v2");
  31
  32 static bool multiplanar;
  33 module_param(multiplanar, bool, 0444);
  34 MODULE_PARM_DESC(multiplanar,
  35                  " use multi-planar API instead of single-planar API");
  36
  37 static unsigned int debug;
  38 module_param(debug, uint, 0644);
  39 MODULE_PARM_DESC(debug, " activates debug info");
  40
  41 #define VICODEC_NAME            "vicodec"
  42 #define MAX_WIDTH               4096U
  43 #define MIN_WIDTH               640U
  44 #define MAX_HEIGHT              2160U
  45 #define MIN_HEIGHT              360U
  46
  47 #define dprintk(dev, fmt, arg...) \
  48         v4l2_dbg(1, debug, &dev->v4l2_dev, "%s: " fmt, __func__, ## arg)
  49
  50
  51 static void vicodec_dev_release(struct device *dev)
  52 {
  53 }
  54
  55 static struct platform_device vicodec_pdev = {
  56         .name           = VICODEC_NAME,
  57         .dev.release    = vicodec_dev_release,
  58 };
  59
  60 /* Per-queue, driver-specific private data */
  61 struct vicodec_q_data {
  62         unsigned int            width;
  63         unsigned int            height;
  64         unsigned int            flags;
  65         unsigned int            sizeimage;
  66         unsigned int            sequence;
  67         u32                     fourcc;
  68 };
  69
  70 enum {
  71         V4L2_M2M_SRC = 0,
  72         V4L2_M2M_DST = 1,
  73 };
  74
  75 struct vicodec_dev {
  76         struct v4l2_device      v4l2_dev;
  77         struct video_device     enc_vfd;
  78         struct video_device     dec_vfd;
  79 #ifdef CONFIG_MEDIA_CONTROLLER
  80         struct media_device     mdev;
  81 #endif
  82
  83         struct mutex            enc_mutex;
  84         struct mutex            dec_mutex;
  85         spinlock_t              enc_lock;
  86         spinlock_t              dec_lock;
  87
  88         struct v4l2_m2m_dev     *enc_dev;
  89         struct v4l2_m2m_dev     *dec_dev;
  90 };
  91
  92 struct vicodec_ctx {
  93         struct v4l2_fh          fh;
  94         struct vicodec_dev      *dev;
  95         bool                    is_enc;
  96         spinlock_t              *lock;
  97
  98         struct v4l2_ctrl_handler hdl;
  99         struct v4l2_ctrl        *ctrl_gop_size;
 100         unsigned int            gop_size;
 101         unsigned int            gop_cnt;
 102
 103         /* Abort requested by m2m */
 104         int                     aborting;
 105         struct vb2_v4l2_buffer *last_src_buf;
 106         struct vb2_v4l2_buffer *last_dst_buf;
 107
 108         enum v4l2_colorspace    colorspace;
 109         enum v4l2_ycbcr_encoding ycbcr_enc;
 110         enum v4l2_xfer_func     xfer_func;
 111         enum v4l2_quantization  quantization;
 112
 113         /* Source and destination queue data */
 114         struct vicodec_q_data   q_data[2];
 115         struct raw_frame        ref_frame;
 116         u8                      *compressed_frame;
 117         u32                     cur_buf_offset;
 118         u32                     comp_max_size;
 119         u32                     comp_size;
 120         u32                     comp_magic_cnt;
 121         u32                     comp_frame_size;
 122         bool                    comp_has_frame;
 123         bool                    comp_has_next_frame;
 124 };
 125
 126 static const u32 pixfmts_yuv[] = {
 127         V4L2_PIX_FMT_YUV420,
 128         V4L2_PIX_FMT_YVU420,
 129         V4L2_PIX_FMT_NV12,
 130         V4L2_PIX_FMT_NV21,
 131 };
 132
 133 static inline struct vicodec_ctx *file2ctx(struct file *file)
 134 {
 135         return container_of(file->private_data, struct vicodec_ctx, fh);
 136 }
 137
 138 static struct vicodec_q_data *get_q_data(struct vicodec_ctx *ctx,
 139                                          enum v4l2_buf_type type)
 140 {
 141         switch (type) {
 142         case V4L2_BUF_TYPE_VIDEO_OUTPUT:
 143         case V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE:
 144                 return &ctx->q_data[V4L2_M2M_SRC];
 145         case V4L2_BUF_TYPE_VIDEO_CAPTURE:
 146         case V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE:
 147                 return &ctx->q_data[V4L2_M2M_DST];
 148         default:
 149                 WARN_ON(1);
 150                 break;
 151         }
 152         return NULL;
 153 }
 154
 155 static void encode(struct vicodec_ctx *ctx,
 156                    struct vicodec_q_data *q_data,
 157                    u8 *p_in, u8 *p_out)
 158 {
 159         unsigned int size = q_data->width * q_data->height;
 160         struct cframe_hdr *p_hdr;
 161         struct cframe cf;
 162         struct raw_frame rf;
 163         u32 encoding;
 164
 165         rf.width = q_data->width;
 166         rf.height = q_data->height;
 167         rf.luma = p_in;
 168
 169         switch (q_data->fourcc) {
 170         case V4L2_PIX_FMT_YUV420:
 171                 rf.cb = rf.luma + size;
 172                 rf.cr = rf.cb + size / 4;
 173                 rf.chroma_step = 1;
 174                 break;
 175         case V4L2_PIX_FMT_YVU420:
 176                 rf.cr = rf.luma + size;
 177                 rf.cb = rf.cr + size / 4;
 178                 rf.chroma_step = 1;
 179                 break;
 180         case V4L2_PIX_FMT_NV12:
 181                 rf.cb = rf.luma + size;
 182                 rf.cr = rf.cb + 1;
 183                 rf.chroma_step = 2;
 184                 break;
 185         case V4L2_PIX_FMT_NV21:
 186                 rf.cr = rf.luma + size;
 187                 rf.cb = rf.cr + 1;
 188                 rf.chroma_step = 2;
 189                 break;
 190         }
 191
 192         cf.width = q_data->width;
 193         cf.height = q_data->height;
 194         cf.rlc_data = (__be16 *)(p_out + sizeof(*p_hdr));
 195
 196         encoding = encode_frame(&rf, &ctx->ref_frame, &cf, !ctx->gop_cnt,
 197                                 ctx->gop_cnt == ctx->gop_size - 1);
 198         if (encoding != FRAME_PCODED)
 199                 ctx->gop_cnt = 0;
 200         if (++ctx->gop_cnt == ctx->gop_size)
 201                 ctx->gop_cnt = 0;
 202
 203         p_hdr = (struct cframe_hdr *)p_out;
 204         p_hdr->magic1 = VICODEC_MAGIC1;
 205         p_hdr->magic2 = VICODEC_MAGIC2;
 206         p_hdr->version = htonl(VICODEC_VERSION);
 207         p_hdr->width = htonl(cf.width);
 208         p_hdr->height = htonl(cf.height);
 209         p_hdr->flags = htonl(q_data->flags);
 210         if (encoding & LUMA_UNENCODED)
 211                 p_hdr->flags |= htonl(VICODEC_FL_LUMA_IS_UNCOMPRESSED);
 212         if (encoding & CB_UNENCODED)
 213                 p_hdr->flags |= htonl(VICODEC_FL_CB_IS_UNCOMPRESSED);
 214         if (encoding & CR_UNENCODED)
 215                 p_hdr->flags |= htonl(VICODEC_FL_CR_IS_UNCOMPRESSED);
 216         p_hdr->colorspace = htonl(ctx->colorspace);
 217         p_hdr->xfer_func = htonl(ctx->xfer_func);
 218         p_hdr->ycbcr_enc = htonl(ctx->ycbcr_enc);
 219         p_hdr->quantization = htonl(ctx->quantization);
 220         p_hdr->size = htonl(cf.size);
 221         ctx->ref_frame.width = cf.width;
 222         ctx->ref_frame.height = cf.height;
 223 }
 224
 225 static int decode(struct vicodec_ctx *ctx,
 226                   struct vicodec_q_data *q_data,
 227                   u8 *p_in, u8 *p_out)
 228 {
 229         unsigned int size = q_data->width * q_data->height;
 230         unsigned int i;
 231         struct cframe_hdr *p_hdr;
 232         struct cframe cf;
 233         u8 *p;
 234
 235         p_hdr = (struct cframe_hdr *)p_in;
 236         cf.width = ntohl(p_hdr->width);
 237         cf.height = ntohl(p_hdr->height);
 238         q_data->flags = ntohl(p_hdr->flags);
 239         ctx->colorspace = ntohl(p_hdr->colorspace);
 240         ctx->xfer_func = ntohl(p_hdr->xfer_func);
 241         ctx->ycbcr_enc = ntohl(p_hdr->ycbcr_enc);
 242         ctx->quantization = ntohl(p_hdr->quantization);
 243         cf.rlc_data = (__be16 *)(p_in + sizeof(*p_hdr));
 244
 245         if (p_hdr->magic1 != VICODEC_MAGIC1 ||
 246             p_hdr->magic2 != VICODEC_MAGIC2 ||
 247             ntohl(p_hdr->version) != VICODEC_VERSION ||
 248             cf.width < VICODEC_MIN_WIDTH ||
 249             cf.width > VICODEC_MAX_WIDTH ||
 250             cf.height < VICODEC_MIN_HEIGHT ||
 251             cf.height > VICODEC_MAX_HEIGHT ||
 252             (cf.width & 7) || (cf.height & 7))
 253                 return -EINVAL;
 254
 255         /* TODO: support resolution changes */
 256         if (cf.width != q_data->width || cf.height != q_data->height)
 257                 return -EINVAL;
 258
 259         decode_frame(&cf, &ctx->ref_frame, q_data->flags);
 260         memcpy(p_out, ctx->ref_frame.luma, size);
 261         p_out += size;
 262
 263         switch (q_data->fourcc) {
 264         case V4L2_PIX_FMT_YUV420:
 265                 memcpy(p_out, ctx->ref_frame.cb, size / 4);
 266                 p_out += size / 4;
 267                 memcpy(p_out, ctx->ref_frame.cr, size / 4);
 268                 break;
 269         case V4L2_PIX_FMT_YVU420:
 270                 memcpy(p_out, ctx->ref_frame.cr, size / 4);
 271                 p_out += size / 4;
 272                 memcpy(p_out, ctx->ref_frame.cb, size / 4);
 273                 break;
 274         case V4L2_PIX_FMT_NV12:
 275                 for (i = 0, p = p_out; i < size / 4; i++, p += 2)
 276                         *p = ctx->ref_frame.cb[i];
 277                 for (i = 0, p = p_out + 1; i < size / 4; i++, p += 2)
 278                         *p = ctx->ref_frame.cr[i];
 279                 break;
 280         case V4L2_PIX_FMT_NV21:
 281                 for (i = 0, p = p_out; i < size / 4; i++, p += 2)
 282                         *p = ctx->ref_frame.cr[i];
 283                 for (i = 0, p = p_out + 1; i < size / 4; i++, p += 2)
 284                         *p = ctx->ref_frame.cb[i];
 285                 break;
 286         }
 287         return 0;
 288 }
 289
 290 static int device_process(struct vicodec_ctx *ctx,
 291                           struct vb2_v4l2_buffer *in_vb,
 292                           struct vb2_v4l2_buffer *out_vb)
 293 {
 294         struct vicodec_dev *dev = ctx->dev;
 295         struct vicodec_q_data *q_out, *q_cap;
 296         u8 *p_in, *p_out;
 297         int ret;
 298
 299         q_out = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_OUTPUT);
 300         q_cap = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE);
 301         if (ctx->is_enc)
 302                 p_in = vb2_plane_vaddr(&in_vb->vb2_buf, 0);
 303         else
 304                 p_in = ctx->compressed_frame;
 305         p_out = vb2_plane_vaddr(&out_vb->vb2_buf, 0);
 306         if (!p_in || !p_out) {
 307                 v4l2_err(&dev->v4l2_dev,
 308                          "Acquiring kernel pointers to buffers failed\n");
 309                 return -EFAULT;
 310         }
 311
 312         if (ctx->is_enc) {
 313                 struct cframe_hdr *p_hdr = (struct cframe_hdr *)p_out;
 314
 315                 encode(ctx, q_out, p_in, p_out);
 316                 vb2_set_plane_payload(&out_vb->vb2_buf, 0,
 317                                       sizeof(*p_hdr) + ntohl(p_hdr->size));
 318         } else {
 319                 ret = decode(ctx, q_cap, p_in, p_out);
 320                 if (ret)
 321                         return ret;
 322                 vb2_set_plane_payload(&out_vb->vb2_buf, 0,
 323                                       q_cap->width * q_cap->height * 3 / 2);
 324         }
 325
 326         out_vb->sequence = q_cap->sequence++;
 327         out_vb->vb2_buf.timestamp = in_vb->vb2_buf.timestamp;
 328
 329         if (in_vb->flags & V4L2_BUF_FLAG_TIMECODE)
 330                 out_vb->timecode = in_vb->timecode;
 331         out_vb->field = in_vb->field;
 332         out_vb->flags &= ~V4L2_BUF_FLAG_LAST;
 333         out_vb->flags |= in_vb->flags &
 334                 (V4L2_BUF_FLAG_TIMECODE |
 335                  V4L2_BUF_FLAG_KEYFRAME |
 336                  V4L2_BUF_FLAG_PFRAME |
 337                  V4L2_BUF_FLAG_BFRAME |
 338                  V4L2_BUF_FLAG_TSTAMP_SRC_MASK);
 339
 340         return 0;
 341 }
 342
 343 /*
 344  * mem2mem callbacks
 345  */
 346
 347 /* device_run() - prepares and starts the device */
 348 static void device_run(void *priv)
 349 {
 350         static const struct v4l2_event eos_event = {
 351                 .type = V4L2_EVENT_EOS
 352         };
 353         struct vicodec_ctx *ctx = priv;
 354         struct vicodec_dev *dev = ctx->dev;
 355         struct vb2_v4l2_buffer *src_buf, *dst_buf;
 356         struct vicodec_q_data *q_out;
 357         u32 state;
 358
 359         src_buf = v4l2_m2m_next_src_buf(ctx->fh.m2m_ctx);
 360         dst_buf = v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
 361         q_out = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_OUTPUT);
 362
 363         state = VB2_BUF_STATE_DONE;
 364         if (device_process(ctx, src_buf, dst_buf))
 365                 state = VB2_BUF_STATE_ERROR;
 366         ctx->last_dst_buf = dst_buf;
 367
 368         spin_lock(ctx->lock);
 369         if (!ctx->comp_has_next_frame && src_buf == ctx->last_src_buf) {
 370                 dst_buf->flags |= V4L2_BUF_FLAG_LAST;
 371                 v4l2_event_queue_fh(&ctx->fh, &eos_event);
 372         }
 373         if (ctx->is_enc) {
 374                 src_buf->sequence = q_out->sequence++;
 375                 src_buf = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
 376                 v4l2_m2m_buf_done(src_buf, state);
 377         } else if (vb2_get_plane_payload(&src_buf->vb2_buf, 0) == ctx->cur_buf_offset) {
 378                 src_buf->sequence = q_out->sequence++;
 379                 src_buf = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
 380                 v4l2_m2m_buf_done(src_buf, state);
 381                 ctx->cur_buf_offset = 0;
 382                 ctx->comp_has_next_frame = false;
 383         }
 384         v4l2_m2m_buf_done(dst_buf, state);
 385         ctx->comp_size = 0;
 386         ctx->comp_magic_cnt = 0;
 387         ctx->comp_has_frame = false;
 388         spin_unlock(ctx->lock);
 389
 390         if (ctx->is_enc)
 391                 v4l2_m2m_job_finish(dev->enc_dev, ctx->fh.m2m_ctx);
 392         else
 393                 v4l2_m2m_job_finish(dev->dec_dev, ctx->fh.m2m_ctx);
 394 }
 395
 396 static void job_remove_out_buf(struct vicodec_ctx *ctx, u32 state)
 397 {
 398         struct vb2_v4l2_buffer *src_buf;
 399         struct vicodec_q_data *q_out;
 400
 401         q_out = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_OUTPUT);
 402         spin_lock(ctx->lock);
 403         src_buf = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
 404         src_buf->sequence = q_out->sequence++;
 405         v4l2_m2m_buf_done(src_buf, state);
 406         ctx->cur_buf_offset = 0;
 407         spin_unlock(ctx->lock);
 408 }
 409
 410 static int job_ready(void *priv)
 411 {
 412         static const u8 magic[] = {
 413                 0x4f, 0x4f, 0x4f, 0x4f, 0xff, 0xff, 0xff, 0xff
 414         };
 415         struct vicodec_ctx *ctx = priv;
 416         struct vb2_v4l2_buffer *src_buf;
 417         u8 *p_out;
 418         u8 *p;
 419         u32 sz;
 420         u32 state;
 421
 422         if (ctx->is_enc || ctx->comp_has_frame)
 423                 return 1;
 424
 425 restart:
 426         ctx->comp_has_next_frame = false;
 427         src_buf = v4l2_m2m_next_src_buf(ctx->fh.m2m_ctx);
 428         if (!src_buf)
 429                 return 0;
 430         p_out = vb2_plane_vaddr(&src_buf->vb2_buf, 0);
 431         sz = vb2_get_plane_payload(&src_buf->vb2_buf, 0);
 432         p = p_out + ctx->cur_buf_offset;
 433
 434         state = VB2_BUF_STATE_DONE;
 435
 436         if (!ctx->comp_size) {
 437                 state = VB2_BUF_STATE_ERROR;
 438                 for (; p < p_out + sz; p++) {
 439                         u32 copy;
 440
 441                         p = memchr(p, magic[ctx->comp_magic_cnt],
 442                                    p_out + sz - p);
 443                         if (!p) {
 444                                 ctx->comp_magic_cnt = 0;
 445                                 break;
 446                         }
 447                         copy = sizeof(magic) - ctx->comp_magic_cnt;
 448                         if (p_out + sz - p < copy)
 449                                 copy = p_out + sz - p;
 450                         memcpy(ctx->compressed_frame + ctx->comp_magic_cnt,
 451                                p, copy);
 452                         ctx->comp_magic_cnt += copy;
 453                         if (!memcmp(ctx->compressed_frame, magic, ctx->comp_magic_cnt)) {
 454                                 p += copy;
 455                                 state = VB2_BUF_STATE_DONE;
 456                                 break;
 457                         }
 458                         ctx->comp_magic_cnt = 0;
 459                 }
 460                 if (ctx->comp_magic_cnt < sizeof(magic)) {
 461                         job_remove_out_buf(ctx, state);
 462                         goto restart;
 463                 }
 464                 ctx->comp_size = sizeof(magic);
 465         }
 466         if (ctx->comp_size < sizeof(struct cframe_hdr)) {
 467                 struct cframe_hdr *p_hdr = (struct cframe_hdr *)ctx->compressed_frame;
 468                 u32 copy = sizeof(struct cframe_hdr) - ctx->comp_size;
 469
 470                 if (copy > p_out + sz - p)
 471                         copy = p_out + sz - p;
 472                 memcpy(ctx->compressed_frame + ctx->comp_size,
 473                        p, copy);
 474                 p += copy;
 475                 ctx->comp_size += copy;
 476                 if (ctx->comp_size < sizeof(struct cframe_hdr)) {
 477                         job_remove_out_buf(ctx, state);
 478                         goto restart;
 479                 }
 480                 ctx->comp_frame_size = ntohl(p_hdr->size) + sizeof(*p_hdr);
 481                 if (ctx->comp_frame_size > ctx->comp_max_size)
 482                         ctx->comp_frame_size = ctx->comp_max_size;
 483         }
 484         if (ctx->comp_size < ctx->comp_frame_size) {
 485                 u32 copy = ctx->comp_frame_size - ctx->comp_size;
 486
 487                 if (copy > p_out + sz - p)
 488                         copy = p_out + sz - p;
 489                 memcpy(ctx->compressed_frame + ctx->comp_size,
 490                        p, copy);
 491                 p += copy;
 492                 ctx->comp_size += copy;
 493                 if (ctx->comp_size < ctx->comp_frame_size) {
 494                         job_remove_out_buf(ctx, state);
 495                         goto restart;
 496                 }
 497         }
 498         ctx->cur_buf_offset = p - p_out;
 499         ctx->comp_has_frame = true;
 500         ctx->comp_has_next_frame = false;
 501         if (sz - ctx->cur_buf_offset >= sizeof(struct cframe_hdr)) {
 502                 struct cframe_hdr *p_hdr = (struct cframe_hdr *)p;
 503                 u32 frame_size = ntohl(p_hdr->size);
 504                 u32 remaining = sz - ctx->cur_buf_offset - sizeof(*p_hdr);
 505
 506                 if (!memcmp(p, magic, sizeof(magic)))
 507                         ctx->comp_has_next_frame = remaining >= frame_size;
 508         }
 509         return 1;
 510 }
 511
 512 static void job_abort(void *priv)
 513 {
 514         struct vicodec_ctx *ctx = priv;
 515
 516         /* Will cancel the transaction in the next interrupt handler */
 517         ctx->aborting = 1;
 518 }
 519
 520 /*
 521  * video ioctls
 522  */
 523
 524 static u32 find_fmt(u32 fmt)
 525 {
 526         unsigned int i;
 527
 528         for (i = 0; i < ARRAY_SIZE(pixfmts_yuv); i++)
 529                 if (pixfmts_yuv[i] == fmt)
 530                         return fmt;
 531         return pixfmts_yuv[0];
 532 }
 533
 534 static int vidioc_querycap(struct file *file, void *priv,
 535                            struct v4l2_capability *cap)
 536 {
 537         strncpy(cap->driver, VICODEC_NAME, sizeof(cap->driver) - 1);
 538         strncpy(cap->card, VICODEC_NAME, sizeof(cap->card) - 1);
 539         snprintf(cap->bus_info, sizeof(cap->bus_info),
 540                         "platform:%s", VICODEC_NAME);
 541         cap->device_caps =  V4L2_CAP_STREAMING |
 542                             (multiplanar ?
 543                              V4L2_CAP_VIDEO_M2M_MPLANE :
 544                              V4L2_CAP_VIDEO_M2M);
 545         cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
 546         return 0;
 547 }
 548
 549 static int enum_fmt(struct v4l2_fmtdesc *f, bool is_enc, bool is_out)
 550 {
 551         bool is_yuv = (is_enc && is_out) || (!is_enc && !is_out);
 552
 553         if (V4L2_TYPE_IS_MULTIPLANAR(f->type) && !multiplanar)
 554                 return -EINVAL;
 555         if (!V4L2_TYPE_IS_MULTIPLANAR(f->type) && multiplanar)
 556                 return -EINVAL;
 557         if (f->index >= (is_yuv ? ARRAY_SIZE(pixfmts_yuv) : 1))
 558                 return -EINVAL;
 559
 560         if (is_yuv)
 561                 f->pixelformat = pixfmts_yuv[f->index];
 562         else
 563                 f->pixelformat = V4L2_PIX_FMT_FWHT;
 564         return 0;
 565 }
 566
 567 static int vidioc_enum_fmt_vid_cap(struct file *file, void *priv,
 568                                    struct v4l2_fmtdesc *f)
 569 {
 570         struct vicodec_ctx *ctx = file2ctx(file);
 571
 572         return enum_fmt(f, ctx->is_enc, false);
 573 }
 574
 575 static int vidioc_enum_fmt_vid_out(struct file *file, void *priv,
 576                                    struct v4l2_fmtdesc *f)
 577 {
 578         struct vicodec_ctx *ctx = file2ctx(file);
 579
 580         return enum_fmt(f, ctx->is_enc, true);
 581 }
 582
 583 static int vidioc_g_fmt(struct vicodec_ctx *ctx, struct v4l2_format *f)
 584 {
 585         struct vb2_queue *vq;
 586         struct vicodec_q_data *q_data;
 587         struct v4l2_pix_format_mplane *pix_mp;
 588         struct v4l2_pix_format *pix;
 589
 590         vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);
 591         if (!vq)
 592                 return -EINVAL;
 593
 594         q_data = get_q_data(ctx, f->type);
 595
 596         switch (f->type) {
 597         case V4L2_BUF_TYPE_VIDEO_CAPTURE:
 598         case V4L2_BUF_TYPE_VIDEO_OUTPUT:
 599                 if (multiplanar)
 600                         return -EINVAL;
 601                 pix = &f->fmt.pix;
 602                 pix->width = q_data->width;
 603                 pix->height = q_data->height;
 604                 pix->field = V4L2_FIELD_NONE;
 605                 pix->pixelformat = q_data->fourcc;
 606                 if (q_data->fourcc == V4L2_PIX_FMT_FWHT)
 607                         pix->bytesperline = 0;
 608                 else
 609                         pix->bytesperline = q_data->width;
 610                 pix->sizeimage = q_data->sizeimage;
 611                 pix->colorspace = ctx->colorspace;
 612                 pix->xfer_func = ctx->xfer_func;
 613                 pix->ycbcr_enc = ctx->ycbcr_enc;
 614                 pix->quantization = ctx->quantization;
 615                 break;
 616
 617         case V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE:
 618         case V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE:
 619                 if (!multiplanar)
 620                         return -EINVAL;
 621                 pix_mp = &f->fmt.pix_mp;
 622                 pix_mp->width = q_data->width;
 623                 pix_mp->height = q_data->height;
 624                 pix_mp->field = V4L2_FIELD_NONE;
 625                 pix_mp->pixelformat = q_data->fourcc;
 626                 pix_mp->num_planes = 1;
 627                 if (q_data->fourcc == V4L2_PIX_FMT_FWHT)
 628                         pix_mp->plane_fmt[0].bytesperline = 0;
 629                 else
 630                         pix_mp->plane_fmt[0].bytesperline = q_data->width;
 631                 pix_mp->plane_fmt[0].sizeimage = q_data->sizeimage;
 632                 pix_mp->colorspace = ctx->colorspace;
 633                 pix_mp->xfer_func = ctx->xfer_func;
 634                 pix_mp->ycbcr_enc = ctx->ycbcr_enc;
 635                 pix_mp->quantization = ctx->quantization;
 636                 memset(pix_mp->reserved, 0, sizeof(pix_mp->reserved));
 637                 memset(pix_mp->plane_fmt[0].reserved, 0,
 638                        sizeof(pix_mp->plane_fmt[0].reserved));
 639                 break;
 640         default:
 641                 return -EINVAL;
 642         }
 643         return 0;
 644 }
 645
 646 static int vidioc_g_fmt_vid_out(struct file *file, void *priv,
 647                                 struct v4l2_format *f)
 648 {
 649         return vidioc_g_fmt(file2ctx(file), f);
 650 }
 651
 652 static int vidioc_g_fmt_vid_cap(struct file *file, void *priv,
 653                                 struct v4l2_format *f)
 654 {
 655         return vidioc_g_fmt(file2ctx(file), f);
 656 }
 657
 658 static int vidioc_try_fmt(struct vicodec_ctx *ctx, struct v4l2_format *f)
 659 {
 660         struct v4l2_pix_format_mplane *pix_mp;
 661         struct v4l2_pix_format *pix;
 662
 663         switch (f->type) {
 664         case V4L2_BUF_TYPE_VIDEO_CAPTURE:
 665         case V4L2_BUF_TYPE_VIDEO_OUTPUT:
 666                 pix = &f->fmt.pix;
 667                 pix->width = clamp(pix->width, MIN_WIDTH, MAX_WIDTH) & ~7;
 668                 pix->height = clamp(pix->height, MIN_HEIGHT, MAX_HEIGHT) & ~7;
 669                 pix->bytesperline = pix->width;
 670                 pix->sizeimage = pix->width * pix->height * 3 / 2;
 671                 pix->field = V4L2_FIELD_NONE;
 672                 if (pix->pixelformat == V4L2_PIX_FMT_FWHT) {
 673                         pix->bytesperline = 0;
 674                         pix->sizeimage += sizeof(struct cframe_hdr);
 675                 }
 676                 break;
 677         case V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE:
 678         case V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE:
 679                 pix_mp = &f->fmt.pix_mp;
 680                 pix_mp->width = clamp(pix_mp->width, MIN_WIDTH, MAX_WIDTH) & ~7;
 681                 pix_mp->height =
 682                         clamp(pix_mp->height, MIN_HEIGHT, MAX_HEIGHT) & ~7;
 683                 pix_mp->plane_fmt[0].bytesperline = pix_mp->width;
 684                 pix_mp->plane_fmt[0].sizeimage =
 685                         pix_mp->width * pix_mp->height * 3 / 2;
 686                 pix_mp->field = V4L2_FIELD_NONE;
 687                 pix_mp->num_planes = 1;
 688                 if (pix_mp->pixelformat == V4L2_PIX_FMT_FWHT) {
 689                         pix_mp->plane_fmt[0].bytesperline = 0;
 690                         pix_mp->plane_fmt[0].sizeimage +=
 691                                         sizeof(struct cframe_hdr);
 692                 }
 693                 memset(pix_mp->reserved, 0, sizeof(pix_mp->reserved));
 694                 memset(pix_mp->plane_fmt[0].reserved, 0,
 695                        sizeof(pix_mp->plane_fmt[0].reserved));
 696                 break;
 697         default:
 698                 return -EINVAL;
 699         }
 700
 701         return 0;
 702 }
 703
 704 static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
 705                                   struct v4l2_format *f)
 706 {
 707         struct vicodec_ctx *ctx = file2ctx(file);
 708         struct v4l2_pix_format_mplane *pix_mp;
 709         struct v4l2_pix_format *pix;
 710
 711         switch (f->type) {
 712         case V4L2_BUF_TYPE_VIDEO_CAPTURE:
 713                 if (multiplanar)
 714                         return -EINVAL;
 715                 pix = &f->fmt.pix;
 716                 pix->pixelformat = ctx->is_enc ? V4L2_PIX_FMT_FWHT :
 717                                    find_fmt(f->fmt.pix.pixelformat);
 718                 pix->colorspace = ctx->colorspace;
 719                 pix->xfer_func = ctx->xfer_func;
 720                 pix->ycbcr_enc = ctx->ycbcr_enc;
 721                 pix->quantization = ctx->quantization;
 722                 break;
 723         case V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE:
 724                 if (!multiplanar)
 725                         return -EINVAL;
 726                 pix_mp = &f->fmt.pix_mp;
 727                 pix_mp->pixelformat = ctx->is_enc ? V4L2_PIX_FMT_FWHT :
 728                                       find_fmt(pix_mp->pixelformat);
 729                 pix_mp->colorspace = ctx->colorspace;
 730                 pix_mp->xfer_func = ctx->xfer_func;
 731                 pix_mp->ycbcr_enc = ctx->ycbcr_enc;
 732                 pix_mp->quantization = ctx->quantization;
 733                 memset(pix_mp->reserved, 0, sizeof(pix_mp->reserved));
 734                 memset(pix_mp->plane_fmt[0].reserved, 0,
 735                        sizeof(pix_mp->plane_fmt[0].reserved));
 736                 break;
 737         default:
 738                 return -EINVAL;
 739         }
 740
 741         return vidioc_try_fmt(ctx, f);
 742 }
 743
 744 static int vidioc_try_fmt_vid_out(struct file *file, void *priv,
 745                                   struct v4l2_format *f)
 746 {
 747         struct vicodec_ctx *ctx = file2ctx(file);
 748         struct v4l2_pix_format_mplane *pix_mp;
 749         struct v4l2_pix_format *pix;
 750
 751         switch (f->type) {
 752         case V4L2_BUF_TYPE_VIDEO_OUTPUT:
 753                 if (multiplanar)
 754                         return -EINVAL;
 755                 pix = &f->fmt.pix;
 756                 pix->pixelformat = !ctx->is_enc ? V4L2_PIX_FMT_FWHT :
 757                                    find_fmt(pix->pixelformat);
 758                 if (!pix->colorspace)
 759                         pix->colorspace = V4L2_COLORSPACE_REC709;
 760                 break;
 761         case V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE:
 762                 if (!multiplanar)
 763                         return -EINVAL;
 764                 pix_mp = &f->fmt.pix_mp;
 765                 pix_mp->pixelformat = !ctx->is_enc ? V4L2_PIX_FMT_FWHT :
 766                                       find_fmt(pix_mp->pixelformat);
 767                 if (!pix_mp->colorspace)
 768                         pix_mp->colorspace = V4L2_COLORSPACE_REC709;
 769                 break;
 770         default:
 771                 return -EINVAL;
 772         }
 773
 774         return vidioc_try_fmt(ctx, f);
 775 }
 776
 777 static int vidioc_s_fmt(struct vicodec_ctx *ctx, struct v4l2_format *f)
 778 {
 779         struct vicodec_q_data *q_data;
 780         struct vb2_queue *vq;
 781         bool fmt_changed = true;
 782         struct v4l2_pix_format_mplane *pix_mp;
 783         struct v4l2_pix_format *pix;
 784
 785         vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);
 786         if (!vq)
 787                 return -EINVAL;
 788
 789         q_data = get_q_data(ctx, f->type);
 790         if (!q_data)
 791                 return -EINVAL;
 792
 793         switch (f->type) {
 794         case V4L2_BUF_TYPE_VIDEO_CAPTURE:
 795         case V4L2_BUF_TYPE_VIDEO_OUTPUT:
 796                 pix = &f->fmt.pix;
 797                 if (ctx->is_enc && V4L2_TYPE_IS_OUTPUT(f->type))
 798                         fmt_changed =
 799                                 q_data->fourcc != pix->pixelformat ||
 800                                 q_data->width != pix->width ||
 801                                 q_data->height != pix->height;
 802
 803                 if (vb2_is_busy(vq) && fmt_changed)
 804                         return -EBUSY;
 805
 806                 q_data->fourcc = pix->pixelformat;
 807                 q_data->width = pix->width;
 808                 q_data->height = pix->height;
 809                 q_data->sizeimage = pix->sizeimage;
 810                 break;
 811         case V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE:
 812         case V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE:
 813                 pix_mp = &f->fmt.pix_mp;
 814                 if (ctx->is_enc && V4L2_TYPE_IS_OUTPUT(f->type))
 815                         fmt_changed =
 816                                 q_data->fourcc != pix_mp->pixelformat ||
 817                                 q_data->width != pix_mp->width ||
 818                                 q_data->height != pix_mp->height;
 819
 820                 if (vb2_is_busy(vq) && fmt_changed)
 821                         return -EBUSY;
 822
 823                 q_data->fourcc = pix_mp->pixelformat;
 824                 q_data->width = pix_mp->width;
 825                 q_data->height = pix_mp->height;
 826                 q_data->sizeimage = pix_mp->plane_fmt[0].sizeimage;
 827                 break;
 828         default:
 829                 return -EINVAL;
 830         }
 831
 832         dprintk(ctx->dev,
 833                 "Setting format for type %d, wxh: %dx%d, fourcc: %08x\n",
 834                 f->type, q_data->width, q_data->height, q_data->fourcc);
 835
 836         return 0;
 837 }
 838
 839 static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
 840                                 struct v4l2_format *f)
 841 {
 842         int ret;
 843
 844         ret = vidioc_try_fmt_vid_cap(file, priv, f);
 845         if (ret)
 846                 return ret;
 847
 848         return vidioc_s_fmt(file2ctx(file), f);
 849 }
 850
 851 static int vidioc_s_fmt_vid_out(struct file *file, void *priv,
 852                                 struct v4l2_format *f)
 853 {
 854         struct vicodec_ctx *ctx = file2ctx(file);
 855         struct v4l2_pix_format_mplane *pix_mp;
 856         struct v4l2_pix_format *pix;
 857         int ret;
 858
 859         ret = vidioc_try_fmt_vid_out(file, priv, f);
 860         if (ret)
 861                 return ret;
 862
 863         ret = vidioc_s_fmt(file2ctx(file), f);
 864         if (!ret) {
 865                 switch (f->type) {
 866                 case V4L2_BUF_TYPE_VIDEO_CAPTURE:
 867                 case V4L2_BUF_TYPE_VIDEO_OUTPUT:
 868                         pix = &f->fmt.pix;
 869                         ctx->colorspace = pix->colorspace;
 870                         ctx->xfer_func = pix->xfer_func;
 871                         ctx->ycbcr_enc = pix->ycbcr_enc;
 872                         ctx->quantization = pix->quantization;
 873                         break;
 874                 case V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE:
 875                 case V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE:
 876                         pix_mp = &f->fmt.pix_mp;
 877                         ctx->colorspace = pix_mp->colorspace;
 878                         ctx->xfer_func = pix_mp->xfer_func;
 879                         ctx->ycbcr_enc = pix_mp->ycbcr_enc;
 880                         ctx->quantization = pix_mp->quantization;
 881                         break;
 882                 default:
 883                         break;
 884                 }
 885         }
 886         return ret;
 887 }
 888
 889 static void vicodec_mark_last_buf(struct vicodec_ctx *ctx)
 890 {
 891         static const struct v4l2_event eos_event = {
 892                 .type = V4L2_EVENT_EOS
 893         };
 894
 895         spin_lock(ctx->lock);
 896         ctx->last_src_buf = v4l2_m2m_last_src_buf(ctx->fh.m2m_ctx);
 897         if (!ctx->last_src_buf && ctx->last_dst_buf) {
 898                 ctx->last_dst_buf->flags |= V4L2_BUF_FLAG_LAST;
 899                 v4l2_event_queue_fh(&ctx->fh, &eos_event);
 900         }
 901         spin_unlock(ctx->lock);
 902 }
 903
 904 static int vicodec_try_encoder_cmd(struct file *file, void *fh,
 905                                 struct v4l2_encoder_cmd *ec)
 906 {
 907         if (ec->cmd != V4L2_ENC_CMD_STOP)
 908                 return -EINVAL;
 909
 910         if (ec->flags & V4L2_ENC_CMD_STOP_AT_GOP_END)
 911                 return -EINVAL;
 912
 913         return 0;
 914 }
 915
 916 static int vicodec_encoder_cmd(struct file *file, void *fh,
 917                             struct v4l2_encoder_cmd *ec)
 918 {
 919         struct vicodec_ctx *ctx = file2ctx(file);
 920         int ret;
 921
 922         ret = vicodec_try_encoder_cmd(file, fh, ec);
 923         if (ret < 0)
 924                 return ret;
 925
 926         vicodec_mark_last_buf(ctx);
 927         return 0;
 928 }
 929
 930 static int vicodec_try_decoder_cmd(struct file *file, void *fh,
 931                                 struct v4l2_decoder_cmd *dc)
 932 {
 933         if (dc->cmd != V4L2_DEC_CMD_STOP)
 934                 return -EINVAL;
 935
 936         if (dc->flags & V4L2_DEC_CMD_STOP_TO_BLACK)
 937                 return -EINVAL;
 938
 939         if (!(dc->flags & V4L2_DEC_CMD_STOP_IMMEDIATELY) && (dc->stop.pts != 0))
 940                 return -EINVAL;
 941
 942         return 0;
 943 }
 944
 945 static int vicodec_decoder_cmd(struct file *file, void *fh,
 946                             struct v4l2_decoder_cmd *dc)
 947 {
 948         struct vicodec_ctx *ctx = file2ctx(file);
 949         int ret;
 950
 951         ret = vicodec_try_decoder_cmd(file, fh, dc);
 952         if (ret < 0)
 953                 return ret;
 954
 955         vicodec_mark_last_buf(ctx);
 956         return 0;
 957 }
 958
 959 static int vicodec_enum_framesizes(struct file *file, void *fh,
 960                                    struct v4l2_frmsizeenum *fsize)
 961 {
 962         switch (fsize->pixel_format) {
 963         case V4L2_PIX_FMT_FWHT:
 964                 break;
 965         default:
 966                 if (find_fmt(fsize->pixel_format) == fsize->pixel_format)
 967                         break;
 968                 return -EINVAL;
 969         }
 970
 971         if (fsize->index)
 972                 return -EINVAL;
 973
 974         fsize->type = V4L2_FRMSIZE_TYPE_STEPWISE;
 975
 976         fsize->stepwise.min_width = MIN_WIDTH;
 977         fsize->stepwise.max_width = MAX_WIDTH;
 978         fsize->stepwise.step_width = 8;
 979         fsize->stepwise.min_height = MIN_HEIGHT;
 980         fsize->stepwise.max_height = MAX_HEIGHT;
 981         fsize->stepwise.step_height = 8;
 982
 983         return 0;
 984 }
 985
 986 static int vicodec_subscribe_event(struct v4l2_fh *fh,
 987                                 const struct v4l2_event_subscription *sub)
 988 {
 989         switch (sub->type) {
 990         case V4L2_EVENT_EOS:
 991                 return v4l2_event_subscribe(fh, sub, 0, NULL);
 992         default:
 993                 return v4l2_ctrl_subscribe_event(fh, sub);
 994         }
 995 }
 996
 997 static const struct v4l2_ioctl_ops vicodec_ioctl_ops = {
 998         .vidioc_querycap        = vidioc_querycap,
 999
1000         .vidioc_enum_fmt_vid_cap = vidioc_enum_fmt_vid_cap,
1001         .vidioc_g_fmt_vid_cap   = vidioc_g_fmt_vid_cap,
1002         .vidioc_try_fmt_vid_cap = vidioc_try_fmt_vid_cap,
1003         .vidioc_s_fmt_vid_cap   = vidioc_s_fmt_vid_cap,
1004
1005         .vidioc_enum_fmt_vid_cap_mplane = vidioc_enum_fmt_vid_cap,
1006         .vidioc_g_fmt_vid_cap_mplane    = vidioc_g_fmt_vid_cap,
1007         .vidioc_try_fmt_vid_cap_mplane  = vidioc_try_fmt_vid_cap,
1008         .vidioc_s_fmt_vid_cap_mplane    = vidioc_s_fmt_vid_cap,
1009
1010         .vidioc_enum_fmt_vid_out = vidioc_enum_fmt_vid_out,
1011         .vidioc_g_fmt_vid_out   = vidioc_g_fmt_vid_out,
1012         .vidioc_try_fmt_vid_out = vidioc_try_fmt_vid_out,
1013         .vidioc_s_fmt_vid_out   = vidioc_s_fmt_vid_out,
1014
1015         .vidioc_enum_fmt_vid_out_mplane = vidioc_enum_fmt_vid_out,
1016         .vidioc_g_fmt_vid_out_mplane    = vidioc_g_fmt_vid_out,
1017         .vidioc_try_fmt_vid_out_mplane  = vidioc_try_fmt_vid_out,
1018         .vidioc_s_fmt_vid_out_mplane    = vidioc_s_fmt_vid_out,
1019
1020         .vidioc_reqbufs         = v4l2_m2m_ioctl_reqbufs,
1021         .vidioc_querybuf        = v4l2_m2m_ioctl_querybuf,
1022         .vidioc_qbuf            = v4l2_m2m_ioctl_qbuf,
1023         .vidioc_dqbuf           = v4l2_m2m_ioctl_dqbuf,
1024         .vidioc_prepare_buf     = v4l2_m2m_ioctl_prepare_buf,
1025         .vidioc_create_bufs     = v4l2_m2m_ioctl_create_bufs,
1026         .vidioc_expbuf          = v4l2_m2m_ioctl_expbuf,
1027
1028         .vidioc_streamon        = v4l2_m2m_ioctl_streamon,
1029         .vidioc_streamoff       = v4l2_m2m_ioctl_streamoff,
1030
1031         .vidioc_try_encoder_cmd = vicodec_try_encoder_cmd,
1032         .vidioc_encoder_cmd     = vicodec_encoder_cmd,
1033         .vidioc_try_decoder_cmd = vicodec_try_decoder_cmd,
1034         .vidioc_decoder_cmd     = vicodec_decoder_cmd,
1035         .vidioc_enum_framesizes = vicodec_enum_framesizes,
1036
1037         .vidioc_subscribe_event = vicodec_subscribe_event,
1038         .vidioc_unsubscribe_event = v4l2_event_unsubscribe,
1039 };
1040
1041
1042 /*
1043  * Queue operations
1044  */
1045
1046 static int vicodec_queue_setup(struct vb2_queue *vq, unsigned int *nbuffers,
1047                                unsigned int *nplanes, unsigned int sizes[],
1048                                struct device *alloc_devs[])
1049 {
1050         struct vicodec_ctx *ctx = vb2_get_drv_priv(vq);
1051         struct vicodec_q_data *q_data = get_q_data(ctx, vq->type);
1052         unsigned int size = q_data->sizeimage;
1053
1054         if (*nplanes)
1055                 return sizes[0] < size ? -EINVAL : 0;
1056
1057         *nplanes = 1;
1058         sizes[0] = size;
1059         return 0;
1060 }
1061
1062 static int vicodec_buf_prepare(struct vb2_buffer *vb)
1063 {
1064         struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
1065         struct vicodec_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
1066         struct vicodec_q_data *q_data;
1067
1068         dprintk(ctx->dev, "type: %d\n", vb->vb2_queue->type);
1069
1070         q_data = get_q_data(ctx, vb->vb2_queue->type);
1071         if (V4L2_TYPE_IS_OUTPUT(vb->vb2_queue->type)) {
1072                 if (vbuf->field == V4L2_FIELD_ANY)
1073                         vbuf->field = V4L2_FIELD_NONE;
1074                 if (vbuf->field != V4L2_FIELD_NONE) {
1075                         dprintk(ctx->dev, "%s field isn't supported\n",
1076                                         __func__);
1077                         return -EINVAL;
1078                 }
1079         }
1080
1081         if (vb2_plane_size(vb, 0) < q_data->sizeimage) {
1082                 dprintk(ctx->dev,
1083                         "%s data will not fit into plane (%lu < %lu)\n",
1084                         __func__, vb2_plane_size(vb, 0),
1085                         (long)q_data->sizeimage);
1086                 return -EINVAL;
1087         }
1088
1089         return 0;
1090 }
1091
1092 static void vicodec_buf_queue(struct vb2_buffer *vb)
1093 {
1094         struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
1095         struct vicodec_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
1096
1097         v4l2_m2m_buf_queue(ctx->fh.m2m_ctx, vbuf);
1098 }
1099
1100 static void vicodec_return_bufs(struct vb2_queue *q, u32 state)
1101 {
1102         struct vicodec_ctx *ctx = vb2_get_drv_priv(q);
1103         struct vb2_v4l2_buffer *vbuf;
1104
1105         for (;;) {
1106                 if (V4L2_TYPE_IS_OUTPUT(q->type))
1107                         vbuf = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
1108                 else
1109                         vbuf = v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
1110                 if (vbuf == NULL)
1111                         return;
1112                 spin_lock(ctx->lock);
1113                 v4l2_m2m_buf_done(vbuf, state);
1114                 spin_unlock(ctx->lock);
1115         }
1116 }
1117
1118 static int vicodec_start_streaming(struct vb2_queue *q,
1119                                    unsigned int count)
1120 {
1121         struct vicodec_ctx *ctx = vb2_get_drv_priv(q);
1122         struct vicodec_q_data *q_data = get_q_data(ctx, q->type);
1123         unsigned int size = q_data->width * q_data->height;
1124
1125         q_data->sequence = 0;
1126
1127         if (!V4L2_TYPE_IS_OUTPUT(q->type))
1128                 return 0;
1129
1130         ctx->ref_frame.width = ctx->ref_frame.height = 0;
1131         ctx->ref_frame.luma = kvmalloc(size * 3 / 2, GFP_KERNEL);
1132         ctx->comp_max_size = size * 3 / 2 + sizeof(struct cframe_hdr);
1133         ctx->compressed_frame = kvmalloc(ctx->comp_max_size, GFP_KERNEL);
1134         if (!ctx->ref_frame.luma || !ctx->compressed_frame) {
1135                 kvfree(ctx->ref_frame.luma);
1136                 kvfree(ctx->compressed_frame);
1137                 vicodec_return_bufs(q, VB2_BUF_STATE_QUEUED);
1138                 return -ENOMEM;
1139         }
1140         ctx->ref_frame.cb = ctx->ref_frame.luma + size;
1141         ctx->ref_frame.cr = ctx->ref_frame.cb + size / 4;
1142         ctx->last_src_buf = NULL;
1143         ctx->last_dst_buf = NULL;
1144         v4l2_ctrl_grab(ctx->ctrl_gop_size, true);
1145         ctx->gop_size = v4l2_ctrl_g_ctrl(ctx->ctrl_gop_size);
1146         ctx->gop_cnt = 0;
1147         ctx->cur_buf_offset = 0;
1148         ctx->comp_size = 0;
1149         ctx->comp_magic_cnt = 0;
1150         ctx->comp_has_frame = false;
1151
1152         return 0;
1153 }
1154
1155 static void vicodec_stop_streaming(struct vb2_queue *q)
1156 {
1157         struct vicodec_ctx *ctx = vb2_get_drv_priv(q);
1158
1159         vicodec_return_bufs(q, VB2_BUF_STATE_ERROR);
1160
1161         if (!V4L2_TYPE_IS_OUTPUT(q->type))
1162                 return;
1163
1164         kvfree(ctx->ref_frame.luma);
1165         kvfree(ctx->compressed_frame);
1166         v4l2_ctrl_grab(ctx->ctrl_gop_size, false);
1167 }
1168
1169 static const struct vb2_ops vicodec_qops = {
1170         .queue_setup     = vicodec_queue_setup,
1171         .buf_prepare     = vicodec_buf_prepare,
1172         .buf_queue       = vicodec_buf_queue,
1173         .start_streaming = vicodec_start_streaming,
1174         .stop_streaming  = vicodec_stop_streaming,
1175         .wait_prepare    = vb2_ops_wait_prepare,
1176         .wait_finish     = vb2_ops_wait_finish,
1177 };
1178
1179 static int queue_init(void *priv, struct vb2_queue *src_vq,
1180                       struct vb2_queue *dst_vq)
1181 {
1182         struct vicodec_ctx *ctx = priv;
1183         int ret;
1184
1185         src_vq->type = (multiplanar ?
1186                         V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE :
1187                         V4L2_BUF_TYPE_VIDEO_OUTPUT);
1188         src_vq->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF;
1189         src_vq->drv_priv = ctx;
1190         src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
1191         src_vq->ops = &vicodec_qops;
1192         src_vq->mem_ops = &vb2_vmalloc_memops;
1193         src_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
1194         src_vq->lock = ctx->is_enc ? &ctx->dev->enc_mutex :
1195                 &ctx->dev->dec_mutex;
1196
1197         ret = vb2_queue_init(src_vq);
1198         if (ret)
1199                 return ret;
1200
1201         dst_vq->type = (multiplanar ?
1202                         V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE :
1203                         V4L2_BUF_TYPE_VIDEO_CAPTURE);
1204         dst_vq->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF;
1205         dst_vq->drv_priv = ctx;
1206         dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
1207         dst_vq->ops = &vicodec_qops;
1208         dst_vq->mem_ops = &vb2_vmalloc_memops;
1209         dst_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
1210         dst_vq->lock = src_vq->lock;
1211
1212         return vb2_queue_init(dst_vq);
1213 }
1214
1215 /*
1216  * File operations
1217  */
1218 static int vicodec_open(struct file *file)
1219 {
1220         struct video_device *vfd = video_devdata(file);
1221         struct vicodec_dev *dev = video_drvdata(file);
1222         struct vicodec_ctx *ctx = NULL;
1223         struct v4l2_ctrl_handler *hdl;
1224         unsigned int size;
1225         int rc = 0;
1226
1227         if (mutex_lock_interruptible(vfd->lock))
1228                 return -ERESTARTSYS;
1229         ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
1230         if (!ctx) {
1231                 rc = -ENOMEM;
1232                 goto open_unlock;
1233         }
1234
1235         if (vfd == &dev->enc_vfd)
1236                 ctx->is_enc = true;
1237
1238         v4l2_fh_init(&ctx->fh, video_devdata(file));
1239         file->private_data = &ctx->fh;
1240         ctx->dev = dev;
1241         hdl = &ctx->hdl;
1242         v4l2_ctrl_handler_init(hdl, 4);
1243         ctx->ctrl_gop_size = v4l2_ctrl_new_std(hdl, NULL,
1244                                                V4L2_CID_MPEG_VIDEO_GOP_SIZE,
1245                                                1, 16, 1, 10);
1246         if (hdl->error) {
1247                 rc = hdl->error;
1248                 v4l2_ctrl_handler_free(hdl);
1249                 kfree(ctx);
1250                 goto open_unlock;
1251         }
1252         ctx->fh.ctrl_handler = hdl;
1253         v4l2_ctrl_handler_setup(hdl);
1254
1255         ctx->q_data[V4L2_M2M_SRC].fourcc =
1256                 ctx->is_enc ? V4L2_PIX_FMT_YUV420 : V4L2_PIX_FMT_FWHT;
1257         ctx->q_data[V4L2_M2M_SRC].width = 1280;
1258         ctx->q_data[V4L2_M2M_SRC].height = 720;
1259         size = 1280 * 720 * 3 / 2;
1260         ctx->q_data[V4L2_M2M_SRC].sizeimage = size;
1261         ctx->q_data[V4L2_M2M_DST] = ctx->q_data[V4L2_M2M_SRC];
1262         ctx->q_data[V4L2_M2M_DST].fourcc =
1263                 ctx->is_enc ? V4L2_PIX_FMT_FWHT : V4L2_PIX_FMT_YUV420;
1264         ctx->colorspace = V4L2_COLORSPACE_REC709;
1265
1266         size += sizeof(struct cframe_hdr);
1267         if (ctx->is_enc) {
1268                 ctx->q_data[V4L2_M2M_DST].sizeimage = size;
1269                 ctx->fh.m2m_ctx = v4l2_m2m_ctx_init(dev->enc_dev, ctx,
1270                                                     &queue_init);
1271                 ctx->lock = &dev->enc_lock;
1272         } else {
1273                 ctx->q_data[V4L2_M2M_SRC].sizeimage = size;
1274                 ctx->fh.m2m_ctx = v4l2_m2m_ctx_init(dev->dec_dev, ctx,
1275                                                     &queue_init);
1276                 ctx->lock = &dev->dec_lock;
1277         }
1278
1279         if (IS_ERR(ctx->fh.m2m_ctx)) {
1280                 rc = PTR_ERR(ctx->fh.m2m_ctx);
1281
1282                 v4l2_ctrl_handler_free(hdl);
1283                 v4l2_fh_exit(&ctx->fh);
1284                 kfree(ctx);
1285                 goto open_unlock;
1286         }
1287
1288         v4l2_fh_add(&ctx->fh);
1289
1290 open_unlock:
1291         mutex_unlock(vfd->lock);
1292         return rc;
1293 }
1294
1295 static int vicodec_release(struct file *file)
1296 {
1297         struct video_device *vfd = video_devdata(file);
1298         struct vicodec_ctx *ctx = file2ctx(file);
1299
1300         mutex_lock(vfd->lock);
1301         v4l2_m2m_ctx_release(ctx->fh.m2m_ctx);
1302         mutex_unlock(vfd->lock);
1303         v4l2_fh_del(&ctx->fh);
1304         v4l2_fh_exit(&ctx->fh);
1305         v4l2_ctrl_handler_free(&ctx->hdl);
1306         kfree(ctx);
1307
1308         return 0;
1309 }
1310
1311 static const struct v4l2_file_operations vicodec_fops = {
1312         .owner          = THIS_MODULE,
1313         .open           = vicodec_open,
1314         .release        = vicodec_release,
1315         .poll           = v4l2_m2m_fop_poll,
1316         .unlocked_ioctl = video_ioctl2,
1317         .mmap           = v4l2_m2m_fop_mmap,
1318 };
1319
1320 static const struct video_device vicodec_videodev = {
1321         .name           = VICODEC_NAME,
1322         .vfl_dir        = VFL_DIR_M2M,
1323         .fops           = &vicodec_fops,
1324         .ioctl_ops      = &vicodec_ioctl_ops,
1325         .minor          = -1,
1326         .release        = video_device_release_empty,
1327 };
1328
1329 static const struct v4l2_m2m_ops m2m_ops = {
1330         .device_run     = device_run,
1331         .job_abort      = job_abort,
1332         .job_ready      = job_ready,
1333 };
1334
1335 static int vicodec_probe(struct platform_device *pdev)
1336 {
1337         struct vicodec_dev *dev;
1338         struct video_device *vfd;
1339         int ret;
1340
1341         dev = devm_kzalloc(&pdev->dev, sizeof(*dev), GFP_KERNEL);
1342         if (!dev)
1343                 return -ENOMEM;
1344
1345         spin_lock_init(&dev->enc_lock);
1346         spin_lock_init(&dev->dec_lock);
1347
1348         ret = v4l2_device_register(&pdev->dev, &dev->v4l2_dev);
1349         if (ret)
1350                 return ret;
1351
1352 #ifdef CONFIG_MEDIA_CONTROLLER
1353         dev->mdev.dev = &pdev->dev;
1354         strlcpy(dev->mdev.model, "vicodec", sizeof(dev->mdev.model));
1355         media_device_init(&dev->mdev);
1356         dev->v4l2_dev.mdev = &dev->mdev;
1357 #endif
1358
1359         mutex_init(&dev->enc_mutex);
1360         mutex_init(&dev->dec_mutex);
1361
1362         platform_set_drvdata(pdev, dev);
1363
1364         dev->enc_dev = v4l2_m2m_init(&m2m_ops);
1365         if (IS_ERR(dev->enc_dev)) {
1366                 v4l2_err(&dev->v4l2_dev, "Failed to init vicodec device\n");
1367                 ret = PTR_ERR(dev->enc_dev);
1368                 goto unreg_dev;
1369         }
1370
1371         dev->dec_dev = v4l2_m2m_init(&m2m_ops);
1372         if (IS_ERR(dev->dec_dev)) {
1373                 v4l2_err(&dev->v4l2_dev, "Failed to init vicodec device\n");
1374                 ret = PTR_ERR(dev->dec_dev);
1375                 goto err_enc_m2m;
1376         }
1377
1378         dev->enc_vfd = vicodec_videodev;
1379         vfd = &dev->enc_vfd;
1380         vfd->lock = &dev->enc_mutex;
1381         vfd->v4l2_dev = &dev->v4l2_dev;
1382         strlcpy(vfd->name, "vicodec-enc", sizeof(vfd->name));
1383         v4l2_disable_ioctl(vfd, VIDIOC_DECODER_CMD);
1384         v4l2_disable_ioctl(vfd, VIDIOC_TRY_DECODER_CMD);
1385         video_set_drvdata(vfd, dev);
1386
1387         ret = video_register_device(vfd, VFL_TYPE_GRABBER, 0);
1388         if (ret) {
1389                 v4l2_err(&dev->v4l2_dev, "Failed to register video device\n");
1390                 goto err_dec_m2m;
1391         }
1392         v4l2_info(&dev->v4l2_dev,
1393                         "Device registered as /dev/video%d\n", vfd->num);
1394
1395         dev->dec_vfd = vicodec_videodev;
1396         vfd = &dev->dec_vfd;
1397         vfd->lock = &dev->dec_mutex;
1398         vfd->v4l2_dev = &dev->v4l2_dev;
1399         strlcpy(vfd->name, "vicodec-dec", sizeof(vfd->name));
1400         v4l2_disable_ioctl(vfd, VIDIOC_ENCODER_CMD);
1401         v4l2_disable_ioctl(vfd, VIDIOC_TRY_ENCODER_CMD);
1402         video_set_drvdata(vfd, dev);
1403
1404         ret = video_register_device(vfd, VFL_TYPE_GRABBER, 0);
1405         if (ret) {
1406                 v4l2_err(&dev->v4l2_dev, "Failed to register video device\n");
1407                 goto unreg_enc;
1408         }
1409         v4l2_info(&dev->v4l2_dev,
1410                         "Device registered as /dev/video%d\n", vfd->num);
1411
1412 #ifdef CONFIG_MEDIA_CONTROLLER
1413         ret = v4l2_m2m_register_media_controller(dev->enc_dev,
1414                         &dev->enc_vfd, MEDIA_ENT_F_PROC_VIDEO_ENCODER);
1415         if (ret) {
1416                 v4l2_err(&dev->v4l2_dev, "Failed to init mem2mem media controller\n");
1417                 goto unreg_m2m;
1418         }
1419
1420         ret = v4l2_m2m_register_media_controller(dev->dec_dev,
1421                         &dev->dec_vfd, MEDIA_ENT_F_PROC_VIDEO_DECODER);
1422         if (ret) {
1423                 v4l2_err(&dev->v4l2_dev, "Failed to init mem2mem media controller\n");
1424                 goto unreg_m2m_enc_mc;
1425         }
1426
1427         ret = media_device_register(&dev->mdev);
1428         if (ret) {
1429                 v4l2_err(&dev->v4l2_dev, "Failed to register mem2mem media device\n");
1430                 goto unreg_m2m_dec_mc;
1431         }
1432 #endif
1433         return 0;
1434
1435 #ifdef CONFIG_MEDIA_CONTROLLER
1436 unreg_m2m_dec_mc:
1437         v4l2_m2m_unregister_media_controller(dev->dec_dev);
1438 unreg_m2m_enc_mc:
1439         v4l2_m2m_unregister_media_controller(dev->enc_dev);
1440 unreg_m2m:
1441         video_unregister_device(&dev->dec_vfd);
1442 #endif
1443 unreg_enc:
1444         video_unregister_device(&dev->enc_vfd);
1445 err_dec_m2m:
1446         v4l2_m2m_release(dev->dec_dev);
1447 err_enc_m2m:
1448         v4l2_m2m_release(dev->enc_dev);
1449 unreg_dev:
1450         v4l2_device_unregister(&dev->v4l2_dev);
1451
1452         return ret;
1453 }
1454
1455 static int vicodec_remove(struct platform_device *pdev)
1456 {
1457         struct vicodec_dev *dev = platform_get_drvdata(pdev);
1458
1459         v4l2_info(&dev->v4l2_dev, "Removing " VICODEC_NAME);
1460
1461 #ifdef CONFIG_MEDIA_CONTROLLER
1462         media_device_unregister(&dev->mdev);
1463         v4l2_m2m_unregister_media_controller(dev->enc_dev);
1464         v4l2_m2m_unregister_media_controller(dev->dec_dev);
1465         media_device_cleanup(&dev->mdev);
1466 #endif
1467
1468         v4l2_m2m_release(dev->enc_dev);
1469         v4l2_m2m_release(dev->dec_dev);
1470         video_unregister_device(&dev->enc_vfd);
1471         video_unregister_device(&dev->dec_vfd);
1472         v4l2_device_unregister(&dev->v4l2_dev);
1473
1474         return 0;
1475 }
1476
1477 static struct platform_driver vicodec_pdrv = {
1478         .probe          = vicodec_probe,
1479         .remove         = vicodec_remove,
1480         .driver         = {
1481                 .name   = VICODEC_NAME,
1482         },
1483 };
1484
1485 static void __exit vicodec_exit(void)
1486 {
1487         platform_driver_unregister(&vicodec_pdrv);
1488         platform_device_unregister(&vicodec_pdev);
1489 }
1490
1491 static int __init vicodec_init(void)
1492 {
1493         int ret;
1494
1495         ret = platform_device_register(&vicodec_pdev);
1496         if (ret)
1497                 return ret;
1498
1499         ret = platform_driver_register(&vicodec_pdrv);
1500         if (ret)
1501                 platform_device_unregister(&vicodec_pdev);
1502
1503         return ret;
1504 }
1505
1506 module_init(vicodec_init);
1507 module_exit(vicodec_exit);