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, n: 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, n: 8, value: &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: (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, n: leading_zero_bits, value: &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, n: leading_zero_bits, value: 0); |
199 | if (ret) |
200 | return ret; |
201 | |
202 | return rbsp_write_bits(rbsp, n: leading_zero_bits + 1, value: *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, value: &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, value: &tmp); |
237 | } |
238 | |
239 | static int __rbsp_write_bit(struct rbsp *rbsp, int *value) |
240 | { |
241 | return rbsp_write_bit(rbsp, value: *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: *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, value: &rbsp_stop_one_bit); |
308 | rbsp_bits(rbsp, round_up(rbsp->pos, 8) - rbsp->pos, |
309 | value: &rbsp_alignment_zero_bit); |
310 | } |
311 | |