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