1 // SPDX-License-Identifier: GPL-2.0
2 /*
3  * Copyright (C) 2019-2020 Pengutronix, Michael Tretter <kernel@pengutronix.de>
4  *
5  * Helper functions to generate a raw byte sequence payload from values.
6  */
7 
8 #include <linux/kernel.h>
9 #include <linux/types.h>
10 #include <linux/string.h>
11 #include <linux/v4l2-controls.h>
12 
13 #include <linux/device.h>
14 #include <linux/export.h>
15 #include <linux/log2.h>
16 
17 #include "nal-rbsp.h"
18 
19 void rbsp_init(struct rbsp *rbsp, void *addr, size_t size,
20 	       struct nal_rbsp_ops *ops)
21 {
22 	if (!rbsp)
23 		return;
24 
25 	rbsp->data = addr;
26 	rbsp->size = size;
27 	rbsp->pos = 0;
28 	rbsp->ops = ops;
29 	rbsp->error = 0;
30 }
31 
32 void rbsp_unsupported(struct rbsp *rbsp)
33 {
34 	rbsp->error = -EINVAL;
35 }
36 
37 static int rbsp_read_bits(struct rbsp *rbsp, int n, unsigned int *value);
38 static int rbsp_write_bits(struct rbsp *rbsp, int n, unsigned int value);
39 
40 /*
41  * When reading or writing, the emulation_prevention_three_byte is detected
42  * only when the 2 one bits need to be inserted. Therefore, we are not
43  * actually adding the 0x3 byte, but the 2 one bits and the six 0 bits of the
44  * next byte.
45  */
46 #define EMULATION_PREVENTION_THREE_BYTE (0x3 << 6)
47 
48 static int add_emulation_prevention_three_byte(struct rbsp *rbsp)
49 {
50 	rbsp->num_consecutive_zeros = 0;
51 	rbsp_write_bits(rbsp, 8, EMULATION_PREVENTION_THREE_BYTE);
52 
53 	return 0;
54 }
55 
56 static int discard_emulation_prevention_three_byte(struct rbsp *rbsp)
57 {
58 	unsigned int tmp = 0;
59 
60 	rbsp->num_consecutive_zeros = 0;
61 	rbsp_read_bits(rbsp, 8, &tmp);
62 	if (tmp != EMULATION_PREVENTION_THREE_BYTE)
63 		return -EINVAL;
64 
65 	return 0;
66 }
67 
68 static inline int rbsp_read_bit(struct rbsp *rbsp)
69 {
70 	int shift;
71 	int ofs;
72 	int bit;
73 	int err;
74 
75 	if (rbsp->num_consecutive_zeros == 22) {
76 		err = discard_emulation_prevention_three_byte(rbsp);
77 		if (err)
78 			return err;
79 	}
80 
81 	shift = 7 - (rbsp->pos % 8);
82 	ofs = rbsp->pos / 8;
83 	if (ofs >= rbsp->size)
84 		return -EINVAL;
85 
86 	bit = (rbsp->data[ofs] >> shift) & 1;
87 
88 	rbsp->pos++;
89 
90 	if (bit == 1 ||
91 	    (rbsp->num_consecutive_zeros < 7 && (rbsp->pos % 8 == 0)))
92 		rbsp->num_consecutive_zeros = 0;
93 	else
94 		rbsp->num_consecutive_zeros++;
95 
96 	return bit;
97 }
98 
99 static inline int rbsp_write_bit(struct rbsp *rbsp, bool value)
100 {
101 	int shift;
102 	int ofs;
103 
104 	if (rbsp->num_consecutive_zeros == 22)
105 		add_emulation_prevention_three_byte(rbsp);
106 
107 	shift = 7 - (rbsp->pos % 8);
108 	ofs = rbsp->pos / 8;
109 	if (ofs >= rbsp->size)
110 		return -EINVAL;
111 
112 	rbsp->data[ofs] &= ~(1 << shift);
113 	rbsp->data[ofs] |= value << shift;
114 
115 	rbsp->pos++;
116 
117 	if (value ||
118 	    (rbsp->num_consecutive_zeros < 7 && (rbsp->pos % 8 == 0))) {
119 		rbsp->num_consecutive_zeros = 0;
120 	} else {
121 		rbsp->num_consecutive_zeros++;
122 	}
123 
124 	return 0;
125 }
126 
127 static inline int rbsp_read_bits(struct rbsp *rbsp, int n, unsigned int *value)
128 {
129 	int i;
130 	int bit;
131 	unsigned int tmp = 0;
132 
133 	if (n > 8 * sizeof(*value))
134 		return -EINVAL;
135 
136 	for (i = n; i > 0; i--) {
137 		bit = rbsp_read_bit(rbsp);
138 		if (bit < 0)
139 			return bit;
140 		tmp |= bit << (i - 1);
141 	}
142 
143 	if (value)
144 		*value = tmp;
145 
146 	return 0;
147 }
148 
149 static int rbsp_write_bits(struct rbsp *rbsp, int n, unsigned int value)
150 {
151 	int ret;
152 
153 	if (n > 8 * sizeof(value))
154 		return -EINVAL;
155 
156 	while (n--) {
157 		ret = rbsp_write_bit(rbsp, (value >> n) & 1);
158 		if (ret)
159 			return ret;
160 	}
161 
162 	return 0;
163 }
164 
165 static int rbsp_read_uev(struct rbsp *rbsp, unsigned int *value)
166 {
167 	int leading_zero_bits = 0;
168 	unsigned int tmp = 0;
169 	int ret;
170 
171 	while ((ret = rbsp_read_bit(rbsp)) == 0)
172 		leading_zero_bits++;
173 	if (ret < 0)
174 		return ret;
175 
176 	if (leading_zero_bits > 0) {
177 		ret = rbsp_read_bits(rbsp, leading_zero_bits, &tmp);
178 		if (ret)
179 			return ret;
180 	}
181 
182 	if (value)
183 		*value = (1 << leading_zero_bits) - 1 + tmp;
184 
185 	return 0;
186 }
187 
188 static int rbsp_write_uev(struct rbsp *rbsp, unsigned int *value)
189 {
190 	int ret;
191 	int leading_zero_bits;
192 
193 	if (!value)
194 		return -EINVAL;
195 
196 	leading_zero_bits = ilog2(*value + 1);
197 
198 	ret = rbsp_write_bits(rbsp, leading_zero_bits, 0);
199 	if (ret)
200 		return ret;
201 
202 	return rbsp_write_bits(rbsp, leading_zero_bits + 1, *value + 1);
203 }
204 
205 static int rbsp_read_sev(struct rbsp *rbsp, int *value)
206 {
207 	int ret;
208 	unsigned int tmp;
209 
210 	ret = rbsp_read_uev(rbsp, &tmp);
211 	if (ret)
212 		return ret;
213 
214 	if (value) {
215 		if (tmp & 1)
216 			*value = (tmp + 1) / 2;
217 		else
218 			*value = -(tmp / 2);
219 	}
220 
221 	return 0;
222 }
223 
224 static int rbsp_write_sev(struct rbsp *rbsp, int *value)
225 {
226 	unsigned int tmp;
227 
228 	if (!value)
229 		return -EINVAL;
230 
231 	if (*value > 0)
232 		tmp = (2 * (*value)) | 1;
233 	else
234 		tmp = -2 * (*value);
235 
236 	return rbsp_write_uev(rbsp, &tmp);
237 }
238 
239 static int __rbsp_write_bit(struct rbsp *rbsp, int *value)
240 {
241 	return rbsp_write_bit(rbsp, *value);
242 }
243 
244 static int __rbsp_write_bits(struct rbsp *rbsp, int n, unsigned int *value)
245 {
246 	return rbsp_write_bits(rbsp, n, *value);
247 }
248 
249 struct nal_rbsp_ops write = {
250 	.rbsp_bit = __rbsp_write_bit,
251 	.rbsp_bits = __rbsp_write_bits,
252 	.rbsp_uev = rbsp_write_uev,
253 	.rbsp_sev = rbsp_write_sev,
254 };
255 
256 static int __rbsp_read_bit(struct rbsp *rbsp, int *value)
257 {
258 	int tmp = rbsp_read_bit(rbsp);
259 
260 	if (tmp < 0)
261 		return tmp;
262 	*value = tmp;
263 
264 	return 0;
265 }
266 
267 struct nal_rbsp_ops read = {
268 	.rbsp_bit = __rbsp_read_bit,
269 	.rbsp_bits = rbsp_read_bits,
270 	.rbsp_uev = rbsp_read_uev,
271 	.rbsp_sev = rbsp_read_sev,
272 };
273 
274 void rbsp_bit(struct rbsp *rbsp, int *value)
275 {
276 	if (rbsp->error)
277 		return;
278 	rbsp->error = rbsp->ops->rbsp_bit(rbsp, value);
279 }
280 
281 void rbsp_bits(struct rbsp *rbsp, int n, int *value)
282 {
283 	if (rbsp->error)
284 		return;
285 	rbsp->error = rbsp->ops->rbsp_bits(rbsp, n, value);
286 }
287 
288 void rbsp_uev(struct rbsp *rbsp, unsigned int *value)
289 {
290 	if (rbsp->error)
291 		return;
292 	rbsp->error = rbsp->ops->rbsp_uev(rbsp, value);
293 }
294 
295 void rbsp_sev(struct rbsp *rbsp, int *value)
296 {
297 	if (rbsp->error)
298 		return;
299 	rbsp->error = rbsp->ops->rbsp_sev(rbsp, value);
300 }
301 
302 void rbsp_trailing_bits(struct rbsp *rbsp)
303 {
304 	unsigned int rbsp_stop_one_bit = 1;
305 	unsigned int rbsp_alignment_zero_bit = 0;
306 
307 	rbsp_bit(rbsp, &rbsp_stop_one_bit);
308 	rbsp_bits(rbsp, round_up(rbsp->pos, 8) - rbsp->pos,
309 		  &rbsp_alignment_zero_bit);
310 }
311