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