platform/allegro-dvt/nal-rbsp.c

0001 // SPDX-License-Identifier: GPL-2.0
0002 /*
0003  * Copyright (C) 2019-2020 Pengutronix, Michael Tretter <kernel@pengutronix.de>
0004  *
0005  * Helper functions to generate a raw byte sequence payload from values.
0006  */
0007
0008 #include <linux/kernel.h>
0009 #include <linux/types.h>
0010 #include <linux/string.h>
0011 #include <linux/v4l2-controls.h>
0012
0013 #include <linux/device.h>
0014 #include <linux/export.h>
0015 #include <linux/log2.h>
0016
0017 #include "nal-rbsp.h"
0018
0019 void rbsp_init(struct rbsp *rbsp, void *addr, size_t size,
0020            struct nal_rbsp_ops *ops)
0021 {
0022     if (!rbsp)
0023         return;
0024
0025     rbsp->data = addr;
0026     rbsp->size = size;
0027     rbsp->pos = 0;
0028     rbsp->ops = ops;
0029     rbsp->error = 0;
0030 }
0031
0032 void rbsp_unsupported(struct rbsp *rbsp)
0033 {
0034     rbsp->error = -EINVAL;
0035 }
0036
0037 static int rbsp_read_bits(struct rbsp *rbsp, int n, unsigned int *value);
0038 static int rbsp_write_bits(struct rbsp *rbsp, int n, unsigned int value);
0039
0040 /*
0041  * When reading or writing, the emulation_prevention_three_byte is detected
0042  * only when the 2 one bits need to be inserted. Therefore, we are not
0043  * actually adding the 0x3 byte, but the 2 one bits and the six 0 bits of the
0044  * next byte.
0045  */
0046 #define EMULATION_PREVENTION_THREE_BYTE (0x3 << 6)
0047
0048 static int add_emulation_prevention_three_byte(struct rbsp *rbsp)
0049 {
0050     rbsp->num_consecutive_zeros = 0;
0051     rbsp_write_bits(rbsp, 8, EMULATION_PREVENTION_THREE_BYTE);
0052
0053     return 0;
0054 }
0055
0056 static int discard_emulation_prevention_three_byte(struct rbsp *rbsp)
0057 {
0058     unsigned int tmp = 0;
0059
0060     rbsp->num_consecutive_zeros = 0;
0061     rbsp_read_bits(rbsp, 8, &tmp);
0062     if (tmp != EMULATION_PREVENTION_THREE_BYTE)
0063         return -EINVAL;
0064
0065     return 0;
0066 }
0067
0068 static inline int rbsp_read_bit(struct rbsp *rbsp)
0069 {
0070     int shift;
0071     int ofs;
0072     int bit;
0073     int err;
0074
0075     if (rbsp->num_consecutive_zeros == 22) {
0076         err = discard_emulation_prevention_three_byte(rbsp);
0077         if (err)
0078             return err;
0079     }
0080
0081     shift = 7 - (rbsp->pos % 8);
0082     ofs = rbsp->pos / 8;
0083     if (ofs >= rbsp->size)
0084         return -EINVAL;
0085
0086     bit = (rbsp->data[ofs] >> shift) & 1;
0087
0088     rbsp->pos++;
0089
0090     if (bit == 1 ||
0091         (rbsp->num_consecutive_zeros < 7 && (rbsp->pos % 8 == 0)))
0092         rbsp->num_consecutive_zeros = 0;
0093     else
0094         rbsp->num_consecutive_zeros++;
0095
0096     return bit;
0097 }
0098
0099 static inline int rbsp_write_bit(struct rbsp *rbsp, bool value)
0100 {
0101     int shift;
0102     int ofs;
0103
0104     if (rbsp->num_consecutive_zeros == 22)
0105         add_emulation_prevention_three_byte(rbsp);
0106
0107     shift = 7 - (rbsp->pos % 8);
0108     ofs = rbsp->pos / 8;
0109     if (ofs >= rbsp->size)
0110         return -EINVAL;
0111
0112     rbsp->data[ofs] &= ~(1 << shift);
0113     rbsp->data[ofs] |= value << shift;
0114
0115     rbsp->pos++;
0116
0117     if (value ||
0118         (rbsp->num_consecutive_zeros < 7 && (rbsp->pos % 8 == 0))) {
0119         rbsp->num_consecutive_zeros = 0;
0120     } else {
0121         rbsp->num_consecutive_zeros++;
0122     }
0123
0124     return 0;
0125 }
0126
0127 static inline int rbsp_read_bits(struct rbsp *rbsp, int n, unsigned int *value)
0128 {
0129     int i;
0130     int bit;
0131     unsigned int tmp = 0;
0132
0133     if (n > 8 * sizeof(*value))
0134         return -EINVAL;
0135
0136     for (i = n; i > 0; i--) {
0137         bit = rbsp_read_bit(rbsp);
0138         if (bit < 0)
0139             return bit;
0140         tmp |= bit << (i - 1);
0141     }
0142
0143     if (value)
0144         *value = tmp;
0145
0146     return 0;
0147 }
0148
0149 static int rbsp_write_bits(struct rbsp *rbsp, int n, unsigned int value)
0150 {
0151     int ret;
0152
0153     if (n > 8 * sizeof(value))
0154         return -EINVAL;
0155
0156     while (n--) {
0157         ret = rbsp_write_bit(rbsp, (value >> n) & 1);
0158         if (ret)
0159             return ret;
0160     }
0161
0162     return 0;
0163 }
0164
0165 static int rbsp_read_uev(struct rbsp *rbsp, unsigned int *value)
0166 {
0167     int leading_zero_bits = 0;
0168     unsigned int tmp = 0;
0169     int ret;
0170
0171     while ((ret = rbsp_read_bit(rbsp)) == 0)
0172         leading_zero_bits++;
0173     if (ret < 0)
0174         return ret;
0175
0176     if (leading_zero_bits > 0) {
0177         ret = rbsp_read_bits(rbsp, leading_zero_bits, &tmp);
0178         if (ret)
0179             return ret;
0180     }
0181
0182     if (value)
0183         *value = (1 << leading_zero_bits) - 1 + tmp;
0184
0185     return 0;
0186 }
0187
0188 static int rbsp_write_uev(struct rbsp *rbsp, unsigned int *value)
0189 {
0190     int ret;
0191     int leading_zero_bits;
0192
0193     if (!value)
0194         return -EINVAL;
0195
0196     leading_zero_bits = ilog2(*value + 1);
0197
0198     ret = rbsp_write_bits(rbsp, leading_zero_bits, 0);
0199     if (ret)
0200         return ret;
0201
0202     return rbsp_write_bits(rbsp, leading_zero_bits + 1, *value + 1);
0203 }
0204
0205 static int rbsp_read_sev(struct rbsp *rbsp, int *value)
0206 {
0207     int ret;
0208     unsigned int tmp;
0209
0210     ret = rbsp_read_uev(rbsp, &tmp);
0211     if (ret)
0212         return ret;
0213
0214     if (value) {
0215         if (tmp & 1)
0216             *value = (tmp + 1) / 2;
0217         else
0218             *value = -(tmp / 2);
0219     }
0220
0221     return 0;
0222 }
0223
0224 static int rbsp_write_sev(struct rbsp *rbsp, int *value)
0225 {
0226     unsigned int tmp;
0227
0228     if (!value)
0229         return -EINVAL;
0230
0231     if (*value > 0)
0232         tmp = (2 * (*value)) | 1;
0233     else
0234         tmp = -2 * (*value);
0235
0236     return rbsp_write_uev(rbsp, &tmp);
0237 }
0238
0239 static int __rbsp_write_bit(struct rbsp *rbsp, int *value)
0240 {
0241     return rbsp_write_bit(rbsp, *value);
0242 }
0243
0244 static int __rbsp_write_bits(struct rbsp *rbsp, int n, unsigned int *value)
0245 {
0246     return rbsp_write_bits(rbsp, n, *value);
0247 }
0248
0249 struct nal_rbsp_ops write = {
0250     .rbsp_bit = __rbsp_write_bit,
0251     .rbsp_bits = __rbsp_write_bits,
0252     .rbsp_uev = rbsp_write_uev,
0253     .rbsp_sev = rbsp_write_sev,
0254 };
0255
0256 static int __rbsp_read_bit(struct rbsp *rbsp, int *value)
0257 {
0258     int tmp = rbsp_read_bit(rbsp);
0259
0260     if (tmp < 0)
0261         return tmp;
0262     *value = tmp;
0263
0264     return 0;
0265 }
0266
0267 struct nal_rbsp_ops read = {
0268     .rbsp_bit = __rbsp_read_bit,
0269     .rbsp_bits = rbsp_read_bits,
0270     .rbsp_uev = rbsp_read_uev,
0271     .rbsp_sev = rbsp_read_sev,
0272 };
0273
0274 void rbsp_bit(struct rbsp *rbsp, int *value)
0275 {
0276     if (rbsp->error)
0277         return;
0278     rbsp->error = rbsp->ops->rbsp_bit(rbsp, value);
0279 }
0280
0281 void rbsp_bits(struct rbsp *rbsp, int n, int *value)
0282 {
0283     if (rbsp->error)
0284         return;
0285     rbsp->error = rbsp->ops->rbsp_bits(rbsp, n, value);
0286 }
0287
0288 void rbsp_uev(struct rbsp *rbsp, unsigned int *value)
0289 {
0290     if (rbsp->error)
0291         return;
0292     rbsp->error = rbsp->ops->rbsp_uev(rbsp, value);
0293 }
0294
0295 void rbsp_sev(struct rbsp *rbsp, int *value)
0296 {
0297     if (rbsp->error)
0298         return;
0299     rbsp->error = rbsp->ops->rbsp_sev(rbsp, value);
0300 }
0301
0302 void rbsp_trailing_bits(struct rbsp *rbsp)
0303 {
0304     unsigned int rbsp_stop_one_bit = 1;
0305     unsigned int rbsp_alignment_zero_bit = 0;
0306
0307     rbsp_bit(rbsp, &rbsp_stop_one_bit);
0308     rbsp_bits(rbsp, round_up(rbsp->pos, 8) - rbsp->pos,
0309           &rbsp_alignment_zero_bit);
0310 }