git.maemo.org Git - mardrone/blob - mardrone/ARDrone_SDK_Version_1_8_20110726/ARDroneLib/VLIB/Platform/arm9_P6/video_p264_p6.c

   1 #include <stdio.h>
   2 #include "libuiomap.h"
   3 #include "dma_malloc.h"
   4 #include "video_config.h"
   5 #include "video_p264_p6.h"
   6 #include "video_utils_p6.h"
   7 #include <VP_Os/vp_os_malloc.h>
   8 #include "P6_h264_reg.h"
   9 #include <VLIB/P264/p264_common.h>
  10 #include <VLIB/P264/p264_zigzag.h>
  11 #include <stdio.h>
  12 #include <VP_Os/vp_os_print.h>
  13 #include <sys/ioctl.h>
  14 #include <sys/types.h>
  15 #include <sys/stat.h>
  16 #include <fcntl.h>
  17 #include <parrot/p264/p264_p6_ioctl.h>
  18
  19
  20 #define CHECK_P264_IOCTL(a) if((a)<0) { PRINT("p264 ioctl failed (%s)\n",#a); return C_FAIL;}
  21 //#define H264_P6_DEBUG
  22 //#define H264_P6_PFRAME_DEBUG
  23
  24 // h264 picture parameters
  25 static uint8_t* current_Y;
  26 static uint8_t* current_Cb;
  27 static uint8_t* current_Cr;
  28 static uint32_t current_width;
  29 static uint32_t current_height;
  30 static bool_t I_encoding;
  31
  32 static uint8_t* h264_ref_frame;  // frame decoded by encoder as a reference
  33 static uint8_t* h264_deb_frame;  // frame decoded by encoder as a reference
  34 static p264_p6_raw_reg_results_t h264_raw_result[400]; // chroma_mode, intra_mode, MV, ...
  35
  36 static int h264_fd = -1;
  37
  38 C_RESULT video_p264_p6_init(void)
  39 {
  40   C_RESULT res = C_OK;
  41   h264_fd = open("/dev/p264_p6", O_RDWR|O_NONBLOCK);
  42   if (!h264_fd)
  43   {
  44     PRINT("p264 driver not found...\n");
  45     res = C_FAIL;
  46   }
  47
  48   // init local variable
  49   // h264 picture parameters
  50   current_Y  = NULL;
  51   current_Cb = NULL;
  52   current_Cr = NULL;
  53   current_width = 0;
  54   current_height = 0;
  55   h264_deb_frame = NULL;
  56   h264_ref_frame = NULL;
  57
  58   return res;
  59 }
  60
  61 #ifdef HAS_P264_FTRANSFORM
  62 C_RESULT video_p264_prepare_slice ( video_controller_t* controller, const vp_api_picture_t* blockline)
  63 {
  64   // picture dimensions change ?
  65   if (controller->width != current_width || controller->height != current_height)
  66   {
  67     // realloc a YUV 4:2:0 reference frame
  68     h264_ref_frame = (uint8_t*)dma_realloc(h264_ref_frame,controller->width*controller->height*3/2);
  69     h264_deb_frame = (uint8_t*)dma_realloc(h264_deb_frame,controller->width*controller->height*3/2);
  70     current_width = controller->width;
  71     current_height = controller->height;
  72
  73     if (current_width > CIF_WIDTH || current_height > CIF_HEIGHT)
  74       PRINT("resolutions above CIF are not supported by P6 h264 ip\n");
  75     // set search window size
  76     // ... use default value
  77
  78     uint32_t value = P264_RES(current_width,current_height);
  79     CHECK_P264_IOCTL(ioctl(h264_fd, P264_SET_DIM, &value))
  80
  81 #ifdef H264_P6_DEBUG
  82     PRINT ("H264 IP : new frame dimensions %dx%d\n",current_width,current_height);
  83 #endif
  84   }
  85
  86   // swap DEB and ref frames
  87   uint8_t* p_tmp;
  88   p_tmp = h264_deb_frame;
  89   h264_deb_frame = h264_ref_frame;
  90   h264_ref_frame = p_tmp;
  91
  92   // set DEB new frame
  93   uint32_t value = (uint32_t)dma_virt2phy(h264_deb_frame);
  94   CHECK_P264_IOCTL(ioctl(h264_fd, P264_SET_DEB_FRAME, &value))
  95
  96   dma_flush_inv( (uint32_t)h264_deb_frame, current_width*current_height*3/2);
  97
  98   // set reference frame
  99   value = (uint32_t)dma_virt2phy(h264_ref_frame);
 100   CHECK_P264_IOCTL(ioctl(h264_fd, P264_SET_REF_FRAME, &value))
 101   dma_flush_inv( (uint32_t)h264_ref_frame, current_width*current_height*3/2);
 102
 103   CHECK_P264_IOCTL(ioctl(h264_fd, P264_SET_FRAME_TYPE, &(controller->picture_type)))
 104
 105   // devrait pouvoir sauter
 106   if (controller->picture_type == VIDEO_PICTURE_INTRA)
 107     I_encoding = TRUE;
 108   else
 109     I_encoding = FALSE;
 110
 111   // retrieve picture pointers
 112   current_Y = blockline->y_buf;
 113   current_Cb = blockline->cb_buf;
 114   current_Cr = blockline->cr_buf;
 115
 116   p264_p6_input_buf_t input_buf;
 117   input_buf.phys_Y = (uint32_t)dma_virt2phy(current_Y);
 118   input_buf.phys_Cb = (uint32_t)dma_virt2phy(current_Cb);
 119   input_buf.phys_Cr = (uint32_t)dma_virt2phy(current_Cr);
 120   CHECK_P264_IOCTL(ioctl(h264_fd, P264_SET_INPUT_BUF, &input_buf))
 121
 122   // flush input frame
 123   dma_flush_inv( (uint32_t)current_Y, current_width*current_height);
 124   dma_flush_inv( (uint32_t)current_Cb, (current_width*current_height>>2));
 125   dma_flush_inv( (uint32_t)current_Cr, (current_width*current_height>>2));
 126
 127   // flush output data
 128   dma_flush_inv( (uint32_t)controller->cache, (controller->width>>4) * (controller->height>>4) * sizeof(MB_p264_t));
 129
 130   return C_OK;
 131 }
 132
 133 static void intra_pred_4x4_p6_to_list (uint32_t intra_pred_4x4_0, uint32_t intra_pred_4x4_1, intra_4x4_mode_t* out)
 134 {
 135   uint32_t i;
 136   for (i=0;i<8;i++)
 137   {
 138     *out++ = (intra_4x4_mode_t) (intra_pred_4x4_0 & 0x0F);
 139     intra_pred_4x4_0 = intra_pred_4x4_0>>4;
 140   }
 141   for (i=0;i<8;i++)
 142   {
 143     *out++ = (intra_4x4_mode_t) (intra_pred_4x4_1 & 0x0F);
 144     intra_pred_4x4_1 = intra_pred_4x4_1>>4;
 145   }
 146 }
 147
 148 void P6_get_MV (MV_XY_t *mv,uint32_t num_mv, uint32_t* raw_mv_tab)
 149 {
 150   uint32_t mv_2k_result;
 151   mv_2k_result = raw_mv_tab[(num_mv&0x0E)>>1];
 152   if ((num_mv&0x01) == 0)
 153   {
 154     mv->x = (mv_2k_result&0x000000FF)>>0;
 155     mv->x >>= 2;
 156     mv->y = (mv_2k_result&0x0000FF00)>>8;
 157     mv->y >>= 2;
 158   }
 159   else
 160   {
 161     mv->x = (mv_2k_result&0x00FF0000)>>16;
 162     mv->x >>= 2;
 163     mv->y = (mv_2k_result&0xFF000000)>>24;
 164     mv->y >>= 2;
 165   }
 166 }
 167
 168
 169 // encode num_macro_blocks MB
 170 C_RESULT video_p264_encode_MB(uint32_t num_macro_blocks, video_macroblock_t* macroblock ,int32_t qp)
 171 {
 172   if (num_macro_blocks > 0)
 173   {
 174     // launch (num_macro_block - num_pending) encoding
 175     MB_p264_t* MB_P6 = (MB_p264_t*)macroblock->data;
 176
 177     // set qp
 178     CHECK_P264_IOCTL(ioctl(h264_fd, P264_SET_QP, &qp))
 179
 180     // set output data addr
 181     p264_p6_output_buf_t output_buf;
 182     output_buf.phys_output = (uint32_t)dma_virt2phy(MB_P6);
 183     output_buf.reg_output =  h264_raw_result;
 184     CHECK_P264_IOCTL(ioctl(h264_fd, P264_SET_OUTPUT_BUF, &output_buf))
 185
 186     // launch encoding
 187     CHECK_P264_IOCTL(ioctl(h264_fd, P264_ENCODE_NEXT_MB, &num_macro_blocks))
 188
 189     return C_OK;
 190   }
 191   else
 192     return C_FAIL;
 193 }
 194
 195 // get encoded num_macro_blocks MB
 196 int32_t video_p264_get_encoded_MB(uint32_t num_macro_blocks, video_macroblock_t* macroblock)
 197 {
 198   // retrieve and process available MBs
 199   if (num_macro_blocks > 0)
 200   {
 201     // wait hardware completion
 202     CHECK_P264_IOCTL(ioctl(h264_fd, P264_WAIT_ENCODE, &num_macro_blocks))
 203
 204     if (num_macro_blocks != 0)
 205     {
 206       // at this point num_macro_block h264_raw_result is filled with me_result and MV
 207       uint32_t i;
 208       for (i=0;i<num_macro_blocks;i++)
 209       {
 210         MB_p264_t* MB_P6 = (MB_p264_t*)(macroblock[i].data);
 211
 212         if (I_encoding == FALSE)
 213         {
 214           // save MB partition and MV
 215           if (INTER_16_8_PARTITION(h264_raw_result[i].me_result) == 0)
 216           {
 217             // 16x16 partition
 218             // save partition mode
 219             macroblock[i].inter_partition_mode[0] = INTER_PART_16x16;
 220             // save corresponding MV
 221             P6_get_MV(&(macroblock[i].inter_MV[0]),0,h264_raw_result[i].pred_result);
 222             macroblock[i].nb_partition = 1;
 223           }
 224           else if (INTER_16_8_PARTITION(h264_raw_result[i].me_result) == 1)
 225           {
 226             // 16x8 partition
 227             // save partition mode
 228             macroblock[i].inter_partition_mode[0] = INTER_PART_16x8;
 229             macroblock[i].inter_partition_mode[1] = INTER_PART_16x8;
 230             // save corresponding MV
 231             P6_get_MV(&macroblock[i].inter_MV[0],0,h264_raw_result[i].pred_result);
 232             P6_get_MV(&macroblock[i].inter_MV[1],8,h264_raw_result[i].pred_result);
 233             macroblock[i].nb_partition = 2;
 234           }
 235           else if (INTER_16_8_PARTITION(h264_raw_result[i].me_result) == 2)
 236           {
 237             // 16x8 partition
 238             // save partition mode
 239             macroblock[i].inter_partition_mode[0] = INTER_PART_8x16;
 240             macroblock[i].inter_partition_mode[1] = INTER_PART_8x16;
 241             // save corresponding MV
 242             P6_get_MV(&macroblock[i].inter_MV[0],0,h264_raw_result[i].pred_result);
 243             P6_get_MV(&macroblock[i].inter_MV[1],4,h264_raw_result[i].pred_result);
 244             macroblock[i].nb_partition = 2;
 245           }
 246           // 8x8 8x4 4x8 4x4 cases missing
 247           else
 248             PRINT ("wrong partition (or not supported) result %d\n",INTER_16_8_PARTITION(h264_raw_result[i].me_result));
 249
 250           // patch DC chroma coeff
 251           // P6 h264 IP performs a zigzag on DC coeff. It should not.
 252           MB_P6->inter.DC_U[3] = MB_P6->inter.dummy_DC_U[0];
 253           MB_P6->inter.DC_V[3] = MB_P6->inter.dummy_DC_V[0];
 254
 255     #ifdef H264_P6_PFRAME_DEBUG
 256           uint32_t i;
 257           for (i=0;i<prev_macroblock->nb_partition;i++)
 258           {
 259             PRINT ("partition[%d]=%d - MV (%d,%d)\n",i,macroblock[i].inter_partition_mode[i],macroblock[i].inter_MV[i].x,macroblock[i].inter_MV[i].y);
 260           }
 261     #endif
 262         }
 263         else
 264         {
 265     #ifdef H264_P6_DEBUG
 266           PRINT ("me result 0x%x\n",h264_raw_result[i].me_result);
 267           PRINT ("--> Y intra mode %d\n",(h264_raw_result[i].me_result&0x1F));
 268           PRINT ("--> Chroma intra mode %d\n",CHROMA_MODE(h264_raw_result[i].me_result));
 269     #endif
 270
 271           if (IS_INTRA_4x4(h264_raw_result[i].me_result))
 272           {
 273             // patch DC chroma coeff
 274             // P6 h264 IP performs a zigzag on DC coeff. It should not.
 275             MB_P6->intra_4x4.DC_U[3] = MB_P6->intra_4x4.dummy_DC_U[0];
 276             MB_P6->intra_4x4.DC_V[3] = MB_P6->intra_4x4.dummy_DC_V[0];
 277             // set macroblock type
 278             macroblock[i].intra_type = INTRA_4x4;
 279             // save 4x4 intra luma result
 280             intra_pred_4x4_p6_to_list (h264_raw_result[i].intra_pred_4x4_0, h264_raw_result[i].intra_pred_4x4_1, macroblock[i].intra_4x4_mode);
 281     #ifdef H264_P6_DEBUG
 282             PRINT ("intra 4x4 pred result 0x%x\n",h264_raw_result[i].intra_pred_4x4_0);
 283             PRINT ("intra 4x4 pred result 0x%x\n",h264_raw_result[i].intra_pred_4x4_1);
 284     #endif
 285           }
 286           else
 287           {
 288             // patch DC chroma coeff
 289             // P6 h264 IP performs a zigzag on DC coeff. It should not.
 290             MB_P6->intra_16x16.DC_U[3] = MB_P6->intra_16x16.dummy_DC_U[0];
 291             MB_P6->intra_16x16.DC_V[3] = MB_P6->intra_16x16.dummy_DC_V[0];
 292             // set macroblock type
 293             macroblock[i].intra_type = INTRA_16x16;
 294             // save 16x16 luma result
 295             macroblock[i].intra_luma_16x16_mode = INTRA_16x16_MODE(h264_raw_result[i].me_result);
 296     #ifdef H264_P6_DEBUG
 297             printf("luma 16x16 pred %d\n",INTRA_16x16_MODE(h264_raw_result[i].me_result));
 298     #endif
 299
 300             // zagzig DC luma (P6 bug fix)
 301             int16_t tmp_zagzig[16];
 302             zagzig_4x4 (MB_P6->intra_16x16.DC_Y, tmp_zagzig);
 303             vp_os_memcpy(MB_P6->intra_16x16.DC_Y,tmp_zagzig,16*sizeof(int16_t));
 304           }
 305
 306           // save chroma mode
 307           macroblock[i].intra_chroma_8x8_mode = CHROMA_MODE(h264_raw_result[i].me_result);
 308         }
 309       }
 310     }
 311   }
 312   return num_macro_blocks;
 313 }
 314 #endif
 315
 316
 317 C_RESULT video_p264_p6_close(void)
 318 {
 319   if (h264_ref_frame != NULL)
 320     dma_free (h264_ref_frame);
 321   h264_ref_frame = NULL;
 322   if (h264_deb_frame != NULL)
 323     dma_free (h264_deb_frame);
 324
 325   if (h264_fd)
 326     close(h264_fd);
 327
 328   h264_deb_frame = NULL;
 329   current_Y  = NULL;
 330   current_Cb = NULL;
 331   current_Cr = NULL;
 332   current_width = 0;
 333   current_height = 0;
 334   return C_OK;
 335 }