| blob | c1475c0a5a77c8c5835cddbf2427e0ab59db1b51 |
1 /***************************************************************************
2 * __________ __ ___.
3 * Open \______ \ ____ ____ | | _\_ |__ _______ ___
4 * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ /
5 * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < <
6 * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \
7 * \/ \/ \/ \/ \/
8 * $Id$
9 *
10 * Copyright (C) 2007 by Jens Arnold
11 *
12 * This program is free software; you can redistribute it and/or
13 * modify it under the terms of the GNU General Public License
14 * as published by the Free Software Foundation; either version 2
15 * of the License, or (at your option) any later version.
16 *
17 * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
18 * KIND, either express or implied.
19 *
20 ****************************************************************************/
22 #include "config.h"
25 #if CONFIG_CPU == PP5002
26 /* Causes ATA retries on iPod G3 probably related to improper controller
27 * setup. Needs investigation. */
28 .section .icode,"ax",%progbits
29 .equ .ata_port, 0xc00031e0
30 #elif defined CPU_PP502x
31 /* Verified working on (PP5020, PP5022) targets */
32 .section .icode,"ax",%progbits
33 .equ .ata_port, 0xc30001e0
34 #elif CONFIG_CPU == S3C2440
35 /* Untested */
36 .text
37 .equ .ata_port, 0x18000000
38 #elif defined(CREATIVE_ZVx) /* Zen Vision could have an other address */
39 .text
40 .equ .ata_port, 0x50FEE000
41 #endif
43 .align 2
44 .global copy_read_sectors
45 .type copy_read_sectors,%function
47 /* Read a number of words from the ATA data port
48 *
49 * Optimised for speed; assumes wordcount >= 10
50 *
51 * Arguments:
52 * r0 - buffer address
53 * r1 - word count
54 *
55 * Register usage:
56 * r0 - current address
57 * r1 - word count
58 * r2 - ata port
59 * r3..r5, r12, lr - read buffers
60 */
62 copy_read_sectors:
63 stmfd sp!, {r4, r5, lr}
64 ldr r2, =.ata_port
65 tst r0, #1 /* 16 bit aligned? */
66 beq .r_aligned
68 /* not 16-bit aligned */
69 sub r1, r1, #1 /* one halfword is handled unconditionally */
70 ldrh r3, [r2] /* read first halfword */
71 strb r3, [r0], #1 /* store low byte */
72 mov r3, r3, lsr #8
74 tst r0, #2 /* 32 bit aligned? */
75 beq .r_noword_u
76 ldrh r4, [r2] /* read second halfword */
77 orr r3, r3, r4, lsl #8 /* combine with old byte */
78 strh r3, [r0], #2 /* store */
79 mov r3, r4, lsr #8
80 sub r1, r1, #1 /* another halfword taken */
81 .r_noword_u:
83 sub r1, r1, #8 /* adjust for zero-check and doing 8 halfwords/loop */
84 .r_loop_u:
85 ldrh r4, [r2] /* Read 8 halfwords and combine them into */
86 orr r3, r3, r4, lsl #8 /* 4 words so that they're properly aligned */
87 ldrh r4, [r2] /* in memory. Bottom byte of first word is */
88 orr r3, r3, r4, lsl #24 /* the top byte from the last round. Write */
89 mov r4, r4, lsr #8 /* all 4 words at once. */
90 ldrh r5, [r2]
91 orr r4, r4, r5, lsl #8
92 ldrh r5, [r2]
93 orr r4, r4, r5, lsl #24
94 mov r5, r5, lsr #8
95 ldrh r12, [r2]
96 orr r5, r5, r12, lsl #8
97 ldrh r12, [r2]
98 orr r5, r5, r12, lsl #24
99 mov r12, r12, lsr #8
100 ldrh lr, [r2]
101 orr r12, r12, lr, lsl #8
102 ldrh lr, [r2]
103 orr r12, r12, lr, lsl #24
104 stmia r0!, {r3, r4, r5, r12}
105 mov r3, lr, lsr #8
106 subs r1, r1, #8 /* 8 or more halfwords left? */
107 bge .r_loop_u
109 /* No need to adjust the count, only checking bits from now on. */
110 tst r1, #4 /* 4 or more halfwords left? */
111 beq .r_end4_u
112 ldrh r4, [r2]
113 orr r3, r3, r4, lsl #8
114 ldrh r4, [r2]
115 orr r3, r3, r4, lsl #24
116 mov r4, r4, lsr #8
117 ldrh r5, [r2]
118 orr r4, r4, r5, lsl #8
119 ldrh r5, [r2]
120 orr r4, r4, r5, lsl #24
121 stmia r0!, {r3, r4}
122 mov r3, r5, lsr #8
123 .r_end4_u:
125 tst r1, #2 /* 2 or more halfwords left? */
126 beq .r_end2_u
127 ldrh r4, [r2]
128 orr r3, r3, r4, lsl #8
129 ldrh r4, [r2]
130 orr r3, r3, r4, lsl #24
131 str r3, [r0], #4
132 mov r3, r4, lsr #8
133 .r_end2_u:
135 tst r1, #1 /* one halfword left? */
136 ldrneh r4, [r2]
137 orrne r3, r3, r4, lsl #8
138 strneh r3, [r0], #2
139 movne r3, r4, lsr #8
141 strb r3, [r0], #1 /* store final byte */
143 ldmfd sp!, {r4, r5, pc}
145 /* 16-bit aligned */
146 .r_aligned:
147 tst r0, #2 /* 32 bit aligned? */
148 ldrneh r3, [r2] /* no: read first halfword */
149 strneh r3, [r0], #2 /* store */
150 subne r1, r1, #1 /* one halfword taken */
152 sub r1, r1, #8 /* adjust for zero-check and doing 8 halfwords/loop */
153 .r_loop_a:
154 ldrh r3, [r2] /* Read 8 halfwords and combine each pair */
155 ldrh r4, [r2] /* into a word, then store all at once. */
156 orr r3, r3, r4, lsl #16
157 ldrh r4, [r2]
158 ldrh r5, [r2]
159 orr r4, r4, r5, lsl #16
160 ldrh r5, [r2]
161 ldrh r12, [r2]
162 orr r5, r5, r12, lsl #16
163 ldrh r12, [r2]
164 ldrh lr, [r2]
165 orr r12, r12, lr, lsl #16
166 stmia r0!, {r3, r4, r5, r12}
167 subs r1, r1, #8 /* 8 or more halfwords left? */
168 bge .r_loop_a
170 /* No need to adjust the count, only checking bits from now on. */
171 tst r1, #4 /* 4 or more halfwords left? */
172 beq .r_end4_a
173 ldrh r3, [r2]
174 ldrh r4, [r2]
175 orr r3, r3, r4, lsl #16
176 ldrh r4, [r2]
177 ldrh r5, [r2]
178 orr r4, r4, r5, lsl #16
179 stmia r0!, {r3, r4}
180 .r_end4_a:
182 tst r1, #2 /* 2 or more halfwords left? */
183 ldrneh r3, [r2]
184 ldrneh r4, [r2]
185 orrne r3, r3, r4, lsl #16
186 strne r3, [r0], #4
188 tst r1, #1 /* one halfword left? */
189 ldrneh r3, [r2]
190 strneh r3, [r0], #2
192 ldmfd sp!, {r4, r5, pc}
194 .r_end:
195 .size copy_read_sectors,.r_end-copy_read_sectors
197 .align 2
198 .global copy_write_sectors
199 .type copy_write_sectors,%function
201 /* Write a number of words to the ATA data port
202 *
203 * Optimised for speed; assumes wordcount >= 10
204 *
205 * Arguments:
206 * r0 - buffer address
207 * r1 - word count
208 *
209 * Register usage:
210 * r0 - current address
211 * r1 - word count
212 * r2 - ata port
213 * r3..r5, r12, lr - read buffers
214 */
216 copy_write_sectors:
217 stmfd sp!, {r4, r5, lr}
218 ldr r2, =.ata_port
219 tst r0, #1 /* 16 bit aligned? */
220 beq .w_aligned
222 /* not 16-bit aligned */
223 sub r1, r1, #1 /* one halfword is done unconditionally */
224 ldrb r3, [r0], #1 /* load 1st byte, now halfword aligned. */
226 tst r0, #2 /* 32 bit aligned? */
227 beq .w_noword_u
228 ldrh r4, [r0], #2 /* load a halfword */
229 orr r3, r3, r4, lsl #8 /* combine with old byte */
230 strh r3, [r2] /* write halfword */
231 mov r3, r4, lsr #8
232 sub r1, r1, #1 /* another halfword taken */
233 .w_noword_u:
235 sub r1, r1, #8 /* adjust for zero-check and doing 8 halfwords/loop */
236 .w_loop_u:
237 ldmia r0!, {r4, r5, r12, lr}
238 orr r3, r3, r4, lsl #8 /* Load 4 words at once and decompose them */
239 strh r3, [r2] /* into 8 halfwords in a way that the words */
240 mov r3, r3, lsr #16 /* are shifted by 8 bits, putting the high */
241 strh r3, [r2] /* byte of one word into the low byte of */
242 mov r4, r4, lsr #24 /* the next. High byte of last word becomes */
243 orr r4, r4, r5, lsl #8 /* low byte of next round. */
244 strh r4, [r2]
245 mov r4, r4, lsr #16
246 strh r4, [r2]
247 mov r5, r5, lsr #24
248 orr r5, r5, r12, lsl #8
249 strh r5, [r2]
250 mov r5, r5, lsr #16
251 strh r5, [r2]
252 mov r12, r12, lsr #24
253 orr r12, r12, lr, lsl #8
254 strh r12, [r2]
255 mov r12, r12, lsr #16
256 strh r12, [r2]
257 mov r3, lr, lsr #24
258 subs r1, r1, #8 /* 8 or more halfwords left? */
259 bge .w_loop_u
261 /* No need to adjust the count, only checking bits from now on. */
262 tst r1, #4 /* 4 or more halfwords left? */
263 beq .w_end4_u
264 ldmia r0!, {r4, r5}
265 orr r3, r3, r4, lsl #8
266 strh r3, [r2]
267 mov r3, r3, lsr #16
268 strh r3, [r2]
269 mov r4, r4, lsr #24
270 orr r4, r4, r5, lsl #8
271 strh r4, [r2]
272 mov r4, r4, lsr #16
273 strh r4, [r2]
274 mov r3, r5, lsr #24
275 .w_end4_u:
277 tst r1, #2 /* 2 or more halfwords left? */
278 beq .w_end2_u
279 ldr r4, [r0], #4
280 orr r3, r3, r4, lsl #8
281 strh r3, [r2]
282 mov r3, r3, lsr #16
283 strh r3, [r2]
284 mov r3, r4, lsr #24
285 .w_end2_u:
287 tst r1, #1 /* one halfword left? */
288 ldrneh r4, [r0], #2
289 orrne r3, r3, r4, lsl #8
290 strneh r3, [r2]
291 movne r3, r3, lsr #16
293 ldrb r4, [r0], #1 /* load final byte */
294 orr r3, r3, r4, lsl #8
295 strh r3, [r2] /* write final halfword */
297 ldmfd sp!, {r4, r5, pc}
299 /* 16-bit aligned */
300 .w_aligned:
301 tst r0, #2 /* 32 bit aligned? */
302 ldrneh r3, [r0], #2 /* no: load first halfword */
303 strneh r3, [r2] /* write */
304 subne r1, r1, #1 /* one halfword taken */
306 sub r1, r1, #8 /* adjust for zero-check and doing 8 halfwords/loop */
307 .w_loop_a:
308 ldmia r0!, {r3, r4, r5, r12}
309 strh r3, [r2] /* Load 4 words and decompose them into */
310 mov r3, r3, lsr #16 /* 2 halfwords each, and write those. */
311 strh r3, [r2]
312 strh r4, [r2]
313 mov r4, r4, lsr #16
314 strh r4, [r2]
315 strh r5, [r2]
316 mov r5, r5, lsr #16
317 strh r5, [r2]
318 strh r12, [r2]
319 mov r12, r12, lsr #16
320 strh r12, [r2]
321 subs r1, r1, #8 /* 8 or more halfwords left? */
322 bge .w_loop_a
324 /* No need to adjust the count, only checking bits from now on. */
325 tst r1, #4 /* 4 or more halfwords left? */
326 beq .w_end4_a
327 ldmia r0!, {r3, r4}
328 strh r3, [r2]
329 mov r3, r3, lsr #16
330 strh r3, [r2]
331 strh r4, [r2]
332 mov r4, r4, lsr #16
333 strh r4, [r2]
334 .w_end4_a:
336 tst r1, #2 /* 2 or more halfwords left? */
337 ldrne r3, [r0], #4
338 strneh r3, [r2]
339 movne r3, r3, lsr #16
340 strneh r3, [r2]
342 tst r1, #1 /* one halfword left? */
343 ldrneh r3, [r0], #2
344 strneh r3, [r2]
346 ldmfd sp!, {r4, r5, pc}
348 .w_end:
349 .size copy_write_sectors,.w_end-copy_write_sectors