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