mirror of https://github.com/Qortal/Brooklyn
You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
310 lines
5.7 KiB
310 lines
5.7 KiB
// SPDX-License-Identifier: GPL-2.0 |
|
/* |
|
* Copyright (C) 2019-2020 Pengutronix, Michael Tretter <[email protected]> |
|
* |
|
* Helper functions to generate a raw byte sequence payload from values. |
|
*/ |
|
|
|
#include <linux/kernel.h> |
|
#include <linux/types.h> |
|
#include <linux/string.h> |
|
#include <linux/v4l2-controls.h> |
|
|
|
#include <linux/device.h> |
|
#include <linux/export.h> |
|
#include <linux/log2.h> |
|
|
|
#include "nal-rbsp.h" |
|
|
|
void rbsp_init(struct rbsp *rbsp, void *addr, size_t size, |
|
struct nal_rbsp_ops *ops) |
|
{ |
|
if (!rbsp) |
|
return; |
|
|
|
rbsp->data = addr; |
|
rbsp->size = size; |
|
rbsp->pos = 0; |
|
rbsp->ops = ops; |
|
rbsp->error = 0; |
|
} |
|
|
|
void rbsp_unsupported(struct rbsp *rbsp) |
|
{ |
|
rbsp->error = -EINVAL; |
|
} |
|
|
|
static int rbsp_read_bits(struct rbsp *rbsp, int n, unsigned int *value); |
|
static int rbsp_write_bits(struct rbsp *rbsp, int n, unsigned int value); |
|
|
|
/* |
|
* When reading or writing, the emulation_prevention_three_byte is detected |
|
* only when the 2 one bits need to be inserted. Therefore, we are not |
|
* actually adding the 0x3 byte, but the 2 one bits and the six 0 bits of the |
|
* next byte. |
|
*/ |
|
#define EMULATION_PREVENTION_THREE_BYTE (0x3 << 6) |
|
|
|
static int add_emulation_prevention_three_byte(struct rbsp *rbsp) |
|
{ |
|
rbsp->num_consecutive_zeros = 0; |
|
rbsp_write_bits(rbsp, 8, EMULATION_PREVENTION_THREE_BYTE); |
|
|
|
return 0; |
|
} |
|
|
|
static int discard_emulation_prevention_three_byte(struct rbsp *rbsp) |
|
{ |
|
unsigned int tmp = 0; |
|
|
|
rbsp->num_consecutive_zeros = 0; |
|
rbsp_read_bits(rbsp, 8, &tmp); |
|
if (tmp != EMULATION_PREVENTION_THREE_BYTE) |
|
return -EINVAL; |
|
|
|
return 0; |
|
} |
|
|
|
static inline int rbsp_read_bit(struct rbsp *rbsp) |
|
{ |
|
int shift; |
|
int ofs; |
|
int bit; |
|
int err; |
|
|
|
if (rbsp->num_consecutive_zeros == 22) { |
|
err = discard_emulation_prevention_three_byte(rbsp); |
|
if (err) |
|
return err; |
|
} |
|
|
|
shift = 7 - (rbsp->pos % 8); |
|
ofs = rbsp->pos / 8; |
|
if (ofs >= rbsp->size) |
|
return -EINVAL; |
|
|
|
bit = (rbsp->data[ofs] >> shift) & 1; |
|
|
|
rbsp->pos++; |
|
|
|
if (bit == 1 || |
|
(rbsp->num_consecutive_zeros < 7 && (rbsp->pos % 8 == 0))) |
|
rbsp->num_consecutive_zeros = 0; |
|
else |
|
rbsp->num_consecutive_zeros++; |
|
|
|
return bit; |
|
} |
|
|
|
static inline int rbsp_write_bit(struct rbsp *rbsp, bool value) |
|
{ |
|
int shift; |
|
int ofs; |
|
|
|
if (rbsp->num_consecutive_zeros == 22) |
|
add_emulation_prevention_three_byte(rbsp); |
|
|
|
shift = 7 - (rbsp->pos % 8); |
|
ofs = rbsp->pos / 8; |
|
if (ofs >= rbsp->size) |
|
return -EINVAL; |
|
|
|
rbsp->data[ofs] &= ~(1 << shift); |
|
rbsp->data[ofs] |= value << shift; |
|
|
|
rbsp->pos++; |
|
|
|
if (value || |
|
(rbsp->num_consecutive_zeros < 7 && (rbsp->pos % 8 == 0))) { |
|
rbsp->num_consecutive_zeros = 0; |
|
} else { |
|
rbsp->num_consecutive_zeros++; |
|
} |
|
|
|
return 0; |
|
} |
|
|
|
static inline int rbsp_read_bits(struct rbsp *rbsp, int n, unsigned int *value) |
|
{ |
|
int i; |
|
int bit; |
|
unsigned int tmp = 0; |
|
|
|
if (n > 8 * sizeof(*value)) |
|
return -EINVAL; |
|
|
|
for (i = n; i > 0; i--) { |
|
bit = rbsp_read_bit(rbsp); |
|
if (bit < 0) |
|
return bit; |
|
tmp |= bit << (i - 1); |
|
} |
|
|
|
if (value) |
|
*value = tmp; |
|
|
|
return 0; |
|
} |
|
|
|
static int rbsp_write_bits(struct rbsp *rbsp, int n, unsigned int value) |
|
{ |
|
int ret; |
|
|
|
if (n > 8 * sizeof(value)) |
|
return -EINVAL; |
|
|
|
while (n--) { |
|
ret = rbsp_write_bit(rbsp, (value >> n) & 1); |
|
if (ret) |
|
return ret; |
|
} |
|
|
|
return 0; |
|
} |
|
|
|
static int rbsp_read_uev(struct rbsp *rbsp, unsigned int *value) |
|
{ |
|
int leading_zero_bits = 0; |
|
unsigned int tmp = 0; |
|
int ret; |
|
|
|
while ((ret = rbsp_read_bit(rbsp)) == 0) |
|
leading_zero_bits++; |
|
if (ret < 0) |
|
return ret; |
|
|
|
if (leading_zero_bits > 0) { |
|
ret = rbsp_read_bits(rbsp, leading_zero_bits, &tmp); |
|
if (ret) |
|
return ret; |
|
} |
|
|
|
if (value) |
|
*value = (1 << leading_zero_bits) - 1 + tmp; |
|
|
|
return 0; |
|
} |
|
|
|
static int rbsp_write_uev(struct rbsp *rbsp, unsigned int *value) |
|
{ |
|
int ret; |
|
int leading_zero_bits; |
|
|
|
if (!value) |
|
return -EINVAL; |
|
|
|
leading_zero_bits = ilog2(*value + 1); |
|
|
|
ret = rbsp_write_bits(rbsp, leading_zero_bits, 0); |
|
if (ret) |
|
return ret; |
|
|
|
return rbsp_write_bits(rbsp, leading_zero_bits + 1, *value + 1); |
|
} |
|
|
|
static int rbsp_read_sev(struct rbsp *rbsp, int *value) |
|
{ |
|
int ret; |
|
unsigned int tmp; |
|
|
|
ret = rbsp_read_uev(rbsp, &tmp); |
|
if (ret) |
|
return ret; |
|
|
|
if (value) { |
|
if (tmp & 1) |
|
*value = (tmp + 1) / 2; |
|
else |
|
*value = -(tmp / 2); |
|
} |
|
|
|
return 0; |
|
} |
|
|
|
static int rbsp_write_sev(struct rbsp *rbsp, int *value) |
|
{ |
|
unsigned int tmp; |
|
|
|
if (!value) |
|
return -EINVAL; |
|
|
|
if (*value > 0) |
|
tmp = (2 * (*value)) | 1; |
|
else |
|
tmp = -2 * (*value); |
|
|
|
return rbsp_write_uev(rbsp, &tmp); |
|
} |
|
|
|
static int __rbsp_write_bit(struct rbsp *rbsp, int *value) |
|
{ |
|
return rbsp_write_bit(rbsp, *value); |
|
} |
|
|
|
static int __rbsp_write_bits(struct rbsp *rbsp, int n, unsigned int *value) |
|
{ |
|
return rbsp_write_bits(rbsp, n, *value); |
|
} |
|
|
|
struct nal_rbsp_ops write = { |
|
.rbsp_bit = __rbsp_write_bit, |
|
.rbsp_bits = __rbsp_write_bits, |
|
.rbsp_uev = rbsp_write_uev, |
|
.rbsp_sev = rbsp_write_sev, |
|
}; |
|
|
|
static int __rbsp_read_bit(struct rbsp *rbsp, int *value) |
|
{ |
|
int tmp = rbsp_read_bit(rbsp); |
|
|
|
if (tmp < 0) |
|
return tmp; |
|
*value = tmp; |
|
|
|
return 0; |
|
} |
|
|
|
struct nal_rbsp_ops read = { |
|
.rbsp_bit = __rbsp_read_bit, |
|
.rbsp_bits = rbsp_read_bits, |
|
.rbsp_uev = rbsp_read_uev, |
|
.rbsp_sev = rbsp_read_sev, |
|
}; |
|
|
|
void rbsp_bit(struct rbsp *rbsp, int *value) |
|
{ |
|
if (rbsp->error) |
|
return; |
|
rbsp->error = rbsp->ops->rbsp_bit(rbsp, value); |
|
} |
|
|
|
void rbsp_bits(struct rbsp *rbsp, int n, int *value) |
|
{ |
|
if (rbsp->error) |
|
return; |
|
rbsp->error = rbsp->ops->rbsp_bits(rbsp, n, value); |
|
} |
|
|
|
void rbsp_uev(struct rbsp *rbsp, unsigned int *value) |
|
{ |
|
if (rbsp->error) |
|
return; |
|
rbsp->error = rbsp->ops->rbsp_uev(rbsp, value); |
|
} |
|
|
|
void rbsp_sev(struct rbsp *rbsp, int *value) |
|
{ |
|
if (rbsp->error) |
|
return; |
|
rbsp->error = rbsp->ops->rbsp_sev(rbsp, value); |
|
} |
|
|
|
void rbsp_trailing_bits(struct rbsp *rbsp) |
|
{ |
|
unsigned int rbsp_stop_one_bit = 1; |
|
unsigned int rbsp_alignment_zero_bit = 0; |
|
|
|
rbsp_bit(rbsp, &rbsp_stop_one_bit); |
|
rbsp_bits(rbsp, round_up(rbsp->pos, 8) - rbsp->pos, |
|
&rbsp_alignment_zero_bit); |
|
}
|
|
|