libandroidplugin added
[mardrone] / mardrone / ARDrone_SDK_Version_1_8_20110726 / ARDroneLib / VLIB / P264 / p264_mb_layer.c
1 #include <VLIB/video_controller.h>
2 #include <VLIB/video_packetizer.h>
3 #include <VLIB/Platform/video_utils.h>
4
5 #include <VP_Os/vp_os_malloc.h>
6 #include <VP_Os/vp_os_assert.h>
7
8 #include "p264_codec.h"
9 #include "p264_layers.h"
10 #include "p264.h"
11 #include "p264_common.h"
12
13 #include <VP_Os/vp_os_print.h>
14
15 #define min(a,b) ((a)<(b)?(a):(b))
16 #define max(a,b) ((a)>(b)?(a):(b))
17
18 // macro helper to read an intra 4x4 mode using prediction
19 #define READ_INTRA_CODE(pred,dest_intra) \
20     code = 0; \
21     video_read_data (stream,&code,1); \
22     if (code==0) \
23     { \
24       dest_intra = pred; \
25     } \
26     else \
27     { \
28       code = 0; \
29       video_read_data (stream,&code,3); \
30       if (code<pred) \
31         dest_intra = code; \
32       else \
33         dest_intra = code+1; \
34     }
35
36 // make a intra 4x4 prediction based on the upper and left 4x4 block intra mode (see p264 standard)
37 intra_4x4_mode_t make_boundary_pred(intra_4x4_mode_t* left_intra_4x4, uint32_t left_index, intra_4x4_mode_t * up_intra_4x4, uint32_t up_index,bool_t boundary_block)
38 {
39   intra_4x4_mode_t pred;
40   intra_4x4_mode_t left_mode,up_mode;
41   if (boundary_block == TRUE)
42     pred = DC_4x4_MODE;
43   else
44   {
45     if (left_intra_4x4 == NULL)
46       left_mode = DC_4x4_MODE;
47     else
48       left_mode = left_intra_4x4[left_index];
49
50     if (up_intra_4x4 == NULL)
51       up_mode = DC_4x4_MODE;
52     else
53       up_mode = up_intra_4x4[up_index];
54
55     pred = min(left_mode,up_mode);
56   }
57
58   return pred;
59 }
60
61 // write 16 intra_4x4 mode. Data should contains prediction on intra_4x4 (see p264 standard)
62 // example : data = [ pred (1bits) | code (3bits) ]
63 // if pred == 0 the prediction made over up and left block is correct. Thus no intra code has to bee transmitted
64 // if pred != 1, the prediction is wrong. The intra 4x4 mode should be transmitted using only 3 bits (see p264 standard)
65 void p264_write_intra_4x4 (video_stream_t* const stream, intra_4x4_mode_t* data)
66 {
67     uint32_t i=16;
68     while(i--)
69     {
70       if (((*data)&0x08) == 0)
71       {
72         // the prediction is good, signal it
73         video_write_data (stream,0,1);
74       }
75       else
76       {
77         // the prediction is false, transmit the intra 4x4 mode
78         video_write_data (stream,*data,4);
79       }
80       data++;
81     }
82 }
83
84 // TODO: this function should be moved to a dedicated file
85 void p264_read_mv(video_stream_t* const stream, video_gob_t*  gobs, uint32_t i_blockline, uint32_t i_mb, uint32_t num_mb_per_line)
86 {
87 // this function reads a 16x16 mvp from a stream and reconstructs the correct mv
88 // it is assumed only 16x16 partition are used
89 //  |D|B|C|
90 //  |A|E
91 // E is the current mb
92 //
93 // case 1 : A is unavailable, use {0,0} as its mv (A becomes available)
94 // case 2 : only A is available, use its mv as the prediction
95 // case 3 : C is unavailable, use C=D instead (C becomes available)
96 // case 4 : A,B,C are available
97 // for case 1,3,4 the prediction is the median(A,B,C)
98
99 // therefore the MB types over a P-frame are
100 //
101 //  --- ---       --- ---
102 // |1&2| 2 | ... | 2 | 2 |
103 //  --- ---       --- ---
104 // | 1 | 4 | ... | 4 | 3 |
105 //  --- ---       --- ---
106 // |      ...            |
107 //  --- ---       --- ---
108
109   int32_t s_code;
110   int32_t pred_x,pred_y;
111   int32_t ax,ay,bx,by,cx,cy;
112
113   if (i_blockline == 0 && i_mb == 0)
114   {
115     // first MB (case 1&2)
116     pred_x = 0;
117     pred_y = 0;
118   }
119   else if (i_blockline == 0)
120   {
121     // first MB line (case 2)
122     pred_x = gobs[i_blockline].macroblocks[i_mb-1].inter_MV[0].x;
123     pred_y = gobs[i_blockline].macroblocks[i_mb-1].inter_MV[0].y;
124   }
125   else if (i_mb == 0)
126   {
127     // first MB column (case 1)
128     ax = 0;
129     ay = 0;
130     bx = gobs[i_blockline-1].macroblocks[i_mb].inter_MV[0].x;
131     by = gobs[i_blockline-1].macroblocks[i_mb].inter_MV[0].y;
132     cx = gobs[i_blockline-1].macroblocks[i_mb+1].inter_MV[0].x;
133     cy = gobs[i_blockline-1].macroblocks[i_mb+1].inter_MV[0].y;
134     pred_x = ax + bx + cx - min(ax,min(bx,cx)) - max(ax,max(bx,cx));
135     pred_y = ay + by + cy - min(ay,min(by,cy)) - max(ay,max(by,cy));
136   }
137   else if (i_mb == (num_mb_per_line-1))
138   {
139     // last column (case 3)
140     ax = gobs[i_blockline].macroblocks[i_mb-1].inter_MV[0].x;
141     ay = gobs[i_blockline].macroblocks[i_mb-1].inter_MV[0].y;
142     bx = gobs[i_blockline-1].macroblocks[i_mb].inter_MV[0].x;
143     by = gobs[i_blockline-1].macroblocks[i_mb].inter_MV[0].y;
144     cx = gobs[i_blockline-1].macroblocks[i_mb-1].inter_MV[0].x; // in fact D
145     cy = gobs[i_blockline-1].macroblocks[i_mb-1].inter_MV[0].y; // in fact D
146     pred_x = ax + bx + cx - min(ax,min(bx,cx)) - max(ax,max(bx,cx));
147     pred_y = ay + by + cy - min(ay,min(by,cy)) - max(ay,max(by,cy));
148   }
149   else
150   {
151     // others MB (case 4)
152     ax = gobs[i_blockline].macroblocks[i_mb-1].inter_MV[0].x;
153     ay = gobs[i_blockline].macroblocks[i_mb-1].inter_MV[0].y;
154     bx = gobs[i_blockline-1].macroblocks[i_mb].inter_MV[0].x;
155     by = gobs[i_blockline-1].macroblocks[i_mb].inter_MV[0].y;
156     cx = gobs[i_blockline-1].macroblocks[i_mb+1].inter_MV[0].x;
157     cy = gobs[i_blockline-1].macroblocks[i_mb+1].inter_MV[0].y;
158     pred_x = ax + bx + cx - min(ax,min(bx,cx)) - max(ax,max(bx,cx));
159     pred_y = ay + by + cy - min(ay,min(by,cy)) - max(ay,max(by,cy));
160   }
161
162   // read mv, add prediction and save
163   s_code=0;
164   p264_decode_int(stream, &s_code);
165   gobs[i_blockline].macroblocks[i_mb].inter_MV[0].x = s_code+pred_x;
166   s_code=0;
167   p264_decode_int(stream, &s_code);
168   gobs[i_blockline].macroblocks[i_mb].inter_MV[0].y = s_code+pred_y;
169 }
170
171 // TODO: this function should be moved to a dedicated file
172 // read 16 intra_4x4 mode transmitted with intra prediction (see p264 standard)
173 void p264_read_intra_4x4 (video_stream_t* const stream, video_gob_t*  gobs, uint32_t i_blockline, uint32_t i_mb)
174 {
175     intra_4x4_mode_t* current_intra_4x4;
176     intra_4x4_mode_t* up_intra_4x4=NULL;
177     intra_4x4_mode_t* left_intra_4x4=NULL;
178     bool_t left_boundary_block = FALSE;
179     bool_t up_boundary_block = FALSE;
180     video_macroblock_t * mb;
181     intra_4x4_mode_t pred = DC_4x4_MODE;
182     uint32_t code;
183
184     // retrieve current mb, up and left intra 4x4 prediction if available
185     current_intra_4x4 = gobs[i_blockline].macroblocks[i_mb].intra_4x4_mode;
186     if (i_blockline > 0)
187     {
188       mb = &gobs[i_blockline-1].macroblocks[i_mb];
189       up_intra_4x4 = mb->intra_4x4_mode;
190       // check whether it's an intra 4x4 mb or not
191       if (mb->intra_type != INTRA_4x4)
192       {
193         up_intra_4x4 = NULL;
194       }
195     }
196     else
197       up_boundary_block = TRUE;
198
199     if (i_mb > 0)
200     {
201       mb = &gobs[i_blockline].macroblocks[i_mb-1];
202       // check whether it's an intra 4x4 mb or not
203       left_intra_4x4 = mb->intra_4x4_mode;
204       if (mb->intra_type != INTRA_4x4)
205       {
206         left_intra_4x4 = NULL;
207       }
208     }
209     else
210       left_boundary_block = TRUE;
211
212   // read intra(0,0)
213   pred = make_boundary_pred(left_intra_4x4,5,up_intra_4x4,10,up_boundary_block|left_boundary_block);
214   READ_INTRA_CODE(pred,current_intra_4x4[0]);
215   // read intra(1,0)
216   pred = make_boundary_pred(current_intra_4x4,0,up_intra_4x4,11,up_boundary_block);
217   READ_INTRA_CODE(pred,current_intra_4x4[1]);
218   // read intra(0,1)
219   pred = make_boundary_pred(left_intra_4x4,7,current_intra_4x4,0,left_boundary_block);
220   READ_INTRA_CODE(pred,current_intra_4x4[2]);
221   // read intra(1,1)
222   pred = min(current_intra_4x4[1],current_intra_4x4[2]);
223   READ_INTRA_CODE(pred,current_intra_4x4[3]);
224
225   // read intra(2,0)
226   pred = make_boundary_pred(current_intra_4x4,1,up_intra_4x4,14,up_boundary_block);
227   READ_INTRA_CODE(pred,current_intra_4x4[4]);
228   // read intra(3,0)
229   pred = make_boundary_pred(current_intra_4x4,4,up_intra_4x4,15,up_boundary_block);
230   READ_INTRA_CODE(pred,current_intra_4x4[5]);
231   // read intra(2,1)
232   pred = min(current_intra_4x4[4],current_intra_4x4[3]);
233   READ_INTRA_CODE(pred,current_intra_4x4[6]);
234   // read intra(3,1)
235   pred = min(current_intra_4x4[5],current_intra_4x4[6]);
236   READ_INTRA_CODE(pred,current_intra_4x4[7]);
237
238   // read intra(0,2)
239   pred = make_boundary_pred(left_intra_4x4,13,current_intra_4x4,2,left_boundary_block);
240   READ_INTRA_CODE(pred,current_intra_4x4[8]);
241   // read intra(1,2)
242   pred = min(current_intra_4x4[8],current_intra_4x4[3]);
243   READ_INTRA_CODE(pred,current_intra_4x4[9]);
244   // read intra(0,3)
245   pred = make_boundary_pred(left_intra_4x4,15,current_intra_4x4,8,left_boundary_block);
246   READ_INTRA_CODE(pred,current_intra_4x4[10]);
247   // read intra(1,3)
248   pred = min(current_intra_4x4[9],current_intra_4x4[10]);
249   READ_INTRA_CODE(pred,current_intra_4x4[11]);
250
251   // read intra(2,2)
252   pred = min(current_intra_4x4[9],current_intra_4x4[6]);
253   READ_INTRA_CODE(pred,current_intra_4x4[12]);
254   // read intra(3,2)
255   pred = min(current_intra_4x4[12],current_intra_4x4[7]);
256   READ_INTRA_CODE(pred,current_intra_4x4[13]);
257     // read intra(2,3)
258   pred = min(current_intra_4x4[11],current_intra_4x4[12]);
259   READ_INTRA_CODE(pred,current_intra_4x4[14]);
260     // read intra(3,3)
261   pred = min(current_intra_4x4[13],current_intra_4x4[14]);
262   READ_INTRA_CODE(pred,current_intra_4x4[15]);
263
264 }
265
266 void p264_write_block( video_stream_t* const stream, int16_t* data, uint32_t length)
267 {
268   int32_t code, run, num_coeff;
269
270   // count number of DC coeff
271   num_coeff = 0;
272   int16_t * p_data = data;
273
274   while (length--)
275   {
276     if (*p_data++ != 0)
277       num_coeff++;
278   }
279
280   if (num_coeff == 0)
281   {
282     video_write_data( stream, 1, 1 ); // signal that there's no coeff
283   }
284   else
285   {
286     video_write_data( stream, 0, 1 ); // signal that there are coeffs
287
288     run = 0;
289     while( num_coeff > 0 )
290     {
291       code = *data++;
292       if( code == 0 )
293       {
294         run ++;
295       }
296       else
297       {
298         num_coeff--;
299         p264_encode( stream, code, run, num_coeff );
300         run = 0;
301       }
302     }
303   }
304 }
305
306 void p264_read_block( video_stream_t* const stream, int16_t* data)
307 {
308   int32_t  index,  run, last;//, nc;
309   uint32_t code;
310
311   code = run = last = 0;
312   video_read_data( stream, &code, 1 ); // signal that there's no DC coeff
313
314   if(code == 0)
315   {
316     index = -1;
317     while( last == 0 )
318     {
319       code = run = last = 0;
320       p264_decode( stream, &run, &code, &last);
321
322       if( last == 0 )
323       {
324         index    += (run+1);
325         data[index] = code;
326       }
327     }
328   }
329 }
330
331 C_RESULT p264_write_mb_layer(video_controller_t* controller, video_stream_t* stream, video_macroblock_t* mb, int32_t num_macro_blocks )
332 {
333   int16_t* data;
334   //uint32_t code;
335   uint32_t i;
336
337
338   while( num_macro_blocks > 0 )
339   {
340     //PRINT ("p264_write_mb_layer : stream size %d  stream used %d\n",stream->size,stream->used);
341
342     if (controller->picture_type == VIDEO_PICTURE_INTER)
343     {
344       // write all partition
345       for (i=0;i<mb->nb_partition;i++)
346       {
347         video_write_data( stream,mb->inter_partition_mode[i],3);
348       }
349
350       // write all motion vector
351       for (i=0;i<mb->nb_partition;i++)
352       {
353         int32_t s_code;
354         s_code = (int32_t)mb->inter_MV[i].x;
355         p264_encode_int(stream, s_code);
356         s_code = (int32_t)mb->inter_MV[i].y;
357         p264_encode_int(stream, s_code);
358       }
359
360       // write residual data as an intra4x4
361       // write all 4x4 block luma AC coeff
362       i=16;
363       data = ((MB_p264_t*)mb->data)->inter.AC_Y;
364       while(i--)
365       {
366         p264_write_block( stream, data, 16);
367         data += BLOCK_SIZE2;
368       }
369
370       // write 4 DC U coeff
371       p264_write_block(stream,((MB_p264_t*)mb->data)->inter.DC_U,4);
372
373       // write AC U coeff
374       i=4;
375       data = &((MB_p264_t*)mb->data)->inter.AC_U[1];
376       while(i--)
377       {
378         p264_write_block( stream, data, 15);
379         data += BLOCK_SIZE2;
380       }
381
382       // write 4 DC V coeff
383       p264_write_block(stream,((MB_p264_t*)mb->data)->inter.DC_V,4);
384
385       // write AC V coeff
386       i=4;
387       data = &((MB_p264_t*)mb->data)->inter.AC_V[1];
388       while(i--)
389       {
390         p264_write_block( stream, data, 15);
391         data += BLOCK_SIZE2;
392       }
393     }
394     else
395     {
396       // write MB intra type (16x16 or 4x4)
397       video_write_data( stream, mb->intra_type, 1 );
398       // write intra chroma type
399       video_write_data (stream, mb->intra_chroma_8x8_mode,2);
400       if (mb->intra_type == INTRA_4x4)
401       {
402         // write all luma 4x4 prediction modes
403         p264_write_intra_4x4(stream,mb->intra_4x4_mode);
404
405         // write all 4x4 block luma AC coeff
406         i=16;
407         data = ((MB_p264_t*)mb->data)->intra_4x4.AC_Y;
408         while(i--)
409         {
410           p264_write_block( stream, data, 16);
411           data += BLOCK_SIZE2;
412         }
413
414         // write 4 DC U coeff
415         p264_write_block(stream,((MB_p264_t*)mb->data)->intra_4x4.DC_U,4);
416
417         // write AC U coeff
418         i=4;
419         data = &((MB_p264_t*)mb->data)->intra_4x4.AC_U[1];
420         while(i--)
421         {
422           p264_write_block( stream, data, 15);
423           data += BLOCK_SIZE2;
424         }
425
426         // write 4 DC V coeff
427         p264_write_block(stream,((MB_p264_t*)mb->data)->intra_4x4.DC_V,4);
428
429         // write AC V coeff
430         i=4;
431         data = &((MB_p264_t*)mb->data)->intra_4x4.AC_V[1];
432         while(i--)
433         {
434           p264_write_block( stream, data, 15);
435           data += BLOCK_SIZE2;
436         }
437       }
438       else
439       {
440         // write luma 16x16 prediction mode
441         video_write_data(stream, mb->intra_luma_16x16_mode,2);
442
443         // write 16 DC luma
444         p264_write_block( stream, ((MB_p264_t*)mb->data)->intra_16x16.DC_Y, 16);
445
446         // write 16 luma AC coeff block
447         i=16;
448         data = &((MB_p264_t*)mb->data)->intra_16x16.AC_Y[1];
449         while(i--)
450         {
451           p264_write_block( stream, data, 15);
452           data += BLOCK_SIZE2;
453         }
454
455         // write 4 DC U coeff
456         p264_write_block(stream,((MB_p264_t*)mb->data)->intra_16x16.DC_U,4);
457
458         // write AC U coeff
459         i=4;
460         data = &((MB_p264_t*)mb->data)->intra_16x16.AC_U[1];
461         while(i--)
462         {
463           p264_write_block( stream, data, 15);
464           data += BLOCK_SIZE2;
465         }
466
467         // write 4 DC V coeff
468         p264_write_block(stream,((MB_p264_t*)mb->data)->intra_16x16.DC_V,4);
469
470         // write AC V coeff
471         i=4;
472         data = &((MB_p264_t*)mb->data)->intra_16x16.AC_V[1];
473         while(i--)
474         {
475           p264_write_block( stream, data, 15);
476           data += BLOCK_SIZE2;
477         }
478       }
479     }
480     mb ++;
481     num_macro_blocks --;
482   }
483   return C_OK;
484 }
485
486
487 C_RESULT p264_read_mb_layer(video_controller_t* controller, video_stream_t* stream, video_gob_t*  gobs, uint32_t i_blockline, uint32_t i_mb)
488 {
489   int16_t* data;
490   uint32_t code;
491   //int8_t mv;
492   uint32_t i;
493   video_macroblock_t* mb;
494   mb = &gobs[i_blockline].macroblocks[i_mb];
495
496   vp_os_memset( mb->data, 0, sizeof(MB_p264_t));
497
498   if (controller->picture_type == VIDEO_PICTURE_INTER)
499   {
500     // for now p264 supports only one partition per macroblock
501     mb->nb_partition = 1;
502     // read all partition
503     for (i=0;i<mb->nb_partition;i++)
504     {
505       code = 0;
506       video_read_data( stream,&code,3);
507       mb->inter_partition_mode[i] = code;
508     }
509
510     // read all motion vector
511     p264_read_mv(stream, gobs, i_blockline, i_mb,controller->mb_blockline);
512
513     // read residual data as an intra 4x4
514     // read 16 4x4 block luma AC coeff
515     i=16;
516     data = ((MB_p264_t*)mb->data)->inter.AC_Y;
517     while(i--)
518     {
519       p264_read_block( stream, data);
520       data += BLOCK_SIZE2;
521     }
522
523     // read 4 DC U coeff
524     p264_read_block(stream,((MB_p264_t*)mb->data)->inter.DC_U);
525
526     // read AC U coeff
527     i=4;
528     data = &((MB_p264_t*)mb->data)->inter.AC_U[1];
529     while(i--)
530     {
531       p264_read_block( stream, data);
532       data += BLOCK_SIZE2;
533     }
534
535     // read 4 DC V coeff
536     p264_read_block(stream,((MB_p264_t*)mb->data)->inter.DC_V);
537
538     // read AC V coeff
539     i=4;
540     data = &((MB_p264_t*)mb->data)->inter.AC_V[1];
541     while(i--)
542     {
543       p264_read_block( stream, data);
544       data += BLOCK_SIZE2;
545     }
546   }
547   else
548   {
549     // read MB intra type
550     code = 0;
551     video_read_data(stream, &code, 1);
552     mb->intra_type = (intra_type_t)code;
553
554     // read intra chroma type
555     code = 0;
556     video_read_data(stream, &code, 2);
557     mb->intra_chroma_8x8_mode = (intra_8x8_chroma_mode_t) code;
558
559     if (mb->intra_type == INTRA_4x4)
560     {
561       // read all 4x4 modes
562       p264_read_intra_4x4(stream, gobs, i_blockline, i_mb);
563
564       // read 16 4x4 block luma AC coeff
565       i=16;
566       data = ((MB_p264_t*)mb->data)->intra_4x4.AC_Y;
567       while(i--)
568       {
569         p264_read_block( stream, data);
570         data += BLOCK_SIZE2;
571       }
572
573       // read 4 DC U coeff
574       p264_read_block(stream,((MB_p264_t*)mb->data)->intra_4x4.DC_U);
575
576       // read AC U coeff
577       i=4;
578       data = &((MB_p264_t*)mb->data)->intra_4x4.AC_U[1];
579       while(i--)
580       {
581         p264_read_block( stream, data);
582         data += BLOCK_SIZE2;
583       }
584
585       // read 4 DC V coeff
586       p264_read_block(stream,((MB_p264_t*)mb->data)->intra_4x4.DC_V);
587
588       // read AC V coeff
589       i=4;
590       data = &((MB_p264_t*)mb->data)->intra_4x4.AC_V[1];
591       while(i--)
592       {
593         p264_read_block( stream, data);
594         data += BLOCK_SIZE2;
595       }
596     }
597     else
598     {
599       // write luma 16x16 prediction mode
600       code = 0;
601       video_read_data(stream, &code ,2);
602       mb->intra_luma_16x16_mode = code;
603
604       // read 16 DC luma
605       p264_read_block( stream, ((MB_p264_t*)mb->data)->intra_16x16.DC_Y);
606
607       // write 256 luma AC coeff
608       // Normally only 15 AC coeff per block 4x4 has to be sent
609       i=16;
610       data = &((MB_p264_t*)mb->data)->intra_16x16.AC_Y[1];
611       while(i--)
612       {
613         p264_read_block( stream, data);
614         data += BLOCK_SIZE2;
615       }
616
617       // read 4 DC U coeff
618       p264_read_block(stream,((MB_p264_t*)mb->data)->intra_16x16.DC_U);
619
620       // read AC U coeff
621       i=4;
622       data = &((MB_p264_t*)mb->data)->intra_16x16.AC_U[1];
623       while(i--)
624       {
625         p264_read_block( stream, data);
626         data += BLOCK_SIZE2;
627       }
628
629       // read 4 DC V coeff
630       p264_read_block(stream,((MB_p264_t*)mb->data)->intra_16x16.DC_V);
631
632       // read AC V coeff
633       i=4;
634       data = &((MB_p264_t*)mb->data)->intra_16x16.AC_V[1];
635       while(i--)
636       {
637         p264_read_block( stream, data);
638         data += BLOCK_SIZE2;
639       }
640     }
641   }
642   return C_OK;
643 }
644