vcs.maemo.org Git - mardrone/blob - mardrone/ARDrone_SDK_Version_1_8_20110726/ARDroneLib/VLIB/Platform/arm9_P6/video_packetizer_p6.S

   1 #include "video_utils_p6.h"
   2
   3 #ifdef HAS_VIDEO_WRITE_DATA
   4
   5         .text
   6         .align  4
   7
   8
   9         .global video_write_data
  10         .type   video_write_data, %function
  11
  12 /* Registers usage for video_write_data (r0 & r3 must be preserved)
  13        r0 = stream
  14        r1 = code
  15        r2 = length
  16        r3 = video_stream->code
  17        r4 = video_stream->used
  18        r5 = video_stream->bytes
  19        ip = video_stream->length
  20        lr = video_stream->index
  21 */
  22
  23 video_write_data:
  24         ldr     ip, [r0]
  25         stmdb   sp!, {r3, r4, r5, lr}         /* save regiter on the stack */
  26         cmp     r2, ip                        /* length > stream->length ? */
  27         ldmib   r0, {r3, r4, r5, lr}
  28
  29         ble     video_write_data_1
  30         add     r4, r4, #4                    /* stream->used += 4 */
  31         str     r4, [r0, #8]                  /* write back stream->used */
  32         mov     r3, r3, lsl ip                /* stream->code <<= stream->length */
  33         mov     r4, #1
  34         sub     r2, r2, ip                    /* length -= stream->length */
  35         rsb     r4, r4, r4, lsl r2            /* code   &= (1 << length) - 1 (rd = shifter operand - rn) */
  36         orr     r3, r3, r1, lsr r2            /* stream->code  |= code >> (length - stream->length) */
  37         and     r1, r1, r4                    /* code   &= (1 << length) - 1 */
  38         str     r3, [r5, lr, lsl #2]          /* stream->bytes[stream->index] = stream->code */
  39         add     lr, lr, #1                    /* stream->index++ */
  40         str     lr, [r0, #16]                 /* write back stream->index */
  41         mov     r3, #0                        /* stream->code = 0 */
  42         mov     ip, #32                       /* stream->length   = 32 */
  43
  44 video_write_data_1:
  45         cmp     r2, #0
  46         orrne   r3, r1, r3, lsl r2            /* stream->code = (stream->code << length) | code */
  47         rsbne   r2, r2, ip                    /* stream->length -= length */
  48         stmia   r0, {r2, r3}
  49         ldmia   sp!, {r3, r4, r5, pc}
  50
  51 #endif // HAS_VIDEO_WRITE_DATA