vcs.maemo.org Git - mardrone/blob - mardrone/ARDrone_SDK_Version_1_8_20110726/ARDroneLib/VLIB/Platform/arm9/video_gen_sad.c

   1 #include <RAMCODE/ramcode.h>
   2 #include "video_gen_sad.h"
   3
   4
   5 sad_fn gen_sad( uint32_t* buffer, uint32_t buffer_length,
   6                 uint32_t bytes_per_entry_ref, uint32_t bytes_per_entry_src,
   7                 uint16_t offset_ref, uint16_t offset_src,
   8                 uint32_t square_length )
   9 {
  10
  11   /* Register usage in generated code
  12       lr/r14 : sad accumulation
  13       r0 : ref - r2 : data read with r0
  14       r1 : src - r3 : data read with r1
  15       ip : counter
  16   */
  17
  18   uint32_t *b;
  19   int8_t *buffer_save, *buffer_current;
  20   sad_fn sad = (sad_fn) NULL;
  21
  22   uint32_t prologue[]   = {
  23                             0xe52de004, // str lr, [sp, #-4]!
  24                             0xe3a0c000, // mov ip, 0x0
  25                             0xe3a0e000  // mov lr, 0x0
  26                           };
  27
  28   uint32_t epilogue[]   = {
  29                             0xe1a0000e, // mov r0, lr
  30                             0xe49df004  // ldr pc, [sp], #4
  31                           };
  32
  33   uint32_t sad_acc[]    = {
  34                             0xe0732002, // rsbs r2, r3, r2
  35                             0xb2622000, // rsblt r2, r2, #0
  36                             0xe08ee002  // add lr, lr, r2
  37                           };
  38
  39   uint32_t dec_count[]  = {
  40                             0xe25cc001  // subs ip, ip, #1
  41                           };
  42
  43   offset_ref += ( 1 - square_length );
  44   offset_src += ( 1 - square_length );
  45
  46   if( offset_ref < 1024 && offset_src < 1024 )
  47   {
  48     sad = (sad_fn) buffer;
  49
  50     b = &buffer[0];
  51     buffer = emit_buffer( buffer, prologue, sizeof(prologue) );
  52     b[1] |= square_length;
  53
  54     switch( bytes_per_entry_ref )
  55     {
  56       case MEM_FMT_8:
  57         buffer = emit_load8_pi( COND_FIELD_ALWAYS, buffer, ARM_REG_0, ARM_REG_2, 1 );
  58         break;
  59
  60       case MEM_FMT_16:
  61         buffer = emit_load16_pi( COND_FIELD_ALWAYS, buffer, ARM_REG_0, ARM_REG_2, 1 );
  62         break;
  63
  64       case MEM_FMT_32:
  65         buffer = emit_load32_pi( COND_FIELD_ALWAYS, buffer, ARM_REG_0, ARM_REG_2, 1 );
  66         break;
  67     }
  68
  69     switch( bytes_per_entry_src )
  70     {
  71       case MEM_FMT_8:
  72         buffer = emit_load8_pi( COND_FIELD_ALWAYS, buffer, ARM_REG_1, ARM_REG_3, 1 );
  73         break;
  74
  75       case MEM_FMT_16:
  76         buffer = emit_load16_pi( COND_FIELD_ALWAYS, buffer, ARM_REG_1, ARM_REG_3, 1 );
  77         break;
  78
  79       case MEM_FMT_32:
  80         buffer = emit_load32_pi( COND_FIELD_ALWAYS, buffer, ARM_REG_1, ARM_REG_3, 1 );
  81         break;
  82     }
  83
  84     buffer_save = (int8_t*)&buffer[0];
  85
  86     square_length --;
  87
  88     while( square_length > 0 )
  89     {
  90       emit_buffer( buffer, sad_acc, sizeof(sad_acc) );
  91
  92       switch( bytes_per_entry_ref )
  93       {
  94         case MEM_FMT_8:
  95           buffer = emit_load8_pi( COND_FIELD_ALWAYS, buffer, ARM_REG_0, ARM_REG_2, 1 );
  96           break;
  97
  98         case MEM_FMT_16:
  99           buffer = emit_load16_pi( COND_FIELD_ALWAYS, buffer, ARM_REG_0, ARM_REG_2, 1 );
 100           break;
 101
 102         case MEM_FMT_32:
 103           buffer = emit_load32_pi( COND_FIELD_ALWAYS, buffer, ARM_REG_0, ARM_REG_2, 1 );
 104           break;
 105       }
 106
 107       switch( bytes_per_entry_src )
 108       {
 109         case MEM_FMT_8:
 110           buffer = emit_load8_pi( COND_FIELD_ALWAYS, buffer, ARM_REG_1, ARM_REG_3, 1 );
 111           break;
 112
 113         case MEM_FMT_16:
 114           buffer = emit_load16_pi( COND_FIELD_ALWAYS, buffer, ARM_REG_1, ARM_REG_3, 1 );
 115           break;
 116
 117         case MEM_FMT_32:
 118           buffer = emit_load32_pi( COND_FIELD_ALWAYS, buffer, ARM_REG_1, ARM_REG_3, 1 );
 119           break;
 120       }
 121
 122       square_length --;
 123     }
 124
 125     buffer = emit_buffer( buffer, sad_acc, sizeof(sad_acc) );
 126     buffer = emit_buffer( buffer, dec_count, sizeof(dec_count) );
 127
 128     switch( bytes_per_entry_ref )
 129     {
 130       case MEM_FMT_8:
 131         buffer = emit_load8_pi( COND_FIELD_NE, buffer, ARM_REG_0, ARM_REG_2, offset_ref );
 132         break;
 133
 134       case MEM_FMT_16:
 135         buffer = emit_load16_pi( COND_FIELD_NE, buffer, ARM_REG_0, ARM_REG_2, offset_ref );
 136         break;
 137
 138       case MEM_FMT_32:
 139         buffer = emit_load32_pi( COND_FIELD_NE, buffer, ARM_REG_0, ARM_REG_2, offset_ref );
 140         break;
 141     }
 142
 143     switch( bytes_per_entry_src )
 144     {
 145       case MEM_FMT_8:
 146         buffer = emit_load8_pi( COND_FIELD_NE, buffer, ARM_REG_1, ARM_REG_3, offset_src );
 147         break;
 148
 149       case MEM_FMT_16:
 150         buffer = emit_load16_pi( COND_FIELD_NE, buffer, ARM_REG_1, ARM_REG_3, offset_src );
 151         break;
 152
 153       case MEM_FMT_32:
 154         buffer = emit_load32_pi( COND_FIELD_NE, buffer, ARM_REG_1, ARM_REG_3, offset_src );
 155         break;
 156     }
 157
 158     buffer_current = (int8_t*)&buffer[0];
 159     buffer = emit_branch( COND_FIELD_NE, buffer, buffer_save - buffer_current - 8 );
 160
 161     buffer = emit_buffer( buffer, epilogue, sizeof(epilogue) );
 162
 163     arm_mmu_flush_dcache();
 164   }
 165
 166   return sad;
 167 }