drivers/media/platform/allegro-dvt/nal-rbsp.c

   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 }