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
vicodec_dev_release(struct device * dev)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
file2ctx(struct file * file)133 static inline struct vicodec_ctx *file2ctx(struct file *file)
134 {
135 return container_of(file->private_data, struct vicodec_ctx, fh);
136 }
137
get_q_data(struct vicodec_ctx * ctx,enum v4l2_buf_type type)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
encode(struct vicodec_ctx * ctx,struct vicodec_q_data * q_data,u8 * p_in,u8 * p_out)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
decode(struct vicodec_ctx * ctx,struct vicodec_q_data * q_data,u8 * p_in,u8 * p_out)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
device_process(struct vicodec_ctx * ctx,struct vb2_v4l2_buffer * in_vb,struct vb2_v4l2_buffer * out_vb)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 */
device_run(void * priv)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
job_remove_out_buf(struct vicodec_ctx * ctx,u32 state)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
job_ready(void * priv)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
job_abort(void * priv)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
find_fmt(u32 fmt)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
vidioc_querycap(struct file * file,void * priv,struct v4l2_capability * cap)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
enum_fmt(struct v4l2_fmtdesc * f,bool is_enc,bool is_out)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
vidioc_enum_fmt_vid_cap(struct file * file,void * priv,struct v4l2_fmtdesc * f)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
vidioc_enum_fmt_vid_out(struct file * file,void * priv,struct v4l2_fmtdesc * f)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
vidioc_g_fmt(struct vicodec_ctx * ctx,struct v4l2_format * f)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
vidioc_g_fmt_vid_out(struct file * file,void * priv,struct v4l2_format * f)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
vidioc_g_fmt_vid_cap(struct file * file,void * priv,struct v4l2_format * f)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
vidioc_try_fmt(struct vicodec_ctx * ctx,struct v4l2_format * f)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
vidioc_try_fmt_vid_cap(struct file * file,void * priv,struct v4l2_format * f)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
vidioc_try_fmt_vid_out(struct file * file,void * priv,struct v4l2_format * f)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
vidioc_s_fmt(struct vicodec_ctx * ctx,struct v4l2_format * f)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
vidioc_s_fmt_vid_cap(struct file * file,void * priv,struct v4l2_format * f)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
vidioc_s_fmt_vid_out(struct file * file,void * priv,struct v4l2_format * f)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
vicodec_mark_last_buf(struct vicodec_ctx * ctx)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
vicodec_try_encoder_cmd(struct file * file,void * fh,struct v4l2_encoder_cmd * ec)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
vicodec_encoder_cmd(struct file * file,void * fh,struct v4l2_encoder_cmd * ec)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
vicodec_try_decoder_cmd(struct file * file,void * fh,struct v4l2_decoder_cmd * dc)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
vicodec_decoder_cmd(struct file * file,void * fh,struct v4l2_decoder_cmd * dc)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
vicodec_enum_framesizes(struct file * file,void * fh,struct v4l2_frmsizeenum * fsize)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
vicodec_subscribe_event(struct v4l2_fh * fh,const struct v4l2_event_subscription * sub)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
vicodec_queue_setup(struct vb2_queue * vq,unsigned int * nbuffers,unsigned int * nplanes,unsigned int sizes[],struct device * alloc_devs[])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
vicodec_buf_prepare(struct vb2_buffer * vb)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
vicodec_buf_queue(struct vb2_buffer * vb)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
vicodec_return_bufs(struct vb2_queue * q,u32 state)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
vicodec_start_streaming(struct vb2_queue * q,unsigned int count)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
vicodec_stop_streaming(struct vb2_queue * q)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
queue_init(void * priv,struct vb2_queue * src_vq,struct vb2_queue * dst_vq)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 */
vicodec_open(struct file * file)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
vicodec_release(struct file * file)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 v4l2_fh_del(&ctx->fh);
1301 v4l2_fh_exit(&ctx->fh);
1302 v4l2_ctrl_handler_free(&ctx->hdl);
1303 mutex_lock(vfd->lock);
1304 v4l2_m2m_ctx_release(ctx->fh.m2m_ctx);
1305 mutex_unlock(vfd->lock);
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
vicodec_probe(struct platform_device * pdev)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
vicodec_remove(struct platform_device * pdev)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
vicodec_exit(void)1485 static void __exit vicodec_exit(void)
1486 {
1487 platform_driver_unregister(&vicodec_pdrv);
1488 platform_device_unregister(&vicodec_pdev);
1489 }
1490
vicodec_init(void)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);
1508