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