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], sz);
 442                         if (!p) {
 443                                 ctx->comp_magic_cnt = 0;
 444                                 break;
 445                         }
 446                         copy = sizeof(magic) - ctx->comp_magic_cnt;
 447                         if (p_out + sz - p < copy)
 448                                 copy = p_out + sz - p;
 449                         memcpy(ctx->compressed_frame + ctx->comp_magic_cnt,
 450                                p, copy);
 451                         ctx->comp_magic_cnt += copy;
 452                         if (!memcmp(ctx->compressed_frame, magic, ctx->comp_magic_cnt)) {
 453                                 p += copy;
 454                                 state = VB2_BUF_STATE_DONE;
 455                                 break;
 456                         }
 457                         ctx->comp_magic_cnt = 0;
 458                 }
 459                 if (ctx->comp_magic_cnt < sizeof(magic)) {
 460                         job_remove_out_buf(ctx, state);
 461                         goto restart;
 462                 }
 463                 ctx->comp_size = sizeof(magic);
 464         }
 465         if (ctx->comp_size < sizeof(struct cframe_hdr)) {
 466                 struct cframe_hdr *p_hdr = (struct cframe_hdr *)ctx->compressed_frame;
 467                 u32 copy = sizeof(struct cframe_hdr) - ctx->comp_size;
 468
 469                 if (copy > p_out + sz - p)
 470                         copy = p_out + sz - p;
 471                 memcpy(ctx->compressed_frame + ctx->comp_size,
 472                        p, copy);
 473                 p += copy;
 474                 ctx->comp_size += copy;
 475                 if (ctx->comp_size < sizeof(struct cframe_hdr)) {
 476                         job_remove_out_buf(ctx, state);
 477                         goto restart;
 478                 }
 479                 ctx->comp_frame_size = ntohl(p_hdr->size) + sizeof(*p_hdr);
 480                 if (ctx->comp_frame_size > ctx->comp_max_size)
 481                         ctx->comp_frame_size = ctx->comp_max_size;
 482         }
 483         if (ctx->comp_size < ctx->comp_frame_size) {
 484                 u32 copy = ctx->comp_frame_size - ctx->comp_size;
 485
 486                 if (copy > p_out + sz - p)
 487                         copy = p_out + sz - p;
 488                 memcpy(ctx->compressed_frame + ctx->comp_size,
 489                        p, copy);
 490                 p += copy;
 491                 ctx->comp_size += copy;
 492                 if (ctx->comp_size < ctx->comp_frame_size) {
 493                         job_remove_out_buf(ctx, state);
 494                         goto restart;
 495                 }
 496         }
 497         ctx->cur_buf_offset = p - p_out;
 498         ctx->comp_has_frame = true;
 499         ctx->comp_has_next_frame = false;
 500         if (sz - ctx->cur_buf_offset >= sizeof(struct cframe_hdr)) {
 501                 struct cframe_hdr *p_hdr = (struct cframe_hdr *)p;
 502                 u32 frame_size = ntohl(p_hdr->size);
 503                 u32 remaining = sz - ctx->cur_buf_offset - sizeof(*p_hdr);
 504
 505                 if (!memcmp(p, magic, sizeof(magic)))
 506                         ctx->comp_has_next_frame = remaining >= frame_size;
 507         }
 508         return 1;
 509 }
 510
 511 static void job_abort(void *priv)
 512 {
 513         struct vicodec_ctx *ctx = priv;
 514
 515         /* Will cancel the transaction in the next interrupt handler */
 516         ctx->aborting = 1;
 517 }
 518
 519 /*
 520  * video ioctls
 521  */
 522
 523 static u32 find_fmt(u32 fmt)
 524 {
 525         unsigned int i;
 526
 527         for (i = 0; i < ARRAY_SIZE(pixfmts_yuv); i++)
 528                 if (pixfmts_yuv[i] == fmt)
 529                         return fmt;
 530         return pixfmts_yuv[0];
 531 }
 532
 533 static int vidioc_querycap(struct file *file, void *priv,
 534                            struct v4l2_capability *cap)
 535 {
 536         strncpy(cap->driver, VICODEC_NAME, sizeof(cap->driver) - 1);
 537         strncpy(cap->card, VICODEC_NAME, sizeof(cap->card) - 1);
 538         snprintf(cap->bus_info, sizeof(cap->bus_info),
 539                         "platform:%s", VICODEC_NAME);
 540         cap->device_caps =  V4L2_CAP_STREAMING |
 541                             (multiplanar ?
 542                              V4L2_CAP_VIDEO_M2M_MPLANE :
 543                              V4L2_CAP_VIDEO_M2M);
 544         cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
 545         return 0;
 546 }
 547
 548 static int enum_fmt(struct v4l2_fmtdesc *f, bool is_enc, bool is_out)
 549 {
 550         bool is_yuv = (is_enc && is_out) || (!is_enc && !is_out);
 551
 552         if (V4L2_TYPE_IS_MULTIPLANAR(f->type) && !multiplanar)
 553                 return -EINVAL;
 554         if (!V4L2_TYPE_IS_MULTIPLANAR(f->type) && multiplanar)
 555                 return -EINVAL;
 556         if (f->index >= (is_yuv ? ARRAY_SIZE(pixfmts_yuv) : 1))
 557                 return -EINVAL;
 558
 559         if (is_yuv)
 560                 f->pixelformat = pixfmts_yuv[f->index];
 561         else
 562                 f->pixelformat = V4L2_PIX_FMT_FWHT;
 563         return 0;
 564 }
 565
 566 static int vidioc_enum_fmt_vid_cap(struct file *file, void *priv,
 567                                    struct v4l2_fmtdesc *f)
 568 {
 569         struct vicodec_ctx *ctx = file2ctx(file);
 570
 571         return enum_fmt(f, ctx->is_enc, false);
 572 }
 573
 574 static int vidioc_enum_fmt_vid_out(struct file *file, void *priv,
 575                                    struct v4l2_fmtdesc *f)
 576 {
 577         struct vicodec_ctx *ctx = file2ctx(file);
 578
 579         return enum_fmt(f, ctx->is_enc, true);
 580 }
 581
 582 static int vidioc_g_fmt(struct vicodec_ctx *ctx, struct v4l2_format *f)
 583 {
 584         struct vb2_queue *vq;
 585         struct vicodec_q_data *q_data;
 586         struct v4l2_pix_format_mplane *pix_mp;
 587         struct v4l2_pix_format *pix;
 588
 589         vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);
 590         if (!vq)
 591                 return -EINVAL;
 592
 593         q_data = get_q_data(ctx, f->type);
 594
 595         switch (f->type) {
 596         case V4L2_BUF_TYPE_VIDEO_CAPTURE:
 597         case V4L2_BUF_TYPE_VIDEO_OUTPUT:
 598                 if (multiplanar)
 599                         return -EINVAL;
 600                 pix = &f->fmt.pix;
 601                 pix->width = q_data->width;
 602                 pix->height = q_data->height;
 603                 pix->field = V4L2_FIELD_NONE;
 604                 pix->pixelformat = q_data->fourcc;
 605                 if (q_data->fourcc == V4L2_PIX_FMT_FWHT)
 606                         pix->bytesperline = 0;
 607                 else
 608                         pix->bytesperline = q_data->width;
 609                 pix->sizeimage = q_data->sizeimage;
 610                 pix->colorspace = ctx->colorspace;
 611                 pix->xfer_func = ctx->xfer_func;
 612                 pix->ycbcr_enc = ctx->ycbcr_enc;
 613                 pix->quantization = ctx->quantization;
 614                 break;
 615
 616         case V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE:
 617         case V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE:
 618                 if (!multiplanar)
 619                         return -EINVAL;
 620                 pix_mp = &f->fmt.pix_mp;
 621                 pix_mp->width = q_data->width;
 622                 pix_mp->height = q_data->height;
 623                 pix_mp->field = V4L2_FIELD_NONE;
 624                 pix_mp->pixelformat = q_data->fourcc;
 625                 pix_mp->num_planes = 1;
 626                 if (q_data->fourcc == V4L2_PIX_FMT_FWHT)
 627                         pix_mp->plane_fmt[0].bytesperline = 0;
 628                 else
 629                         pix_mp->plane_fmt[0].bytesperline = q_data->width;
 630                 pix_mp->plane_fmt[0].sizeimage = q_data->sizeimage;
 631                 pix_mp->colorspace = ctx->colorspace;
 632                 pix_mp->xfer_func = ctx->xfer_func;
 633                 pix_mp->ycbcr_enc = ctx->ycbcr_enc;
 634                 pix_mp->quantization = ctx->quantization;
 635                 memset(pix_mp->reserved, 0, sizeof(pix_mp->reserved));
 636                 memset(pix_mp->plane_fmt[0].reserved, 0,
 637                        sizeof(pix_mp->plane_fmt[0].reserved));
 638                 break;
 639         default:
 640                 return -EINVAL;
 641         }
 642         return 0;
 643 }
 644
 645 static int vidioc_g_fmt_vid_out(struct file *file, void *priv,
 646                                 struct v4l2_format *f)
 647 {
 648         return vidioc_g_fmt(file2ctx(file), f);
 649 }
 650
 651 static int vidioc_g_fmt_vid_cap(struct file *file, void *priv,
 652                                 struct v4l2_format *f)
 653 {
 654         return vidioc_g_fmt(file2ctx(file), f);
 655 }
 656
 657 static int vidioc_try_fmt(struct vicodec_ctx *ctx, struct v4l2_format *f)
 658 {
 659         struct v4l2_pix_format_mplane *pix_mp;
 660         struct v4l2_pix_format *pix;
 661
 662         switch (f->type) {
 663         case V4L2_BUF_TYPE_VIDEO_CAPTURE:
 664         case V4L2_BUF_TYPE_VIDEO_OUTPUT:
 665                 pix = &f->fmt.pix;
 666                 pix->width = clamp(pix->width, MIN_WIDTH, MAX_WIDTH) & ~7;
 667                 pix->height = clamp(pix->height, MIN_HEIGHT, MAX_HEIGHT) & ~7;
 668                 pix->bytesperline = pix->width;
 669                 pix->sizeimage = pix->width * pix->height * 3 / 2;
 670                 pix->field = V4L2_FIELD_NONE;
 671                 if (pix->pixelformat == V4L2_PIX_FMT_FWHT) {
 672                         pix->bytesperline = 0;
 673                         pix->sizeimage += sizeof(struct cframe_hdr);
 674                 }
 675                 break;
 676         case V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE:
 677         case V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE:
 678                 pix_mp = &f->fmt.pix_mp;
 679                 pix_mp->width = clamp(pix_mp->width, MIN_WIDTH, MAX_WIDTH) & ~7;
 680                 pix_mp->height =
 681                         clamp(pix_mp->height, MIN_HEIGHT, MAX_HEIGHT) & ~7;
 682                 pix_mp->plane_fmt[0].bytesperline = pix_mp->width;
 683                 pix_mp->plane_fmt[0].sizeimage =
 684                         pix_mp->width * pix_mp->height * 3 / 2;
 685                 pix_mp->field = V4L2_FIELD_NONE;
 686                 pix_mp->num_planes = 1;
 687                 if (pix_mp->pixelformat == V4L2_PIX_FMT_FWHT) {
 688                         pix_mp->plane_fmt[0].bytesperline = 0;
 689                         pix_mp->plane_fmt[0].sizeimage +=
 690                                         sizeof(struct cframe_hdr);
 691                 }
 692                 memset(pix_mp->reserved, 0, sizeof(pix_mp->reserved));
 693                 memset(pix_mp->plane_fmt[0].reserved, 0,
 694                        sizeof(pix_mp->plane_fmt[0].reserved));
 695                 break;
 696         default:
 697                 return -EINVAL;
 698         }
 699
 700         return 0;
 701 }
 702
 703 static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
 704                                   struct v4l2_format *f)
 705 {
 706         struct vicodec_ctx *ctx = file2ctx(file);
 707         struct v4l2_pix_format_mplane *pix_mp;
 708         struct v4l2_pix_format *pix;
 709
 710         switch (f->type) {
 711         case V4L2_BUF_TYPE_VIDEO_CAPTURE:
 712                 if (multiplanar)
 713                         return -EINVAL;
 714                 pix = &f->fmt.pix;
 715                 pix->pixelformat = ctx->is_enc ? V4L2_PIX_FMT_FWHT :
 716                                    find_fmt(f->fmt.pix.pixelformat);
 717                 pix->colorspace = ctx->colorspace;
 718                 pix->xfer_func = ctx->xfer_func;
 719                 pix->ycbcr_enc = ctx->ycbcr_enc;
 720                 pix->quantization = ctx->quantization;
 721                 break;
 722         case V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE:
 723                 if (!multiplanar)
 724                         return -EINVAL;
 725                 pix_mp = &f->fmt.pix_mp;
 726                 pix_mp->pixelformat = ctx->is_enc ? V4L2_PIX_FMT_FWHT :
 727                                       find_fmt(pix_mp->pixelformat);
 728                 pix_mp->colorspace = ctx->colorspace;
 729                 pix_mp->xfer_func = ctx->xfer_func;
 730                 pix_mp->ycbcr_enc = ctx->ycbcr_enc;
 731                 pix_mp->quantization = ctx->quantization;
 732                 memset(pix_mp->reserved, 0, sizeof(pix_mp->reserved));
 733                 memset(pix_mp->plane_fmt[0].reserved, 0,
 734                        sizeof(pix_mp->plane_fmt[0].reserved));
 735                 break;
 736         default:
 737                 return -EINVAL;
 738         }
 739
 740         return vidioc_try_fmt(ctx, f);
 741 }
 742
 743 static int vidioc_try_fmt_vid_out(struct file *file, void *priv,
 744                                   struct v4l2_format *f)
 745 {
 746         struct vicodec_ctx *ctx = file2ctx(file);
 747         struct v4l2_pix_format_mplane *pix_mp;
 748         struct v4l2_pix_format *pix;
 749
 750         switch (f->type) {
 751         case V4L2_BUF_TYPE_VIDEO_OUTPUT:
 752                 if (multiplanar)
 753                         return -EINVAL;
 754                 pix = &f->fmt.pix;
 755                 pix->pixelformat = !ctx->is_enc ? V4L2_PIX_FMT_FWHT :
 756                                    find_fmt(pix->pixelformat);
 757                 if (!pix->colorspace)
 758                         pix->colorspace = V4L2_COLORSPACE_REC709;
 759                 break;
 760         case V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE:
 761                 if (!multiplanar)
 762                         return -EINVAL;
 763                 pix_mp = &f->fmt.pix_mp;
 764                 pix_mp->pixelformat = !ctx->is_enc ? V4L2_PIX_FMT_FWHT :
 765                                       find_fmt(pix_mp->pixelformat);
 766                 if (!pix_mp->colorspace)
 767                         pix_mp->colorspace = V4L2_COLORSPACE_REC709;
 768                 break;
 769         default:
 770                 return -EINVAL;
 771         }
 772
 773         return vidioc_try_fmt(ctx, f);
 774 }
 775
 776 static int vidioc_s_fmt(struct vicodec_ctx *ctx, struct v4l2_format *f)
 777 {
 778         struct vicodec_q_data *q_data;
 779         struct vb2_queue *vq;
 780         bool fmt_changed = true;
 781         struct v4l2_pix_format_mplane *pix_mp;
 782         struct v4l2_pix_format *pix;
 783
 784         vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);
 785         if (!vq)
 786                 return -EINVAL;
 787
 788         q_data = get_q_data(ctx, f->type);
 789         if (!q_data)
 790                 return -EINVAL;
 791
 792         switch (f->type) {
 793         case V4L2_BUF_TYPE_VIDEO_CAPTURE:
 794         case V4L2_BUF_TYPE_VIDEO_OUTPUT:
 795                 pix = &f->fmt.pix;
 796                 if (ctx->is_enc && V4L2_TYPE_IS_OUTPUT(f->type))
 797                         fmt_changed =
 798                                 q_data->fourcc != pix->pixelformat ||
 799                                 q_data->width != pix->width ||
 800                                 q_data->height != pix->height;
 801
 802                 if (vb2_is_busy(vq) && fmt_changed)
 803                         return -EBUSY;
 804
 805                 q_data->fourcc = pix->pixelformat;
 806                 q_data->width = pix->width;
 807                 q_data->height = pix->height;
 808                 q_data->sizeimage = pix->sizeimage;
 809                 break;
 810         case V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE:
 811         case V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE:
 812                 pix_mp = &f->fmt.pix_mp;
 813                 if (ctx->is_enc && V4L2_TYPE_IS_OUTPUT(f->type))
 814                         fmt_changed =
 815                                 q_data->fourcc != pix_mp->pixelformat ||
 816                                 q_data->width != pix_mp->width ||
 817                                 q_data->height != pix_mp->height;
 818
 819                 if (vb2_is_busy(vq) && fmt_changed)
 820                         return -EBUSY;
 821
 822                 q_data->fourcc = pix_mp->pixelformat;
 823                 q_data->width = pix_mp->width;
 824                 q_data->height = pix_mp->height;
 825                 q_data->sizeimage = pix_mp->plane_fmt[0].sizeimage;
 826                 break;
 827         default:
 828                 return -EINVAL;
 829         }
 830
 831         dprintk(ctx->dev,
 832                 "Setting format for type %d, wxh: %dx%d, fourcc: %08x\n",
 833                 f->type, q_data->width, q_data->height, q_data->fourcc);
 834
 835         return 0;
 836 }
 837
 838 static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
 839                                 struct v4l2_format *f)
 840 {
 841         int ret;
 842
 843         ret = vidioc_try_fmt_vid_cap(file, priv, f);
 844         if (ret)
 845                 return ret;
 846
 847         return vidioc_s_fmt(file2ctx(file), f);
 848 }
 849
 850 static int vidioc_s_fmt_vid_out(struct file *file, void *priv,
 851                                 struct v4l2_format *f)
 852 {
 853         struct vicodec_ctx *ctx = file2ctx(file);
 854         struct v4l2_pix_format_mplane *pix_mp;
 855         struct v4l2_pix_format *pix;
 856         int ret;
 857
 858         ret = vidioc_try_fmt_vid_out(file, priv, f);
 859         if (ret)
 860                 return ret;
 861
 862         ret = vidioc_s_fmt(file2ctx(file), f);
 863         if (!ret) {
 864                 switch (f->type) {
 865                 case V4L2_BUF_TYPE_VIDEO_CAPTURE:
 866                 case V4L2_BUF_TYPE_VIDEO_OUTPUT:
 867                         pix = &f->fmt.pix;
 868                         ctx->colorspace = pix->colorspace;
 869                         ctx->xfer_func = pix->xfer_func;
 870                         ctx->ycbcr_enc = pix->ycbcr_enc;
 871                         ctx->quantization = pix->quantization;
 872                         break;
 873                 case V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE:
 874                 case V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE:
 875                         pix_mp = &f->fmt.pix_mp;
 876                         ctx->colorspace = pix_mp->colorspace;
 877                         ctx->xfer_func = pix_mp->xfer_func;
 878                         ctx->ycbcr_enc = pix_mp->ycbcr_enc;
 879                         ctx->quantization = pix_mp->quantization;
 880                         break;
 881                 default:
 882                         break;
 883                 }
 884         }
 885         return ret;
 886 }
 887
 888 static void vicodec_mark_last_buf(struct vicodec_ctx *ctx)
 889 {
 890         static const struct v4l2_event eos_event = {
 891                 .type = V4L2_EVENT_EOS
 892         };
 893
 894         spin_lock(ctx->lock);
 895         ctx->last_src_buf = v4l2_m2m_last_src_buf(ctx->fh.m2m_ctx);
 896         if (!ctx->last_src_buf && ctx->last_dst_buf) {
 897                 ctx->last_dst_buf->flags |= V4L2_BUF_FLAG_LAST;
 898                 v4l2_event_queue_fh(&ctx->fh, &eos_event);
 899         }
 900         spin_unlock(ctx->lock);
 901 }
 902
 903 static int vicodec_try_encoder_cmd(struct file *file, void *fh,
 904                                 struct v4l2_encoder_cmd *ec)
 905 {
 906         if (ec->cmd != V4L2_ENC_CMD_STOP)
 907                 return -EINVAL;
 908
 909         if (ec->flags & V4L2_ENC_CMD_STOP_AT_GOP_END)
 910                 return -EINVAL;
 911
 912         return 0;
 913 }
 914
 915 static int vicodec_encoder_cmd(struct file *file, void *fh,
 916                             struct v4l2_encoder_cmd *ec)
 917 {
 918         struct vicodec_ctx *ctx = file2ctx(file);
 919         int ret;
 920
 921         ret = vicodec_try_encoder_cmd(file, fh, ec);
 922         if (ret < 0)
 923                 return ret;
 924
 925         vicodec_mark_last_buf(ctx);
 926         return 0;
 927 }
 928
 929 static int vicodec_try_decoder_cmd(struct file *file, void *fh,
 930                                 struct v4l2_decoder_cmd *dc)
 931 {
 932         if (dc->cmd != V4L2_DEC_CMD_STOP)
 933                 return -EINVAL;
 934
 935         if (dc->flags & V4L2_DEC_CMD_STOP_TO_BLACK)
 936                 return -EINVAL;
 937
 938         if (!(dc->flags & V4L2_DEC_CMD_STOP_IMMEDIATELY) && (dc->stop.pts != 0))
 939                 return -EINVAL;
 940
 941         return 0;
 942 }
 943
 944 static int vicodec_decoder_cmd(struct file *file, void *fh,
 945                             struct v4l2_decoder_cmd *dc)
 946 {
 947         struct vicodec_ctx *ctx = file2ctx(file);
 948         int ret;
 949
 950         ret = vicodec_try_decoder_cmd(file, fh, dc);
 951         if (ret < 0)
 952                 return ret;
 953
 954         vicodec_mark_last_buf(ctx);
 955         return 0;
 956 }
 957
 958 static int vicodec_enum_framesizes(struct file *file, void *fh,
 959                                    struct v4l2_frmsizeenum *fsize)
 960 {
 961         switch (fsize->pixel_format) {
 962         case V4L2_PIX_FMT_FWHT:
 963                 break;
 964         default:
 965                 if (find_fmt(fsize->pixel_format) == fsize->pixel_format)
 966                         break;
 967                 return -EINVAL;
 968         }
 969
 970         if (fsize->index)
 971                 return -EINVAL;
 972
 973         fsize->type = V4L2_FRMSIZE_TYPE_STEPWISE;
 974
 975         fsize->stepwise.min_width = MIN_WIDTH;
 976         fsize->stepwise.max_width = MAX_WIDTH;
 977         fsize->stepwise.step_width = 8;
 978         fsize->stepwise.min_height = MIN_HEIGHT;
 979         fsize->stepwise.max_height = MAX_HEIGHT;
 980         fsize->stepwise.step_height = 8;
 981
 982         return 0;
 983 }
 984
 985 static int vicodec_subscribe_event(struct v4l2_fh *fh,
 986                                 const struct v4l2_event_subscription *sub)
 987 {
 988         switch (sub->type) {
 989         case V4L2_EVENT_EOS:
 990                 return v4l2_event_subscribe(fh, sub, 0, NULL);
 991         default:
 992                 return v4l2_ctrl_subscribe_event(fh, sub);
 993         }
 994 }
 995
 996 static const struct v4l2_ioctl_ops vicodec_ioctl_ops = {
 997         .vidioc_querycap        = vidioc_querycap,
 998
 999         .vidioc_enum_fmt_vid_cap = vidioc_enum_fmt_vid_cap,
1000         .vidioc_g_fmt_vid_cap   = vidioc_g_fmt_vid_cap,
1001         .vidioc_try_fmt_vid_cap = vidioc_try_fmt_vid_cap,
1002         .vidioc_s_fmt_vid_cap   = vidioc_s_fmt_vid_cap,
1003
1004         .vidioc_enum_fmt_vid_cap_mplane = vidioc_enum_fmt_vid_cap,
1005         .vidioc_g_fmt_vid_cap_mplane    = vidioc_g_fmt_vid_cap,
1006         .vidioc_try_fmt_vid_cap_mplane  = vidioc_try_fmt_vid_cap,
1007         .vidioc_s_fmt_vid_cap_mplane    = vidioc_s_fmt_vid_cap,
1008
1009         .vidioc_enum_fmt_vid_out = vidioc_enum_fmt_vid_out,
1010         .vidioc_g_fmt_vid_out   = vidioc_g_fmt_vid_out,
1011         .vidioc_try_fmt_vid_out = vidioc_try_fmt_vid_out,
1012         .vidioc_s_fmt_vid_out   = vidioc_s_fmt_vid_out,
1013
1014         .vidioc_enum_fmt_vid_out_mplane = vidioc_enum_fmt_vid_out,
1015         .vidioc_g_fmt_vid_out_mplane    = vidioc_g_fmt_vid_out,
1016         .vidioc_try_fmt_vid_out_mplane  = vidioc_try_fmt_vid_out,
1017         .vidioc_s_fmt_vid_out_mplane    = vidioc_s_fmt_vid_out,
1018
1019         .vidioc_reqbufs         = v4l2_m2m_ioctl_reqbufs,
1020         .vidioc_querybuf        = v4l2_m2m_ioctl_querybuf,
1021         .vidioc_qbuf            = v4l2_m2m_ioctl_qbuf,
1022         .vidioc_dqbuf           = v4l2_m2m_ioctl_dqbuf,
1023         .vidioc_prepare_buf     = v4l2_m2m_ioctl_prepare_buf,
1024         .vidioc_create_bufs     = v4l2_m2m_ioctl_create_bufs,
1025         .vidioc_expbuf          = v4l2_m2m_ioctl_expbuf,
1026
1027         .vidioc_streamon        = v4l2_m2m_ioctl_streamon,
1028         .vidioc_streamoff       = v4l2_m2m_ioctl_streamoff,
1029
1030         .vidioc_try_encoder_cmd = vicodec_try_encoder_cmd,
1031         .vidioc_encoder_cmd     = vicodec_encoder_cmd,
1032         .vidioc_try_decoder_cmd = vicodec_try_decoder_cmd,
1033         .vidioc_decoder_cmd     = vicodec_decoder_cmd,
1034         .vidioc_enum_framesizes = vicodec_enum_framesizes,
1035
1036         .vidioc_subscribe_event = vicodec_subscribe_event,
1037         .vidioc_unsubscribe_event = v4l2_event_unsubscribe,
1038 };
1039
1040
1041 /*
1042  * Queue operations
1043  */
1044
1045 static int vicodec_queue_setup(struct vb2_queue *vq, unsigned int *nbuffers,
1046                                unsigned int *nplanes, unsigned int sizes[],
1047                                struct device *alloc_devs[])
1048 {
1049         struct vicodec_ctx *ctx = vb2_get_drv_priv(vq);
1050         struct vicodec_q_data *q_data = get_q_data(ctx, vq->type);
1051         unsigned int size = q_data->sizeimage;
1052
1053         if (*nplanes)
1054                 return sizes[0] < size ? -EINVAL : 0;
1055
1056         *nplanes = 1;
1057         sizes[0] = size;
1058         return 0;
1059 }
1060
1061 static int vicodec_buf_prepare(struct vb2_buffer *vb)
1062 {
1063         struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
1064         struct vicodec_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
1065         struct vicodec_q_data *q_data;
1066
1067         dprintk(ctx->dev, "type: %d\n", vb->vb2_queue->type);
1068
1069         q_data = get_q_data(ctx, vb->vb2_queue->type);
1070         if (V4L2_TYPE_IS_OUTPUT(vb->vb2_queue->type)) {
1071                 if (vbuf->field == V4L2_FIELD_ANY)
1072                         vbuf->field = V4L2_FIELD_NONE;
1073                 if (vbuf->field != V4L2_FIELD_NONE) {
1074                         dprintk(ctx->dev, "%s field isn't supported\n",
1075                                         __func__);
1076                         return -EINVAL;
1077                 }
1078         }
1079
1080         if (vb2_plane_size(vb, 0) < q_data->sizeimage) {
1081                 dprintk(ctx->dev,
1082                         "%s data will not fit into plane (%lu < %lu)\n",
1083                         __func__, vb2_plane_size(vb, 0),
1084                         (long)q_data->sizeimage);
1085                 return -EINVAL;
1086         }
1087
1088         return 0;
1089 }
1090
1091 static void vicodec_buf_queue(struct vb2_buffer *vb)
1092 {
1093         struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
1094         struct vicodec_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
1095
1096         v4l2_m2m_buf_queue(ctx->fh.m2m_ctx, vbuf);
1097 }
1098
1099 static void vicodec_return_bufs(struct vb2_queue *q, u32 state)
1100 {
1101         struct vicodec_ctx *ctx = vb2_get_drv_priv(q);
1102         struct vb2_v4l2_buffer *vbuf;
1103
1104         for (;;) {
1105                 if (V4L2_TYPE_IS_OUTPUT(q->type))
1106                         vbuf = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
1107                 else
1108                         vbuf = v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
1109                 if (vbuf == NULL)
1110                         return;
1111                 spin_lock(ctx->lock);
1112                 v4l2_m2m_buf_done(vbuf, state);
1113                 spin_unlock(ctx->lock);
1114         }
1115 }
1116
1117 static int vicodec_start_streaming(struct vb2_queue *q,
1118                                    unsigned int count)
1119 {
1120         struct vicodec_ctx *ctx = vb2_get_drv_priv(q);
1121         struct vicodec_q_data *q_data = get_q_data(ctx, q->type);
1122         unsigned int size = q_data->width * q_data->height;
1123
1124         q_data->sequence = 0;
1125
1126         if (!V4L2_TYPE_IS_OUTPUT(q->type))
1127                 return 0;
1128
1129         ctx->ref_frame.width = ctx->ref_frame.height = 0;
1130         ctx->ref_frame.luma = kvmalloc(size * 3 / 2, GFP_KERNEL);
1131         ctx->comp_max_size = size * 3 / 2 + sizeof(struct cframe_hdr);
1132         ctx->compressed_frame = kvmalloc(ctx->comp_max_size, GFP_KERNEL);
1133         if (!ctx->ref_frame.luma || !ctx->compressed_frame) {
1134                 kvfree(ctx->ref_frame.luma);
1135                 kvfree(ctx->compressed_frame);
1136                 vicodec_return_bufs(q, VB2_BUF_STATE_QUEUED);
1137                 return -ENOMEM;
1138         }
1139         ctx->ref_frame.cb = ctx->ref_frame.luma + size;
1140         ctx->ref_frame.cr = ctx->ref_frame.cb + size / 4;
1141         ctx->last_src_buf = NULL;
1142         ctx->last_dst_buf = NULL;
1143         v4l2_ctrl_grab(ctx->ctrl_gop_size, true);
1144         ctx->gop_size = v4l2_ctrl_g_ctrl(ctx->ctrl_gop_size);
1145         ctx->gop_cnt = 0;
1146         ctx->cur_buf_offset = 0;
1147         ctx->comp_size = 0;
1148         ctx->comp_magic_cnt = 0;
1149         ctx->comp_has_frame = false;
1150
1151         return 0;
1152 }
1153
1154 static void vicodec_stop_streaming(struct vb2_queue *q)
1155 {
1156         struct vicodec_ctx *ctx = vb2_get_drv_priv(q);
1157
1158         vicodec_return_bufs(q, VB2_BUF_STATE_ERROR);
1159
1160         if (!V4L2_TYPE_IS_OUTPUT(q->type))
1161                 return;
1162
1163         kvfree(ctx->ref_frame.luma);
1164         kvfree(ctx->compressed_frame);
1165         v4l2_ctrl_grab(ctx->ctrl_gop_size, false);
1166 }
1167
1168 static const struct vb2_ops vicodec_qops = {
1169         .queue_setup     = vicodec_queue_setup,
1170         .buf_prepare     = vicodec_buf_prepare,
1171         .buf_queue       = vicodec_buf_queue,
1172         .start_streaming = vicodec_start_streaming,
1173         .stop_streaming  = vicodec_stop_streaming,
1174         .wait_prepare    = vb2_ops_wait_prepare,
1175         .wait_finish     = vb2_ops_wait_finish,
1176 };
1177
1178 static int queue_init(void *priv, struct vb2_queue *src_vq,
1179                       struct vb2_queue *dst_vq)
1180 {
1181         struct vicodec_ctx *ctx = priv;
1182         int ret;
1183
1184         src_vq->type = (multiplanar ?
1185                         V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE :
1186                         V4L2_BUF_TYPE_VIDEO_OUTPUT);
1187         src_vq->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF;
1188         src_vq->drv_priv = ctx;
1189         src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
1190         src_vq->ops = &vicodec_qops;
1191         src_vq->mem_ops = &vb2_vmalloc_memops;
1192         src_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
1193         src_vq->lock = ctx->is_enc ? &ctx->dev->enc_mutex :
1194                 &ctx->dev->dec_mutex;
1195
1196         ret = vb2_queue_init(src_vq);
1197         if (ret)
1198                 return ret;
1199
1200         dst_vq->type = (multiplanar ?
1201                         V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE :
1202                         V4L2_BUF_TYPE_VIDEO_CAPTURE);
1203         dst_vq->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF;
1204         dst_vq->drv_priv = ctx;
1205         dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
1206         dst_vq->ops = &vicodec_qops;
1207         dst_vq->mem_ops = &vb2_vmalloc_memops;
1208         dst_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
1209         dst_vq->lock = src_vq->lock;
1210
1211         return vb2_queue_init(dst_vq);
1212 }
1213
1214 /*
1215  * File operations
1216  */
1217 static int vicodec_open(struct file *file)
1218 {
1219         struct video_device *vfd = video_devdata(file);
1220         struct vicodec_dev *dev = video_drvdata(file);
1221         struct vicodec_ctx *ctx = NULL;
1222         struct v4l2_ctrl_handler *hdl;
1223         unsigned int size;
1224         int rc = 0;
1225
1226         if (mutex_lock_interruptible(vfd->lock))
1227                 return -ERESTARTSYS;
1228         ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
1229         if (!ctx) {
1230                 rc = -ENOMEM;
1231                 goto open_unlock;
1232         }
1233
1234         if (vfd == &dev->enc_vfd)
1235                 ctx->is_enc = true;
1236
1237         v4l2_fh_init(&ctx->fh, video_devdata(file));
1238         file->private_data = &ctx->fh;
1239         ctx->dev = dev;
1240         hdl = &ctx->hdl;
1241         v4l2_ctrl_handler_init(hdl, 4);
1242         ctx->ctrl_gop_size = v4l2_ctrl_new_std(hdl, NULL,
1243                                                V4L2_CID_MPEG_VIDEO_GOP_SIZE,
1244                                                1, 16, 1, 10);
1245         if (hdl->error) {
1246                 rc = hdl->error;
1247                 v4l2_ctrl_handler_free(hdl);
1248                 kfree(ctx);
1249                 goto open_unlock;
1250         }
1251         ctx->fh.ctrl_handler = hdl;
1252         v4l2_ctrl_handler_setup(hdl);
1253
1254         ctx->q_data[V4L2_M2M_SRC].fourcc =
1255                 ctx->is_enc ? V4L2_PIX_FMT_YUV420 : V4L2_PIX_FMT_FWHT;
1256         ctx->q_data[V4L2_M2M_SRC].width = 1280;
1257         ctx->q_data[V4L2_M2M_SRC].height = 720;
1258         size = 1280 * 720 * 3 / 2;
1259         ctx->q_data[V4L2_M2M_SRC].sizeimage = size;
1260         ctx->q_data[V4L2_M2M_DST] = ctx->q_data[V4L2_M2M_SRC];
1261         ctx->q_data[V4L2_M2M_DST].fourcc =
1262                 ctx->is_enc ? V4L2_PIX_FMT_FWHT : V4L2_PIX_FMT_YUV420;
1263         ctx->colorspace = V4L2_COLORSPACE_REC709;
1264
1265         size += sizeof(struct cframe_hdr);
1266         if (ctx->is_enc) {
1267                 ctx->q_data[V4L2_M2M_DST].sizeimage = size;
1268                 ctx->fh.m2m_ctx = v4l2_m2m_ctx_init(dev->enc_dev, ctx,
1269                                                     &queue_init);
1270                 ctx->lock = &dev->enc_lock;
1271         } else {
1272                 ctx->q_data[V4L2_M2M_SRC].sizeimage = size;
1273                 ctx->fh.m2m_ctx = v4l2_m2m_ctx_init(dev->dec_dev, ctx,
1274                                                     &queue_init);
1275                 ctx->lock = &dev->dec_lock;
1276         }
1277
1278         if (IS_ERR(ctx->fh.m2m_ctx)) {
1279                 rc = PTR_ERR(ctx->fh.m2m_ctx);
1280
1281                 v4l2_ctrl_handler_free(hdl);
1282                 v4l2_fh_exit(&ctx->fh);
1283                 kfree(ctx);
1284                 goto open_unlock;
1285         }
1286
1287         v4l2_fh_add(&ctx->fh);
1288
1289 open_unlock:
1290         mutex_unlock(vfd->lock);
1291         return rc;
1292 }
1293
1294 static int vicodec_release(struct file *file)
1295 {
1296         struct video_device *vfd = video_devdata(file);
1297         struct vicodec_ctx *ctx = file2ctx(file);
1298
1299         v4l2_fh_del(&ctx->fh);
1300         v4l2_fh_exit(&ctx->fh);
1301         v4l2_ctrl_handler_free(&ctx->hdl);
1302         mutex_lock(vfd->lock);
1303         v4l2_m2m_ctx_release(ctx->fh.m2m_ctx);
1304         mutex_unlock(vfd->lock);
1305         kfree(ctx);
1306
1307         return 0;
1308 }
1309
1310 static const struct v4l2_file_operations vicodec_fops = {
1311         .owner          = THIS_MODULE,
1312         .open           = vicodec_open,
1313         .release        = vicodec_release,
1314         .poll           = v4l2_m2m_fop_poll,
1315         .unlocked_ioctl = video_ioctl2,
1316         .mmap           = v4l2_m2m_fop_mmap,
1317 };
1318
1319 static const struct video_device vicodec_videodev = {
1320         .name           = VICODEC_NAME,
1321         .vfl_dir        = VFL_DIR_M2M,
1322         .fops           = &vicodec_fops,
1323         .ioctl_ops      = &vicodec_ioctl_ops,
1324         .minor          = -1,
1325         .release        = video_device_release_empty,
1326 };
1327
1328 static const struct v4l2_m2m_ops m2m_ops = {
1329         .device_run     = device_run,
1330         .job_abort      = job_abort,
1331         .job_ready      = job_ready,
1332 };
1333
1334 static int vicodec_probe(struct platform_device *pdev)
1335 {
1336         struct vicodec_dev *dev;
1337         struct video_device *vfd;
1338         int ret;
1339
1340         dev = devm_kzalloc(&pdev->dev, sizeof(*dev), GFP_KERNEL);
1341         if (!dev)
1342                 return -ENOMEM;
1343
1344         spin_lock_init(&dev->enc_lock);
1345         spin_lock_init(&dev->dec_lock);
1346
1347         ret = v4l2_device_register(&pdev->dev, &dev->v4l2_dev);
1348         if (ret)
1349                 return ret;
1350
1351 #ifdef CONFIG_MEDIA_CONTROLLER
1352         dev->mdev.dev = &pdev->dev;
1353         strlcpy(dev->mdev.model, "vicodec", sizeof(dev->mdev.model));
1354         media_device_init(&dev->mdev);
1355         dev->v4l2_dev.mdev = &dev->mdev;
1356 #endif
1357
1358         mutex_init(&dev->enc_mutex);
1359         mutex_init(&dev->dec_mutex);
1360
1361         platform_set_drvdata(pdev, dev);
1362
1363         dev->enc_dev = v4l2_m2m_init(&m2m_ops);
1364         if (IS_ERR(dev->enc_dev)) {
1365                 v4l2_err(&dev->v4l2_dev, "Failed to init vicodec device\n");
1366                 ret = PTR_ERR(dev->enc_dev);
1367                 goto unreg_dev;
1368         }
1369
1370         dev->dec_dev = v4l2_m2m_init(&m2m_ops);
1371         if (IS_ERR(dev->dec_dev)) {
1372                 v4l2_err(&dev->v4l2_dev, "Failed to init vicodec device\n");
1373                 ret = PTR_ERR(dev->dec_dev);
1374                 goto err_enc_m2m;
1375         }
1376
1377         dev->enc_vfd = vicodec_videodev;
1378         vfd = &dev->enc_vfd;
1379         vfd->lock = &dev->enc_mutex;
1380         vfd->v4l2_dev = &dev->v4l2_dev;
1381         strlcpy(vfd->name, "vicodec-enc", sizeof(vfd->name));
1382         v4l2_disable_ioctl(vfd, VIDIOC_DECODER_CMD);
1383         v4l2_disable_ioctl(vfd, VIDIOC_TRY_DECODER_CMD);
1384         video_set_drvdata(vfd, dev);
1385
1386         ret = video_register_device(vfd, VFL_TYPE_GRABBER, 0);
1387         if (ret) {
1388                 v4l2_err(&dev->v4l2_dev, "Failed to register video device\n");
1389                 goto err_dec_m2m;
1390         }
1391         v4l2_info(&dev->v4l2_dev,
1392                         "Device registered as /dev/video%d\n", vfd->num);
1393
1394         dev->dec_vfd = vicodec_videodev;
1395         vfd = &dev->dec_vfd;
1396         vfd->lock = &dev->dec_mutex;
1397         vfd->v4l2_dev = &dev->v4l2_dev;
1398         strlcpy(vfd->name, "vicodec-dec", sizeof(vfd->name));
1399         v4l2_disable_ioctl(vfd, VIDIOC_ENCODER_CMD);
1400         v4l2_disable_ioctl(vfd, VIDIOC_TRY_ENCODER_CMD);
1401         video_set_drvdata(vfd, dev);
1402
1403         ret = video_register_device(vfd, VFL_TYPE_GRABBER, 0);
1404         if (ret) {
1405                 v4l2_err(&dev->v4l2_dev, "Failed to register video device\n");
1406                 goto unreg_enc;
1407         }
1408         v4l2_info(&dev->v4l2_dev,
1409                         "Device registered as /dev/video%d\n", vfd->num);
1410
1411 #ifdef CONFIG_MEDIA_CONTROLLER
1412         ret = v4l2_m2m_register_media_controller(dev->enc_dev,
1413                         &dev->enc_vfd, MEDIA_ENT_F_PROC_VIDEO_ENCODER);
1414         if (ret) {
1415                 v4l2_err(&dev->v4l2_dev, "Failed to init mem2mem media controller\n");
1416                 goto unreg_m2m;
1417         }
1418
1419         ret = v4l2_m2m_register_media_controller(dev->dec_dev,
1420                         &dev->dec_vfd, MEDIA_ENT_F_PROC_VIDEO_DECODER);
1421         if (ret) {
1422                 v4l2_err(&dev->v4l2_dev, "Failed to init mem2mem media controller\n");
1423                 goto unreg_m2m_enc_mc;
1424         }
1425
1426         ret = media_device_register(&dev->mdev);
1427         if (ret) {
1428                 v4l2_err(&dev->v4l2_dev, "Failed to register mem2mem media device\n");
1429                 goto unreg_m2m_dec_mc;
1430         }
1431 #endif
1432         return 0;
1433
1434 #ifdef CONFIG_MEDIA_CONTROLLER
1435 unreg_m2m_dec_mc:
1436         v4l2_m2m_unregister_media_controller(dev->dec_dev);
1437 unreg_m2m_enc_mc:
1438         v4l2_m2m_unregister_media_controller(dev->enc_dev);
1439 unreg_m2m:
1440         video_unregister_device(&dev->dec_vfd);
1441 #endif
1442 unreg_enc:
1443         video_unregister_device(&dev->enc_vfd);
1444 err_dec_m2m:
1445         v4l2_m2m_release(dev->dec_dev);
1446 err_enc_m2m:
1447         v4l2_m2m_release(dev->enc_dev);
1448 unreg_dev:
1449         v4l2_device_unregister(&dev->v4l2_dev);
1450
1451         return ret;
1452 }
1453
1454 static int vicodec_remove(struct platform_device *pdev)
1455 {
1456         struct vicodec_dev *dev = platform_get_drvdata(pdev);
1457
1458         v4l2_info(&dev->v4l2_dev, "Removing " VICODEC_NAME);
1459
1460 #ifdef CONFIG_MEDIA_CONTROLLER
1461         media_device_unregister(&dev->mdev);
1462         v4l2_m2m_unregister_media_controller(dev->enc_dev);
1463         v4l2_m2m_unregister_media_controller(dev->dec_dev);
1464         media_device_cleanup(&dev->mdev);
1465 #endif
1466
1467         v4l2_m2m_release(dev->enc_dev);
1468         v4l2_m2m_release(dev->dec_dev);
1469         video_unregister_device(&dev->enc_vfd);
1470         video_unregister_device(&dev->dec_vfd);
1471         v4l2_device_unregister(&dev->v4l2_dev);
1472
1473         return 0;
1474 }
1475
1476 static struct platform_driver vicodec_pdrv = {
1477         .probe          = vicodec_probe,
1478         .remove         = vicodec_remove,
1479         .driver         = {
1480                 .name   = VICODEC_NAME,
1481         },
1482 };
1483
1484 static void __exit vicodec_exit(void)
1485 {
1486         platform_driver_unregister(&vicodec_pdrv);
1487         platform_device_unregister(&vicodec_pdev);
1488 }
1489
1490 static int __init vicodec_init(void)
1491 {
1492         int ret;
1493
1494         ret = platform_device_register(&vicodec_pdev);
1495         if (ret)
1496                 return ret;
1497
1498         ret = platform_driver_register(&vicodec_pdrv);
1499         if (ret)
1500                 platform_device_unregister(&vicodec_pdev);
1501
1502         return ret;
1503 }
1504
1505 module_init(vicodec_init);
1506 module_exit(vicodec_exit);