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

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