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
19void 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
32void rbsp_unsupported(struct rbsp *rbsp)
33{
34 rbsp->error = -EINVAL;
35}
36
37static int rbsp_read_bits(struct rbsp *rbsp, int n, unsigned int *value);
38static 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
48static 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
56static 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
68static 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
99static 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
127static 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
149static 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
165static 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
188static 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
205static 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
224static 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
239static int __rbsp_write_bit(struct rbsp *rbsp, int *value)
240{
241 return rbsp_write_bit(rbsp, value: *value);
242}
243
244static int __rbsp_write_bits(struct rbsp *rbsp, int n, unsigned int *value)
245{
246 return rbsp_write_bits(rbsp, n, value: *value);
247}
248
249struct 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
256static 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
267struct 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
274void 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
281void 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
288void 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
295void 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
302void 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

source code of linux/drivers/media/platform/allegro-dvt/nal-rbsp.c