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 480U
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], 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
job_abort(void * priv)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
find_fmt(u32 fmt)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
vidioc_querycap(struct file * file,void * priv,struct v4l2_capability * cap)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
enum_fmt(struct v4l2_fmtdesc * f,bool is_enc,bool is_out)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
vidioc_enum_fmt_vid_cap(struct file * file,void * priv,struct v4l2_fmtdesc * f)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
vidioc_enum_fmt_vid_out(struct file * file,void * priv,struct v4l2_fmtdesc * f)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
vidioc_g_fmt(struct vicodec_ctx * ctx,struct v4l2_format * f)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
vidioc_g_fmt_vid_out(struct file * file,void * priv,struct v4l2_format * f)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
vidioc_g_fmt_vid_cap(struct file * file,void * priv,struct v4l2_format * f)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
vidioc_try_fmt(struct vicodec_ctx * ctx,struct v4l2_format * f)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
vidioc_try_fmt_vid_cap(struct file * file,void * priv,struct v4l2_format * f)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
vidioc_try_fmt_vid_out(struct file * file,void * priv,struct v4l2_format * f)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
vidioc_s_fmt(struct vicodec_ctx * ctx,struct v4l2_format * f)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
vidioc_s_fmt_vid_cap(struct file * file,void * priv,struct v4l2_format * f)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
vidioc_s_fmt_vid_out(struct file * file,void * priv,struct v4l2_format * f)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
vicodec_mark_last_buf(struct vicodec_ctx * ctx)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
vicodec_try_encoder_cmd(struct file * file,void * fh,struct v4l2_encoder_cmd * ec)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
vicodec_encoder_cmd(struct file * file,void * fh,struct v4l2_encoder_cmd * ec)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
vicodec_try_decoder_cmd(struct file * file,void * fh,struct v4l2_decoder_cmd * dc)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
vicodec_decoder_cmd(struct file * file,void * fh,struct v4l2_decoder_cmd * dc)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
vicodec_enum_framesizes(struct file * file,void * fh,struct v4l2_frmsizeenum * fsize)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
vicodec_subscribe_event(struct v4l2_fh * fh,const struct v4l2_event_subscription * sub)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
vicodec_queue_setup(struct vb2_queue * vq,unsigned int * nbuffers,unsigned int * nplanes,unsigned int sizes[],struct device * alloc_devs[])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
vicodec_buf_prepare(struct vb2_buffer * vb)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
vicodec_buf_queue(struct vb2_buffer * vb)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
vicodec_return_bufs(struct vb2_queue * q,u32 state)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
vicodec_start_streaming(struct vb2_queue * q,unsigned int count)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
vicodec_stop_streaming(struct vb2_queue * q)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
queue_init(void * priv,struct vb2_queue * src_vq,struct vb2_queue * dst_vq)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 */
vicodec_open(struct file * file)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
vicodec_release(struct file * file)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
vicodec_probe(struct platform_device * pdev)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
vicodec_remove(struct platform_device * pdev)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
vicodec_exit(void)1484 static void __exit vicodec_exit(void)
1485 {
1486 platform_driver_unregister(&vicodec_pdrv);
1487 platform_device_unregister(&vicodec_pdev);
1488 }
1489
vicodec_init(void)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);
1507
