z80/ym2612 reset handling improved
[picodrive.git] / Pico / Misc.s
... / ...
CommitLineData
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
9memcpy16:
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
22mcp16_loop:
23 ldmia r1!, {r3,r12}
24 subs r2, r2, #4
25 stmia r0!, {r3,r12}
26 bpl mcp16_loop
27
28mcp16_fin:
29 tst r2, #2
30 ldrne r3, [r1], #4
31 strne r3, [r0], #4
32 ands r2, r2, #1
33 bxeq lr
34
35mcp16_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
57memcpy16bswap:
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
73mcp16bs_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
81mcp16bs_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
89mcp16bs_fin4:
90 tst r2, #2
91 beq mcp16bs_fin2
92 ldr r3, [r1], #4
93 bswap r3
94 str r3, [r0], #4
95
96mcp16bs_fin2:
97 ldmfd sp!, {r4,lr}
98 ands r2, r2, #1
99 bxeq lr
100
101mcp16bs_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
111mcp16bs_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
125memcpy32:
126 stmfd sp!, {r4,lr}
127
128 subs r2, r2, #4
129 bmi mcp32_fin
130
131mcp32_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
137mcp32_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
144mcp32_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
155memset32:
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
165mst32_loop:
166 subs r2, r2, #4
167 stmia r0!, {r1,r3,r12,lr}
168 bpl mst32_loop
169
170mst32_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