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 "codec-v4l2-fwht.h"
27 
28 MODULE_DESCRIPTION("Virtual codec device");
29 MODULE_AUTHOR("Hans Verkuil <hans.verkuil@cisco.com>");
30 MODULE_LICENSE("GPL v2");
31 
32 static bool multiplanar;
33 module_param(multiplanar, bool, 0444);
34 MODULE_PARM_DESC(multiplanar,
35 		 " use multi-planar API instead of single-planar API");
36 
37 static unsigned int debug;
38 module_param(debug, uint, 0644);
39 MODULE_PARM_DESC(debug, " activates debug info");
40 
41 #define VICODEC_NAME		"vicodec"
42 #define MAX_WIDTH		4096U
43 #define MIN_WIDTH		640U
44 #define MAX_HEIGHT		2160U
45 #define MIN_HEIGHT		360U
46 
47 #define dprintk(dev, fmt, arg...) \
48 	v4l2_dbg(1, debug, &dev->v4l2_dev, "%s: " fmt, __func__, ## arg)
49 
50 
51 struct pixfmt_info {
52 	u32 id;
53 	unsigned int bytesperline_mult;
54 	unsigned int sizeimage_mult;
55 	unsigned int sizeimage_div;
56 	unsigned int luma_step;
57 	unsigned int chroma_step;
58 	/* Chroma plane subsampling */
59 	unsigned int width_div;
60 	unsigned int height_div;
61 };
62 
63 static const struct v4l2_fwht_pixfmt_info pixfmt_fwht = {
64 	V4L2_PIX_FMT_FWHT, 0, 3, 1, 1, 1, 1, 1, 0, 1
65 };
66 
67 static const struct v4l2_fwht_pixfmt_info pixfmt_stateless_fwht = {
68 	V4L2_PIX_FMT_FWHT_STATELESS, 0, 3, 1, 1, 1, 1, 1, 0, 1
69 };
70 
vicodec_dev_release(struct device * dev)71 static void vicodec_dev_release(struct device *dev)
72 {
73 }
74 
75 static struct platform_device vicodec_pdev = {
76 	.name		= VICODEC_NAME,
77 	.dev.release	= vicodec_dev_release,
78 };
79 
80 /* Per-queue, driver-specific private data */
81 struct vicodec_q_data {
82 	unsigned int		coded_width;
83 	unsigned int		coded_height;
84 	unsigned int		visible_width;
85 	unsigned int		visible_height;
86 	unsigned int		sizeimage;
87 	unsigned int		vb2_sizeimage;
88 	unsigned int		sequence;
89 	const struct v4l2_fwht_pixfmt_info *info;
90 };
91 
92 enum {
93 	V4L2_M2M_SRC = 0,
94 	V4L2_M2M_DST = 1,
95 };
96 
97 struct vicodec_dev_instance {
98 	struct video_device     vfd;
99 	struct mutex            mutex;
100 	spinlock_t              lock;
101 	struct v4l2_m2m_dev     *m2m_dev;
102 };
103 
104 struct vicodec_dev {
105 	struct v4l2_device	v4l2_dev;
106 	struct vicodec_dev_instance stateful_enc;
107 	struct vicodec_dev_instance stateful_dec;
108 	struct vicodec_dev_instance stateless_dec;
109 #ifdef CONFIG_MEDIA_CONTROLLER
110 	struct media_device	mdev;
111 #endif
112 
113 };
114 
115 struct vicodec_ctx {
116 	struct v4l2_fh		fh;
117 	struct vicodec_dev	*dev;
118 	bool			is_enc;
119 	bool			is_stateless;
120 	spinlock_t		*lock;
121 
122 	struct v4l2_ctrl_handler hdl;
123 
124 	/* Source and destination queue data */
125 	struct vicodec_q_data   q_data[2];
126 	struct v4l2_fwht_state	state;
127 
128 	u32			cur_buf_offset;
129 	u32			comp_max_size;
130 	u32			comp_size;
131 	u32			header_size;
132 	u32			comp_magic_cnt;
133 	bool			comp_has_frame;
134 	bool			comp_has_next_frame;
135 	bool			first_source_change_sent;
136 	bool			source_changed;
137 };
138 
139 static const struct v4l2_event vicodec_eos_event = {
140 	.type = V4L2_EVENT_EOS
141 };
142 
file2ctx(struct file * file)143 static inline struct vicodec_ctx *file2ctx(struct file *file)
144 {
145 	return container_of(file->private_data, struct vicodec_ctx, fh);
146 }
147 
get_q_data(struct vicodec_ctx * ctx,enum v4l2_buf_type type)148 static struct vicodec_q_data *get_q_data(struct vicodec_ctx *ctx,
149 					 enum v4l2_buf_type type)
150 {
151 	switch (type) {
152 	case V4L2_BUF_TYPE_VIDEO_OUTPUT:
153 	case V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE:
154 		return &ctx->q_data[V4L2_M2M_SRC];
155 	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
156 	case V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE:
157 		return &ctx->q_data[V4L2_M2M_DST];
158 	default:
159 		break;
160 	}
161 	return NULL;
162 }
163 
copy_cap_to_ref(const u8 * cap,const struct v4l2_fwht_pixfmt_info * info,struct v4l2_fwht_state * state)164 static void copy_cap_to_ref(const u8 *cap, const struct v4l2_fwht_pixfmt_info *info,
165 		struct v4l2_fwht_state *state)
166 {
167 	int plane_idx;
168 	u8 *p_ref = state->ref_frame.buf;
169 	unsigned int cap_stride = state->stride;
170 	unsigned int ref_stride = state->ref_stride;
171 
172 	for (plane_idx = 0; plane_idx < info->planes_num; plane_idx++) {
173 		int i;
174 		unsigned int h_div = (plane_idx == 1 || plane_idx == 2) ?
175 			info->height_div : 1;
176 		const u8 *row_cap = cap;
177 		u8 *row_ref = p_ref;
178 
179 		if (info->planes_num == 3 && plane_idx == 1) {
180 			cap_stride /= 2;
181 			ref_stride /= 2;
182 		}
183 
184 		if (plane_idx == 1 &&
185 		    (info->id == V4L2_PIX_FMT_NV24 ||
186 		     info->id == V4L2_PIX_FMT_NV42)) {
187 			cap_stride *= 2;
188 			ref_stride *= 2;
189 		}
190 
191 		for (i = 0; i < state->visible_height / h_div; i++) {
192 			memcpy(row_ref, row_cap, ref_stride);
193 			row_ref += ref_stride;
194 			row_cap += cap_stride;
195 		}
196 		cap += cap_stride * (state->coded_height / h_div);
197 		p_ref += ref_stride * (state->coded_height / h_div);
198 	}
199 }
200 
validate_by_version(unsigned int flags,unsigned int version)201 static bool validate_by_version(unsigned int flags, unsigned int version)
202 {
203 	if (!version || version > V4L2_FWHT_VERSION)
204 		return false;
205 
206 	if (version >= 2) {
207 		unsigned int components_num = 1 +
208 			((flags & V4L2_FWHT_FL_COMPONENTS_NUM_MSK) >>
209 			 V4L2_FWHT_FL_COMPONENTS_NUM_OFFSET);
210 		unsigned int pixenc = flags & V4L2_FWHT_FL_PIXENC_MSK;
211 
212 		if (components_num == 0 || components_num > 4 || !pixenc)
213 			return false;
214 	}
215 	return true;
216 }
217 
validate_stateless_params_flags(const struct v4l2_ctrl_fwht_params * params,const struct v4l2_fwht_pixfmt_info * cur_info)218 static bool validate_stateless_params_flags(const struct v4l2_ctrl_fwht_params *params,
219 					    const struct v4l2_fwht_pixfmt_info *cur_info)
220 {
221 	unsigned int width_div =
222 		(params->flags & V4L2_FWHT_FL_CHROMA_FULL_WIDTH) ? 1 : 2;
223 	unsigned int height_div =
224 		(params->flags & V4L2_FWHT_FL_CHROMA_FULL_HEIGHT) ? 1 : 2;
225 	unsigned int components_num = 3;
226 	unsigned int pixenc = 0;
227 
228 	if (params->version < 3)
229 		return false;
230 
231 	components_num = 1 + ((params->flags & V4L2_FWHT_FL_COMPONENTS_NUM_MSK) >>
232 			      V4L2_FWHT_FL_COMPONENTS_NUM_OFFSET);
233 	pixenc = (params->flags & V4L2_FWHT_FL_PIXENC_MSK);
234 	if (v4l2_fwht_validate_fmt(cur_info, width_div, height_div,
235 				    components_num, pixenc))
236 		return true;
237 	return false;
238 }
239 
240 
update_state_from_header(struct vicodec_ctx * ctx)241 static void update_state_from_header(struct vicodec_ctx *ctx)
242 {
243 	const struct fwht_cframe_hdr *p_hdr = &ctx->state.header;
244 
245 	ctx->state.visible_width = ntohl(p_hdr->width);
246 	ctx->state.visible_height = ntohl(p_hdr->height);
247 	ctx->state.colorspace = ntohl(p_hdr->colorspace);
248 	ctx->state.xfer_func = ntohl(p_hdr->xfer_func);
249 	ctx->state.ycbcr_enc = ntohl(p_hdr->ycbcr_enc);
250 	ctx->state.quantization = ntohl(p_hdr->quantization);
251 }
252 
device_process(struct vicodec_ctx * ctx,struct vb2_v4l2_buffer * src_vb,struct vb2_v4l2_buffer * dst_vb)253 static int device_process(struct vicodec_ctx *ctx,
254 			  struct vb2_v4l2_buffer *src_vb,
255 			  struct vb2_v4l2_buffer *dst_vb)
256 {
257 	struct vicodec_dev *dev = ctx->dev;
258 	struct v4l2_fwht_state *state = &ctx->state;
259 	u8 *p_src, *p_dst;
260 	int ret = 0;
261 
262 	if (ctx->is_enc || ctx->is_stateless)
263 		p_src = vb2_plane_vaddr(&src_vb->vb2_buf, 0);
264 	else
265 		p_src = state->compressed_frame;
266 
267 	if (ctx->is_stateless) {
268 		struct media_request *src_req = src_vb->vb2_buf.req_obj.req;
269 
270 		ret = v4l2_ctrl_request_setup(src_req, &ctx->hdl);
271 		if (ret)
272 			return ret;
273 		update_state_from_header(ctx);
274 
275 		ctx->state.header.size =
276 			htonl(vb2_get_plane_payload(&src_vb->vb2_buf, 0));
277 		/*
278 		 * set the reference buffer from the reference timestamp
279 		 * only if this is a P-frame
280 		 */
281 		if (!(ntohl(ctx->state.header.flags) & V4L2_FWHT_FL_I_FRAME)) {
282 			struct vb2_buffer *ref_vb2_buf;
283 			struct vb2_queue *vq_cap =
284 				v4l2_m2m_get_vq(ctx->fh.m2m_ctx,
285 						V4L2_BUF_TYPE_VIDEO_CAPTURE);
286 
287 			ref_vb2_buf = vb2_find_buffer(vq_cap, ctx->state.ref_frame_ts);
288 			if (!ref_vb2_buf)
289 				return -EINVAL;
290 			if (ref_vb2_buf->state == VB2_BUF_STATE_ERROR)
291 				ret = -EINVAL;
292 			ctx->state.ref_frame.buf =
293 				vb2_plane_vaddr(ref_vb2_buf, 0);
294 		} else {
295 			ctx->state.ref_frame.buf = NULL;
296 		}
297 	}
298 	p_dst = vb2_plane_vaddr(&dst_vb->vb2_buf, 0);
299 	if (!p_src || !p_dst) {
300 		v4l2_err(&dev->v4l2_dev,
301 			 "Acquiring kernel pointers to buffers failed\n");
302 		return -EFAULT;
303 	}
304 
305 	if (ctx->is_enc) {
306 		struct vicodec_q_data *q_src;
307 		int comp_sz_or_errcode;
308 
309 		q_src = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_OUTPUT);
310 		state->info = q_src->info;
311 		comp_sz_or_errcode = v4l2_fwht_encode(state, p_src, p_dst);
312 		if (comp_sz_or_errcode < 0)
313 			return comp_sz_or_errcode;
314 		vb2_set_plane_payload(&dst_vb->vb2_buf, 0, comp_sz_or_errcode);
315 	} else {
316 		struct vicodec_q_data *q_dst;
317 		unsigned int comp_frame_size = ntohl(ctx->state.header.size);
318 
319 		q_dst = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE);
320 		if (comp_frame_size > ctx->comp_max_size)
321 			return -EINVAL;
322 		state->info = q_dst->info;
323 		ret = v4l2_fwht_decode(state, p_src, p_dst);
324 		if (ret < 0)
325 			return ret;
326 		if (!ctx->is_stateless)
327 			copy_cap_to_ref(p_dst, ctx->state.info, &ctx->state);
328 
329 		vb2_set_plane_payload(&dst_vb->vb2_buf, 0, q_dst->sizeimage);
330 		if (ntohl(ctx->state.header.flags) & V4L2_FWHT_FL_I_FRAME)
331 			dst_vb->flags |= V4L2_BUF_FLAG_KEYFRAME;
332 		else
333 			dst_vb->flags |= V4L2_BUF_FLAG_PFRAME;
334 	}
335 	return ret;
336 }
337 
338 /*
339  * mem2mem callbacks
340  */
get_next_header(struct vicodec_ctx * ctx,u8 ** pp,u32 sz)341 static enum vb2_buffer_state get_next_header(struct vicodec_ctx *ctx,
342 					     u8 **pp, u32 sz)
343 {
344 	static const u8 magic[] = {
345 		0x4f, 0x4f, 0x4f, 0x4f, 0xff, 0xff, 0xff, 0xff
346 	};
347 	u8 *p = *pp;
348 	u32 state;
349 	u8 *header = (u8 *)&ctx->state.header;
350 
351 	state = VB2_BUF_STATE_DONE;
352 
353 	if (!ctx->header_size) {
354 		state = VB2_BUF_STATE_ERROR;
355 		for (; p < *pp + sz; p++) {
356 			u32 copy;
357 
358 			p = memchr(p, magic[ctx->comp_magic_cnt],
359 				   *pp + sz - p);
360 			if (!p) {
361 				ctx->comp_magic_cnt = 0;
362 				p = *pp + sz;
363 				break;
364 			}
365 			copy = sizeof(magic) - ctx->comp_magic_cnt;
366 			if (*pp + sz - p < copy)
367 				copy = *pp + sz - p;
368 
369 			memcpy(header + ctx->comp_magic_cnt, p, copy);
370 			ctx->comp_magic_cnt += copy;
371 			if (!memcmp(header, magic, ctx->comp_magic_cnt)) {
372 				p += copy;
373 				state = VB2_BUF_STATE_DONE;
374 				break;
375 			}
376 			ctx->comp_magic_cnt = 0;
377 		}
378 		if (ctx->comp_magic_cnt < sizeof(magic)) {
379 			*pp = p;
380 			return state;
381 		}
382 		ctx->header_size = sizeof(magic);
383 	}
384 
385 	if (ctx->header_size < sizeof(struct fwht_cframe_hdr)) {
386 		u32 copy = sizeof(struct fwht_cframe_hdr) - ctx->header_size;
387 
388 		if (*pp + sz - p < copy)
389 			copy = *pp + sz - p;
390 
391 		memcpy(header + ctx->header_size, p, copy);
392 		p += copy;
393 		ctx->header_size += copy;
394 	}
395 	*pp = p;
396 	return state;
397 }
398 
399 /* device_run() - prepares and starts the device */
device_run(void * priv)400 static void device_run(void *priv)
401 {
402 	struct vicodec_ctx *ctx = priv;
403 	struct vicodec_dev *dev = ctx->dev;
404 	struct vb2_v4l2_buffer *src_buf, *dst_buf;
405 	struct vicodec_q_data *q_src, *q_dst;
406 	u32 state;
407 	struct media_request *src_req;
408 
409 	src_buf = v4l2_m2m_next_src_buf(ctx->fh.m2m_ctx);
410 	dst_buf = v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
411 	src_req = src_buf->vb2_buf.req_obj.req;
412 
413 	q_src = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_OUTPUT);
414 	q_dst = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE);
415 
416 	state = VB2_BUF_STATE_DONE;
417 	if (device_process(ctx, src_buf, dst_buf))
418 		state = VB2_BUF_STATE_ERROR;
419 	else
420 		dst_buf->sequence = q_dst->sequence++;
421 	dst_buf->flags &= ~V4L2_BUF_FLAG_LAST;
422 	v4l2_m2m_buf_copy_metadata(src_buf, dst_buf, false);
423 
424 	spin_lock(ctx->lock);
425 	if (!ctx->comp_has_next_frame &&
426 	    v4l2_m2m_is_last_draining_src_buf(ctx->fh.m2m_ctx, src_buf)) {
427 		dst_buf->flags |= V4L2_BUF_FLAG_LAST;
428 		v4l2_event_queue_fh(&ctx->fh, &vicodec_eos_event);
429 		v4l2_m2m_mark_stopped(ctx->fh.m2m_ctx);
430 	}
431 	if (ctx->is_enc || ctx->is_stateless) {
432 		src_buf->sequence = q_src->sequence++;
433 		src_buf = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
434 		v4l2_m2m_buf_done(src_buf, state);
435 	} else if (vb2_get_plane_payload(&src_buf->vb2_buf, 0) == ctx->cur_buf_offset) {
436 		src_buf->sequence = q_src->sequence++;
437 		src_buf = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
438 		v4l2_m2m_buf_done(src_buf, state);
439 		ctx->cur_buf_offset = 0;
440 		ctx->comp_has_next_frame = false;
441 	}
442 	v4l2_m2m_buf_done(dst_buf, state);
443 
444 	ctx->comp_size = 0;
445 	ctx->header_size = 0;
446 	ctx->comp_magic_cnt = 0;
447 	ctx->comp_has_frame = false;
448 	spin_unlock(ctx->lock);
449 	if (ctx->is_stateless && src_req)
450 		v4l2_ctrl_request_complete(src_req, &ctx->hdl);
451 
452 	if (ctx->is_enc)
453 		v4l2_m2m_job_finish(dev->stateful_enc.m2m_dev, ctx->fh.m2m_ctx);
454 	else if (ctx->is_stateless)
455 		v4l2_m2m_job_finish(dev->stateless_dec.m2m_dev,
456 				    ctx->fh.m2m_ctx);
457 	else
458 		v4l2_m2m_job_finish(dev->stateful_dec.m2m_dev, ctx->fh.m2m_ctx);
459 }
460 
job_remove_src_buf(struct vicodec_ctx * ctx,u32 state)461 static void job_remove_src_buf(struct vicodec_ctx *ctx, u32 state)
462 {
463 	struct vb2_v4l2_buffer *src_buf;
464 	struct vicodec_q_data *q_src;
465 
466 	q_src = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_OUTPUT);
467 	spin_lock(ctx->lock);
468 	src_buf = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
469 	src_buf->sequence = q_src->sequence++;
470 	v4l2_m2m_buf_done(src_buf, state);
471 	ctx->cur_buf_offset = 0;
472 	spin_unlock(ctx->lock);
473 }
474 
475 static const struct v4l2_fwht_pixfmt_info *
info_from_header(const struct fwht_cframe_hdr * p_hdr)476 info_from_header(const struct fwht_cframe_hdr *p_hdr)
477 {
478 	unsigned int flags = ntohl(p_hdr->flags);
479 	unsigned int width_div = (flags & V4L2_FWHT_FL_CHROMA_FULL_WIDTH) ? 1 : 2;
480 	unsigned int height_div = (flags & V4L2_FWHT_FL_CHROMA_FULL_HEIGHT) ? 1 : 2;
481 	unsigned int components_num = 3;
482 	unsigned int pixenc = 0;
483 	unsigned int version = ntohl(p_hdr->version);
484 
485 	if (version >= 2) {
486 		components_num = 1 + ((flags & V4L2_FWHT_FL_COMPONENTS_NUM_MSK) >>
487 				V4L2_FWHT_FL_COMPONENTS_NUM_OFFSET);
488 		pixenc = (flags & V4L2_FWHT_FL_PIXENC_MSK);
489 	}
490 	return v4l2_fwht_find_nth_fmt(width_div, height_div,
491 				     components_num, pixenc, 0);
492 }
493 
is_header_valid(const struct fwht_cframe_hdr * p_hdr)494 static bool is_header_valid(const struct fwht_cframe_hdr *p_hdr)
495 {
496 	const struct v4l2_fwht_pixfmt_info *info;
497 	unsigned int w = ntohl(p_hdr->width);
498 	unsigned int h = ntohl(p_hdr->height);
499 	unsigned int version = ntohl(p_hdr->version);
500 	unsigned int flags = ntohl(p_hdr->flags);
501 
502 	if (w < MIN_WIDTH || w > MAX_WIDTH || h < MIN_HEIGHT || h > MAX_HEIGHT)
503 		return false;
504 
505 	if (!validate_by_version(flags, version))
506 		return false;
507 
508 	info = info_from_header(p_hdr);
509 	if (!info)
510 		return false;
511 	return true;
512 }
513 
update_capture_data_from_header(struct vicodec_ctx * ctx)514 static void update_capture_data_from_header(struct vicodec_ctx *ctx)
515 {
516 	struct vicodec_q_data *q_dst = get_q_data(ctx,
517 						  V4L2_BUF_TYPE_VIDEO_CAPTURE);
518 	const struct fwht_cframe_hdr *p_hdr = &ctx->state.header;
519 	const struct v4l2_fwht_pixfmt_info *info = info_from_header(p_hdr);
520 	unsigned int flags = ntohl(p_hdr->flags);
521 	unsigned int hdr_width_div = (flags & V4L2_FWHT_FL_CHROMA_FULL_WIDTH) ? 1 : 2;
522 	unsigned int hdr_height_div = (flags & V4L2_FWHT_FL_CHROMA_FULL_HEIGHT) ? 1 : 2;
523 
524 	/*
525 	 * This function should not be used by a stateless codec since
526 	 * it changes values in q_data that are not request specific
527 	 */
528 	WARN_ON(ctx->is_stateless);
529 
530 	q_dst->info = info;
531 	q_dst->visible_width = ntohl(p_hdr->width);
532 	q_dst->visible_height = ntohl(p_hdr->height);
533 	q_dst->coded_width = vic_round_dim(q_dst->visible_width, hdr_width_div);
534 	q_dst->coded_height = vic_round_dim(q_dst->visible_height,
535 					    hdr_height_div);
536 
537 	q_dst->sizeimage = q_dst->coded_width * q_dst->coded_height *
538 		q_dst->info->sizeimage_mult / q_dst->info->sizeimage_div;
539 	ctx->state.colorspace = ntohl(p_hdr->colorspace);
540 
541 	ctx->state.xfer_func = ntohl(p_hdr->xfer_func);
542 	ctx->state.ycbcr_enc = ntohl(p_hdr->ycbcr_enc);
543 	ctx->state.quantization = ntohl(p_hdr->quantization);
544 }
545 
set_last_buffer(struct vb2_v4l2_buffer * dst_buf,const struct vb2_v4l2_buffer * src_buf,struct vicodec_ctx * ctx)546 static void set_last_buffer(struct vb2_v4l2_buffer *dst_buf,
547 			    const struct vb2_v4l2_buffer *src_buf,
548 			    struct vicodec_ctx *ctx)
549 {
550 	struct vicodec_q_data *q_dst = get_q_data(ctx,
551 						  V4L2_BUF_TYPE_VIDEO_CAPTURE);
552 
553 	vb2_set_plane_payload(&dst_buf->vb2_buf, 0, 0);
554 	dst_buf->sequence = q_dst->sequence++;
555 
556 	v4l2_m2m_buf_copy_metadata(src_buf, dst_buf, !ctx->is_enc);
557 	dst_buf->flags |= V4L2_BUF_FLAG_LAST;
558 	v4l2_m2m_buf_done(dst_buf, VB2_BUF_STATE_DONE);
559 }
560 
job_ready(void * priv)561 static int job_ready(void *priv)
562 {
563 	static const u8 magic[] = {
564 		0x4f, 0x4f, 0x4f, 0x4f, 0xff, 0xff, 0xff, 0xff
565 	};
566 	struct vicodec_ctx *ctx = priv;
567 	struct vb2_v4l2_buffer *src_buf;
568 	u8 *p_src;
569 	u8 *p;
570 	u32 sz;
571 	u32 state;
572 	struct vicodec_q_data *q_dst = get_q_data(ctx,
573 						  V4L2_BUF_TYPE_VIDEO_CAPTURE);
574 	unsigned int flags;
575 	unsigned int hdr_width_div;
576 	unsigned int hdr_height_div;
577 	unsigned int max_to_copy;
578 	unsigned int comp_frame_size;
579 
580 	if (ctx->source_changed)
581 		return 0;
582 	if (ctx->is_stateless || ctx->is_enc || ctx->comp_has_frame)
583 		return 1;
584 
585 restart:
586 	ctx->comp_has_next_frame = false;
587 	src_buf = v4l2_m2m_next_src_buf(ctx->fh.m2m_ctx);
588 	if (!src_buf)
589 		return 0;
590 	p_src = vb2_plane_vaddr(&src_buf->vb2_buf, 0);
591 	sz = vb2_get_plane_payload(&src_buf->vb2_buf, 0);
592 	p = p_src + ctx->cur_buf_offset;
593 
594 	state = VB2_BUF_STATE_DONE;
595 
596 	if (ctx->header_size < sizeof(struct fwht_cframe_hdr)) {
597 		state = get_next_header(ctx, &p, p_src + sz - p);
598 		if (ctx->header_size < sizeof(struct fwht_cframe_hdr)) {
599 			if (v4l2_m2m_is_last_draining_src_buf(ctx->fh.m2m_ctx,
600 							      src_buf))
601 				return 1;
602 			job_remove_src_buf(ctx, state);
603 			goto restart;
604 		}
605 	}
606 
607 	comp_frame_size = ntohl(ctx->state.header.size);
608 
609 	/*
610 	 * The current scanned frame might be the first frame of a new
611 	 * resolution so its size might be larger than ctx->comp_max_size.
612 	 * In that case it is copied up to the current buffer capacity and
613 	 * the copy will continue after allocating new large enough buffer
614 	 * when restreaming
615 	 */
616 	max_to_copy = min(comp_frame_size, ctx->comp_max_size);
617 
618 	if (ctx->comp_size < max_to_copy) {
619 		u32 copy = max_to_copy - ctx->comp_size;
620 
621 		if (copy > p_src + sz - p)
622 			copy = p_src + sz - p;
623 
624 		memcpy(ctx->state.compressed_frame + ctx->comp_size,
625 		       p, copy);
626 		p += copy;
627 		ctx->comp_size += copy;
628 		if (ctx->comp_size < max_to_copy) {
629 			if (v4l2_m2m_is_last_draining_src_buf(ctx->fh.m2m_ctx,
630 							      src_buf))
631 				return 1;
632 			job_remove_src_buf(ctx, state);
633 			goto restart;
634 		}
635 	}
636 	ctx->cur_buf_offset = p - p_src;
637 	if (ctx->comp_size == comp_frame_size)
638 		ctx->comp_has_frame = true;
639 	ctx->comp_has_next_frame = false;
640 	if (ctx->comp_has_frame && sz - ctx->cur_buf_offset >=
641 			sizeof(struct fwht_cframe_hdr)) {
642 		struct fwht_cframe_hdr *p_hdr = (struct fwht_cframe_hdr *)p;
643 		u32 frame_size = ntohl(p_hdr->size);
644 		u32 remaining = sz - ctx->cur_buf_offset - sizeof(*p_hdr);
645 
646 		if (!memcmp(p, magic, sizeof(magic)))
647 			ctx->comp_has_next_frame = remaining >= frame_size;
648 	}
649 	/*
650 	 * if the header is invalid the device_run will just drop the frame
651 	 * with an error
652 	 */
653 	if (!is_header_valid(&ctx->state.header) && ctx->comp_has_frame)
654 		return 1;
655 	flags = ntohl(ctx->state.header.flags);
656 	hdr_width_div = (flags & V4L2_FWHT_FL_CHROMA_FULL_WIDTH) ? 1 : 2;
657 	hdr_height_div = (flags & V4L2_FWHT_FL_CHROMA_FULL_HEIGHT) ? 1 : 2;
658 
659 	if (ntohl(ctx->state.header.width) != q_dst->visible_width ||
660 	    ntohl(ctx->state.header.height) != q_dst->visible_height ||
661 	    !q_dst->info ||
662 	    hdr_width_div != q_dst->info->width_div ||
663 	    hdr_height_div != q_dst->info->height_div) {
664 		static const struct v4l2_event rs_event = {
665 			.type = V4L2_EVENT_SOURCE_CHANGE,
666 			.u.src_change.changes = V4L2_EVENT_SRC_CH_RESOLUTION,
667 		};
668 
669 		struct vb2_v4l2_buffer *dst_buf =
670 			v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
671 
672 		update_capture_data_from_header(ctx);
673 		v4l2_event_queue_fh(&ctx->fh, &rs_event);
674 		set_last_buffer(dst_buf, src_buf, ctx);
675 		ctx->source_changed = true;
676 		return 0;
677 	}
678 	return 1;
679 }
680 
681 /*
682  * video ioctls
683  */
684 
find_fmt(u32 fmt)685 static const struct v4l2_fwht_pixfmt_info *find_fmt(u32 fmt)
686 {
687 	const struct v4l2_fwht_pixfmt_info *info =
688 		v4l2_fwht_find_pixfmt(fmt);
689 
690 	if (!info)
691 		info = v4l2_fwht_get_pixfmt(0);
692 	return info;
693 }
694 
vidioc_querycap(struct file * file,void * priv,struct v4l2_capability * cap)695 static int vidioc_querycap(struct file *file, void *priv,
696 			   struct v4l2_capability *cap)
697 {
698 	strscpy(cap->driver, VICODEC_NAME, sizeof(cap->driver));
699 	strscpy(cap->card, VICODEC_NAME, sizeof(cap->card));
700 	snprintf(cap->bus_info, sizeof(cap->bus_info),
701 			"platform:%s", VICODEC_NAME);
702 	return 0;
703 }
704 
enum_fmt(struct v4l2_fmtdesc * f,struct vicodec_ctx * ctx,bool is_out)705 static int enum_fmt(struct v4l2_fmtdesc *f, struct vicodec_ctx *ctx,
706 		    bool is_out)
707 {
708 	bool is_uncomp = (ctx->is_enc && is_out) || (!ctx->is_enc && !is_out);
709 
710 	if (V4L2_TYPE_IS_MULTIPLANAR(f->type) && !multiplanar)
711 		return -EINVAL;
712 	if (!V4L2_TYPE_IS_MULTIPLANAR(f->type) && multiplanar)
713 		return -EINVAL;
714 
715 	if (is_uncomp) {
716 		const struct v4l2_fwht_pixfmt_info *info =
717 					get_q_data(ctx, f->type)->info;
718 
719 		if (ctx->is_enc ||
720 		    !vb2_is_streaming(&ctx->fh.m2m_ctx->cap_q_ctx.q))
721 			info = v4l2_fwht_get_pixfmt(f->index);
722 		else
723 			info = v4l2_fwht_find_nth_fmt(info->width_div,
724 						     info->height_div,
725 						     info->components_num,
726 						     info->pixenc,
727 						     f->index);
728 		if (!info)
729 			return -EINVAL;
730 		f->pixelformat = info->id;
731 	} else {
732 		if (f->index)
733 			return -EINVAL;
734 		f->pixelformat = ctx->is_stateless ?
735 			V4L2_PIX_FMT_FWHT_STATELESS : V4L2_PIX_FMT_FWHT;
736 		if (!ctx->is_enc && !ctx->is_stateless)
737 			f->flags = V4L2_FMT_FLAG_DYN_RESOLUTION |
738 				   V4L2_FMT_FLAG_CONTINUOUS_BYTESTREAM;
739 	}
740 	return 0;
741 }
742 
vidioc_enum_fmt_vid_cap(struct file * file,void * priv,struct v4l2_fmtdesc * f)743 static int vidioc_enum_fmt_vid_cap(struct file *file, void *priv,
744 				   struct v4l2_fmtdesc *f)
745 {
746 	struct vicodec_ctx *ctx = file2ctx(file);
747 
748 	return enum_fmt(f, ctx, false);
749 }
750 
vidioc_enum_fmt_vid_out(struct file * file,void * priv,struct v4l2_fmtdesc * f)751 static int vidioc_enum_fmt_vid_out(struct file *file, void *priv,
752 				   struct v4l2_fmtdesc *f)
753 {
754 	struct vicodec_ctx *ctx = file2ctx(file);
755 
756 	return enum_fmt(f, ctx, true);
757 }
758 
vidioc_g_fmt(struct vicodec_ctx * ctx,struct v4l2_format * f)759 static int vidioc_g_fmt(struct vicodec_ctx *ctx, struct v4l2_format *f)
760 {
761 	struct vb2_queue *vq;
762 	struct vicodec_q_data *q_data;
763 	struct v4l2_pix_format_mplane *pix_mp;
764 	struct v4l2_pix_format *pix;
765 	const struct v4l2_fwht_pixfmt_info *info;
766 
767 	vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);
768 	if (!vq)
769 		return -EINVAL;
770 
771 	q_data = get_q_data(ctx, f->type);
772 	info = q_data->info;
773 
774 	switch (f->type) {
775 	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
776 	case V4L2_BUF_TYPE_VIDEO_OUTPUT:
777 		if (multiplanar)
778 			return -EINVAL;
779 		pix = &f->fmt.pix;
780 		pix->width = q_data->coded_width;
781 		pix->height = q_data->coded_height;
782 		pix->field = V4L2_FIELD_NONE;
783 		pix->pixelformat = info->id;
784 		pix->bytesperline = q_data->coded_width *
785 					info->bytesperline_mult;
786 		pix->sizeimage = q_data->sizeimage;
787 		pix->colorspace = ctx->state.colorspace;
788 		pix->xfer_func = ctx->state.xfer_func;
789 		pix->ycbcr_enc = ctx->state.ycbcr_enc;
790 		pix->quantization = ctx->state.quantization;
791 		break;
792 
793 	case V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE:
794 	case V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE:
795 		if (!multiplanar)
796 			return -EINVAL;
797 		pix_mp = &f->fmt.pix_mp;
798 		pix_mp->width = q_data->coded_width;
799 		pix_mp->height = q_data->coded_height;
800 		pix_mp->field = V4L2_FIELD_NONE;
801 		pix_mp->pixelformat = info->id;
802 		pix_mp->num_planes = 1;
803 		pix_mp->plane_fmt[0].bytesperline =
804 				q_data->coded_width * info->bytesperline_mult;
805 		pix_mp->plane_fmt[0].sizeimage = q_data->sizeimage;
806 		pix_mp->colorspace = ctx->state.colorspace;
807 		pix_mp->xfer_func = ctx->state.xfer_func;
808 		pix_mp->ycbcr_enc = ctx->state.ycbcr_enc;
809 		pix_mp->quantization = ctx->state.quantization;
810 		break;
811 	default:
812 		return -EINVAL;
813 	}
814 	return 0;
815 }
816 
vidioc_g_fmt_vid_out(struct file * file,void * priv,struct v4l2_format * f)817 static int vidioc_g_fmt_vid_out(struct file *file, void *priv,
818 				struct v4l2_format *f)
819 {
820 	return vidioc_g_fmt(file2ctx(file), f);
821 }
822 
vidioc_g_fmt_vid_cap(struct file * file,void * priv,struct v4l2_format * f)823 static int vidioc_g_fmt_vid_cap(struct file *file, void *priv,
824 				struct v4l2_format *f)
825 {
826 	return vidioc_g_fmt(file2ctx(file), f);
827 }
828 
vidioc_try_fmt(struct vicodec_ctx * ctx,struct v4l2_format * f)829 static int vidioc_try_fmt(struct vicodec_ctx *ctx, struct v4l2_format *f)
830 {
831 	struct v4l2_pix_format_mplane *pix_mp;
832 	struct v4l2_pix_format *pix;
833 	struct v4l2_plane_pix_format *plane;
834 	const struct v4l2_fwht_pixfmt_info *info = ctx->is_stateless ?
835 		&pixfmt_stateless_fwht : &pixfmt_fwht;
836 
837 	switch (f->type) {
838 	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
839 	case V4L2_BUF_TYPE_VIDEO_OUTPUT:
840 		pix = &f->fmt.pix;
841 		if (pix->pixelformat != V4L2_PIX_FMT_FWHT &&
842 		    pix->pixelformat != V4L2_PIX_FMT_FWHT_STATELESS)
843 			info = find_fmt(pix->pixelformat);
844 
845 		pix->width = clamp(pix->width, MIN_WIDTH, MAX_WIDTH);
846 		pix->width = vic_round_dim(pix->width, info->width_div);
847 
848 		pix->height = clamp(pix->height, MIN_HEIGHT, MAX_HEIGHT);
849 		pix->height = vic_round_dim(pix->height, info->height_div);
850 
851 		pix->field = V4L2_FIELD_NONE;
852 		pix->bytesperline =
853 			pix->width * info->bytesperline_mult;
854 		pix->sizeimage = pix->width * pix->height *
855 			info->sizeimage_mult / info->sizeimage_div;
856 		if (pix->pixelformat == V4L2_PIX_FMT_FWHT)
857 			pix->sizeimage += sizeof(struct fwht_cframe_hdr);
858 		break;
859 	case V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE:
860 	case V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE:
861 		pix_mp = &f->fmt.pix_mp;
862 		plane = pix_mp->plane_fmt;
863 		if (pix_mp->pixelformat != V4L2_PIX_FMT_FWHT &&
864 		    pix_mp->pixelformat != V4L2_PIX_FMT_FWHT_STATELESS)
865 			info = find_fmt(pix_mp->pixelformat);
866 		pix_mp->num_planes = 1;
867 
868 		pix_mp->width = clamp(pix_mp->width, MIN_WIDTH, MAX_WIDTH);
869 		pix_mp->width = vic_round_dim(pix_mp->width, info->width_div);
870 
871 		pix_mp->height = clamp(pix_mp->height, MIN_HEIGHT, MAX_HEIGHT);
872 		pix_mp->height = vic_round_dim(pix_mp->height,
873 					       info->height_div);
874 
875 		pix_mp->field = V4L2_FIELD_NONE;
876 		plane->bytesperline =
877 			pix_mp->width * info->bytesperline_mult;
878 		plane->sizeimage = pix_mp->width * pix_mp->height *
879 			info->sizeimage_mult / info->sizeimage_div;
880 		if (pix_mp->pixelformat == V4L2_PIX_FMT_FWHT)
881 			plane->sizeimage += sizeof(struct fwht_cframe_hdr);
882 		break;
883 	default:
884 		return -EINVAL;
885 	}
886 
887 	return 0;
888 }
889 
vidioc_try_fmt_vid_cap(struct file * file,void * priv,struct v4l2_format * f)890 static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
891 				  struct v4l2_format *f)
892 {
893 	struct vicodec_ctx *ctx = file2ctx(file);
894 	struct v4l2_pix_format_mplane *pix_mp;
895 	struct v4l2_pix_format *pix;
896 
897 	switch (f->type) {
898 	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
899 		if (multiplanar)
900 			return -EINVAL;
901 		pix = &f->fmt.pix;
902 		pix->pixelformat = ctx->is_enc ? V4L2_PIX_FMT_FWHT :
903 				   find_fmt(f->fmt.pix.pixelformat)->id;
904 		pix->colorspace = ctx->state.colorspace;
905 		pix->xfer_func = ctx->state.xfer_func;
906 		pix->ycbcr_enc = ctx->state.ycbcr_enc;
907 		pix->quantization = ctx->state.quantization;
908 		break;
909 	case V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE:
910 		if (!multiplanar)
911 			return -EINVAL;
912 		pix_mp = &f->fmt.pix_mp;
913 		pix_mp->pixelformat = ctx->is_enc ? V4L2_PIX_FMT_FWHT :
914 				      find_fmt(pix_mp->pixelformat)->id;
915 		pix_mp->colorspace = ctx->state.colorspace;
916 		pix_mp->xfer_func = ctx->state.xfer_func;
917 		pix_mp->ycbcr_enc = ctx->state.ycbcr_enc;
918 		pix_mp->quantization = ctx->state.quantization;
919 		break;
920 	default:
921 		return -EINVAL;
922 	}
923 
924 	return vidioc_try_fmt(ctx, f);
925 }
926 
vidioc_try_fmt_vid_out(struct file * file,void * priv,struct v4l2_format * f)927 static int vidioc_try_fmt_vid_out(struct file *file, void *priv,
928 				  struct v4l2_format *f)
929 {
930 	struct vicodec_ctx *ctx = file2ctx(file);
931 	struct v4l2_pix_format_mplane *pix_mp;
932 	struct v4l2_pix_format *pix;
933 
934 	switch (f->type) {
935 	case V4L2_BUF_TYPE_VIDEO_OUTPUT:
936 		if (multiplanar)
937 			return -EINVAL;
938 		pix = &f->fmt.pix;
939 		if (ctx->is_enc)
940 			pix->pixelformat = find_fmt(pix->pixelformat)->id;
941 		else if (ctx->is_stateless)
942 			pix->pixelformat = V4L2_PIX_FMT_FWHT_STATELESS;
943 		else
944 			pix->pixelformat = V4L2_PIX_FMT_FWHT;
945 		if (!pix->colorspace)
946 			pix->colorspace = V4L2_COLORSPACE_REC709;
947 		break;
948 	case V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE:
949 		if (!multiplanar)
950 			return -EINVAL;
951 		pix_mp = &f->fmt.pix_mp;
952 		if (ctx->is_enc)
953 			pix_mp->pixelformat = find_fmt(pix_mp->pixelformat)->id;
954 		else if (ctx->is_stateless)
955 			pix_mp->pixelformat = V4L2_PIX_FMT_FWHT_STATELESS;
956 		else
957 			pix_mp->pixelformat = V4L2_PIX_FMT_FWHT;
958 		if (!pix_mp->colorspace)
959 			pix_mp->colorspace = V4L2_COLORSPACE_REC709;
960 		break;
961 	default:
962 		return -EINVAL;
963 	}
964 
965 	return vidioc_try_fmt(ctx, f);
966 }
967 
vidioc_s_fmt(struct vicodec_ctx * ctx,struct v4l2_format * f)968 static int vidioc_s_fmt(struct vicodec_ctx *ctx, struct v4l2_format *f)
969 {
970 	struct vicodec_q_data *q_data;
971 	struct vb2_queue *vq;
972 	bool fmt_changed = true;
973 	struct v4l2_pix_format_mplane *pix_mp;
974 	struct v4l2_pix_format *pix;
975 
976 	vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);
977 	if (!vq)
978 		return -EINVAL;
979 
980 	q_data = get_q_data(ctx, f->type);
981 	if (!q_data)
982 		return -EINVAL;
983 
984 	switch (f->type) {
985 	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
986 	case V4L2_BUF_TYPE_VIDEO_OUTPUT:
987 		pix = &f->fmt.pix;
988 		if (ctx->is_enc && V4L2_TYPE_IS_OUTPUT(f->type))
989 			fmt_changed =
990 				!q_data->info ||
991 				q_data->info->id != pix->pixelformat ||
992 				q_data->coded_width != pix->width ||
993 				q_data->coded_height != pix->height;
994 
995 		if (vb2_is_busy(vq) && fmt_changed)
996 			return -EBUSY;
997 
998 		if (pix->pixelformat == V4L2_PIX_FMT_FWHT)
999 			q_data->info = &pixfmt_fwht;
1000 		else if (pix->pixelformat == V4L2_PIX_FMT_FWHT_STATELESS)
1001 			q_data->info = &pixfmt_stateless_fwht;
1002 		else
1003 			q_data->info = find_fmt(pix->pixelformat);
1004 		q_data->coded_width = pix->width;
1005 		q_data->coded_height = pix->height;
1006 		q_data->sizeimage = pix->sizeimage;
1007 		break;
1008 	case V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE:
1009 	case V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE:
1010 		pix_mp = &f->fmt.pix_mp;
1011 		if (ctx->is_enc && V4L2_TYPE_IS_OUTPUT(f->type))
1012 			fmt_changed =
1013 				!q_data->info ||
1014 				q_data->info->id != pix_mp->pixelformat ||
1015 				q_data->coded_width != pix_mp->width ||
1016 				q_data->coded_height != pix_mp->height;
1017 
1018 		if (vb2_is_busy(vq) && fmt_changed)
1019 			return -EBUSY;
1020 
1021 		if (pix_mp->pixelformat == V4L2_PIX_FMT_FWHT)
1022 			q_data->info = &pixfmt_fwht;
1023 		else if (pix_mp->pixelformat == V4L2_PIX_FMT_FWHT_STATELESS)
1024 			q_data->info = &pixfmt_stateless_fwht;
1025 		else
1026 			q_data->info = find_fmt(pix_mp->pixelformat);
1027 		q_data->coded_width = pix_mp->width;
1028 		q_data->coded_height = pix_mp->height;
1029 		q_data->sizeimage = pix_mp->plane_fmt[0].sizeimage;
1030 		break;
1031 	default:
1032 		return -EINVAL;
1033 	}
1034 
1035 	dprintk(ctx->dev,
1036 		"Setting format for type %d, coded wxh: %dx%d, fourcc: 0x%08x\n",
1037 		f->type, q_data->coded_width, q_data->coded_height,
1038 		q_data->info->id);
1039 
1040 	return 0;
1041 }
1042 
vidioc_s_fmt_vid_cap(struct file * file,void * priv,struct v4l2_format * f)1043 static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
1044 				struct v4l2_format *f)
1045 {
1046 	int ret;
1047 
1048 	ret = vidioc_try_fmt_vid_cap(file, priv, f);
1049 	if (ret)
1050 		return ret;
1051 
1052 	return vidioc_s_fmt(file2ctx(file), f);
1053 }
1054 
vidioc_s_fmt_vid_out(struct file * file,void * priv,struct v4l2_format * f)1055 static int vidioc_s_fmt_vid_out(struct file *file, void *priv,
1056 				struct v4l2_format *f)
1057 {
1058 	struct vicodec_ctx *ctx = file2ctx(file);
1059 	struct vicodec_q_data *q_data;
1060 	struct vicodec_q_data *q_data_cap;
1061 	struct v4l2_pix_format *pix;
1062 	struct v4l2_pix_format_mplane *pix_mp;
1063 	u32 coded_w = 0, coded_h = 0;
1064 	unsigned int size = 0;
1065 	int ret;
1066 
1067 	q_data = get_q_data(ctx, f->type);
1068 	q_data_cap = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE);
1069 
1070 	ret = vidioc_try_fmt_vid_out(file, priv, f);
1071 	if (ret)
1072 		return ret;
1073 
1074 	if (ctx->is_enc) {
1075 		struct vb2_queue *vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);
1076 		struct vb2_queue *vq_cap = v4l2_m2m_get_vq(ctx->fh.m2m_ctx,
1077 							   V4L2_BUF_TYPE_VIDEO_CAPTURE);
1078 		const struct v4l2_fwht_pixfmt_info *info = ctx->is_stateless ?
1079 			&pixfmt_stateless_fwht : &pixfmt_fwht;
1080 
1081 		if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT) {
1082 			coded_w = f->fmt.pix.width;
1083 			coded_h = f->fmt.pix.height;
1084 		} else {
1085 			coded_w = f->fmt.pix_mp.width;
1086 			coded_h = f->fmt.pix_mp.height;
1087 		}
1088 		if (vb2_is_busy(vq) && (coded_w != q_data->coded_width ||
1089 					coded_h != q_data->coded_height))
1090 			return -EBUSY;
1091 		size = coded_w * coded_h *
1092 			info->sizeimage_mult / info->sizeimage_div;
1093 		if (!ctx->is_stateless)
1094 			size += sizeof(struct fwht_cframe_hdr);
1095 
1096 		if (vb2_is_busy(vq_cap) && size > q_data_cap->sizeimage)
1097 			return -EBUSY;
1098 	}
1099 
1100 	ret = vidioc_s_fmt(file2ctx(file), f);
1101 	if (!ret) {
1102 		if (ctx->is_enc) {
1103 			q_data->visible_width = coded_w;
1104 			q_data->visible_height = coded_h;
1105 			q_data_cap->coded_width = coded_w;
1106 			q_data_cap->coded_height = coded_h;
1107 			q_data_cap->sizeimage = size;
1108 		}
1109 
1110 		switch (f->type) {
1111 		case V4L2_BUF_TYPE_VIDEO_OUTPUT:
1112 			pix = &f->fmt.pix;
1113 			ctx->state.colorspace = pix->colorspace;
1114 			ctx->state.xfer_func = pix->xfer_func;
1115 			ctx->state.ycbcr_enc = pix->ycbcr_enc;
1116 			ctx->state.quantization = pix->quantization;
1117 			break;
1118 		case V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE:
1119 			pix_mp = &f->fmt.pix_mp;
1120 			ctx->state.colorspace = pix_mp->colorspace;
1121 			ctx->state.xfer_func = pix_mp->xfer_func;
1122 			ctx->state.ycbcr_enc = pix_mp->ycbcr_enc;
1123 			ctx->state.quantization = pix_mp->quantization;
1124 			break;
1125 		default:
1126 			break;
1127 		}
1128 	}
1129 	return ret;
1130 }
1131 
vidioc_g_selection(struct file * file,void * priv,struct v4l2_selection * s)1132 static int vidioc_g_selection(struct file *file, void *priv,
1133 			      struct v4l2_selection *s)
1134 {
1135 	struct vicodec_ctx *ctx = file2ctx(file);
1136 	struct vicodec_q_data *q_data;
1137 
1138 	q_data = get_q_data(ctx, s->type);
1139 	if (!q_data)
1140 		return -EINVAL;
1141 	/*
1142 	 * encoder supports only cropping on the OUTPUT buffer
1143 	 * decoder supports only composing on the CAPTURE buffer
1144 	 */
1145 	if (ctx->is_enc && s->type == V4L2_BUF_TYPE_VIDEO_OUTPUT) {
1146 		switch (s->target) {
1147 		case V4L2_SEL_TGT_CROP:
1148 			s->r.left = 0;
1149 			s->r.top = 0;
1150 			s->r.width = q_data->visible_width;
1151 			s->r.height = q_data->visible_height;
1152 			return 0;
1153 		case V4L2_SEL_TGT_CROP_DEFAULT:
1154 		case V4L2_SEL_TGT_CROP_BOUNDS:
1155 			s->r.left = 0;
1156 			s->r.top = 0;
1157 			s->r.width = q_data->coded_width;
1158 			s->r.height = q_data->coded_height;
1159 			return 0;
1160 		}
1161 	} else if (!ctx->is_enc && s->type == V4L2_BUF_TYPE_VIDEO_CAPTURE) {
1162 		switch (s->target) {
1163 		case V4L2_SEL_TGT_COMPOSE:
1164 			s->r.left = 0;
1165 			s->r.top = 0;
1166 			s->r.width = q_data->visible_width;
1167 			s->r.height = q_data->visible_height;
1168 			return 0;
1169 		case V4L2_SEL_TGT_COMPOSE_DEFAULT:
1170 		case V4L2_SEL_TGT_COMPOSE_BOUNDS:
1171 			s->r.left = 0;
1172 			s->r.top = 0;
1173 			s->r.width = q_data->coded_width;
1174 			s->r.height = q_data->coded_height;
1175 			return 0;
1176 		}
1177 	}
1178 	return -EINVAL;
1179 }
1180 
vidioc_s_selection(struct file * file,void * priv,struct v4l2_selection * s)1181 static int vidioc_s_selection(struct file *file, void *priv,
1182 			      struct v4l2_selection *s)
1183 {
1184 	struct vicodec_ctx *ctx = file2ctx(file);
1185 	struct vicodec_q_data *q_data;
1186 
1187 	if (s->type != V4L2_BUF_TYPE_VIDEO_OUTPUT)
1188 		return -EINVAL;
1189 
1190 	q_data = get_q_data(ctx, s->type);
1191 	if (!q_data)
1192 		return -EINVAL;
1193 
1194 	if (!ctx->is_enc || s->target != V4L2_SEL_TGT_CROP)
1195 		return -EINVAL;
1196 
1197 	s->r.left = 0;
1198 	s->r.top = 0;
1199 	q_data->visible_width = clamp(s->r.width, MIN_WIDTH,
1200 				      q_data->coded_width);
1201 	s->r.width = q_data->visible_width;
1202 	q_data->visible_height = clamp(s->r.height, MIN_HEIGHT,
1203 				       q_data->coded_height);
1204 	s->r.height = q_data->visible_height;
1205 	return 0;
1206 }
1207 
vicodec_encoder_cmd(struct file * file,void * fh,struct v4l2_encoder_cmd * ec)1208 static int vicodec_encoder_cmd(struct file *file, void *fh,
1209 			    struct v4l2_encoder_cmd *ec)
1210 {
1211 	struct vicodec_ctx *ctx = file2ctx(file);
1212 	int ret;
1213 
1214 	ret = v4l2_m2m_ioctl_try_encoder_cmd(file, fh, ec);
1215 	if (ret < 0)
1216 		return ret;
1217 
1218 	if (!vb2_is_streaming(&ctx->fh.m2m_ctx->cap_q_ctx.q) ||
1219 	    !vb2_is_streaming(&ctx->fh.m2m_ctx->out_q_ctx.q))
1220 		return 0;
1221 
1222 	ret = v4l2_m2m_ioctl_encoder_cmd(file, fh, ec);
1223 	if (ret < 0)
1224 		return ret;
1225 
1226 	if (ec->cmd == V4L2_ENC_CMD_STOP &&
1227 	    v4l2_m2m_has_stopped(ctx->fh.m2m_ctx))
1228 		v4l2_event_queue_fh(&ctx->fh, &vicodec_eos_event);
1229 
1230 	if (ec->cmd == V4L2_ENC_CMD_START &&
1231 	    v4l2_m2m_has_stopped(ctx->fh.m2m_ctx))
1232 		vb2_clear_last_buffer_dequeued(&ctx->fh.m2m_ctx->cap_q_ctx.q);
1233 
1234 	return 0;
1235 }
1236 
vicodec_decoder_cmd(struct file * file,void * fh,struct v4l2_decoder_cmd * dc)1237 static int vicodec_decoder_cmd(struct file *file, void *fh,
1238 			    struct v4l2_decoder_cmd *dc)
1239 {
1240 	struct vicodec_ctx *ctx = file2ctx(file);
1241 	int ret;
1242 
1243 	ret = v4l2_m2m_ioctl_try_decoder_cmd(file, fh, dc);
1244 	if (ret < 0)
1245 		return ret;
1246 
1247 	if (!vb2_is_streaming(&ctx->fh.m2m_ctx->cap_q_ctx.q) ||
1248 	    !vb2_is_streaming(&ctx->fh.m2m_ctx->out_q_ctx.q))
1249 		return 0;
1250 
1251 	ret = v4l2_m2m_ioctl_decoder_cmd(file, fh, dc);
1252 	if (ret < 0)
1253 		return ret;
1254 
1255 	if (dc->cmd == V4L2_DEC_CMD_STOP &&
1256 	    v4l2_m2m_has_stopped(ctx->fh.m2m_ctx))
1257 		v4l2_event_queue_fh(&ctx->fh, &vicodec_eos_event);
1258 
1259 	if (dc->cmd == V4L2_DEC_CMD_START &&
1260 	    v4l2_m2m_has_stopped(ctx->fh.m2m_ctx))
1261 		vb2_clear_last_buffer_dequeued(&ctx->fh.m2m_ctx->cap_q_ctx.q);
1262 
1263 	return 0;
1264 }
1265 
vicodec_enum_framesizes(struct file * file,void * fh,struct v4l2_frmsizeenum * fsize)1266 static int vicodec_enum_framesizes(struct file *file, void *fh,
1267 				   struct v4l2_frmsizeenum *fsize)
1268 {
1269 	switch (fsize->pixel_format) {
1270 	case V4L2_PIX_FMT_FWHT_STATELESS:
1271 		break;
1272 	case V4L2_PIX_FMT_FWHT:
1273 		break;
1274 	default:
1275 		if (find_fmt(fsize->pixel_format)->id == fsize->pixel_format)
1276 			break;
1277 		return -EINVAL;
1278 	}
1279 
1280 	if (fsize->index)
1281 		return -EINVAL;
1282 
1283 	fsize->type = V4L2_FRMSIZE_TYPE_STEPWISE;
1284 
1285 	fsize->stepwise.min_width = MIN_WIDTH;
1286 	fsize->stepwise.max_width = MAX_WIDTH;
1287 	fsize->stepwise.step_width = 8;
1288 	fsize->stepwise.min_height = MIN_HEIGHT;
1289 	fsize->stepwise.max_height = MAX_HEIGHT;
1290 	fsize->stepwise.step_height = 8;
1291 
1292 	return 0;
1293 }
1294 
vicodec_subscribe_event(struct v4l2_fh * fh,const struct v4l2_event_subscription * sub)1295 static int vicodec_subscribe_event(struct v4l2_fh *fh,
1296 				const struct v4l2_event_subscription *sub)
1297 {
1298 	struct vicodec_ctx *ctx = container_of(fh, struct vicodec_ctx, fh);
1299 
1300 	switch (sub->type) {
1301 	case V4L2_EVENT_SOURCE_CHANGE:
1302 		if (ctx->is_enc)
1303 			return -EINVAL;
1304 		fallthrough;
1305 	case V4L2_EVENT_EOS:
1306 		if (ctx->is_stateless)
1307 			return -EINVAL;
1308 		return v4l2_event_subscribe(fh, sub, 0, NULL);
1309 	default:
1310 		return v4l2_ctrl_subscribe_event(fh, sub);
1311 	}
1312 }
1313 
1314 static const struct v4l2_ioctl_ops vicodec_ioctl_ops = {
1315 	.vidioc_querycap	= vidioc_querycap,
1316 
1317 	.vidioc_enum_fmt_vid_cap = vidioc_enum_fmt_vid_cap,
1318 	.vidioc_g_fmt_vid_cap	= vidioc_g_fmt_vid_cap,
1319 	.vidioc_try_fmt_vid_cap	= vidioc_try_fmt_vid_cap,
1320 	.vidioc_s_fmt_vid_cap	= vidioc_s_fmt_vid_cap,
1321 
1322 	.vidioc_g_fmt_vid_cap_mplane	= vidioc_g_fmt_vid_cap,
1323 	.vidioc_try_fmt_vid_cap_mplane	= vidioc_try_fmt_vid_cap,
1324 	.vidioc_s_fmt_vid_cap_mplane	= vidioc_s_fmt_vid_cap,
1325 
1326 	.vidioc_enum_fmt_vid_out = vidioc_enum_fmt_vid_out,
1327 	.vidioc_g_fmt_vid_out	= vidioc_g_fmt_vid_out,
1328 	.vidioc_try_fmt_vid_out	= vidioc_try_fmt_vid_out,
1329 	.vidioc_s_fmt_vid_out	= vidioc_s_fmt_vid_out,
1330 
1331 	.vidioc_g_fmt_vid_out_mplane	= vidioc_g_fmt_vid_out,
1332 	.vidioc_try_fmt_vid_out_mplane	= vidioc_try_fmt_vid_out,
1333 	.vidioc_s_fmt_vid_out_mplane	= vidioc_s_fmt_vid_out,
1334 
1335 	.vidioc_reqbufs		= v4l2_m2m_ioctl_reqbufs,
1336 	.vidioc_querybuf	= v4l2_m2m_ioctl_querybuf,
1337 	.vidioc_qbuf		= v4l2_m2m_ioctl_qbuf,
1338 	.vidioc_dqbuf		= v4l2_m2m_ioctl_dqbuf,
1339 	.vidioc_prepare_buf	= v4l2_m2m_ioctl_prepare_buf,
1340 	.vidioc_create_bufs	= v4l2_m2m_ioctl_create_bufs,
1341 	.vidioc_expbuf		= v4l2_m2m_ioctl_expbuf,
1342 
1343 	.vidioc_streamon	= v4l2_m2m_ioctl_streamon,
1344 	.vidioc_streamoff	= v4l2_m2m_ioctl_streamoff,
1345 
1346 	.vidioc_g_selection	= vidioc_g_selection,
1347 	.vidioc_s_selection	= vidioc_s_selection,
1348 
1349 	.vidioc_try_encoder_cmd	= v4l2_m2m_ioctl_try_encoder_cmd,
1350 	.vidioc_encoder_cmd	= vicodec_encoder_cmd,
1351 	.vidioc_try_decoder_cmd	= v4l2_m2m_ioctl_try_decoder_cmd,
1352 	.vidioc_decoder_cmd	= vicodec_decoder_cmd,
1353 	.vidioc_enum_framesizes = vicodec_enum_framesizes,
1354 
1355 	.vidioc_subscribe_event = vicodec_subscribe_event,
1356 	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
1357 };
1358 
1359 
1360 /*
1361  * Queue operations
1362  */
1363 
vicodec_queue_setup(struct vb2_queue * vq,unsigned int * nbuffers,unsigned int * nplanes,unsigned int sizes[],struct device * alloc_devs[])1364 static int vicodec_queue_setup(struct vb2_queue *vq, unsigned int *nbuffers,
1365 			       unsigned int *nplanes, unsigned int sizes[],
1366 			       struct device *alloc_devs[])
1367 {
1368 	struct vicodec_ctx *ctx = vb2_get_drv_priv(vq);
1369 	struct vicodec_q_data *q_data = get_q_data(ctx, vq->type);
1370 	unsigned int size = q_data->sizeimage;
1371 
1372 	if (*nplanes)
1373 		return sizes[0] < size ? -EINVAL : 0;
1374 
1375 	*nplanes = 1;
1376 	sizes[0] = size;
1377 	q_data->vb2_sizeimage = size;
1378 	return 0;
1379 }
1380 
vicodec_buf_out_validate(struct vb2_buffer * vb)1381 static int vicodec_buf_out_validate(struct vb2_buffer *vb)
1382 {
1383 	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
1384 
1385 	vbuf->field = V4L2_FIELD_NONE;
1386 	return 0;
1387 }
1388 
vicodec_buf_prepare(struct vb2_buffer * vb)1389 static int vicodec_buf_prepare(struct vb2_buffer *vb)
1390 {
1391 	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
1392 	struct vicodec_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
1393 	struct vicodec_q_data *q_data;
1394 
1395 	dprintk(ctx->dev, "type: %d\n", vb->vb2_queue->type);
1396 
1397 	q_data = get_q_data(ctx, vb->vb2_queue->type);
1398 	if (V4L2_TYPE_IS_OUTPUT(vb->vb2_queue->type)) {
1399 		if (vbuf->field == V4L2_FIELD_ANY)
1400 			vbuf->field = V4L2_FIELD_NONE;
1401 		if (vbuf->field != V4L2_FIELD_NONE) {
1402 			dprintk(ctx->dev, "%s field isn't supported\n",
1403 					__func__);
1404 			return -EINVAL;
1405 		}
1406 	}
1407 
1408 	if (vb2_plane_size(vb, 0) < q_data->vb2_sizeimage) {
1409 		dprintk(ctx->dev,
1410 			"%s data will not fit into plane (%lu < %lu)\n",
1411 			__func__, vb2_plane_size(vb, 0),
1412 			(long)q_data->vb2_sizeimage);
1413 		return -EINVAL;
1414 	}
1415 
1416 	return 0;
1417 }
1418 
vicodec_buf_queue(struct vb2_buffer * vb)1419 static void vicodec_buf_queue(struct vb2_buffer *vb)
1420 {
1421 	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
1422 	struct vicodec_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
1423 	unsigned int sz = vb2_get_plane_payload(&vbuf->vb2_buf, 0);
1424 	u8 *p_src = vb2_plane_vaddr(&vbuf->vb2_buf, 0);
1425 	u8 *p = p_src;
1426 	struct vb2_queue *vq_out = v4l2_m2m_get_vq(ctx->fh.m2m_ctx,
1427 						   V4L2_BUF_TYPE_VIDEO_OUTPUT);
1428 	struct vb2_queue *vq_cap = v4l2_m2m_get_vq(ctx->fh.m2m_ctx,
1429 						   V4L2_BUF_TYPE_VIDEO_CAPTURE);
1430 	bool header_valid = false;
1431 	static const struct v4l2_event rs_event = {
1432 		.type = V4L2_EVENT_SOURCE_CHANGE,
1433 		.u.src_change.changes = V4L2_EVENT_SRC_CH_RESOLUTION,
1434 	};
1435 
1436 	if (V4L2_TYPE_IS_CAPTURE(vb->vb2_queue->type) &&
1437 	    vb2_is_streaming(vb->vb2_queue) &&
1438 	    v4l2_m2m_dst_buf_is_last(ctx->fh.m2m_ctx)) {
1439 		unsigned int i;
1440 
1441 		for (i = 0; i < vb->num_planes; i++)
1442 			vb2_set_plane_payload(vb, i, 0);
1443 
1444 		vbuf->field = V4L2_FIELD_NONE;
1445 		vbuf->sequence =
1446 			get_q_data(ctx, vb->vb2_queue->type)->sequence++;
1447 
1448 		v4l2_m2m_last_buffer_done(ctx->fh.m2m_ctx, vbuf);
1449 		v4l2_event_queue_fh(&ctx->fh, &vicodec_eos_event);
1450 		return;
1451 	}
1452 
1453 	/* buf_queue handles only the first source change event */
1454 	if (ctx->first_source_change_sent) {
1455 		v4l2_m2m_buf_queue(ctx->fh.m2m_ctx, vbuf);
1456 		return;
1457 	}
1458 
1459 	/*
1460 	 * if both queues are streaming, the source change event is
1461 	 * handled in job_ready
1462 	 */
1463 	if (vb2_is_streaming(vq_cap) && vb2_is_streaming(vq_out)) {
1464 		v4l2_m2m_buf_queue(ctx->fh.m2m_ctx, vbuf);
1465 		return;
1466 	}
1467 
1468 	/*
1469 	 * source change event is relevant only for the stateful decoder
1470 	 * in the compressed stream
1471 	 */
1472 	if (ctx->is_stateless || ctx->is_enc ||
1473 	    V4L2_TYPE_IS_CAPTURE(vb->vb2_queue->type)) {
1474 		v4l2_m2m_buf_queue(ctx->fh.m2m_ctx, vbuf);
1475 		return;
1476 	}
1477 
1478 	do {
1479 		enum vb2_buffer_state state =
1480 			get_next_header(ctx, &p, p_src + sz - p);
1481 
1482 		if (ctx->header_size < sizeof(struct fwht_cframe_hdr)) {
1483 			v4l2_m2m_buf_done(vbuf, state);
1484 			return;
1485 		}
1486 		header_valid = is_header_valid(&ctx->state.header);
1487 		/*
1488 		 * p points right after the end of the header in the
1489 		 * buffer. If the header is invalid we set p to point
1490 		 * to the next byte after the start of the header
1491 		 */
1492 		if (!header_valid) {
1493 			p = p - sizeof(struct fwht_cframe_hdr) + 1;
1494 			if (p < p_src)
1495 				p = p_src;
1496 			ctx->header_size = 0;
1497 			ctx->comp_magic_cnt = 0;
1498 		}
1499 
1500 	} while (!header_valid);
1501 
1502 	ctx->cur_buf_offset = p - p_src;
1503 	update_capture_data_from_header(ctx);
1504 	ctx->first_source_change_sent = true;
1505 	v4l2_event_queue_fh(&ctx->fh, &rs_event);
1506 	v4l2_m2m_buf_queue(ctx->fh.m2m_ctx, vbuf);
1507 }
1508 
vicodec_return_bufs(struct vb2_queue * q,u32 state)1509 static void vicodec_return_bufs(struct vb2_queue *q, u32 state)
1510 {
1511 	struct vicodec_ctx *ctx = vb2_get_drv_priv(q);
1512 	struct vb2_v4l2_buffer *vbuf;
1513 
1514 	for (;;) {
1515 		if (V4L2_TYPE_IS_OUTPUT(q->type))
1516 			vbuf = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
1517 		else
1518 			vbuf = v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
1519 		if (vbuf == NULL)
1520 			return;
1521 		v4l2_ctrl_request_complete(vbuf->vb2_buf.req_obj.req,
1522 					   &ctx->hdl);
1523 		spin_lock(ctx->lock);
1524 		v4l2_m2m_buf_done(vbuf, state);
1525 		spin_unlock(ctx->lock);
1526 	}
1527 }
1528 
total_frame_size(struct vicodec_q_data * q_data)1529 static unsigned int total_frame_size(struct vicodec_q_data *q_data)
1530 {
1531 	unsigned int size;
1532 	unsigned int chroma_div;
1533 
1534 	if (!q_data->info) {
1535 		WARN_ON(1);
1536 		return 0;
1537 	}
1538 	size = q_data->coded_width * q_data->coded_height;
1539 	chroma_div = q_data->info->width_div * q_data->info->height_div;
1540 
1541 	if (q_data->info->components_num == 4)
1542 		return 2 * size + 2 * (size / chroma_div);
1543 	else if (q_data->info->components_num == 3)
1544 		return size + 2 * (size / chroma_div);
1545 	return size;
1546 }
1547 
vicodec_start_streaming(struct vb2_queue * q,unsigned int count)1548 static int vicodec_start_streaming(struct vb2_queue *q,
1549 				   unsigned int count)
1550 {
1551 	struct vicodec_ctx *ctx = vb2_get_drv_priv(q);
1552 	struct vicodec_q_data *q_data = get_q_data(ctx, q->type);
1553 	struct v4l2_fwht_state *state = &ctx->state;
1554 	const struct v4l2_fwht_pixfmt_info *info = q_data->info;
1555 	unsigned int size = q_data->coded_width * q_data->coded_height;
1556 	unsigned int chroma_div;
1557 	unsigned int total_planes_size;
1558 	u8 *new_comp_frame = NULL;
1559 
1560 	chroma_div = info->width_div * info->height_div;
1561 	q_data->sequence = 0;
1562 
1563 	v4l2_m2m_update_start_streaming_state(ctx->fh.m2m_ctx, q);
1564 
1565 	state->gop_cnt = 0;
1566 
1567 	if ((V4L2_TYPE_IS_OUTPUT(q->type) && !ctx->is_enc) ||
1568 	    (V4L2_TYPE_IS_CAPTURE(q->type) && ctx->is_enc))
1569 		return 0;
1570 
1571 	if (info->id == V4L2_PIX_FMT_FWHT ||
1572 	    info->id == V4L2_PIX_FMT_FWHT_STATELESS) {
1573 		vicodec_return_bufs(q, VB2_BUF_STATE_QUEUED);
1574 		return -EINVAL;
1575 	}
1576 	total_planes_size = total_frame_size(q_data);
1577 	ctx->comp_max_size = total_planes_size;
1578 
1579 	state->visible_width = q_data->visible_width;
1580 	state->visible_height = q_data->visible_height;
1581 	state->coded_width = q_data->coded_width;
1582 	state->coded_height = q_data->coded_height;
1583 	state->stride = q_data->coded_width *
1584 				info->bytesperline_mult;
1585 
1586 	if (ctx->is_stateless) {
1587 		state->ref_stride = state->stride;
1588 		return 0;
1589 	}
1590 	state->ref_stride = q_data->coded_width * info->luma_alpha_step;
1591 
1592 	state->ref_frame.buf = kvmalloc(total_planes_size, GFP_KERNEL);
1593 	state->ref_frame.luma = state->ref_frame.buf;
1594 	new_comp_frame = kvmalloc(ctx->comp_max_size, GFP_KERNEL);
1595 
1596 	if (!state->ref_frame.luma || !new_comp_frame) {
1597 		kvfree(state->ref_frame.luma);
1598 		kvfree(new_comp_frame);
1599 		vicodec_return_bufs(q, VB2_BUF_STATE_QUEUED);
1600 		return -ENOMEM;
1601 	}
1602 	/*
1603 	 * if state->compressed_frame was already allocated then
1604 	 * it contain data of the first frame of the new resolution
1605 	 */
1606 	if (state->compressed_frame) {
1607 		if (ctx->comp_size > ctx->comp_max_size)
1608 			ctx->comp_size = ctx->comp_max_size;
1609 
1610 		memcpy(new_comp_frame,
1611 		       state->compressed_frame, ctx->comp_size);
1612 	}
1613 
1614 	kvfree(state->compressed_frame);
1615 	state->compressed_frame = new_comp_frame;
1616 
1617 	if (info->components_num < 3) {
1618 		state->ref_frame.cb = NULL;
1619 		state->ref_frame.cr = NULL;
1620 		state->ref_frame.alpha = NULL;
1621 		return 0;
1622 	}
1623 
1624 	state->ref_frame.cb = state->ref_frame.luma + size;
1625 	state->ref_frame.cr = state->ref_frame.cb + size / chroma_div;
1626 
1627 	if (info->components_num == 4)
1628 		state->ref_frame.alpha =
1629 			state->ref_frame.cr + size / chroma_div;
1630 	else
1631 		state->ref_frame.alpha = NULL;
1632 
1633 	return 0;
1634 }
1635 
vicodec_stop_streaming(struct vb2_queue * q)1636 static void vicodec_stop_streaming(struct vb2_queue *q)
1637 {
1638 	struct vicodec_ctx *ctx = vb2_get_drv_priv(q);
1639 
1640 	vicodec_return_bufs(q, VB2_BUF_STATE_ERROR);
1641 
1642 	v4l2_m2m_update_stop_streaming_state(ctx->fh.m2m_ctx, q);
1643 
1644 	if (V4L2_TYPE_IS_OUTPUT(q->type) &&
1645 	    v4l2_m2m_has_stopped(ctx->fh.m2m_ctx))
1646 		v4l2_event_queue_fh(&ctx->fh, &vicodec_eos_event);
1647 
1648 	if (!ctx->is_enc && V4L2_TYPE_IS_OUTPUT(q->type))
1649 		ctx->first_source_change_sent = false;
1650 
1651 	if ((!V4L2_TYPE_IS_OUTPUT(q->type) && !ctx->is_enc) ||
1652 	    (V4L2_TYPE_IS_OUTPUT(q->type) && ctx->is_enc)) {
1653 		if (!ctx->is_stateless)
1654 			kvfree(ctx->state.ref_frame.buf);
1655 		ctx->state.ref_frame.buf = NULL;
1656 		ctx->state.ref_frame.luma = NULL;
1657 		ctx->comp_max_size = 0;
1658 		ctx->source_changed = false;
1659 	}
1660 	if (V4L2_TYPE_IS_OUTPUT(q->type) && !ctx->is_enc) {
1661 		ctx->cur_buf_offset = 0;
1662 		ctx->comp_size = 0;
1663 		ctx->header_size = 0;
1664 		ctx->comp_magic_cnt = 0;
1665 		ctx->comp_has_frame = false;
1666 		ctx->comp_has_next_frame = false;
1667 	}
1668 }
1669 
vicodec_buf_request_complete(struct vb2_buffer * vb)1670 static void vicodec_buf_request_complete(struct vb2_buffer *vb)
1671 {
1672 	struct vicodec_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
1673 
1674 	v4l2_ctrl_request_complete(vb->req_obj.req, &ctx->hdl);
1675 }
1676 
1677 
1678 static const struct vb2_ops vicodec_qops = {
1679 	.queue_setup		= vicodec_queue_setup,
1680 	.buf_out_validate	= vicodec_buf_out_validate,
1681 	.buf_prepare		= vicodec_buf_prepare,
1682 	.buf_queue		= vicodec_buf_queue,
1683 	.buf_request_complete	= vicodec_buf_request_complete,
1684 	.start_streaming	= vicodec_start_streaming,
1685 	.stop_streaming		= vicodec_stop_streaming,
1686 	.wait_prepare		= vb2_ops_wait_prepare,
1687 	.wait_finish		= vb2_ops_wait_finish,
1688 };
1689 
queue_init(void * priv,struct vb2_queue * src_vq,struct vb2_queue * dst_vq)1690 static int queue_init(void *priv, struct vb2_queue *src_vq,
1691 		      struct vb2_queue *dst_vq)
1692 {
1693 	struct vicodec_ctx *ctx = priv;
1694 	int ret;
1695 
1696 	src_vq->type = (multiplanar ?
1697 			V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE :
1698 			V4L2_BUF_TYPE_VIDEO_OUTPUT);
1699 	src_vq->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF;
1700 	src_vq->drv_priv = ctx;
1701 	src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
1702 	src_vq->ops = &vicodec_qops;
1703 	src_vq->mem_ops = &vb2_vmalloc_memops;
1704 	src_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
1705 	if (ctx->is_enc)
1706 		src_vq->lock = &ctx->dev->stateful_enc.mutex;
1707 	else if (ctx->is_stateless)
1708 		src_vq->lock = &ctx->dev->stateless_dec.mutex;
1709 	else
1710 		src_vq->lock = &ctx->dev->stateful_dec.mutex;
1711 	src_vq->supports_requests = ctx->is_stateless;
1712 	src_vq->requires_requests = ctx->is_stateless;
1713 	ret = vb2_queue_init(src_vq);
1714 	if (ret)
1715 		return ret;
1716 
1717 	dst_vq->type = (multiplanar ?
1718 			V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE :
1719 			V4L2_BUF_TYPE_VIDEO_CAPTURE);
1720 	dst_vq->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF;
1721 	dst_vq->drv_priv = ctx;
1722 	dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
1723 	dst_vq->ops = &vicodec_qops;
1724 	dst_vq->mem_ops = &vb2_vmalloc_memops;
1725 	dst_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
1726 	dst_vq->lock = src_vq->lock;
1727 
1728 	return vb2_queue_init(dst_vq);
1729 }
1730 
vicodec_try_ctrl(struct v4l2_ctrl * ctrl)1731 static int vicodec_try_ctrl(struct v4l2_ctrl *ctrl)
1732 {
1733 	struct vicodec_ctx *ctx = container_of(ctrl->handler,
1734 			struct vicodec_ctx, hdl);
1735 	const struct v4l2_ctrl_fwht_params *params;
1736 	struct vicodec_q_data *q_dst = get_q_data(ctx,
1737 			V4L2_BUF_TYPE_VIDEO_CAPTURE);
1738 
1739 	switch (ctrl->id) {
1740 	case V4L2_CID_STATELESS_FWHT_PARAMS:
1741 		if (!q_dst->info)
1742 			return -EINVAL;
1743 		params = ctrl->p_new.p_fwht_params;
1744 		if (params->width > q_dst->coded_width ||
1745 		    params->width < MIN_WIDTH ||
1746 		    params->height > q_dst->coded_height ||
1747 		    params->height < MIN_HEIGHT)
1748 			return -EINVAL;
1749 		if (!validate_by_version(params->flags, params->version))
1750 			return -EINVAL;
1751 		if (!validate_stateless_params_flags(params, q_dst->info))
1752 			return -EINVAL;
1753 		return 0;
1754 	default:
1755 		return 0;
1756 	}
1757 	return 0;
1758 }
1759 
update_header_from_stateless_params(struct vicodec_ctx * ctx,const struct v4l2_ctrl_fwht_params * params)1760 static void update_header_from_stateless_params(struct vicodec_ctx *ctx,
1761 						const struct v4l2_ctrl_fwht_params *params)
1762 {
1763 	struct fwht_cframe_hdr *p_hdr = &ctx->state.header;
1764 
1765 	p_hdr->magic1 = FWHT_MAGIC1;
1766 	p_hdr->magic2 = FWHT_MAGIC2;
1767 	p_hdr->version = htonl(params->version);
1768 	p_hdr->width = htonl(params->width);
1769 	p_hdr->height = htonl(params->height);
1770 	p_hdr->flags = htonl(params->flags);
1771 	p_hdr->colorspace = htonl(params->colorspace);
1772 	p_hdr->xfer_func = htonl(params->xfer_func);
1773 	p_hdr->ycbcr_enc = htonl(params->ycbcr_enc);
1774 	p_hdr->quantization = htonl(params->quantization);
1775 }
1776 
vicodec_s_ctrl(struct v4l2_ctrl * ctrl)1777 static int vicodec_s_ctrl(struct v4l2_ctrl *ctrl)
1778 {
1779 	struct vicodec_ctx *ctx = container_of(ctrl->handler,
1780 					       struct vicodec_ctx, hdl);
1781 	const struct v4l2_ctrl_fwht_params *params;
1782 
1783 	switch (ctrl->id) {
1784 	case V4L2_CID_MPEG_VIDEO_GOP_SIZE:
1785 		ctx->state.gop_size = ctrl->val;
1786 		return 0;
1787 	case V4L2_CID_FWHT_I_FRAME_QP:
1788 		ctx->state.i_frame_qp = ctrl->val;
1789 		return 0;
1790 	case V4L2_CID_FWHT_P_FRAME_QP:
1791 		ctx->state.p_frame_qp = ctrl->val;
1792 		return 0;
1793 	case V4L2_CID_STATELESS_FWHT_PARAMS:
1794 		params = ctrl->p_new.p_fwht_params;
1795 		update_header_from_stateless_params(ctx, params);
1796 		ctx->state.ref_frame_ts = params->backward_ref_ts;
1797 		return 0;
1798 	}
1799 	return -EINVAL;
1800 }
1801 
1802 static const struct v4l2_ctrl_ops vicodec_ctrl_ops = {
1803 	.s_ctrl = vicodec_s_ctrl,
1804 	.try_ctrl = vicodec_try_ctrl,
1805 };
1806 
1807 static const struct v4l2_ctrl_config vicodec_ctrl_stateless_state = {
1808 	.ops		= &vicodec_ctrl_ops,
1809 	.id		= V4L2_CID_STATELESS_FWHT_PARAMS,
1810 	.elem_size      = sizeof(struct v4l2_ctrl_fwht_params),
1811 };
1812 
1813 /*
1814  * File operations
1815  */
vicodec_open(struct file * file)1816 static int vicodec_open(struct file *file)
1817 {
1818 	const struct v4l2_fwht_pixfmt_info *info = v4l2_fwht_get_pixfmt(0);
1819 	struct video_device *vfd = video_devdata(file);
1820 	struct vicodec_dev *dev = video_drvdata(file);
1821 	struct vicodec_ctx *ctx = NULL;
1822 	struct v4l2_ctrl_handler *hdl;
1823 	unsigned int raw_size;
1824 	unsigned int comp_size;
1825 	int rc = 0;
1826 
1827 	if (mutex_lock_interruptible(vfd->lock))
1828 		return -ERESTARTSYS;
1829 	ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
1830 	if (!ctx) {
1831 		rc = -ENOMEM;
1832 		goto open_unlock;
1833 	}
1834 
1835 	if (vfd == &dev->stateful_enc.vfd)
1836 		ctx->is_enc = true;
1837 	else if (vfd == &dev->stateless_dec.vfd)
1838 		ctx->is_stateless = true;
1839 
1840 	v4l2_fh_init(&ctx->fh, video_devdata(file));
1841 	file->private_data = &ctx->fh;
1842 	ctx->dev = dev;
1843 	hdl = &ctx->hdl;
1844 	v4l2_ctrl_handler_init(hdl, 5);
1845 	v4l2_ctrl_new_std(hdl, &vicodec_ctrl_ops, V4L2_CID_MPEG_VIDEO_GOP_SIZE,
1846 			  1, 16, 1, 10);
1847 	v4l2_ctrl_new_std(hdl, &vicodec_ctrl_ops, V4L2_CID_FWHT_I_FRAME_QP,
1848 			  1, 31, 1, 20);
1849 	v4l2_ctrl_new_std(hdl, &vicodec_ctrl_ops, V4L2_CID_FWHT_P_FRAME_QP,
1850 			  1, 31, 1, 20);
1851 	if (ctx->is_enc)
1852 		v4l2_ctrl_new_std(hdl, &vicodec_ctrl_ops,
1853 				  V4L2_CID_MIN_BUFFERS_FOR_OUTPUT, 1, 1, 1, 1);
1854 	if (ctx->is_stateless)
1855 		v4l2_ctrl_new_custom(hdl, &vicodec_ctrl_stateless_state, NULL);
1856 	if (hdl->error) {
1857 		rc = hdl->error;
1858 		v4l2_ctrl_handler_free(hdl);
1859 		kfree(ctx);
1860 		goto open_unlock;
1861 	}
1862 	ctx->fh.ctrl_handler = hdl;
1863 	v4l2_ctrl_handler_setup(hdl);
1864 
1865 	if (ctx->is_enc)
1866 		ctx->q_data[V4L2_M2M_SRC].info = info;
1867 	else if (ctx->is_stateless)
1868 		ctx->q_data[V4L2_M2M_SRC].info = &pixfmt_stateless_fwht;
1869 	else
1870 		ctx->q_data[V4L2_M2M_SRC].info = &pixfmt_fwht;
1871 	ctx->q_data[V4L2_M2M_SRC].coded_width = 1280;
1872 	ctx->q_data[V4L2_M2M_SRC].coded_height = 720;
1873 	ctx->q_data[V4L2_M2M_SRC].visible_width = 1280;
1874 	ctx->q_data[V4L2_M2M_SRC].visible_height = 720;
1875 	raw_size = 1280 * 720 * info->sizeimage_mult / info->sizeimage_div;
1876 	comp_size = 1280 * 720 * pixfmt_fwht.sizeimage_mult /
1877 				 pixfmt_fwht.sizeimage_div;
1878 	if (ctx->is_enc)
1879 		ctx->q_data[V4L2_M2M_SRC].sizeimage = raw_size;
1880 	else if (ctx->is_stateless)
1881 		ctx->q_data[V4L2_M2M_SRC].sizeimage = comp_size;
1882 	else
1883 		ctx->q_data[V4L2_M2M_SRC].sizeimage =
1884 			comp_size + sizeof(struct fwht_cframe_hdr);
1885 	ctx->q_data[V4L2_M2M_DST] = ctx->q_data[V4L2_M2M_SRC];
1886 	if (ctx->is_enc) {
1887 		ctx->q_data[V4L2_M2M_DST].info = &pixfmt_fwht;
1888 		ctx->q_data[V4L2_M2M_DST].sizeimage =
1889 			comp_size + sizeof(struct fwht_cframe_hdr);
1890 	} else {
1891 		ctx->q_data[V4L2_M2M_DST].info = info;
1892 		ctx->q_data[V4L2_M2M_DST].sizeimage = raw_size;
1893 	}
1894 
1895 	ctx->state.colorspace = V4L2_COLORSPACE_REC709;
1896 
1897 	if (ctx->is_enc) {
1898 		ctx->fh.m2m_ctx = v4l2_m2m_ctx_init(dev->stateful_enc.m2m_dev,
1899 						    ctx, &queue_init);
1900 		ctx->lock = &dev->stateful_enc.lock;
1901 	} else if (ctx->is_stateless) {
1902 		ctx->fh.m2m_ctx = v4l2_m2m_ctx_init(dev->stateless_dec.m2m_dev,
1903 						    ctx, &queue_init);
1904 		ctx->lock = &dev->stateless_dec.lock;
1905 	} else {
1906 		ctx->fh.m2m_ctx = v4l2_m2m_ctx_init(dev->stateful_dec.m2m_dev,
1907 						    ctx, &queue_init);
1908 		ctx->lock = &dev->stateful_dec.lock;
1909 	}
1910 
1911 	if (IS_ERR(ctx->fh.m2m_ctx)) {
1912 		rc = PTR_ERR(ctx->fh.m2m_ctx);
1913 
1914 		v4l2_ctrl_handler_free(hdl);
1915 		v4l2_fh_exit(&ctx->fh);
1916 		kfree(ctx);
1917 		goto open_unlock;
1918 	}
1919 
1920 	v4l2_fh_add(&ctx->fh);
1921 
1922 open_unlock:
1923 	mutex_unlock(vfd->lock);
1924 	return rc;
1925 }
1926 
vicodec_release(struct file * file)1927 static int vicodec_release(struct file *file)
1928 {
1929 	struct video_device *vfd = video_devdata(file);
1930 	struct vicodec_ctx *ctx = file2ctx(file);
1931 
1932 	mutex_lock(vfd->lock);
1933 	v4l2_m2m_ctx_release(ctx->fh.m2m_ctx);
1934 	mutex_unlock(vfd->lock);
1935 	v4l2_fh_del(&ctx->fh);
1936 	v4l2_fh_exit(&ctx->fh);
1937 	v4l2_ctrl_handler_free(&ctx->hdl);
1938 	kvfree(ctx->state.compressed_frame);
1939 	kfree(ctx);
1940 
1941 	return 0;
1942 }
1943 
vicodec_request_validate(struct media_request * req)1944 static int vicodec_request_validate(struct media_request *req)
1945 {
1946 	struct media_request_object *obj;
1947 	struct v4l2_ctrl_handler *parent_hdl, *hdl;
1948 	struct vicodec_ctx *ctx = NULL;
1949 	struct v4l2_ctrl *ctrl;
1950 	unsigned int count;
1951 
1952 	list_for_each_entry(obj, &req->objects, list) {
1953 		struct vb2_buffer *vb;
1954 
1955 		if (vb2_request_object_is_buffer(obj)) {
1956 			vb = container_of(obj, struct vb2_buffer, req_obj);
1957 			ctx = vb2_get_drv_priv(vb->vb2_queue);
1958 
1959 			break;
1960 		}
1961 	}
1962 
1963 	if (!ctx) {
1964 		pr_err("No buffer was provided with the request\n");
1965 		return -ENOENT;
1966 	}
1967 
1968 	count = vb2_request_buffer_cnt(req);
1969 	if (!count) {
1970 		v4l2_info(&ctx->dev->v4l2_dev,
1971 			  "No buffer was provided with the request\n");
1972 		return -ENOENT;
1973 	} else if (count > 1) {
1974 		v4l2_info(&ctx->dev->v4l2_dev,
1975 			  "More than one buffer was provided with the request\n");
1976 		return -EINVAL;
1977 	}
1978 
1979 	parent_hdl = &ctx->hdl;
1980 
1981 	hdl = v4l2_ctrl_request_hdl_find(req, parent_hdl);
1982 	if (!hdl) {
1983 		v4l2_info(&ctx->dev->v4l2_dev, "Missing codec control\n");
1984 		return -ENOENT;
1985 	}
1986 	ctrl = v4l2_ctrl_request_hdl_ctrl_find(hdl,
1987 					       vicodec_ctrl_stateless_state.id);
1988 	v4l2_ctrl_request_hdl_put(hdl);
1989 	if (!ctrl) {
1990 		v4l2_info(&ctx->dev->v4l2_dev,
1991 			  "Missing required codec control\n");
1992 		return -ENOENT;
1993 	}
1994 
1995 	return vb2_request_validate(req);
1996 }
1997 
1998 static const struct v4l2_file_operations vicodec_fops = {
1999 	.owner		= THIS_MODULE,
2000 	.open		= vicodec_open,
2001 	.release	= vicodec_release,
2002 	.poll		= v4l2_m2m_fop_poll,
2003 	.unlocked_ioctl	= video_ioctl2,
2004 	.mmap		= v4l2_m2m_fop_mmap,
2005 };
2006 
2007 static const struct video_device vicodec_videodev = {
2008 	.name		= VICODEC_NAME,
2009 	.vfl_dir	= VFL_DIR_M2M,
2010 	.fops		= &vicodec_fops,
2011 	.ioctl_ops	= &vicodec_ioctl_ops,
2012 	.minor		= -1,
2013 	.release	= video_device_release_empty,
2014 };
2015 
2016 static const struct media_device_ops vicodec_m2m_media_ops = {
2017 	.req_validate	= vicodec_request_validate,
2018 	.req_queue	= v4l2_m2m_request_queue,
2019 };
2020 
2021 static const struct v4l2_m2m_ops m2m_ops = {
2022 	.device_run	= device_run,
2023 	.job_ready	= job_ready,
2024 };
2025 
register_instance(struct vicodec_dev * dev,struct vicodec_dev_instance * dev_instance,const char * name,bool is_enc)2026 static int register_instance(struct vicodec_dev *dev,
2027 			     struct vicodec_dev_instance *dev_instance,
2028 			     const char *name, bool is_enc)
2029 {
2030 	struct video_device *vfd;
2031 	int ret;
2032 
2033 	spin_lock_init(&dev_instance->lock);
2034 	mutex_init(&dev_instance->mutex);
2035 	dev_instance->m2m_dev = v4l2_m2m_init(&m2m_ops);
2036 	if (IS_ERR(dev_instance->m2m_dev)) {
2037 		v4l2_err(&dev->v4l2_dev, "Failed to init vicodec enc device\n");
2038 		return PTR_ERR(dev_instance->m2m_dev);
2039 	}
2040 
2041 	dev_instance->vfd = vicodec_videodev;
2042 	vfd = &dev_instance->vfd;
2043 	vfd->lock = &dev_instance->mutex;
2044 	vfd->v4l2_dev = &dev->v4l2_dev;
2045 	strscpy(vfd->name, name, sizeof(vfd->name));
2046 	vfd->device_caps = V4L2_CAP_STREAMING |
2047 		(multiplanar ? V4L2_CAP_VIDEO_M2M_MPLANE : V4L2_CAP_VIDEO_M2M);
2048 	if (is_enc) {
2049 		v4l2_disable_ioctl(vfd, VIDIOC_DECODER_CMD);
2050 		v4l2_disable_ioctl(vfd, VIDIOC_TRY_DECODER_CMD);
2051 	} else {
2052 		v4l2_disable_ioctl(vfd, VIDIOC_ENCODER_CMD);
2053 		v4l2_disable_ioctl(vfd, VIDIOC_TRY_ENCODER_CMD);
2054 	}
2055 	video_set_drvdata(vfd, dev);
2056 
2057 	ret = video_register_device(vfd, VFL_TYPE_VIDEO, 0);
2058 	if (ret) {
2059 		v4l2_err(&dev->v4l2_dev, "Failed to register video device '%s'\n", name);
2060 		v4l2_m2m_release(dev_instance->m2m_dev);
2061 		return ret;
2062 	}
2063 	v4l2_info(&dev->v4l2_dev, "Device '%s' registered as /dev/video%d\n",
2064 		  name, vfd->num);
2065 	return 0;
2066 }
2067 
vicodec_v4l2_dev_release(struct v4l2_device * v4l2_dev)2068 static void vicodec_v4l2_dev_release(struct v4l2_device *v4l2_dev)
2069 {
2070 	struct vicodec_dev *dev = container_of(v4l2_dev, struct vicodec_dev, v4l2_dev);
2071 
2072 	v4l2_device_unregister(&dev->v4l2_dev);
2073 	v4l2_m2m_release(dev->stateful_enc.m2m_dev);
2074 	v4l2_m2m_release(dev->stateful_dec.m2m_dev);
2075 	v4l2_m2m_release(dev->stateless_dec.m2m_dev);
2076 #ifdef CONFIG_MEDIA_CONTROLLER
2077 	media_device_cleanup(&dev->mdev);
2078 #endif
2079 	kfree(dev);
2080 }
2081 
vicodec_probe(struct platform_device * pdev)2082 static int vicodec_probe(struct platform_device *pdev)
2083 {
2084 	struct vicodec_dev *dev;
2085 	int ret;
2086 
2087 	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
2088 	if (!dev)
2089 		return -ENOMEM;
2090 
2091 	ret = v4l2_device_register(&pdev->dev, &dev->v4l2_dev);
2092 	if (ret)
2093 		goto free_dev;
2094 
2095 	dev->v4l2_dev.release = vicodec_v4l2_dev_release;
2096 
2097 #ifdef CONFIG_MEDIA_CONTROLLER
2098 	dev->mdev.dev = &pdev->dev;
2099 	strscpy(dev->mdev.model, "vicodec", sizeof(dev->mdev.model));
2100 	strscpy(dev->mdev.bus_info, "platform:vicodec",
2101 		sizeof(dev->mdev.bus_info));
2102 	media_device_init(&dev->mdev);
2103 	dev->mdev.ops = &vicodec_m2m_media_ops;
2104 	dev->v4l2_dev.mdev = &dev->mdev;
2105 #endif
2106 
2107 	platform_set_drvdata(pdev, dev);
2108 
2109 	ret = register_instance(dev, &dev->stateful_enc, "stateful-encoder",
2110 				true);
2111 	if (ret)
2112 		goto unreg_dev;
2113 
2114 	ret = register_instance(dev, &dev->stateful_dec, "stateful-decoder",
2115 				false);
2116 	if (ret)
2117 		goto unreg_sf_enc;
2118 
2119 	ret = register_instance(dev, &dev->stateless_dec, "stateless-decoder",
2120 				false);
2121 	if (ret)
2122 		goto unreg_sf_dec;
2123 
2124 #ifdef CONFIG_MEDIA_CONTROLLER
2125 	ret = v4l2_m2m_register_media_controller(dev->stateful_enc.m2m_dev,
2126 						 &dev->stateful_enc.vfd,
2127 						 MEDIA_ENT_F_PROC_VIDEO_ENCODER);
2128 	if (ret) {
2129 		v4l2_err(&dev->v4l2_dev, "Failed to init mem2mem media controller for enc\n");
2130 		goto unreg_m2m;
2131 	}
2132 
2133 	ret = v4l2_m2m_register_media_controller(dev->stateful_dec.m2m_dev,
2134 						 &dev->stateful_dec.vfd,
2135 						 MEDIA_ENT_F_PROC_VIDEO_DECODER);
2136 	if (ret) {
2137 		v4l2_err(&dev->v4l2_dev, "Failed to init mem2mem media controller for dec\n");
2138 		goto unreg_m2m_sf_enc_mc;
2139 	}
2140 
2141 	ret = v4l2_m2m_register_media_controller(dev->stateless_dec.m2m_dev,
2142 						 &dev->stateless_dec.vfd,
2143 						 MEDIA_ENT_F_PROC_VIDEO_DECODER);
2144 	if (ret) {
2145 		v4l2_err(&dev->v4l2_dev, "Failed to init mem2mem media controller for stateless dec\n");
2146 		goto unreg_m2m_sf_dec_mc;
2147 	}
2148 
2149 	ret = media_device_register(&dev->mdev);
2150 	if (ret) {
2151 		v4l2_err(&dev->v4l2_dev, "Failed to register mem2mem media device\n");
2152 		goto unreg_m2m_sl_dec_mc;
2153 	}
2154 #endif
2155 	return 0;
2156 
2157 #ifdef CONFIG_MEDIA_CONTROLLER
2158 unreg_m2m_sl_dec_mc:
2159 	v4l2_m2m_unregister_media_controller(dev->stateless_dec.m2m_dev);
2160 unreg_m2m_sf_dec_mc:
2161 	v4l2_m2m_unregister_media_controller(dev->stateful_dec.m2m_dev);
2162 unreg_m2m_sf_enc_mc:
2163 	v4l2_m2m_unregister_media_controller(dev->stateful_enc.m2m_dev);
2164 unreg_m2m:
2165 	video_unregister_device(&dev->stateless_dec.vfd);
2166 	v4l2_m2m_release(dev->stateless_dec.m2m_dev);
2167 #endif
2168 unreg_sf_dec:
2169 	video_unregister_device(&dev->stateful_dec.vfd);
2170 	v4l2_m2m_release(dev->stateful_dec.m2m_dev);
2171 unreg_sf_enc:
2172 	video_unregister_device(&dev->stateful_enc.vfd);
2173 	v4l2_m2m_release(dev->stateful_enc.m2m_dev);
2174 unreg_dev:
2175 	v4l2_device_unregister(&dev->v4l2_dev);
2176 free_dev:
2177 	kfree(dev);
2178 
2179 	return ret;
2180 }
2181 
vicodec_remove(struct platform_device * pdev)2182 static void vicodec_remove(struct platform_device *pdev)
2183 {
2184 	struct vicodec_dev *dev = platform_get_drvdata(pdev);
2185 
2186 	v4l2_info(&dev->v4l2_dev, "Removing " VICODEC_NAME);
2187 
2188 #ifdef CONFIG_MEDIA_CONTROLLER
2189 	media_device_unregister(&dev->mdev);
2190 	v4l2_m2m_unregister_media_controller(dev->stateful_enc.m2m_dev);
2191 	v4l2_m2m_unregister_media_controller(dev->stateful_dec.m2m_dev);
2192 	v4l2_m2m_unregister_media_controller(dev->stateless_dec.m2m_dev);
2193 #endif
2194 
2195 	video_unregister_device(&dev->stateful_enc.vfd);
2196 	video_unregister_device(&dev->stateful_dec.vfd);
2197 	video_unregister_device(&dev->stateless_dec.vfd);
2198 	v4l2_device_put(&dev->v4l2_dev);
2199 }
2200 
2201 static struct platform_driver vicodec_pdrv = {
2202 	.probe		= vicodec_probe,
2203 	.remove_new	= vicodec_remove,
2204 	.driver		= {
2205 		.name	= VICODEC_NAME,
2206 	},
2207 };
2208 
vicodec_exit(void)2209 static void __exit vicodec_exit(void)
2210 {
2211 	platform_driver_unregister(&vicodec_pdrv);
2212 	platform_device_unregister(&vicodec_pdev);
2213 }
2214 
vicodec_init(void)2215 static int __init vicodec_init(void)
2216 {
2217 	int ret;
2218 
2219 	ret = platform_device_register(&vicodec_pdev);
2220 	if (ret)
2221 		return ret;
2222 
2223 	ret = platform_driver_register(&vicodec_pdrv);
2224 	if (ret)
2225 		platform_device_unregister(&vicodec_pdev);
2226 
2227 	return ret;
2228 }
2229 
2230 module_init(vicodec_init);
2231 module_exit(vicodec_exit);
2232