311 lines · c
1// SPDX-License-Identifier: GPL-2.02/*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 detected42 * only when the 2 one bits need to be inserted. Therefore, we are not43 * actually adding the 0x3 byte, but the 2 one bits and the six 0 bits of the44 * 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 else94 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 else218 *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 else234 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}311