git.maemo.org Git - mardrone/blob - mardrone/ARDrone_SDK_Version_1_8_20110726/ARDroneLib/VP_SDK/Examples/linux/api_serial_MJPEG_sdl.c

   1 #include <stdlib.h>
   2 #include <ctype.h>
   3
   4 #include <VP_Api/vp_api.h>
   5 #include <VP_Api/vp_api_thread_helper.h>
   6 #include <VP_Api/vp_api_error.h>
   7 #include <VP_Stages/vp_stages_configs.h>
   8 #include <VP_Stages/vp_stages_io_console.h>
   9 #include <VP_Stages/vp_stages_o_sdl.h>
  10 #include <VP_Stages/vp_stages_io_com.h>
  11 #include <VP_Stages/vp_stages_io_file.h>
  12 #include <VP_Os/vp_os_print.h>
  13 #include <VP_Os/vp_os_malloc.h>
  14 #include <VP_Os/vp_os_delay.h>
  15
  16 #include <MJPEG/mjpeg.h>
  17 #include <MJPEG/stream.h>
  18
  19 #define NB_STAGES 3
  20
  21 #define ACQ_WIDTH   320
  22 #define ACQ_HEIGHT  240
  23
  24
  25 static PIPELINE_HANDLE pipeline_handle;
  26
  27 ////////////////////////////////////////////////////////////////////////////////
  28 // cpu load information part
  29 //
  30 #define NOMFICH_CPUINFO "/proc/cpuinfo"
  31 double RDTSC(void)
  32 {
  33   unsigned long long x;
  34   __asm__ volatile  (".byte 0x0f, 0x31" : "=A"(x));
  35   return (double)x;
  36 }
  37
  38 int read_cpu_freq (double* freq)
  39 {
  40   const char* prefix_cpu_mhz = "cpu MHz";
  41   FILE* F;
  42   char line[300+1];
  43   char *pos;
  44   int ok=0;
  45
  46   F = fopen(NOMFICH_CPUINFO, "r");
  47   if (!F) return 0;
  48
  49   while (!feof(F))
  50   {
  51     fgets (line, sizeof(line), F);
  52
  53     if (!strncmp(line, prefix_cpu_mhz, strlen(prefix_cpu_mhz)))
  54     {
  55       pos = strrchr (line, ':') +2;
  56       if (!pos) break;
  57       if (pos[strlen(pos)-1] == '\n') pos[strlen(pos)-1] = '\0';
  58       strcpy (line, pos);
  59       strcat (line,"e6");
  60       *freq = atof (line);
  61       ok = 1;
  62       break;
  63     }
  64   }
  65   fclose (F);
  66   return ok;
  67 }
  68 ////////////////////////////////////////////////////////////////////////////////
  69
  70 PROTO_THREAD_ROUTINE(escaper,nomParams);
  71 PROTO_THREAD_ROUTINE(app,nomParams);
  72
  73 BEGIN_THREAD_TABLE
  74   THREAD_TABLE_ENTRY(escaper,20)
  75   THREAD_TABLE_ENTRY(app,10)
  76 END_THREAD_TABLE
  77
  78
  79 typedef struct _mjpeg_stage_decoding_config_t
  80 {
  81   stream_t          stream;
  82   mjpeg_t           mjpeg;
  83   vp_api_picture_t* picture;
  84
  85   uint32_t          out_buffer_size;
  86
  87 } mjpeg_stage_decoding_config_t;
  88
  89 double cpufreq;
  90 double t1,t2, t;
  91 C_RESULT mjpeg_stage_decoding_open(mjpeg_stage_decoding_config_t *cfg)
  92 {
  93   stream_new( &cfg->stream, OUTPUT_STREAM );
  94
  95   read_cpu_freq(&cpufreq);
  96
  97   return mjpeg_init( &cfg->mjpeg, MJPEG_DECODE, cfg->picture->width, cfg->picture->height, cfg->picture->format );
  98 }
  99
 100 C_RESULT mjpeg_stage_decoding_transform(mjpeg_stage_decoding_config_t *cfg, vp_api_io_data_t *in, vp_api_io_data_t *out)
 101 {
 102   uint32_t success;
 103   bool_t got_image;
 104   static FILE* fp=NULL;
 105
 106   vp_os_mutex_lock( &out->lock );
 107
 108   if(out->status == VP_API_STATUS_INIT)
 109   {
 110     out->numBuffers   = 1;
 111     out->buffers      = (int8_t**)cfg->picture;
 112     out->indexBuffer  = 0;
 113     out->lineSize     = 0;
 114
 115     out->status = VP_API_STATUS_PROCESSING;
 116
 117         fp = fopen("tmp.mjpg", "wb");
 118         //mjpeg_init_block_mode( &cfg->mjpeg, 15 );
 119   }
 120
 121   if( in->status == VP_API_STATUS_ENDED )
 122     out->status = in->status;
 123
 124   // Several cases must be handled in this stage
 125   // 1st: Input buffer is too small to decode a complete picture
 126   // 2nd: Input buffer is big enough to decode 1 frame
 127   // 3rd: Input buffer is so big we can decode more than 1 frame
 128
 129   if( in->size > 0 )
 130   {
 131     if( out->status == VP_API_STATUS_PROCESSING )
 132     {
 133           if( NULL != fp )
 134                   fwrite( in->buffers[in->indexBuffer] , 1, in->size, fp );
 135
 136       // Reinit stream with new data
 137       stream_config( &cfg->stream, in->size, in->buffers[in->indexBuffer] );
 138     }
 139
 140     if(out->status == VP_API_STATUS_PROCESSING || out->status == VP_API_STATUS_STILL_RUNNING)
 141     {
 142       // If out->size == 1 it means picture is ready
 143       out->size = 0;
 144       out->status = VP_API_STATUS_PROCESSING;
 145           success = SUCCEED(mjpeg_decode( &cfg->mjpeg, cfg->picture, &cfg->stream , &got_image ));
 146       if( got_image )
 147       {
 148         // we got one picture (handle case 1)
 149         out->size = 1;
 150       }
 151       // handle case 2 & 3
 152       if( FAILED(stream_is_empty( &cfg->stream )) )
 153       {
 154         // Some data are still in stream
 155         // Next time we run this stage we don't want this data to be lost
 156         // So flag it!
 157         out->status = VP_API_STATUS_STILL_RUNNING;
 158       }
 159     }
 160   }
 161
 162   vp_os_mutex_unlock( &out->lock );
 163
 164   return C_OK;
 165 }
 166
 167 C_RESULT mjpeg_stage_decoding_close(mjpeg_stage_decoding_config_t *cfg)
 168 {
 169   stream_delete( &cfg->stream );
 170
 171   return mjpeg_release( &cfg->mjpeg );
 172 }
 173
 174
 175 const vp_api_stage_funcs_t mjpeg_decoding_funcs = {
 176   (vp_api_stage_handle_msg_t) NULL,
 177   (vp_api_stage_open_t) mjpeg_stage_decoding_open,
 178   (vp_api_stage_transform_t) mjpeg_stage_decoding_transform,
 179   (vp_api_stage_close_t) mjpeg_stage_decoding_close
 180 };
 181
 182 int
 183 main(int argc, char **argv)
 184 {
 185   START_THREAD(escaper, NO_PARAM);
 186   START_THREAD(app, argv);
 187
 188   JOIN_THREAD(escaper);
 189   JOIN_THREAD(app);
 190
 191   return EXIT_SUCCESS;
 192 }
 193
 194 PROTO_THREAD_ROUTINE(app,argv)
 195 {
 196   int32_t success, curstage=0;
 197   vp_api_picture_t picture;
 198
 199   vp_api_io_pipeline_t    pipeline;
 200   vp_api_io_data_t        out;
 201   vp_api_io_stage_t       stages[NB_STAGES];
 202
 203   vp_stages_input_com_config_t    icc;
 204   mjpeg_stage_decoding_config_t   dec;
 205   vp_stages_output_sdl_config_t   osc;
 206   vp_com_serial_config_t          config;
 207
 208   vp_com_t                        com;
 209
 210   /// Picture configuration
 211   picture.format        = PIX_FMT_YUV420P;
 212   picture.width         = ACQ_WIDTH;
 213   picture.height        = ACQ_HEIGHT;
 214   picture.framerate     = 30;
 215   picture.y_buf         = vp_os_malloc( ACQ_WIDTH*ACQ_HEIGHT );
 216   picture.cr_buf        = vp_os_malloc( ACQ_WIDTH*ACQ_HEIGHT/4 );
 217   picture.cb_buf        = vp_os_malloc( ACQ_WIDTH*ACQ_HEIGHT/4 );
 218   picture.y_line_size   = ACQ_WIDTH;
 219   picture.cb_line_size  = ACQ_WIDTH / 2;
 220   picture.cr_line_size  = ACQ_WIDTH / 2;
 221   picture.y_pad         = 0;
 222   picture.c_pad         = 0;
 223
 224   dec.picture         = &picture;
 225   dec.out_buffer_size = 4096;
 226
 227   vp_os_memset( &icc,         0, sizeof(vp_stages_input_com_config_t)  );
 228   vp_os_memset( &osc,         0, sizeof(vp_stages_output_sdl_config_t) );
 229   vp_os_memset( &com,         0, sizeof(vp_com_t)                      );
 230
 231   strcpy(config.itfName, "/dev/ttyUSB1");
 232
 233   //
 234   config.initial_baudrate = VP_COM_BAUDRATE_921600;
 235   config.baudrate = VP_COM_BAUDRATE_921600;
 236   //
 237   //config.initial_baudrate = VP_COM_BAUDRATE_460800;
 238   //config.baudrate = VP_COM_BAUDRATE_460800;
 239   //
 240
 241   config.sync = 0;
 242   config.blocking = 1;
 243   com.type = VP_COM_SERIAL;
 244
 245   icc.com                           = &com;
 246   icc.config                        = (vp_com_config_t*)&config;
 247   icc.socket.type                   = VP_COM_CLIENT;
 248   icc.buffer_size                   = 320*240;
 249
 250   //osc.width           = 640;
 251   //osc.height          = 480;
 252   osc.width           = 320;
 253   osc.height          = 240;
 254   osc.bpp             = 16;
 255   //osc.window_width    = 640;
 256   //osc.window_height   = 480;
 257   osc.window_width    = 320;
 258   osc.window_height   = 240;
 259   osc.pic_width       = ACQ_WIDTH;
 260   osc.pic_height      = ACQ_HEIGHT;
 261   osc.y_size          = ACQ_WIDTH*ACQ_HEIGHT;
 262   osc.c_size          = (ACQ_WIDTH*ACQ_HEIGHT) >> 2;
 263
 264   stages[0].type                    = VP_API_INPUT_SOCKET;
 265   stages[0].cfg                     = (void *)&icc;
 266   stages[0].funcs                   = vp_stages_input_com_funcs;
 267
 268   stages[1].type                    = VP_API_FILTER_DECODER;
 269   stages[1].cfg                     = (void*)&dec;
 270   stages[1].funcs                   = mjpeg_decoding_funcs;
 271
 272   stages[2].type                    = VP_API_OUTPUT_SDL;
 273   stages[2].cfg                     = (void *)&osc;
 274   stages[2].funcs                   = vp_stages_output_sdl_funcs;
 275
 276   pipeline.nb_stages                = NB_STAGES;
 277   pipeline.stages                   = &stages[0];
 278
 279   vp_api_open(&pipeline, &pipeline_handle);
 280   out.status = VP_API_STATUS_PROCESSING;
 281   while(SUCCEED(vp_api_run(&pipeline, &out)) && (out.status == VP_API_STATUS_PROCESSING || out.status == VP_API_STATUS_STILL_RUNNING));
 282   /*
 283   do {
 284           t1 = RDTSC();
 285           success = SUCCEED(vp_api_run(&pipeline, &out));
 286           t2 = RDTSC();
 287
 288           t = (t2-t1)*1000 / cpufreq ;
 289           if( t>4 )printf("%d\t%f\n", curstage, t );
 290           if( ++curstage == 3 ) curstage = 0;
 291   } while( success && (out.status == VP_API_STATUS_PROCESSING || out.status == VP_API_STATUS_STILL_RUNNING) );
 292   */
 293
 294   vp_api_close(&pipeline, &pipeline_handle);
 295
 296   return EXIT_SUCCESS;
 297 }
 298
 299 ///*******************************************************************************************************************///
 300
 301
 302 // static THREAD_HANDLE dct_thread_handle;
 303
 304 static dct_io_buffer_t* current_io_buffer;
 305 static dct_io_buffer_t* result_io_buffer;
 306
 307 static void fdct(const unsigned short* in, short* out);
 308 static void idct(const short* in, unsigned short* out);
 309
 310
 311 //-----------------------------------------------------------------------------
 312 // DCT API
 313 //-----------------------------------------------------------------------------
 314
 315
 316 bool_t dct_init(void)
 317 {
 318   current_io_buffer = NULL;
 319   result_io_buffer  = NULL;
 320
 321   return TRUE;
 322 }
 323
 324 bool_t dct_compute( dct_io_buffer_t* io_buffer )
 325 {
 326   bool_t res = FALSE;
 327
 328   assert(io_buffer != NULL);
 329
 330   if( current_io_buffer == NULL && result_io_buffer == NULL )
 331   {
 332     current_io_buffer = io_buffer;
 333     res = TRUE;
 334
 335   }
 336
 337   return res;
 338 }
 339
 340 dct_io_buffer_t* dct_result( void )
 341 {
 342   uint32_t i;
 343   dct_io_buffer_t* io_buffer;
 344
 345   io_buffer = NULL;
 346
 347   if( current_io_buffer != NULL)
 348   {
 349     if( current_io_buffer->dct_mode == DCT_MODE_FDCT )
 350     {
 351       for( i = 0; i < current_io_buffer->num_total_blocks; i++ )
 352       {
 353         fdct(current_io_buffer->input[i], current_io_buffer->output[i]);
 354       }
 355     }
 356     else if( current_io_buffer->dct_mode == DCT_MODE_IDCT )
 357     {
 358       for( i = 0; i < current_io_buffer->num_total_blocks; i++ )
 359       {
 360         idct(current_io_buffer->input[i], current_io_buffer->output[i]);
 361       }
 362     }
 363
 364     io_buffer = current_io_buffer;
 365     current_io_buffer = NULL;
 366   }
 367
 368   return io_buffer;
 369 }
 370
 371
 372 //-----------------------------------------------------------------------------
 373 // DCT Computation
 374 //-----------------------------------------------------------------------------
 375
 376
 377 #define FIX_0_298631336  ((INT32)  2446)        /* FIX(0.298631336) */
 378 #define FIX_0_390180644  ((INT32)  3196)        /* FIX(0.390180644) */
 379 #define FIX_0_541196100  ((INT32)  4433)        /* FIX(0.541196100) */
 380 #define FIX_0_765366865  ((INT32)  6270)        /* FIX(0.765366865) */
 381 #define FIX_0_899976223  ((INT32)  7373)        /* FIX(0.899976223) */
 382 #define FIX_1_175875602  ((INT32)  9633)        /* FIX(1.175875602) */
 383 #define FIX_1_501321110  ((INT32)  12299)       /* FIX(1.501321110) */
 384 #define FIX_1_847759065  ((INT32)  15137)       /* FIX(1.847759065) */
 385 #define FIX_1_961570560  ((INT32)  16069)       /* FIX(1.961570560) */
 386 #define FIX_2_053119869  ((INT32)  16819)       /* FIX(2.053119869) */
 387 #define FIX_2_562915447  ((INT32)  20995)       /* FIX(2.562915447) */
 388 #define FIX_3_072711026  ((INT32)  25172)       /* FIX(3.072711026) */
 389
 390 #define INT32       int
 391 #define DCTELEM     int
 392 #define DCTSIZE     8
 393 #define DCTSIZE2    64
 394 #define CONST_BITS  13
 395 #define PASS1_BITS  1
 396 #define ONE     ((INT32) 1)
 397 #define MULTIPLY(var,const)  ((var) * (const))
 398 #define DESCALE(x,n)  RIGHT_SHIFT((x) + (ONE << ((n)-1)), n)
 399 #define RIGHT_SHIFT(x,shft)     ((x) >> (shft))
 400
 401 static void fdct(const unsigned short* in, short* out)
 402 {
 403   INT32 tmp0, tmp1, tmp2, tmp3, tmp4, tmp5, tmp6, tmp7;
 404   INT32 tmp10, tmp11, tmp12, tmp13;
 405   INT32 z1, z2, z3, z4, z5;
 406   int ctr;
 407   // SHIFT_TEMPS
 408
 409   int data[DCTSIZE * DCTSIZE];
 410   int i, j;
 411   int* dataptr = data;
 412
 413   for( i = 0; i < DCTSIZE; i++ )
 414   {
 415     for( j = 0; j < DCTSIZE; j++ )
 416     {
 417       int temp;
 418
 419       temp = in[i*DCTSIZE + j];
 420       dataptr[i*DCTSIZE + j] = temp;
 421     }
 422   }
 423
 424   /* Pass 1: process rows. */
 425   /* Note results are scaled up by sqrt(8) compared to a true DCT; */
 426   /* furthermore, we scale the results by 2**PASS1_BITS. */
 427
 428   dataptr = data;
 429   for (ctr = DCTSIZE-1; ctr >= 0; ctr--) {
 430     tmp0 = dataptr[0] + dataptr[7];
 431     tmp7 = dataptr[0] - dataptr[7];
 432     tmp1 = dataptr[1] + dataptr[6];
 433     tmp6 = dataptr[1] - dataptr[6];
 434     tmp2 = dataptr[2] + dataptr[5];
 435     tmp5 = dataptr[2] - dataptr[5];
 436     tmp3 = dataptr[3] + dataptr[4];
 437     tmp4 = dataptr[3] - dataptr[4];
 438
 439     /* Even part per LL&M figure 1 --- note that published figure is faulty;
 440      * rotator "sqrt(2)*c1" should be "sqrt(2)*c6".
 441      */
 442
 443     tmp10 = tmp0 + tmp3;
 444     tmp13 = tmp0 - tmp3;
 445     tmp11 = tmp1 + tmp2;
 446     tmp12 = tmp1 - tmp2;
 447
 448     dataptr[0] = (DCTELEM) ((tmp10 + tmp11) << PASS1_BITS);
 449     dataptr[4] = (DCTELEM) ((tmp10 - tmp11) << PASS1_BITS);
 450
 451     z1 = MULTIPLY(tmp12 + tmp13, FIX_0_541196100);
 452     dataptr[2] = (DCTELEM) DESCALE(z1 + MULTIPLY(tmp13, FIX_0_765366865), CONST_BITS-PASS1_BITS);
 453     dataptr[6] = (DCTELEM) DESCALE(z1 + MULTIPLY(tmp12, - FIX_1_847759065), CONST_BITS-PASS1_BITS);
 454
 455     /* Odd part per figure 8 --- note paper omits factor of sqrt(2).
 456      * cK represents cos(K*pi/16).
 457      * i0..i3 in the paper are tmp4..tmp7 here.
 458      */
 459
 460     z1 = tmp4 + tmp7;
 461     z2 = tmp5 + tmp6;
 462     z3 = tmp4 + tmp6;
 463     z4 = tmp5 + tmp7;
 464     z5 = MULTIPLY(z3 + z4, FIX_1_175875602); /* sqrt(2) * c3 */
 465
 466     tmp4 = MULTIPLY(tmp4, FIX_0_298631336); /* sqrt(2) * (-c1+c3+c5-c7) */
 467     tmp5 = MULTIPLY(tmp5, FIX_2_053119869); /* sqrt(2) * ( c1+c3-c5+c7) */
 468     tmp6 = MULTIPLY(tmp6, FIX_3_072711026); /* sqrt(2) * ( c1+c3+c5-c7) */
 469     tmp7 = MULTIPLY(tmp7, FIX_1_501321110); /* sqrt(2) * ( c1+c3-c5-c7) */
 470     z1 = MULTIPLY(z1, - FIX_0_899976223); /* sqrt(2) * (c7-c3) */
 471     z2 = MULTIPLY(z2, - FIX_2_562915447); /* sqrt(2) * (-c1-c3) */
 472     z3 = MULTIPLY(z3, - FIX_1_961570560); /* sqrt(2) * (-c3-c5) */
 473     z4 = MULTIPLY(z4, - FIX_0_390180644); /* sqrt(2) * (c5-c3) */
 474
 475     z3 += z5;
 476     z4 += z5;
 477
 478     dataptr[7] = (DCTELEM) DESCALE(tmp4 + z1 + z3, CONST_BITS-PASS1_BITS);
 479     dataptr[5] = (DCTELEM) DESCALE(tmp5 + z2 + z4, CONST_BITS-PASS1_BITS);
 480     dataptr[3] = (DCTELEM) DESCALE(tmp6 + z2 + z3, CONST_BITS-PASS1_BITS);
 481     dataptr[1] = (DCTELEM) DESCALE(tmp7 + z1 + z4, CONST_BITS-PASS1_BITS);
 482
 483     dataptr += DCTSIZE;         /* advance pointer to next row */
 484   }
 485
 486   /* Pass 2: process columns.
 487    * We remove the PASS1_BITS scaling, but leave the results scaled up
 488    * by an overall factor of 8.
 489    */
 490
 491   dataptr = data;
 492   for (ctr = DCTSIZE-1; ctr >= 0; ctr--) {
 493     tmp0 = dataptr[DCTSIZE*0] + dataptr[DCTSIZE*7];
 494     tmp7 = dataptr[DCTSIZE*0] - dataptr[DCTSIZE*7];
 495     tmp1 = dataptr[DCTSIZE*1] + dataptr[DCTSIZE*6];
 496     tmp6 = dataptr[DCTSIZE*1] - dataptr[DCTSIZE*6];
 497     tmp2 = dataptr[DCTSIZE*2] + dataptr[DCTSIZE*5];
 498     tmp5 = dataptr[DCTSIZE*2] - dataptr[DCTSIZE*5];
 499     tmp3 = dataptr[DCTSIZE*3] + dataptr[DCTSIZE*4];
 500     tmp4 = dataptr[DCTSIZE*3] - dataptr[DCTSIZE*4];
 501
 502     /* Even part per LL&M figure 1 --- note that published figure is faulty;
 503      * rotator "sqrt(2)*c1" should be "sqrt(2)*c6".
 504      */
 505
 506     tmp10 = tmp0 + tmp3;
 507     tmp13 = tmp0 - tmp3;
 508     tmp11 = tmp1 + tmp2;
 509     tmp12 = tmp1 - tmp2;
 510
 511     dataptr[DCTSIZE*0] = (DCTELEM) DESCALE(tmp10 + tmp11, PASS1_BITS);
 512     dataptr[DCTSIZE*4] = (DCTELEM) DESCALE(tmp10 - tmp11, PASS1_BITS);
 513
 514     z1 = MULTIPLY(tmp12 + tmp13, FIX_0_541196100);
 515     dataptr[DCTSIZE*2] = (DCTELEM) DESCALE(z1 + MULTIPLY(tmp13, FIX_0_765366865), CONST_BITS+PASS1_BITS);
 516     dataptr[DCTSIZE*6] = (DCTELEM) DESCALE(z1 + MULTIPLY(tmp12, - FIX_1_847759065), CONST_BITS+PASS1_BITS);
 517
 518     /* Odd part per figure 8 --- note paper omits factor of sqrt(2).
 519      * cK represents cos(K*pi/16).
 520      * i0..i3 in the paper are tmp4..tmp7 here.
 521      */
 522
 523     z1 = tmp4 + tmp7;
 524     z2 = tmp5 + tmp6;
 525     z3 = tmp4 + tmp6;
 526     z4 = tmp5 + tmp7;
 527     z5 = MULTIPLY(z3 + z4, FIX_1_175875602); /* sqrt(2) * c3 */
 528
 529     tmp4 = MULTIPLY(tmp4, FIX_0_298631336); /* sqrt(2) * (-c1+c3+c5-c7) */
 530     tmp5 = MULTIPLY(tmp5, FIX_2_053119869); /* sqrt(2) * ( c1+c3-c5+c7) */
 531     tmp6 = MULTIPLY(tmp6, FIX_3_072711026); /* sqrt(2) * ( c1+c3+c5-c7) */
 532     tmp7 = MULTIPLY(tmp7, FIX_1_501321110); /* sqrt(2) * ( c1+c3-c5-c7) */
 533     z1 = MULTIPLY(z1, - FIX_0_899976223); /* sqrt(2) * (c7-c3) */
 534     z2 = MULTIPLY(z2, - FIX_2_562915447); /* sqrt(2) * (-c1-c3) */
 535     z3 = MULTIPLY(z3, - FIX_1_961570560); /* sqrt(2) * (-c3-c5) */
 536     z4 = MULTIPLY(z4, - FIX_0_390180644); /* sqrt(2) * (c5-c3) */
 537
 538     z3 += z5;
 539     z4 += z5;
 540
 541     dataptr[DCTSIZE*7] = (DCTELEM) DESCALE(tmp4 + z1 + z3, CONST_BITS+PASS1_BITS);
 542     dataptr[DCTSIZE*5] = (DCTELEM) DESCALE(tmp5 + z2 + z4, CONST_BITS+PASS1_BITS);
 543     dataptr[DCTSIZE*3] = (DCTELEM) DESCALE(tmp6 + z2 + z3, CONST_BITS+PASS1_BITS);
 544     dataptr[DCTSIZE*1] = (DCTELEM) DESCALE(tmp7 + z1 + z4, CONST_BITS+PASS1_BITS);
 545
 546     dataptr++;  /* advance pointer to next column */
 547   }
 548
 549   for( i = 0; i < DCTSIZE; i++ )
 550     for( j = 0; j < DCTSIZE; j++ )
 551       out[i*DCTSIZE + j] = data[i*DCTSIZE + j] >> 3;
 552 }
 553
 554 static void idct(const short* in, unsigned short* out)
 555 {
 556   INT32 tmp0, tmp1, tmp2, tmp3;
 557   INT32 tmp10, tmp11, tmp12, tmp13;
 558   INT32 z1, z2, z3, z4, z5;
 559   int* wsptr;
 560   int* outptr;
 561   const short* inptr;
 562   int ctr;
 563   int workspace[DCTSIZE2];      /* buffers data between passes */
 564   int data[DCTSIZE2];
 565   // SHIFT_TEMPS
 566
 567   /* Pass 1: process columns from input, store into work array. */
 568   /* Note results are scaled up by sqrt(8) compared to a true IDCT; */
 569   /* furthermore, we scale the results by 2**PASS1_BITS. */
 570
 571   inptr = in;
 572   wsptr = workspace;
 573   for (ctr = DCTSIZE; ctr > 0; ctr--) {
 574     /* Due to quantization, we will usually find that many of the input
 575      * coefficients are zero, especially the AC terms.  We can exploit this
 576      * by short-circuiting the IDCT calculation for any column in which all
 577      * the AC terms are zero.  In that case each output is equal to the
 578      * DC coefficient (with scale factor as needed).
 579      * With typical images and quantization tables, half or more of the
 580      * column DCT calculations can be simplified this way.
 581      */
 582
 583     if( inptr[DCTSIZE*1] == 0 && inptr[DCTSIZE*2] == 0 &&
 584         inptr[DCTSIZE*3] == 0 && inptr[DCTSIZE*4] == 0 &&
 585         inptr[DCTSIZE*5] == 0 && inptr[DCTSIZE*6] == 0 &&
 586         inptr[DCTSIZE*7] == 0 ) {
 587       /* AC terms all zero */
 588       int dcval = inptr[DCTSIZE*0] << PASS1_BITS;
 589
 590       wsptr[DCTSIZE*0] = dcval;
 591       wsptr[DCTSIZE*1] = dcval;
 592       wsptr[DCTSIZE*2] = dcval;
 593       wsptr[DCTSIZE*3] = dcval;
 594       wsptr[DCTSIZE*4] = dcval;
 595       wsptr[DCTSIZE*5] = dcval;
 596       wsptr[DCTSIZE*6] = dcval;
 597       wsptr[DCTSIZE*7] = dcval;
 598
 599       inptr++;  /* advance pointers to next column */
 600       wsptr++;
 601       continue;
 602     }
 603
 604     /* Even part: reverse the even part of the forward DCT. */
 605     /* The rotator is sqrt(2)*c(-6). */
 606
 607     z2 = inptr[DCTSIZE*2];
 608     z3 = inptr[DCTSIZE*6];
 609
 610     z1 = MULTIPLY(z2 + z3, FIX_0_541196100);
 611     tmp2 = z1 + MULTIPLY(z3, - FIX_1_847759065);
 612     tmp3 = z1 + MULTIPLY(z2, FIX_0_765366865);
 613
 614     z2 = inptr[DCTSIZE*0];
 615     z3 = inptr[DCTSIZE*4];
 616
 617     tmp0 = (z2 + z3) << CONST_BITS;
 618     tmp1 = (z2 - z3) << CONST_BITS;
 619
 620     tmp10 = tmp0 + tmp3;
 621     tmp13 = tmp0 - tmp3;
 622     tmp11 = tmp1 + tmp2;
 623     tmp12 = tmp1 - tmp2;
 624
 625     /* Odd part per figure 8; the matrix is unitary and hence its
 626      * transpose is its inverse.  i0..i3 are y7,y5,y3,y1 respectively.
 627      */
 628
 629     tmp0 = inptr[DCTSIZE*7];
 630     tmp1 = inptr[DCTSIZE*5];
 631     tmp2 = inptr[DCTSIZE*3];
 632     tmp3 = inptr[DCTSIZE*1];
 633
 634     z1 = tmp0 + tmp3;
 635     z2 = tmp1 + tmp2;
 636     z3 = tmp0 + tmp2;
 637     z4 = tmp1 + tmp3;
 638     z5 = MULTIPLY(z3 + z4, FIX_1_175875602); /* sqrt(2) * c3 */
 639
 640     tmp0 = MULTIPLY(tmp0, FIX_0_298631336); /* sqrt(2) * (-c1+c3+c5-c7) */
 641     tmp1 = MULTIPLY(tmp1, FIX_2_053119869); /* sqrt(2) * ( c1+c3-c5+c7) */
 642     tmp2 = MULTIPLY(tmp2, FIX_3_072711026); /* sqrt(2) * ( c1+c3+c5-c7) */
 643     tmp3 = MULTIPLY(tmp3, FIX_1_501321110); /* sqrt(2) * ( c1+c3-c5-c7) */
 644     z1 = MULTIPLY(z1, - FIX_0_899976223); /* sqrt(2) * (c7-c3) */
 645     z2 = MULTIPLY(z2, - FIX_2_562915447); /* sqrt(2) * (-c1-c3) */
 646     z3 = MULTIPLY(z3, - FIX_1_961570560); /* sqrt(2) * (-c3-c5) */
 647     z4 = MULTIPLY(z4, - FIX_0_390180644); /* sqrt(2) * (c5-c3) */
 648
 649     z3 += z5;
 650     z4 += z5;
 651
 652     tmp0 += z1 + z3;
 653     tmp1 += z2 + z4;
 654     tmp2 += z2 + z3;
 655     tmp3 += z1 + z4;
 656
 657     /* Final output stage: inputs are tmp10..tmp13, tmp0..tmp3 */
 658
 659     wsptr[DCTSIZE*0] = (int) DESCALE(tmp10 + tmp3, CONST_BITS-PASS1_BITS);
 660     wsptr[DCTSIZE*7] = (int) DESCALE(tmp10 - tmp3, CONST_BITS-PASS1_BITS);
 661     wsptr[DCTSIZE*1] = (int) DESCALE(tmp11 + tmp2, CONST_BITS-PASS1_BITS);
 662     wsptr[DCTSIZE*6] = (int) DESCALE(tmp11 - tmp2, CONST_BITS-PASS1_BITS);
 663     wsptr[DCTSIZE*2] = (int) DESCALE(tmp12 + tmp1, CONST_BITS-PASS1_BITS);
 664     wsptr[DCTSIZE*5] = (int) DESCALE(tmp12 - tmp1, CONST_BITS-PASS1_BITS);
 665     wsptr[DCTSIZE*3] = (int) DESCALE(tmp13 + tmp0, CONST_BITS-PASS1_BITS);
 666     wsptr[DCTSIZE*4] = (int) DESCALE(tmp13 - tmp0, CONST_BITS-PASS1_BITS);
 667
 668     inptr++;  /* advance pointers to next column */
 669     wsptr++;
 670   }
 671
 672   /* Pass 2: process rows from work array, store into output array. */
 673   /* Note that we must descale the results by a factor of 8 == 2**3, */
 674   /* and also undo the PASS1_BITS scaling. */
 675
 676   wsptr = workspace;
 677   outptr = data;
 678   for (ctr = 0; ctr < DCTSIZE; ctr++) {
 679     /* Even part: reverse the even part of the forward DCT. */
 680     /* The rotator is sqrt(2)*c(-6). */
 681
 682     z2 = (INT32) wsptr[2];
 683     z3 = (INT32) wsptr[6];
 684
 685     z1 = MULTIPLY(z2 + z3, FIX_0_541196100);
 686     tmp2 = z1 + MULTIPLY(z3, - FIX_1_847759065);
 687     tmp3 = z1 + MULTIPLY(z2, FIX_0_765366865);
 688
 689     tmp0 = ((INT32) wsptr[0] + (INT32) wsptr[4]) << CONST_BITS;
 690     tmp1 = ((INT32) wsptr[0] - (INT32) wsptr[4]) << CONST_BITS;
 691
 692     tmp10 = tmp0 + tmp3;
 693     tmp13 = tmp0 - tmp3;
 694     tmp11 = tmp1 + tmp2;
 695     tmp12 = tmp1 - tmp2;
 696
 697     /* Odd part per figure 8; the matrix is unitary and hence its
 698      * transpose is its inverse.  i0..i3 are y7,y5,y3,y1 respectively.
 699      */
 700
 701     tmp0 = (INT32) wsptr[7];
 702     tmp1 = (INT32) wsptr[5];
 703     tmp2 = (INT32) wsptr[3];
 704     tmp3 = (INT32) wsptr[1];
 705
 706     z1 = tmp0 + tmp3;
 707     z2 = tmp1 + tmp2;
 708     z3 = tmp0 + tmp2;
 709     z4 = tmp1 + tmp3;
 710     z5 = MULTIPLY(z3 + z4, FIX_1_175875602); /* sqrt(2) * c3 */
 711
 712     tmp0 = MULTIPLY(tmp0, FIX_0_298631336); /* sqrt(2) * (-c1+c3+c5-c7) */
 713     tmp1 = MULTIPLY(tmp1, FIX_2_053119869); /* sqrt(2) * ( c1+c3-c5+c7) */
 714     tmp2 = MULTIPLY(tmp2, FIX_3_072711026); /* sqrt(2) * ( c1+c3+c5-c7) */
 715     tmp3 = MULTIPLY(tmp3, FIX_1_501321110); /* sqrt(2) * ( c1+c3-c5-c7) */
 716     z1 = MULTIPLY(z1, - FIX_0_899976223); /* sqrt(2) * (c7-c3) */
 717     z2 = MULTIPLY(z2, - FIX_2_562915447); /* sqrt(2) * (-c1-c3) */
 718     z3 = MULTIPLY(z3, - FIX_1_961570560); /* sqrt(2) * (-c3-c5) */
 719     z4 = MULTIPLY(z4, - FIX_0_390180644); /* sqrt(2) * (c5-c3) */
 720
 721     z3 += z5;
 722     z4 += z5;
 723
 724     tmp0 += z1 + z3;
 725     tmp1 += z2 + z4;
 726     tmp2 += z2 + z3;
 727     tmp3 += z1 + z4;
 728
 729     /* Final output stage: inputs are tmp10..tmp13, tmp0..tmp3 */
 730
 731     outptr[0] = (tmp10 + tmp3) >> ( CONST_BITS+PASS1_BITS+3 );
 732     outptr[7] = (tmp10 - tmp3) >> ( CONST_BITS+PASS1_BITS+3 );
 733     outptr[1] = (tmp11 + tmp2) >> ( CONST_BITS+PASS1_BITS+3 );
 734     outptr[6] = (tmp11 - tmp2) >> ( CONST_BITS+PASS1_BITS+3 );
 735     outptr[2] = (tmp12 + tmp1) >> ( CONST_BITS+PASS1_BITS+3 );
 736     outptr[5] = (tmp12 - tmp1) >> ( CONST_BITS+PASS1_BITS+3 );
 737     outptr[3] = (tmp13 + tmp0) >> ( CONST_BITS+PASS1_BITS+3 );
 738     outptr[4] = (tmp13 - tmp0) >> ( CONST_BITS+PASS1_BITS+3 );
 739
 740     wsptr += DCTSIZE; /* advance pointer to next row */
 741     outptr += DCTSIZE;
 742   }
 743
 744   for(ctr = 0; ctr < DCTSIZE2; ctr++)
 745     out[ctr] = data[ctr];
 746 }