nal-rbsp.c

  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, 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, 8, &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 >> 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, 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
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, 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
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, &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, &tmp);
237}
238
239static int __rbsp_write_bit(struct rbsp *rbsp, int *value)
240{
241	return rbsp_write_bit(rbsp, *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);
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, &rbsp_stop_one_bit);
308	rbsp_bits(rbsp, round_up(rbsp->pos, 8) - rbsp->pos,
309		  &rbsp_alignment_zero_bit);
310}