drivers/media/platform/coda/coda-h264.c

   1 // SPDX-License-Identifier: GPL-2.0-or-later
   2 /*
   3  * Coda multi-standard codec IP - H.264 helper functions
   4  *
   5  * Copyright (C) 2012 Vista Silicon S.L.
   6  *    Javier Martin, <javier.martin@vista-silicon.com>
   7  *    Xavier Duret
   8  */
   9
  10 #include <linux/kernel.h>
  11 #include <linux/string.h>
  12 #include <linux/videodev2.h>
  13
  14 #include "coda.h"
  15
  16 static const u8 coda_filler_size[8] = { 0, 7, 14, 13, 12, 11, 10, 9 };
  17
  18 static const u8 *coda_find_nal_header(const u8 *buf, const u8 *end)
  19 {
  20         u32 val = 0xffffffff;
  21
  22         do {
  23                 val = val << 8 | *buf++;
  24                 if (buf >= end)
  25                         return NULL;
  26         } while (val != 0x00000001);
  27
  28         return buf;
  29 }
  30
  31 int coda_sps_parse_profile(struct coda_ctx *ctx, struct vb2_buffer *vb)
  32 {
  33         const u8 *buf = vb2_plane_vaddr(vb, 0);
  34         const u8 *end = buf + vb2_get_plane_payload(vb, 0);
  35
  36         /* Find SPS header */
  37         do {
  38                 buf = coda_find_nal_header(buf, end);
  39                 if (!buf)
  40                         return -EINVAL;
  41         } while ((*buf++ & 0x1f) != 0x7);
  42
  43         ctx->params.h264_profile_idc = buf[0];
  44         ctx->params.h264_level_idc = buf[2];
  45
  46         return 0;
  47 }
  48
  49 int coda_h264_filler_nal(int size, char *p)
  50 {
  51         if (size < 6)
  52                 return -EINVAL;
  53
  54         p[0] = 0x00;
  55         p[1] = 0x00;
  56         p[2] = 0x00;
  57         p[3] = 0x01;
  58         p[4] = 0x0c;
  59         memset(p + 5, 0xff, size - 6);
  60         /* Add rbsp stop bit and trailing at the end */
  61         p[size - 1] = 0x80;
  62
  63         return 0;
  64 }
  65
  66 int coda_h264_padding(int size, char *p)
  67 {
  68         int nal_size;
  69         int diff;
  70
  71         diff = size - (size & ~0x7);
  72         if (diff == 0)
  73                 return 0;
  74
  75         nal_size = coda_filler_size[diff];
  76         coda_h264_filler_nal(nal_size, p);
  77
  78         return nal_size;
  79 }
  80
  81 int coda_h264_profile(int profile_idc)
  82 {
  83         switch (profile_idc) {
  84         case 66: return V4L2_MPEG_VIDEO_H264_PROFILE_BASELINE;
  85         case 77: return V4L2_MPEG_VIDEO_H264_PROFILE_MAIN;
  86         case 88: return V4L2_MPEG_VIDEO_H264_PROFILE_EXTENDED;
  87         case 100: return V4L2_MPEG_VIDEO_H264_PROFILE_HIGH;
  88         default: return -EINVAL;
  89         }
  90 }
  91
  92 int coda_h264_level(int level_idc)
  93 {
  94         switch (level_idc) {
  95         case 10: return V4L2_MPEG_VIDEO_H264_LEVEL_1_0;
  96         case 9:  return V4L2_MPEG_VIDEO_H264_LEVEL_1B;
  97         case 11: return V4L2_MPEG_VIDEO_H264_LEVEL_1_1;
  98         case 12: return V4L2_MPEG_VIDEO_H264_LEVEL_1_2;
  99         case 13: return V4L2_MPEG_VIDEO_H264_LEVEL_1_3;
 100         case 20: return V4L2_MPEG_VIDEO_H264_LEVEL_2_0;
 101         case 21: return V4L2_MPEG_VIDEO_H264_LEVEL_2_1;
 102         case 22: return V4L2_MPEG_VIDEO_H264_LEVEL_2_2;
 103         case 30: return V4L2_MPEG_VIDEO_H264_LEVEL_3_0;
 104         case 31: return V4L2_MPEG_VIDEO_H264_LEVEL_3_1;
 105         case 32: return V4L2_MPEG_VIDEO_H264_LEVEL_3_2;
 106         case 40: return V4L2_MPEG_VIDEO_H264_LEVEL_4_0;
 107         case 41: return V4L2_MPEG_VIDEO_H264_LEVEL_4_1;
 108         case 42: return V4L2_MPEG_VIDEO_H264_LEVEL_4_2;
 109         case 50: return V4L2_MPEG_VIDEO_H264_LEVEL_5_0;
 110         case 51: return V4L2_MPEG_VIDEO_H264_LEVEL_5_1;
 111         default: return -EINVAL;
 112         }
 113 }
 114
 115 struct rbsp {
 116         char *buf;
 117         int size;
 118         int pos;
 119 };
 120
 121 static inline int rbsp_read_bit(struct rbsp *rbsp)
 122 {
 123         int shift = 7 - (rbsp->pos % 8);
 124         int ofs = rbsp->pos++ / 8;
 125
 126         if (ofs >= rbsp->size)
 127                 return -EINVAL;
 128
 129         return (rbsp->buf[ofs] >> shift) & 1;
 130 }
 131
 132 static inline int rbsp_write_bit(struct rbsp *rbsp, int bit)
 133 {
 134         int shift = 7 - (rbsp->pos % 8);
 135         int ofs = rbsp->pos++ / 8;
 136
 137         if (ofs >= rbsp->size)
 138                 return -EINVAL;
 139
 140         rbsp->buf[ofs] &= ~(1 << shift);
 141         rbsp->buf[ofs] |= bit << shift;
 142
 143         return 0;
 144 }
 145
 146 static inline int rbsp_read_bits(struct rbsp *rbsp, int num, int *val)
 147 {
 148         int i, ret;
 149         int tmp = 0;
 150
 151         if (num > 32)
 152                 return -EINVAL;
 153
 154         for (i = 0; i < num; i++) {
 155                 ret = rbsp_read_bit(rbsp);
 156                 if (ret < 0)
 157                         return ret;
 158                 tmp |= ret << (num - i - 1);
 159         }
 160
 161         if (val)
 162                 *val = tmp;
 163
 164         return 0;
 165 }
 166
 167 static int rbsp_write_bits(struct rbsp *rbsp, int num, int value)
 168 {
 169         int ret;
 170
 171         while (num--) {
 172                 ret = rbsp_write_bit(rbsp, (value >> num) & 1);
 173                 if (ret)
 174                         return ret;
 175         }
 176
 177         return 0;
 178 }
 179
 180 static int rbsp_read_uev(struct rbsp *rbsp, unsigned int *val)
 181 {
 182         int leading_zero_bits = 0;
 183         unsigned int tmp = 0;
 184         int ret;
 185
 186         while ((ret = rbsp_read_bit(rbsp)) == 0)
 187                 leading_zero_bits++;
 188         if (ret < 0)
 189                 return ret;
 190
 191         if (leading_zero_bits > 0) {
 192                 ret = rbsp_read_bits(rbsp, leading_zero_bits, &tmp);
 193                 if (ret)
 194                         return ret;
 195         }
 196
 197         if (val)
 198                 *val = (1 << leading_zero_bits) - 1 + tmp;
 199
 200         return 0;
 201 }
 202
 203 static int rbsp_write_uev(struct rbsp *rbsp, unsigned int value)
 204 {
 205         int i;
 206         int ret;
 207         int tmp = value + 1;
 208         int leading_zero_bits = fls(tmp) - 1;
 209
 210         for (i = 0; i < leading_zero_bits; i++) {
 211                 ret = rbsp_write_bit(rbsp, 0);
 212                 if (ret)
 213                         return ret;
 214         }
 215
 216         return rbsp_write_bits(rbsp, leading_zero_bits + 1, tmp);
 217 }
 218
 219 static int rbsp_read_sev(struct rbsp *rbsp, int *val)
 220 {
 221         unsigned int tmp;
 222         int ret;
 223
 224         ret = rbsp_read_uev(rbsp, &tmp);
 225         if (ret)
 226                 return ret;
 227
 228         if (val) {
 229                 if (tmp & 1)
 230                         *val = (tmp + 1) / 2;
 231                 else
 232                         *val = -(tmp / 2);
 233         }
 234
 235         return 0;
 236 }
 237
 238 /**
 239  * coda_h264_sps_fixup - fixes frame cropping values in h.264 SPS
 240  * @ctx: encoder context
 241  * @width: visible width
 242  * @height: visible height
 243  * @buf: buffer containing h.264 SPS RBSP, starting with NAL header
 244  * @size: modified RBSP size return value
 245  * @max_size: available size in buf
 246  *
 247  * Rewrites the frame cropping values in an h.264 SPS RBSP correctly for the
 248  * given visible width and height.
 249  */
 250 int coda_h264_sps_fixup(struct coda_ctx *ctx, int width, int height, char *buf,
 251                         int *size, int max_size)
 252 {
 253         int profile_idc;
 254         unsigned int pic_order_cnt_type;
 255         int pic_width_in_mbs_minus1, pic_height_in_map_units_minus1;
 256         int frame_mbs_only_flag, frame_cropping_flag;
 257         int vui_parameters_present_flag;
 258         unsigned int crop_right, crop_bottom;
 259         struct rbsp sps;
 260         int pos;
 261         int ret;
 262
 263         if (*size < 8 || *size >= max_size)
 264                 return -EINVAL;
 265
 266         sps.buf = buf + 5; /* Skip NAL header */
 267         sps.size = *size - 5;
 268
 269         profile_idc = sps.buf[0];
 270         /* Skip constraint_set[0-5]_flag, reserved_zero_2bits */
 271         /* Skip level_idc */
 272         sps.pos = 24;
 273
 274         /* seq_parameter_set_id */
 275         ret = rbsp_read_uev(&sps, NULL);
 276         if (ret)
 277                 return ret;
 278
 279         if (profile_idc == 100 || profile_idc == 110 || profile_idc == 122 ||
 280             profile_idc == 244 || profile_idc == 44 || profile_idc == 83 ||
 281             profile_idc == 86 || profile_idc == 118 || profile_idc == 128 ||
 282             profile_idc == 138 || profile_idc == 139 || profile_idc == 134 ||
 283             profile_idc == 135) {
 284                 dev_err(ctx->fh.vdev->dev_parent,
 285                         "%s: Handling profile_idc %d not implemented\n",
 286                         __func__, profile_idc);
 287                 return -EINVAL;
 288         }
 289
 290         /* log2_max_frame_num_minus4 */
 291         ret = rbsp_read_uev(&sps, NULL);
 292         if (ret)
 293                 return ret;
 294
 295         ret = rbsp_read_uev(&sps, &pic_order_cnt_type);
 296         if (ret)
 297                 return ret;
 298
 299         if (pic_order_cnt_type == 0) {
 300                 /* log2_max_pic_order_cnt_lsb_minus4 */
 301                 ret = rbsp_read_uev(&sps, NULL);
 302                 if (ret)
 303                         return ret;
 304         } else if (pic_order_cnt_type == 1) {
 305                 unsigned int i, num_ref_frames_in_pic_order_cnt_cycle;
 306
 307                 /* delta_pic_order_always_zero_flag */
 308                 ret = rbsp_read_bit(&sps);
 309                 if (ret < 0)
 310                         return ret;
 311                 /* offset_for_non_ref_pic */
 312                 ret = rbsp_read_sev(&sps, NULL);
 313                 if (ret)
 314                         return ret;
 315                 /* offset_for_top_to_bottom_field */
 316                 ret = rbsp_read_sev(&sps, NULL);
 317                 if (ret)
 318                         return ret;
 319
 320                 ret = rbsp_read_uev(&sps,
 321                                     &num_ref_frames_in_pic_order_cnt_cycle);
 322                 if (ret)
 323                         return ret;
 324                 for (i = 0; i < num_ref_frames_in_pic_order_cnt_cycle; i++) {
 325                         /* offset_for_ref_frame */
 326                         ret = rbsp_read_sev(&sps, NULL);
 327                         if (ret)
 328                                 return ret;
 329                 }
 330         }
 331
 332         /* max_num_ref_frames */
 333         ret = rbsp_read_uev(&sps, NULL);
 334         if (ret)
 335                 return ret;
 336
 337         /* gaps_in_frame_num_value_allowed_flag */
 338         ret = rbsp_read_bit(&sps);
 339         if (ret < 0)
 340                 return ret;
 341         ret = rbsp_read_uev(&sps, &pic_width_in_mbs_minus1);
 342         if (ret)
 343                 return ret;
 344         ret = rbsp_read_uev(&sps, &pic_height_in_map_units_minus1);
 345         if (ret)
 346                 return ret;
 347         frame_mbs_only_flag = ret = rbsp_read_bit(&sps);
 348         if (ret < 0)
 349                 return ret;
 350         if (!frame_mbs_only_flag) {
 351                 /* mb_adaptive_frame_field_flag */
 352                 ret = rbsp_read_bit(&sps);
 353                 if (ret < 0)
 354                         return ret;
 355         }
 356         /* direct_8x8_inference_flag */
 357         ret = rbsp_read_bit(&sps);
 358         if (ret < 0)
 359                 return ret;
 360
 361         /* Mark position of the frame cropping flag */
 362         pos = sps.pos;
 363         frame_cropping_flag = ret = rbsp_read_bit(&sps);
 364         if (ret < 0)
 365                 return ret;
 366         if (frame_cropping_flag) {
 367                 unsigned int crop_left, crop_top;
 368
 369                 ret = rbsp_read_uev(&sps, &crop_left);
 370                 if (ret)
 371                         return ret;
 372                 ret = rbsp_read_uev(&sps, &crop_right);
 373                 if (ret)
 374                         return ret;
 375                 ret = rbsp_read_uev(&sps, &crop_top);
 376                 if (ret)
 377                         return ret;
 378                 ret = rbsp_read_uev(&sps, &crop_bottom);
 379                 if (ret)
 380                         return ret;
 381         }
 382         vui_parameters_present_flag = ret = rbsp_read_bit(&sps);
 383         if (ret < 0)
 384                 return ret;
 385         if (vui_parameters_present_flag) {
 386                 dev_err(ctx->fh.vdev->dev_parent,
 387                         "%s: Handling vui_parameters not implemented\n",
 388                         __func__);
 389                 return -EINVAL;
 390         }
 391
 392         crop_right = round_up(width, 16) - width;
 393         crop_bottom = round_up(height, 16) - height;
 394         crop_right /= 2;
 395         if (frame_mbs_only_flag)
 396                 crop_bottom /= 2;
 397         else
 398                 crop_bottom /= 4;
 399
 400
 401         sps.size = max_size - 5;
 402         sps.pos = pos;
 403         frame_cropping_flag = 1;
 404         ret = rbsp_write_bit(&sps, frame_cropping_flag);
 405         if (ret)
 406                 return ret;
 407         ret = rbsp_write_uev(&sps, 0); /* crop_left */
 408         if (ret)
 409                 return ret;
 410         ret = rbsp_write_uev(&sps, crop_right);
 411         if (ret)
 412                 return ret;
 413         ret = rbsp_write_uev(&sps, 0); /* crop_top */
 414         if (ret)
 415                 return ret;
 416         ret = rbsp_write_uev(&sps, crop_bottom);
 417         if (ret)
 418                 return ret;
 419         ret = rbsp_write_bit(&sps, 0); /* vui_parameters_present_flag */
 420         if (ret)
 421                 return ret;
 422         ret = rbsp_write_bit(&sps, 1);
 423         if (ret)
 424                 return ret;
 425
 426         *size = 5 + DIV_ROUND_UP(sps.pos, 8);
 427
 428         return 0;
 429 }