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
16static const u8 coda_filler_size[8] = { 0, 7, 14, 13, 12, 11, 10, 9 };
17
18static 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
31int 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
49int 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
66int 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
81int 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
92int 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
115struct rbsp {
116	char *buf;
117	int size;
118	int pos;
119};
120
121static 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
132static 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
146static 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
167static 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
180static 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
203static 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
219static 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 */
250int 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