1 // SPDX-License-Identifier: GPL-2.0-or-later
2 /*
3  * Coda multi-standard codec IP - H.264 helper functions
4  *
5  * Copyright (C) 2012 Vista Silicon S.L.
6  *    Javier Martin, <javier.martin@vista-silicon.com>
7  *    Xavier Duret
8  */
9 
10 #include <linux/kernel.h>
11 #include <linux/string.h>
12 #include <linux/videodev2.h>
13 
14 #include "coda.h"
15 
16 static const u8 coda_filler_size[8] = { 0, 7, 14, 13, 12, 11, 10, 9 };
17 
18 static const u8 *coda_find_nal_header(const u8 *buf, const u8 *end)
19 {
20 	u32 val = 0xffffffff;
21 
22 	do {
23 		val = val << 8 | *buf++;
24 		if (buf >= end)
25 			return NULL;
26 	} while (val != 0x00000001);
27 
28 	return buf;
29 }
30 
31 int coda_sps_parse_profile(struct coda_ctx *ctx, struct vb2_buffer *vb)
32 {
33 	const u8 *buf = vb2_plane_vaddr(vb, 0);
34 	const u8 *end = buf + vb2_get_plane_payload(vb, 0);
35 
36 	/* Find SPS header */
37 	do {
38 		buf = coda_find_nal_header(buf, end);
39 		if (!buf)
40 			return -EINVAL;
41 	} while ((*buf++ & 0x1f) != 0x7);
42 
43 	ctx->params.h264_profile_idc = buf[0];
44 	ctx->params.h264_level_idc = buf[2];
45 
46 	return 0;
47 }
48 
49 int coda_h264_filler_nal(int size, char *p)
50 {
51 	if (size < 6)
52 		return -EINVAL;
53 
54 	p[0] = 0x00;
55 	p[1] = 0x00;
56 	p[2] = 0x00;
57 	p[3] = 0x01;
58 	p[4] = 0x0c;
59 	memset(p + 5, 0xff, size - 6);
60 	/* Add rbsp stop bit and trailing at the end */
61 	p[size - 1] = 0x80;
62 
63 	return 0;
64 }
65 
66 int coda_h264_padding(int size, char *p)
67 {
68 	int nal_size;
69 	int diff;
70 
71 	diff = size - (size & ~0x7);
72 	if (diff == 0)
73 		return 0;
74 
75 	nal_size = coda_filler_size[diff];
76 	coda_h264_filler_nal(nal_size, p);
77 
78 	return nal_size;
79 }
80 
81 int coda_h264_profile(int profile_idc)
82 {
83 	switch (profile_idc) {
84 	case 66: return V4L2_MPEG_VIDEO_H264_PROFILE_BASELINE;
85 	case 77: return V4L2_MPEG_VIDEO_H264_PROFILE_MAIN;
86 	case 88: return V4L2_MPEG_VIDEO_H264_PROFILE_EXTENDED;
87 	case 100: return V4L2_MPEG_VIDEO_H264_PROFILE_HIGH;
88 	default: return -EINVAL;
89 	}
90 }
91 
92 int coda_h264_level(int level_idc)
93 {
94 	switch (level_idc) {
95 	case 10: return V4L2_MPEG_VIDEO_H264_LEVEL_1_0;
96 	case 9:  return V4L2_MPEG_VIDEO_H264_LEVEL_1B;
97 	case 11: return V4L2_MPEG_VIDEO_H264_LEVEL_1_1;
98 	case 12: return V4L2_MPEG_VIDEO_H264_LEVEL_1_2;
99 	case 13: return V4L2_MPEG_VIDEO_H264_LEVEL_1_3;
100 	case 20: return V4L2_MPEG_VIDEO_H264_LEVEL_2_0;
101 	case 21: return V4L2_MPEG_VIDEO_H264_LEVEL_2_1;
102 	case 22: return V4L2_MPEG_VIDEO_H264_LEVEL_2_2;
103 	case 30: return V4L2_MPEG_VIDEO_H264_LEVEL_3_0;
104 	case 31: return V4L2_MPEG_VIDEO_H264_LEVEL_3_1;
105 	case 32: return V4L2_MPEG_VIDEO_H264_LEVEL_3_2;
106 	case 40: return V4L2_MPEG_VIDEO_H264_LEVEL_4_0;
107 	case 41: return V4L2_MPEG_VIDEO_H264_LEVEL_4_1;
108 	case 42: return V4L2_MPEG_VIDEO_H264_LEVEL_4_2;
109 	case 50: return V4L2_MPEG_VIDEO_H264_LEVEL_5_0;
110 	case 51: return V4L2_MPEG_VIDEO_H264_LEVEL_5_1;
111 	default: return -EINVAL;
112 	}
113 }
114 
115 struct rbsp {
116 	char *buf;
117 	int size;
118 	int pos;
119 };
120 
121 static inline int rbsp_read_bit(struct rbsp *rbsp)
122 {
123 	int shift = 7 - (rbsp->pos % 8);
124 	int ofs = rbsp->pos++ / 8;
125 
126 	if (ofs >= rbsp->size)
127 		return -EINVAL;
128 
129 	return (rbsp->buf[ofs] >> shift) & 1;
130 }
131 
132 static inline int rbsp_write_bit(struct rbsp *rbsp, int bit)
133 {
134 	int shift = 7 - (rbsp->pos % 8);
135 	int ofs = rbsp->pos++ / 8;
136 
137 	if (ofs >= rbsp->size)
138 		return -EINVAL;
139 
140 	rbsp->buf[ofs] &= ~(1 << shift);
141 	rbsp->buf[ofs] |= bit << shift;
142 
143 	return 0;
144 }
145 
146 static inline int rbsp_read_bits(struct rbsp *rbsp, int num, int *val)
147 {
148 	int i, ret;
149 	int tmp = 0;
150 
151 	if (num > 32)
152 		return -EINVAL;
153 
154 	for (i = 0; i < num; i++) {
155 		ret = rbsp_read_bit(rbsp);
156 		if (ret < 0)
157 			return ret;
158 		tmp |= ret << (num - i - 1);
159 	}
160 
161 	if (val)
162 		*val = tmp;
163 
164 	return 0;
165 }
166 
167 static int rbsp_write_bits(struct rbsp *rbsp, int num, int value)
168 {
169 	int ret;
170 
171 	while (num--) {
172 		ret = rbsp_write_bit(rbsp, (value >> num) & 1);
173 		if (ret)
174 			return ret;
175 	}
176 
177 	return 0;
178 }
179 
180 static int rbsp_read_uev(struct rbsp *rbsp, unsigned int *val)
181 {
182 	int leading_zero_bits = 0;
183 	unsigned int tmp = 0;
184 	int ret;
185 
186 	while ((ret = rbsp_read_bit(rbsp)) == 0)
187 		leading_zero_bits++;
188 	if (ret < 0)
189 		return ret;
190 
191 	if (leading_zero_bits > 0) {
192 		ret = rbsp_read_bits(rbsp, leading_zero_bits, &tmp);
193 		if (ret)
194 			return ret;
195 	}
196 
197 	if (val)
198 		*val = (1 << leading_zero_bits) - 1 + tmp;
199 
200 	return 0;
201 }
202 
203 static int rbsp_write_uev(struct rbsp *rbsp, unsigned int value)
204 {
205 	int i;
206 	int ret;
207 	int tmp = value + 1;
208 	int leading_zero_bits = fls(tmp) - 1;
209 
210 	for (i = 0; i < leading_zero_bits; i++) {
211 		ret = rbsp_write_bit(rbsp, 0);
212 		if (ret)
213 			return ret;
214 	}
215 
216 	return rbsp_write_bits(rbsp, leading_zero_bits + 1, tmp);
217 }
218 
219 static int rbsp_read_sev(struct rbsp *rbsp, int *val)
220 {
221 	unsigned int tmp;
222 	int ret;
223 
224 	ret = rbsp_read_uev(rbsp, &tmp);
225 	if (ret)
226 		return ret;
227 
228 	if (val) {
229 		if (tmp & 1)
230 			*val = (tmp + 1) / 2;
231 		else
232 			*val = -(tmp / 2);
233 	}
234 
235 	return 0;
236 }
237 
238 /**
239  * coda_h264_sps_fixup - fixes frame cropping values in h.264 SPS
240  * @ctx: encoder context
241  * @width: visible width
242  * @height: visible height
243  * @buf: buffer containing h.264 SPS RBSP, starting with NAL header
244  * @size: modified RBSP size return value
245  * @max_size: available size in buf
246  *
247  * Rewrites the frame cropping values in an h.264 SPS RBSP correctly for the
248  * given visible width and height.
249  */
250 int coda_h264_sps_fixup(struct coda_ctx *ctx, int width, int height, char *buf,
251 			int *size, int max_size)
252 {
253 	int profile_idc;
254 	unsigned int pic_order_cnt_type;
255 	int pic_width_in_mbs_minus1, pic_height_in_map_units_minus1;
256 	int frame_mbs_only_flag, frame_cropping_flag;
257 	int vui_parameters_present_flag;
258 	unsigned int crop_right, crop_bottom;
259 	struct rbsp sps;
260 	int pos;
261 	int ret;
262 
263 	if (*size < 8 || *size >= max_size)
264 		return -EINVAL;
265 
266 	sps.buf = buf + 5; /* Skip NAL header */
267 	sps.size = *size - 5;
268 
269 	profile_idc = sps.buf[0];
270 	/* Skip constraint_set[0-5]_flag, reserved_zero_2bits */
271 	/* Skip level_idc */
272 	sps.pos = 24;
273 
274 	/* seq_parameter_set_id */
275 	ret = rbsp_read_uev(&sps, NULL);
276 	if (ret)
277 		return ret;
278 
279 	if (profile_idc == 100 || profile_idc == 110 || profile_idc == 122 ||
280 	    profile_idc == 244 || profile_idc == 44 || profile_idc == 83 ||
281 	    profile_idc == 86 || profile_idc == 118 || profile_idc == 128 ||
282 	    profile_idc == 138 || profile_idc == 139 || profile_idc == 134 ||
283 	    profile_idc == 135) {
284 		dev_err(ctx->fh.vdev->dev_parent,
285 			"%s: Handling profile_idc %d not implemented\n",
286 			__func__, profile_idc);
287 		return -EINVAL;
288 	}
289 
290 	/* log2_max_frame_num_minus4 */
291 	ret = rbsp_read_uev(&sps, NULL);
292 	if (ret)
293 		return ret;
294 
295 	ret = rbsp_read_uev(&sps, &pic_order_cnt_type);
296 	if (ret)
297 		return ret;
298 
299 	if (pic_order_cnt_type == 0) {
300 		/* log2_max_pic_order_cnt_lsb_minus4 */
301 		ret = rbsp_read_uev(&sps, NULL);
302 		if (ret)
303 			return ret;
304 	} else if (pic_order_cnt_type == 1) {
305 		unsigned int i, num_ref_frames_in_pic_order_cnt_cycle;
306 
307 		/* delta_pic_order_always_zero_flag */
308 		ret = rbsp_read_bit(&sps);
309 		if (ret < 0)
310 			return ret;
311 		/* offset_for_non_ref_pic */
312 		ret = rbsp_read_sev(&sps, NULL);
313 		if (ret)
314 			return ret;
315 		/* offset_for_top_to_bottom_field */
316 		ret = rbsp_read_sev(&sps, NULL);
317 		if (ret)
318 			return ret;
319 
320 		ret = rbsp_read_uev(&sps,
321 				    &num_ref_frames_in_pic_order_cnt_cycle);
322 		if (ret)
323 			return ret;
324 		for (i = 0; i < num_ref_frames_in_pic_order_cnt_cycle; i++) {
325 			/* offset_for_ref_frame */
326 			ret = rbsp_read_sev(&sps, NULL);
327 			if (ret)
328 				return ret;
329 		}
330 	}
331 
332 	/* max_num_ref_frames */
333 	ret = rbsp_read_uev(&sps, NULL);
334 	if (ret)
335 		return ret;
336 
337 	/* gaps_in_frame_num_value_allowed_flag */
338 	ret = rbsp_read_bit(&sps);
339 	if (ret < 0)
340 		return ret;
341 	ret = rbsp_read_uev(&sps, &pic_width_in_mbs_minus1);
342 	if (ret)
343 		return ret;
344 	ret = rbsp_read_uev(&sps, &pic_height_in_map_units_minus1);
345 	if (ret)
346 		return ret;
347 	frame_mbs_only_flag = ret = rbsp_read_bit(&sps);
348 	if (ret < 0)
349 		return ret;
350 	if (!frame_mbs_only_flag) {
351 		/* mb_adaptive_frame_field_flag */
352 		ret = rbsp_read_bit(&sps);
353 		if (ret < 0)
354 			return ret;
355 	}
356 	/* direct_8x8_inference_flag */
357 	ret = rbsp_read_bit(&sps);
358 	if (ret < 0)
359 		return ret;
360 
361 	/* Mark position of the frame cropping flag */
362 	pos = sps.pos;
363 	frame_cropping_flag = ret = rbsp_read_bit(&sps);
364 	if (ret < 0)
365 		return ret;
366 	if (frame_cropping_flag) {
367 		unsigned int crop_left, crop_top;
368 
369 		ret = rbsp_read_uev(&sps, &crop_left);
370 		if (ret)
371 			return ret;
372 		ret = rbsp_read_uev(&sps, &crop_right);
373 		if (ret)
374 			return ret;
375 		ret = rbsp_read_uev(&sps, &crop_top);
376 		if (ret)
377 			return ret;
378 		ret = rbsp_read_uev(&sps, &crop_bottom);
379 		if (ret)
380 			return ret;
381 	}
382 	vui_parameters_present_flag = ret = rbsp_read_bit(&sps);
383 	if (ret < 0)
384 		return ret;
385 	if (vui_parameters_present_flag) {
386 		dev_err(ctx->fh.vdev->dev_parent,
387 			"%s: Handling vui_parameters not implemented\n",
388 			__func__);
389 		return -EINVAL;
390 	}
391 
392 	crop_right = round_up(width, 16) - width;
393 	crop_bottom = round_up(height, 16) - height;
394 	crop_right /= 2;
395 	if (frame_mbs_only_flag)
396 		crop_bottom /= 2;
397 	else
398 		crop_bottom /= 4;
399 
400 
401 	sps.size = max_size - 5;
402 	sps.pos = pos;
403 	frame_cropping_flag = 1;
404 	ret = rbsp_write_bit(&sps, frame_cropping_flag);
405 	if (ret)
406 		return ret;
407 	ret = rbsp_write_uev(&sps, 0); /* crop_left */
408 	if (ret)
409 		return ret;
410 	ret = rbsp_write_uev(&sps, crop_right);
411 	if (ret)
412 		return ret;
413 	ret = rbsp_write_uev(&sps, 0); /* crop_top */
414 	if (ret)
415 		return ret;
416 	ret = rbsp_write_uev(&sps, crop_bottom);
417 	if (ret)
418 		return ret;
419 	ret = rbsp_write_bit(&sps, 0); /* vui_parameters_present_flag */
420 	if (ret)
421 		return ret;
422 	ret = rbsp_write_bit(&sps, 1);
423 	if (ret)
424 		return ret;
425 
426 	*size = 5 + DIV_ROUND_UP(sps.pos, 8);
427 
428 	return 0;
429 }
430