Pico/misc.s

   1
   2 @ vim:filetype=armasm
   3
   4
   5 .global memcpy16 @ unsigned short *dest, unsigned short *src, int count
   6
   7 memcpy16:
   8     eor     r3, r0, r1
   9     tst     r3, #2
  10     bne     mcp16_cant_align
  11
  12     tst     r0, #2
  13     ldrneh  r3, [r1], #2
  14     subne   r2, r2, #1
  15     strneh  r3, [r0], #2
  16
  17     subs    r2, r2, #4
  18     bmi     mcp16_fin
  19
  20 mcp16_loop:
  21     ldmia   r1!, {r3,r12}
  22     subs    r2, r2, #4
  23     stmia   r0!, {r3,r12}
  24     bpl     mcp16_loop
  25
  26 mcp16_fin:
  27     tst     r2, #2
  28     ldrne   r3, [r1], #4
  29     strne   r3, [r0], #4
  30     ands    r2, r2, #1
  31     bxeq    lr
  32
  33 mcp16_cant_align:
  34     ldrh    r3, [r1], #2
  35     subs    r2, r2, #1
  36     strh    r3, [r0], #2
  37     bne     mcp16_cant_align
  38
  39     bx      lr
  40
  41
  42
  43 @ 0x12345678 -> 0x34127856
  44 @ r4=temp, lr=0x00ff00ff
  45 .macro bswap reg
  46     and     r4,   \reg, lr
  47     and     \reg, lr,   \reg, lsr #8
  48     orr     \reg, \reg, r4,   lsl #8
  49 .endm
  50
  51
  52 @ dest must be halfword aligned, src can be unaligned
  53 .global memcpy16bswap @ unsigned short *dest, void *src, int count
  54
  55 memcpy16bswap:
  56     tst     r1, #1
  57     bne     mcp16bs_cant_align2
  58
  59     eor     r3, r0, r1
  60     tst     r3, #2
  61     bne     mcp16bs_cant_align
  62
  63     tst     r0, #2
  64     beq     mcp16bs_aligned
  65     ldrh    r3, [r1], #2
  66     sub     r2, r2, #1
  67     orr     r3, r3, r3, lsl #16
  68     mov     r3, r3, lsr #8
  69     strh    r3, [r0], #2
  70
  71 mcp16bs_aligned:
  72     stmfd   sp!, {r4,lr}
  73     mov     lr, #0xff
  74     orr     lr, lr, lr, lsl #16
  75
  76     subs    r2, r2, #4
  77     bmi     mcp16bs_fin4
  78
  79 mcp16bs_loop:
  80     ldmia   r1!, {r3,r12}
  81     subs    r2, r2, #4
  82     bswap   r3
  83     bswap   r12
  84     stmia   r0!, {r3,r12}
  85     bpl     mcp16bs_loop
  86
  87 mcp16bs_fin4:
  88     tst     r2, #2
  89     beq     mcp16bs_fin2
  90     ldr     r3, [r1], #4
  91     bswap   r3
  92     str     r3, [r0], #4
  93
  94 mcp16bs_fin2:
  95     ldmfd   sp!, {r4,lr}
  96     ands    r2, r2, #1
  97     bxeq    lr
  98
  99 mcp16bs_cant_align:
 100     ldrh    r3, [r1], #2
 101     subs    r2, r2, #1
 102     orr     r3, r3, r3, lsl #16
 103     mov     r3, r3, lsr #8
 104     strh    r3, [r0], #2
 105     bne     mcp16bs_cant_align
 106     bx      lr
 107
 108     @ worst case
 109 mcp16bs_cant_align2:
 110     ldrb    r3, [r1], #1
 111     ldrb    r12,[r1], #1
 112     subs    r2, r2, #1
 113     mov     r3, r3, lsl #8
 114     orr     r3, r3, r12
 115     strh    r3, [r0], #2
 116     bne     mcp16bs_cant_align2
 117     bx      lr
 118
 119
 120
 121 .global memcpy32 @ int *dest, int *src, int count
 122
 123 memcpy32:
 124     stmfd   sp!, {r4,lr}
 125
 126     subs    r2, r2, #4
 127     bmi     mcp32_fin
 128
 129 mcp32_loop:
 130     ldmia   r1!, {r3,r4,r12,lr}
 131     subs    r2, r2, #4
 132     stmia   r0!, {r3,r4,r12,lr}
 133     bpl     mcp32_loop
 134
 135 mcp32_fin:
 136     tst     r2, #3
 137     ldmeqfd sp!, {r4,pc}
 138     tst     r2, #1
 139     ldrne   r3, [r1], #4
 140     strne   r3, [r0], #4
 141
 142 mcp32_no_unal1:
 143     tst     r2, #2
 144     ldmneia r1!, {r3,r12}
 145     ldmfd   sp!, {r4,lr}
 146     stmneia r0!, {r3,r12}
 147     bx      lr
 148
 149
 150
 151 .global memset32 @ int *dest, int c, int count
 152
 153 memset32:
 154     stmfd   sp!, {lr}
 155
 156     mov     r3, r1
 157     subs    r2, r2, #4
 158     bmi     mst32_fin
 159
 160     mov     r12,r1
 161     mov     lr, r1
 162
 163 mst32_loop:
 164     subs    r2, r2, #4
 165     stmia   r0!, {r1,r3,r12,lr}
 166     bpl     mst32_loop
 167
 168 mst32_fin:
 169     tst     r2, #1
 170     strne   r1, [r0], #4
 171
 172     tst     r2, #2
 173     stmneia r0!, {r1,r3}
 174
 175     ldmfd   sp!, {lr}
 176     bx      lr
 177
 178
 179