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 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 32 static int rbsp_read_bits(struct rbsp *rbsp, int n, unsigned int *value); 33 static int rbsp_write_bits(struct rbsp *rbsp, int n, unsigned int value); 34 35 /* 36 * When reading or writing, the emulation_prevention_three_byte is detected 37 * only when the 2 one bits need to be inserted. Therefore, we are not 38 * actually adding the 0x3 byte, but the 2 one bits and the six 0 bits of the 39 * next byte. 40 */ 41 #define EMULATION_PREVENTION_THREE_BYTE (0x3 << 6) 42 43 static int add_emulation_prevention_three_byte(struct rbsp *rbsp) 44 { 45 rbsp->num_consecutive_zeros = 0; 46 rbsp_write_bits(rbsp, 8, EMULATION_PREVENTION_THREE_BYTE); 47 48 return 0; 49 } 50 51 static int discard_emulation_prevention_three_byte(struct rbsp *rbsp) 52 { 53 unsigned int tmp = 0; 54 55 rbsp->num_consecutive_zeros = 0; 56 rbsp_read_bits(rbsp, 8, &tmp); 57 if (tmp != EMULATION_PREVENTION_THREE_BYTE) 58 return -EINVAL; 59 60 return 0; 61 } 62 63 static inline int rbsp_read_bit(struct rbsp *rbsp) 64 { 65 int shift; 66 int ofs; 67 int bit; 68 int err; 69 70 if (rbsp->num_consecutive_zeros == 22) { 71 err = discard_emulation_prevention_three_byte(rbsp); 72 if (err) 73 return err; 74 } 75 76 shift = 7 - (rbsp->pos % 8); 77 ofs = rbsp->pos / 8; 78 if (ofs >= rbsp->size) 79 return -EINVAL; 80 81 bit = (rbsp->data[ofs] >> shift) & 1; 82 83 rbsp->pos++; 84 85 if (bit == 1 || 86 (rbsp->num_consecutive_zeros < 7 && (rbsp->pos % 8 == 0))) 87 rbsp->num_consecutive_zeros = 0; 88 else 89 rbsp->num_consecutive_zeros++; 90 91 return bit; 92 } 93 94 static inline int rbsp_write_bit(struct rbsp *rbsp, bool value) 95 { 96 int shift; 97 int ofs; 98 99 if (rbsp->num_consecutive_zeros == 22) 100 add_emulation_prevention_three_byte(rbsp); 101 102 shift = 7 - (rbsp->pos % 8); 103 ofs = rbsp->pos / 8; 104 if (ofs >= rbsp->size) 105 return -EINVAL; 106 107 rbsp->data[ofs] &= ~(1 << shift); 108 rbsp->data[ofs] |= value << shift; 109 110 rbsp->pos++; 111 112 if (value || 113 (rbsp->num_consecutive_zeros < 7 && (rbsp->pos % 8 == 0))) { 114 rbsp->num_consecutive_zeros = 0; 115 } else { 116 rbsp->num_consecutive_zeros++; 117 } 118 119 return 0; 120 } 121 122 static inline int rbsp_read_bits(struct rbsp *rbsp, int n, unsigned int *value) 123 { 124 int i; 125 int bit; 126 unsigned int tmp = 0; 127 128 if (n > 8 * sizeof(*value)) 129 return -EINVAL; 130 131 for (i = n; i > 0; i--) { 132 bit = rbsp_read_bit(rbsp); 133 if (bit < 0) 134 return bit; 135 tmp |= bit << (i - 1); 136 } 137 138 if (value) 139 *value = tmp; 140 141 return 0; 142 } 143 144 static int rbsp_write_bits(struct rbsp *rbsp, int n, unsigned int value) 145 { 146 int ret; 147 148 if (n > 8 * sizeof(value)) 149 return -EINVAL; 150 151 while (n--) { 152 ret = rbsp_write_bit(rbsp, (value >> n) & 1); 153 if (ret) 154 return ret; 155 } 156 157 return 0; 158 } 159 160 static int rbsp_read_uev(struct rbsp *rbsp, unsigned int *value) 161 { 162 int leading_zero_bits = 0; 163 unsigned int tmp = 0; 164 int ret; 165 166 while ((ret = rbsp_read_bit(rbsp)) == 0) 167 leading_zero_bits++; 168 if (ret < 0) 169 return ret; 170 171 if (leading_zero_bits > 0) { 172 ret = rbsp_read_bits(rbsp, leading_zero_bits, &tmp); 173 if (ret) 174 return ret; 175 } 176 177 if (value) 178 *value = (1 << leading_zero_bits) - 1 + tmp; 179 180 return 0; 181 } 182 183 static int rbsp_write_uev(struct rbsp *rbsp, unsigned int *value) 184 { 185 int ret; 186 int leading_zero_bits; 187 188 if (!value) 189 return -EINVAL; 190 191 leading_zero_bits = ilog2(*value + 1); 192 193 ret = rbsp_write_bits(rbsp, leading_zero_bits, 0); 194 if (ret) 195 return ret; 196 197 return rbsp_write_bits(rbsp, leading_zero_bits + 1, *value + 1); 198 } 199 200 static int rbsp_read_sev(struct rbsp *rbsp, int *value) 201 { 202 int ret; 203 unsigned int tmp; 204 205 ret = rbsp_read_uev(rbsp, &tmp); 206 if (ret) 207 return ret; 208 209 if (value) { 210 if (tmp & 1) 211 *value = (tmp + 1) / 2; 212 else 213 *value = -(tmp / 2); 214 } 215 216 return 0; 217 } 218 219 static int rbsp_write_sev(struct rbsp *rbsp, int *value) 220 { 221 unsigned int tmp; 222 223 if (!value) 224 return -EINVAL; 225 226 if (*value > 0) 227 tmp = (2 * (*value)) | 1; 228 else 229 tmp = -2 * (*value); 230 231 return rbsp_write_uev(rbsp, &tmp); 232 } 233 234 static int __rbsp_write_bit(struct rbsp *rbsp, int *value) 235 { 236 return rbsp_write_bit(rbsp, *value); 237 } 238 239 static int __rbsp_write_bits(struct rbsp *rbsp, int n, unsigned int *value) 240 { 241 return rbsp_write_bits(rbsp, n, *value); 242 } 243 244 struct nal_rbsp_ops write = { 245 .rbsp_bit = __rbsp_write_bit, 246 .rbsp_bits = __rbsp_write_bits, 247 .rbsp_uev = rbsp_write_uev, 248 .rbsp_sev = rbsp_write_sev, 249 }; 250 251 static int __rbsp_read_bit(struct rbsp *rbsp, int *value) 252 { 253 int tmp = rbsp_read_bit(rbsp); 254 255 if (tmp < 0) 256 return tmp; 257 *value = tmp; 258 259 return 0; 260 } 261 262 struct nal_rbsp_ops read = { 263 .rbsp_bit = __rbsp_read_bit, 264 .rbsp_bits = rbsp_read_bits, 265 .rbsp_uev = rbsp_read_uev, 266 .rbsp_sev = rbsp_read_sev, 267 }; 268 269 void rbsp_bit(struct rbsp *rbsp, int *value) 270 { 271 if (rbsp->error) 272 return; 273 rbsp->error = rbsp->ops->rbsp_bit(rbsp, value); 274 } 275 276 void rbsp_bits(struct rbsp *rbsp, int n, int *value) 277 { 278 if (rbsp->error) 279 return; 280 rbsp->error = rbsp->ops->rbsp_bits(rbsp, n, value); 281 } 282 283 void rbsp_uev(struct rbsp *rbsp, unsigned int *value) 284 { 285 if (rbsp->error) 286 return; 287 rbsp->error = rbsp->ops->rbsp_uev(rbsp, value); 288 } 289 290 void rbsp_sev(struct rbsp *rbsp, int *value) 291 { 292 if (rbsp->error) 293 return; 294 rbsp->error = rbsp->ops->rbsp_sev(rbsp, value); 295 } 296 297 void rbsp_trailing_bits(struct rbsp *rbsp) 298 { 299 unsigned int rbsp_stop_one_bit = 1; 300 unsigned int rbsp_alignment_zero_bit = 0; 301 302 rbsp_bit(rbsp, &rbsp_stop_one_bit); 303 rbsp_bits(rbsp, round_up(rbsp->pos, 8) - rbsp->pos, 304 &rbsp_alignment_zero_bit); 305 } 306