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