1 // SPDX-License-Identifier: LGPL-2.1
2 /*
3  * A V4L2 frontend for the FWHT codec
4  *
5  * Copyright 2018 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
6  */
7 
8 #include <linux/errno.h>
9 #include <linux/string.h>
10 #include <linux/videodev2.h>
11 #include "codec-v4l2-fwht.h"
12 
13 static const struct v4l2_fwht_pixfmt_info v4l2_fwht_pixfmts[] = {
14 	{ V4L2_PIX_FMT_YUV420,  1, 3, 2, 1, 1, 2, 2, 3, 3, V4L2_FWHT_FL_PIXENC_YUV},
15 	{ V4L2_PIX_FMT_YVU420,  1, 3, 2, 1, 1, 2, 2, 3, 3, V4L2_FWHT_FL_PIXENC_YUV},
16 	{ V4L2_PIX_FMT_YUV422P, 1, 2, 1, 1, 1, 2, 1, 3, 3, V4L2_FWHT_FL_PIXENC_YUV},
17 	{ V4L2_PIX_FMT_NV12,    1, 3, 2, 1, 2, 2, 2, 3, 2, V4L2_FWHT_FL_PIXENC_YUV},
18 	{ V4L2_PIX_FMT_NV21,    1, 3, 2, 1, 2, 2, 2, 3, 2, V4L2_FWHT_FL_PIXENC_YUV},
19 	{ V4L2_PIX_FMT_NV16,    1, 2, 1, 1, 2, 2, 1, 3, 2, V4L2_FWHT_FL_PIXENC_YUV},
20 	{ V4L2_PIX_FMT_NV61,    1, 2, 1, 1, 2, 2, 1, 3, 2, V4L2_FWHT_FL_PIXENC_YUV},
21 	{ V4L2_PIX_FMT_NV24,    1, 3, 1, 1, 2, 1, 1, 3, 2, V4L2_FWHT_FL_PIXENC_YUV},
22 	{ V4L2_PIX_FMT_NV42,    1, 3, 1, 1, 2, 1, 1, 3, 2, V4L2_FWHT_FL_PIXENC_YUV},
23 	{ V4L2_PIX_FMT_YUYV,    2, 2, 1, 2, 4, 2, 1, 3, 1, V4L2_FWHT_FL_PIXENC_YUV},
24 	{ V4L2_PIX_FMT_YVYU,    2, 2, 1, 2, 4, 2, 1, 3, 1, V4L2_FWHT_FL_PIXENC_YUV},
25 	{ V4L2_PIX_FMT_UYVY,    2, 2, 1, 2, 4, 2, 1, 3, 1, V4L2_FWHT_FL_PIXENC_YUV},
26 	{ V4L2_PIX_FMT_VYUY,    2, 2, 1, 2, 4, 2, 1, 3, 1, V4L2_FWHT_FL_PIXENC_YUV},
27 	{ V4L2_PIX_FMT_BGR24,   3, 3, 1, 3, 3, 1, 1, 3, 1, V4L2_FWHT_FL_PIXENC_RGB},
28 	{ V4L2_PIX_FMT_RGB24,   3, 3, 1, 3, 3, 1, 1, 3, 1, V4L2_FWHT_FL_PIXENC_RGB},
29 	{ V4L2_PIX_FMT_HSV24,   3, 3, 1, 3, 3, 1, 1, 3, 1, V4L2_FWHT_FL_PIXENC_HSV},
30 	{ V4L2_PIX_FMT_BGR32,   4, 4, 1, 4, 4, 1, 1, 4, 1, V4L2_FWHT_FL_PIXENC_RGB},
31 	{ V4L2_PIX_FMT_XBGR32,  4, 4, 1, 4, 4, 1, 1, 4, 1, V4L2_FWHT_FL_PIXENC_RGB},
32 	{ V4L2_PIX_FMT_ABGR32,  4, 4, 1, 4, 4, 1, 1, 4, 1, V4L2_FWHT_FL_PIXENC_RGB},
33 	{ V4L2_PIX_FMT_RGB32,   4, 4, 1, 4, 4, 1, 1, 4, 1, V4L2_FWHT_FL_PIXENC_RGB},
34 	{ V4L2_PIX_FMT_XRGB32,  4, 4, 1, 4, 4, 1, 1, 4, 1, V4L2_FWHT_FL_PIXENC_RGB},
35 	{ V4L2_PIX_FMT_ARGB32,  4, 4, 1, 4, 4, 1, 1, 4, 1, V4L2_FWHT_FL_PIXENC_RGB},
36 	{ V4L2_PIX_FMT_BGRX32,  4, 4, 1, 4, 4, 1, 1, 4, 1, V4L2_FWHT_FL_PIXENC_RGB},
37 	{ V4L2_PIX_FMT_BGRA32,  4, 4, 1, 4, 4, 1, 1, 4, 1, V4L2_FWHT_FL_PIXENC_RGB},
38 	{ V4L2_PIX_FMT_RGBX32,  4, 4, 1, 4, 4, 1, 1, 4, 1, V4L2_FWHT_FL_PIXENC_RGB},
39 	{ V4L2_PIX_FMT_RGBA32,  4, 4, 1, 4, 4, 1, 1, 4, 1, V4L2_FWHT_FL_PIXENC_RGB},
40 	{ V4L2_PIX_FMT_HSV32,   4, 4, 1, 4, 4, 1, 1, 4, 1, V4L2_FWHT_FL_PIXENC_HSV},
41 	{ V4L2_PIX_FMT_GREY,    1, 1, 1, 1, 0, 1, 1, 1, 1, V4L2_FWHT_FL_PIXENC_RGB},
42 };
43 
v4l2_fwht_validate_fmt(const struct v4l2_fwht_pixfmt_info * info,u32 width_div,u32 height_div,u32 components_num,u32 pixenc)44 bool v4l2_fwht_validate_fmt(const struct v4l2_fwht_pixfmt_info *info,
45 			    u32 width_div, u32 height_div, u32 components_num,
46 			    u32 pixenc)
47 {
48 	if (info->width_div == width_div &&
49 	    info->height_div == height_div &&
50 	    (!pixenc || info->pixenc == pixenc) &&
51 	    info->components_num == components_num)
52 		return true;
53 	return false;
54 }
55 
v4l2_fwht_find_nth_fmt(u32 width_div,u32 height_div,u32 components_num,u32 pixenc,unsigned int start_idx)56 const struct v4l2_fwht_pixfmt_info *v4l2_fwht_find_nth_fmt(u32 width_div,
57 							  u32 height_div,
58 							  u32 components_num,
59 							  u32 pixenc,
60 							  unsigned int start_idx)
61 {
62 	unsigned int i;
63 
64 	for (i = 0; i < ARRAY_SIZE(v4l2_fwht_pixfmts); i++) {
65 		bool is_valid = v4l2_fwht_validate_fmt(&v4l2_fwht_pixfmts[i],
66 						       width_div, height_div,
67 						       components_num, pixenc);
68 		if (is_valid) {
69 			if (start_idx == 0)
70 				return v4l2_fwht_pixfmts + i;
71 			start_idx--;
72 		}
73 	}
74 	return NULL;
75 }
76 
v4l2_fwht_find_pixfmt(u32 pixelformat)77 const struct v4l2_fwht_pixfmt_info *v4l2_fwht_find_pixfmt(u32 pixelformat)
78 {
79 	unsigned int i;
80 
81 	for (i = 0; i < ARRAY_SIZE(v4l2_fwht_pixfmts); i++)
82 		if (v4l2_fwht_pixfmts[i].id == pixelformat)
83 			return v4l2_fwht_pixfmts + i;
84 	return NULL;
85 }
86 
v4l2_fwht_get_pixfmt(u32 idx)87 const struct v4l2_fwht_pixfmt_info *v4l2_fwht_get_pixfmt(u32 idx)
88 {
89 	if (idx >= ARRAY_SIZE(v4l2_fwht_pixfmts))
90 		return NULL;
91 	return v4l2_fwht_pixfmts + idx;
92 }
93 
prepare_raw_frame(struct fwht_raw_frame * rf,const struct v4l2_fwht_pixfmt_info * info,u8 * buf,unsigned int size)94 static int prepare_raw_frame(struct fwht_raw_frame *rf,
95 			 const struct v4l2_fwht_pixfmt_info *info, u8 *buf,
96 			 unsigned int size)
97 {
98 	rf->luma = buf;
99 	rf->width_div = info->width_div;
100 	rf->height_div = info->height_div;
101 	rf->luma_alpha_step = info->luma_alpha_step;
102 	rf->chroma_step = info->chroma_step;
103 	rf->alpha = NULL;
104 	rf->components_num = info->components_num;
105 
106 	/*
107 	 * The buffer is NULL if it is the reference
108 	 * frame of an I-frame in the stateless decoder
109 	 */
110 	if (!buf) {
111 		rf->luma = NULL;
112 		rf->cb = NULL;
113 		rf->cr = NULL;
114 		rf->alpha = NULL;
115 		return 0;
116 	}
117 	switch (info->id) {
118 	case V4L2_PIX_FMT_GREY:
119 		rf->cb = NULL;
120 		rf->cr = NULL;
121 		break;
122 	case V4L2_PIX_FMT_YUV420:
123 		rf->cb = rf->luma + size;
124 		rf->cr = rf->cb + size / 4;
125 		break;
126 	case V4L2_PIX_FMT_YVU420:
127 		rf->cr = rf->luma + size;
128 		rf->cb = rf->cr + size / 4;
129 		break;
130 	case V4L2_PIX_FMT_YUV422P:
131 		rf->cb = rf->luma + size;
132 		rf->cr = rf->cb + size / 2;
133 		break;
134 	case V4L2_PIX_FMT_NV12:
135 	case V4L2_PIX_FMT_NV16:
136 	case V4L2_PIX_FMT_NV24:
137 		rf->cb = rf->luma + size;
138 		rf->cr = rf->cb + 1;
139 		break;
140 	case V4L2_PIX_FMT_NV21:
141 	case V4L2_PIX_FMT_NV61:
142 	case V4L2_PIX_FMT_NV42:
143 		rf->cr = rf->luma + size;
144 		rf->cb = rf->cr + 1;
145 		break;
146 	case V4L2_PIX_FMT_YUYV:
147 		rf->cb = rf->luma + 1;
148 		rf->cr = rf->cb + 2;
149 		break;
150 	case V4L2_PIX_FMT_YVYU:
151 		rf->cr = rf->luma + 1;
152 		rf->cb = rf->cr + 2;
153 		break;
154 	case V4L2_PIX_FMT_UYVY:
155 		rf->cb = rf->luma;
156 		rf->cr = rf->cb + 2;
157 		rf->luma++;
158 		break;
159 	case V4L2_PIX_FMT_VYUY:
160 		rf->cr = rf->luma;
161 		rf->cb = rf->cr + 2;
162 		rf->luma++;
163 		break;
164 	case V4L2_PIX_FMT_RGB24:
165 	case V4L2_PIX_FMT_HSV24:
166 		rf->cr = rf->luma;
167 		rf->cb = rf->cr + 2;
168 		rf->luma++;
169 		break;
170 	case V4L2_PIX_FMT_BGR24:
171 		rf->cb = rf->luma;
172 		rf->cr = rf->cb + 2;
173 		rf->luma++;
174 		break;
175 	case V4L2_PIX_FMT_RGB32:
176 	case V4L2_PIX_FMT_XRGB32:
177 	case V4L2_PIX_FMT_HSV32:
178 	case V4L2_PIX_FMT_ARGB32:
179 		rf->alpha = rf->luma;
180 		rf->cr = rf->luma + 1;
181 		rf->cb = rf->cr + 2;
182 		rf->luma += 2;
183 		break;
184 	case V4L2_PIX_FMT_BGR32:
185 	case V4L2_PIX_FMT_XBGR32:
186 	case V4L2_PIX_FMT_ABGR32:
187 		rf->cb = rf->luma;
188 		rf->cr = rf->cb + 2;
189 		rf->luma++;
190 		rf->alpha = rf->cr + 1;
191 		break;
192 	case V4L2_PIX_FMT_BGRX32:
193 	case V4L2_PIX_FMT_BGRA32:
194 		rf->alpha = rf->luma;
195 		rf->cb = rf->luma + 1;
196 		rf->cr = rf->cb + 2;
197 		rf->luma += 2;
198 		break;
199 	case V4L2_PIX_FMT_RGBX32:
200 	case V4L2_PIX_FMT_RGBA32:
201 		rf->alpha = rf->luma + 3;
202 		rf->cr = rf->luma;
203 		rf->cb = rf->cr + 2;
204 		rf->luma++;
205 		break;
206 	default:
207 		return -EINVAL;
208 	}
209 	return 0;
210 }
211 
v4l2_fwht_encode(struct v4l2_fwht_state * state,u8 * p_in,u8 * p_out)212 int v4l2_fwht_encode(struct v4l2_fwht_state *state, u8 *p_in, u8 *p_out)
213 {
214 	unsigned int size = state->stride * state->coded_height;
215 	unsigned int chroma_stride = state->stride;
216 	const struct v4l2_fwht_pixfmt_info *info = state->info;
217 	struct fwht_cframe_hdr *p_hdr;
218 	struct fwht_cframe cf;
219 	struct fwht_raw_frame rf;
220 	u32 encoding;
221 	u32 flags = 0;
222 
223 	if (!info)
224 		return -EINVAL;
225 
226 	if (prepare_raw_frame(&rf, info, p_in, size))
227 		return -EINVAL;
228 
229 	if (info->planes_num == 3)
230 		chroma_stride /= 2;
231 
232 	if (info->id == V4L2_PIX_FMT_NV24 ||
233 	    info->id == V4L2_PIX_FMT_NV42)
234 		chroma_stride *= 2;
235 
236 	cf.i_frame_qp = state->i_frame_qp;
237 	cf.p_frame_qp = state->p_frame_qp;
238 	cf.rlc_data = (__be16 *)(p_out + sizeof(*p_hdr));
239 
240 	encoding = fwht_encode_frame(&rf, &state->ref_frame, &cf,
241 				     !state->gop_cnt,
242 				     state->gop_cnt == state->gop_size - 1,
243 				     state->visible_width,
244 				     state->visible_height,
245 				     state->stride, chroma_stride);
246 	if (!(encoding & FWHT_FRAME_PCODED))
247 		state->gop_cnt = 0;
248 	if (++state->gop_cnt >= state->gop_size)
249 		state->gop_cnt = 0;
250 
251 	p_hdr = (struct fwht_cframe_hdr *)p_out;
252 	p_hdr->magic1 = FWHT_MAGIC1;
253 	p_hdr->magic2 = FWHT_MAGIC2;
254 	p_hdr->version = htonl(V4L2_FWHT_VERSION);
255 	p_hdr->width = htonl(state->visible_width);
256 	p_hdr->height = htonl(state->visible_height);
257 	flags |= (info->components_num - 1) << V4L2_FWHT_FL_COMPONENTS_NUM_OFFSET;
258 	flags |= info->pixenc;
259 	if (encoding & FWHT_LUMA_UNENCODED)
260 		flags |= V4L2_FWHT_FL_LUMA_IS_UNCOMPRESSED;
261 	if (encoding & FWHT_CB_UNENCODED)
262 		flags |= V4L2_FWHT_FL_CB_IS_UNCOMPRESSED;
263 	if (encoding & FWHT_CR_UNENCODED)
264 		flags |= V4L2_FWHT_FL_CR_IS_UNCOMPRESSED;
265 	if (encoding & FWHT_ALPHA_UNENCODED)
266 		flags |= V4L2_FWHT_FL_ALPHA_IS_UNCOMPRESSED;
267 	if (!(encoding & FWHT_FRAME_PCODED))
268 		flags |= V4L2_FWHT_FL_I_FRAME;
269 	if (rf.height_div == 1)
270 		flags |= V4L2_FWHT_FL_CHROMA_FULL_HEIGHT;
271 	if (rf.width_div == 1)
272 		flags |= V4L2_FWHT_FL_CHROMA_FULL_WIDTH;
273 	p_hdr->flags = htonl(flags);
274 	p_hdr->colorspace = htonl(state->colorspace);
275 	p_hdr->xfer_func = htonl(state->xfer_func);
276 	p_hdr->ycbcr_enc = htonl(state->ycbcr_enc);
277 	p_hdr->quantization = htonl(state->quantization);
278 	p_hdr->size = htonl(cf.size);
279 	return cf.size + sizeof(*p_hdr);
280 }
281 
v4l2_fwht_decode(struct v4l2_fwht_state * state,u8 * p_in,u8 * p_out)282 int v4l2_fwht_decode(struct v4l2_fwht_state *state, u8 *p_in, u8 *p_out)
283 {
284 	u32 flags;
285 	struct fwht_cframe cf;
286 	unsigned int components_num = 3;
287 	unsigned int version;
288 	const struct v4l2_fwht_pixfmt_info *info;
289 	unsigned int hdr_width_div, hdr_height_div;
290 	struct fwht_raw_frame dst_rf;
291 	unsigned int dst_chroma_stride = state->stride;
292 	unsigned int ref_chroma_stride = state->ref_stride;
293 	unsigned int dst_size = state->stride * state->coded_height;
294 	unsigned int ref_size;
295 
296 	if (!state->info)
297 		return -EINVAL;
298 
299 	info = state->info;
300 
301 	version = ntohl(state->header.version);
302 	if (!version || version > V4L2_FWHT_VERSION) {
303 		pr_err("version %d is not supported, current version is %d\n",
304 		       version, V4L2_FWHT_VERSION);
305 		return -EINVAL;
306 	}
307 
308 	if (state->header.magic1 != FWHT_MAGIC1 ||
309 	    state->header.magic2 != FWHT_MAGIC2)
310 		return -EINVAL;
311 
312 	/* TODO: support resolution changes */
313 	if (ntohl(state->header.width)  != state->visible_width ||
314 	    ntohl(state->header.height) != state->visible_height)
315 		return -EINVAL;
316 
317 	flags = ntohl(state->header.flags);
318 
319 	if (version >= 2) {
320 		if ((flags & V4L2_FWHT_FL_PIXENC_MSK) != info->pixenc)
321 			return -EINVAL;
322 		components_num = 1 + ((flags & V4L2_FWHT_FL_COMPONENTS_NUM_MSK) >>
323 				V4L2_FWHT_FL_COMPONENTS_NUM_OFFSET);
324 	}
325 
326 	if (components_num != info->components_num)
327 		return -EINVAL;
328 
329 	state->colorspace = ntohl(state->header.colorspace);
330 	state->xfer_func = ntohl(state->header.xfer_func);
331 	state->ycbcr_enc = ntohl(state->header.ycbcr_enc);
332 	state->quantization = ntohl(state->header.quantization);
333 	cf.rlc_data = (__be16 *)p_in;
334 	cf.size = ntohl(state->header.size);
335 
336 	hdr_width_div = (flags & V4L2_FWHT_FL_CHROMA_FULL_WIDTH) ? 1 : 2;
337 	hdr_height_div = (flags & V4L2_FWHT_FL_CHROMA_FULL_HEIGHT) ? 1 : 2;
338 	if (hdr_width_div != info->width_div ||
339 	    hdr_height_div != info->height_div)
340 		return -EINVAL;
341 
342 	if (prepare_raw_frame(&dst_rf, info, p_out, dst_size))
343 		return -EINVAL;
344 	if (info->planes_num == 3) {
345 		dst_chroma_stride /= 2;
346 		ref_chroma_stride /= 2;
347 	}
348 	if (info->id == V4L2_PIX_FMT_NV24 ||
349 	    info->id == V4L2_PIX_FMT_NV42) {
350 		dst_chroma_stride *= 2;
351 		ref_chroma_stride *= 2;
352 	}
353 
354 
355 	ref_size = state->ref_stride * state->coded_height;
356 
357 	if (prepare_raw_frame(&state->ref_frame, info, state->ref_frame.buf,
358 			      ref_size))
359 		return -EINVAL;
360 
361 	if (!fwht_decode_frame(&cf, flags, components_num,
362 			state->visible_width, state->visible_height,
363 			&state->ref_frame, state->ref_stride, ref_chroma_stride,
364 			&dst_rf, state->stride, dst_chroma_stride))
365 		return -EINVAL;
366 	return 0;
367 }
368