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
coda_find_nal_header(const u8 * buf,const u8 * end)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
coda_sps_parse_profile(struct coda_ctx * ctx,struct vb2_buffer * vb)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
coda_h264_filler_nal(int size,char * p)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
coda_h264_padding(int size,char * p)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
coda_h264_profile(int profile_idc)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
coda_h264_level(int level_idc)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
rbsp_read_bit(struct rbsp * rbsp)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
rbsp_write_bit(struct rbsp * rbsp,int bit)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
rbsp_read_bits(struct rbsp * rbsp,int num,int * val)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
rbsp_write_bits(struct rbsp * rbsp,int num,int value)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
rbsp_read_uev(struct rbsp * rbsp,unsigned int * val)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
rbsp_write_uev(struct rbsp * rbsp,unsigned int value)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
rbsp_read_sev(struct rbsp * rbsp,int * val)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 */
coda_h264_sps_fixup(struct coda_ctx * ctx,int width,int height,char * buf,int * size,int max_size)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 }
433
