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
rbsp_init(struct rbsp * rbsp,void * addr,size_t size,struct nal_rbsp_ops * ops)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
rbsp_unsupported(struct rbsp * rbsp)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
add_emulation_prevention_three_byte(struct rbsp * rbsp)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
discard_emulation_prevention_three_byte(struct rbsp * rbsp)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
rbsp_read_bit(struct rbsp * rbsp)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
rbsp_write_bit(struct rbsp * rbsp,bool value)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
rbsp_read_bits(struct rbsp * rbsp,int n,unsigned int * value)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
rbsp_write_bits(struct rbsp * rbsp,int n,unsigned int value)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
rbsp_read_uev(struct rbsp * rbsp,unsigned int * value)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
rbsp_write_uev(struct rbsp * rbsp,unsigned int * value)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
rbsp_read_sev(struct rbsp * rbsp,int * value)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
rbsp_write_sev(struct rbsp * rbsp,int * value)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
__rbsp_write_bit(struct rbsp * rbsp,int * value)239 static int __rbsp_write_bit(struct rbsp *rbsp, int *value)
240 {
241 return rbsp_write_bit(rbsp, *value);
242 }
243
__rbsp_write_bits(struct rbsp * rbsp,int n,unsigned int * value)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
__rbsp_read_bit(struct rbsp * rbsp,int * value)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
rbsp_bit(struct rbsp * rbsp,int * value)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
rbsp_bits(struct rbsp * rbsp,int n,int * value)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
rbsp_uev(struct rbsp * rbsp,unsigned int * value)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
rbsp_sev(struct rbsp * rbsp,int * value)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
rbsp_trailing_bits(struct rbsp * rbsp)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