diff options
Diffstat (limited to 'firmware/target/coldfire/mpio/ata-as-mpio.S')
-rw-r--r-- | firmware/target/coldfire/mpio/ata-as-mpio.S | 757 |
1 files changed, 757 insertions, 0 deletions
diff --git a/firmware/target/coldfire/mpio/ata-as-mpio.S b/firmware/target/coldfire/mpio/ata-as-mpio.S new file mode 100644 index 0000000000..4a6bd3e613 --- /dev/null +++ b/firmware/target/coldfire/mpio/ata-as-mpio.S | |||
@@ -0,0 +1,757 @@ | |||
1 | /*************************************************************************** | ||
2 | * __________ __ ___. | ||
3 | * Open \______ \ ____ ____ | | _\_ |__ _______ ___ | ||
4 | * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ / | ||
5 | * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < < | ||
6 | * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \ | ||
7 | * \/ \/ \/ \/ \/ | ||
8 | * $Id: ata-as-coldfire.S 17847 2008-06-28 18:10:04Z bagder $ | ||
9 | * | ||
10 | * Copyright (C) 2006 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 | ****************************************************************************/ | ||
21 | |||
22 | .section .icode,"ax",@progbits | ||
23 | |||
24 | .equ .ata_port, 0x20000020 | ||
25 | .equ .swapmask, 0x00FF00FF | ||
26 | .align 2 | ||
27 | .global copy_read_sectors | ||
28 | .type copy_read_sectors,@function | ||
29 | |||
30 | /* Read a number of words from the ATA data port | ||
31 | * | ||
32 | * Utilises line bursts, assumes there is at least one full line to copy. | ||
33 | * | ||
34 | * Arguments: | ||
35 | * (4,%sp) - buffer address | ||
36 | * (8,%sp) - word count | ||
37 | * | ||
38 | * Register usage: | ||
39 | * %a0 - current address | ||
40 | * %a1 - end address | ||
41 | * %a2 - ata port | ||
42 | * %d0 - scratch | ||
43 | * %d1 - shift count | ||
44 | * %d2-%d6 - read buffers | ||
45 | * | ||
46 | * %d7 - byte swap scrach register | ||
47 | * %a3 - byte swap mask | ||
48 | */ | ||
49 | |||
50 | copy_read_sectors: | ||
51 | lea.l (-32, %sp), %sp | ||
52 | movem.l %d2-%d7/%a2-%a3, (%sp) | ||
53 | movem.l (36, %sp), %a0-%a1 | ||
54 | add.l %a1, %a1 | ||
55 | add.l %a0, %a1 | ||
56 | lea.l .ata_port, %a2 | ||
57 | lea.l .swapmask, %a3 | ||
58 | |||
59 | move.l %a0, %d0 | ||
60 | btst.l #0, %d0 /* 16-bit aligned? */ | ||
61 | jeq .r_aligned /* yes, do word copy */ | ||
62 | |||
63 | /* not 16-bit aligned */ | ||
64 | subq.l #1, %a1 /* last byte is done unconditionally */ | ||
65 | moveq.l #24, %d1 /* preload shift count */ | ||
66 | |||
67 | move.w (%a2), %d2 /* load initial word */ | ||
68 | move.b %d2, (%a0)+ /* write high byte of it, aligns dest addr */ | ||
69 | /* we have byte swapped */ | ||
70 | |||
71 | btst.l #1, %d0 /* longword aligned? (testing old d0 value!) */ | ||
72 | bne.b .r_end_u_w1 /* yes, skip leading word handling */ | ||
73 | |||
74 | swap %d2 /* move initial word up */ | ||
75 | move.w (%a2), %d2 /* combine with second word */ | ||
76 | |||
77 | /* byte swap d2 */ | ||
78 | move.l %a3, %d7 /* d7 = 0x00FF00FF */ | ||
79 | and.l %d2, %d7 /* d7 = .B.D */ | ||
80 | eor.l %d7, %d2 /* d2 = A.C. */ | ||
81 | lsl.l #8, %d7 /* d7 = B.D. */ | ||
82 | lsr.l #8, %d2 /* d2 = .A.C */ | ||
83 | or.l %d7, %d2 /* d2 = BADC */ | ||
84 | |||
85 | move.l %d2, %d3 | ||
86 | lsr.l #8, %d3 | ||
87 | move.w %d3, (%a0)+ /* write bytes 2 and 3 as word */ | ||
88 | |||
89 | .r_end_u_w1: | ||
90 | moveq.l #12, %d0 | ||
91 | add.l %a0, %d0 | ||
92 | and.l #0xFFFFFFF0,%d0 /* d0 == first line bound */ | ||
93 | cmp.l %a0, %d0 /* any leading longwords? */ | ||
94 | bls.b .r_end_u_l1 /* no: skip loop */ | ||
95 | |||
96 | .r_loop_u_l1: | ||
97 | move.w (%a2), %d3 /* load first word */ | ||
98 | swap %d3 /* move to upper 16 bit */ | ||
99 | move.w (%a2), %d3 /* load second word */ | ||
100 | |||
101 | /* byte swap d3 */ | ||
102 | move.l %a3, %d7 /* d7 = 0x00FF00FF */ | ||
103 | and.l %d3, %d7 /* d7 = .B.D */ | ||
104 | eor.l %d7, %d3 /* d3 = A.C. */ | ||
105 | lsl.l #8, %d7 /* d7 = B.D. */ | ||
106 | lsr.l #8, %d3 /* d3 = .A.C */ | ||
107 | or.l %d7, %d3 /* d3 = BADC */ | ||
108 | |||
109 | move.l %d3, %d4 | ||
110 | lsl.l %d1, %d2 | ||
111 | lsr.l #8, %d3 | ||
112 | or.l %d3, %d2 /* combine old low byte with new top 3 bytes */ | ||
113 | move.l %d2, (%a0)+ /* store as long */ | ||
114 | move.l %d4, %d2 | ||
115 | cmp.l %a0, %d0 /* run up to first line bound */ | ||
116 | bhi.b .r_loop_u_l1 | ||
117 | |||
118 | .r_end_u_l1: | ||
119 | lea.l (-14, %a1), %a1 /* adjust end addr. to 16 bytes/pass */ | ||
120 | |||
121 | .r_loop_u_line: | ||
122 | move.w (%a2), %d3 /* load 1st word */ | ||
123 | swap %d3 /* move to upper 16 bit */ | ||
124 | move.w (%a2), %d3 /* load 2nd word */ | ||
125 | |||
126 | /* byte swap d3 */ | ||
127 | move.l %a3, %d7 /* d7 = 0x00FF00FF */ | ||
128 | and.l %d3, %d7 /* d7 = .B.D */ | ||
129 | eor.l %d7, %d3 /* d3 = A.C. */ | ||
130 | lsl.l #8, %d7 /* d7 = B.D. */ | ||
131 | lsr.l #8, %d3 /* d3 = .A.C */ | ||
132 | or.l %d7, %d3 /* d3 = BADC */ | ||
133 | |||
134 | move.l %d3, %d0 | ||
135 | lsl.l %d1, %d2 | ||
136 | lsr.l #8, %d0 | ||
137 | or.l %d0, %d2 /* combine old low byte with new top 3 bytes */ | ||
138 | move.w (%a2), %d4 /* load 3rd word */ | ||
139 | swap %d4 /* move to upper 16 bit */ | ||
140 | move.w (%a2), %d4 /* load 4th word */ | ||
141 | |||
142 | /* byte swap d4 */ | ||
143 | move.l %a3, %d7 /* d7 = 0x00FF00FF */ | ||
144 | and.l %d4, %d7 /* d7 = .B.D */ | ||
145 | eor.l %d7, %d4 /* d4 = A.C. */ | ||
146 | lsl.l #8, %d7 /* d7 = B.D. */ | ||
147 | lsr.l #8, %d4 /* d4 = .A.C */ | ||
148 | or.l %d7, %d4 /* d4 = BADC */ | ||
149 | |||
150 | move.l %d4, %d0 | ||
151 | lsl.l %d1, %d3 | ||
152 | lsr.l #8, %d0 | ||
153 | or.l %d0, %d3 /* combine old low byte with new top 3 bytes */ | ||
154 | move.w (%a2), %d5 /* load 5th word */ | ||
155 | swap %d5 /* move to upper 16 bit */ | ||
156 | move.w (%a2), %d5 /* load 6th word */ | ||
157 | |||
158 | /* byte swap d5 */ | ||
159 | move.l %a3, %d7 /* d7 = 0x00FF00FF */ | ||
160 | and.l %d5, %d7 /* d7 = .B.D */ | ||
161 | eor.l %d7, %d5 /* d5 = A.C. */ | ||
162 | lsl.l #8, %d7 /* d7 = B.D. */ | ||
163 | lsr.l #8, %d5 /* d5 = .A.C */ | ||
164 | or.l %d7, %d5 /* d5 = BADC */ | ||
165 | |||
166 | move.l %d5, %d0 | ||
167 | lsl.l %d1, %d4 | ||
168 | lsr.l #8, %d0 | ||
169 | or.l %d0, %d4 /* combine old low byte with new top 3 bytes */ | ||
170 | move.w (%a2), %d6 /* load 7th word */ | ||
171 | swap %d6 /* move to upper 16 bit */ | ||
172 | move.w (%a2), %d6 /* load 8th word */ | ||
173 | |||
174 | /* byte swap d6 */ | ||
175 | move.l %a3, %d7 /* d7 = 0x00FF00FF */ | ||
176 | and.l %d6, %d7 /* d7 = .B.D */ | ||
177 | eor.l %d7, %d6 /* d6 = A.C. */ | ||
178 | lsl.l #8, %d7 /* d7 = B.D. */ | ||
179 | lsr.l #8, %d6 /* d6 = .A.C */ | ||
180 | or.l %d7, %d6 /* d6 = BADC */ | ||
181 | |||
182 | move.l %d6, %d0 | ||
183 | lsl.l %d1, %d5 | ||
184 | lsr.l #8, %d0 | ||
185 | or.l %d0, %d5 /* combine old low byte with new top 3 bytes */ | ||
186 | movem.l %d2-%d5, (%a0) /* store line */ | ||
187 | lea.l (16, %a0), %a0 | ||
188 | move.l %d6, %d2 | ||
189 | cmp.l %a0, %a1 /* run up to last line bound */ | ||
190 | bhi.b .r_loop_u_line | ||
191 | |||
192 | lea.l (12, %a1), %a1 /* readjust for longword loop */ | ||
193 | cmp.l %a0, %a1 /* any trailing longwords? */ | ||
194 | bls.b .r_end_u_l2 /* no: skip loop */ | ||
195 | |||
196 | .r_loop_u_l2: | ||
197 | move.w (%a2), %d3 /* load first word */ | ||
198 | swap %d3 /* move to upper 16 bit */ | ||
199 | move.w (%a2), %d3 /* load second word */ | ||
200 | |||
201 | /* byte swap d3 */ | ||
202 | move.l %a3, %d7 /* d7 = 0x00FF00FF */ | ||
203 | and.l %d3, %d7 /* d7 = .B.D */ | ||
204 | eor.l %d7, %d3 /* d3 = A.C. */ | ||
205 | lsl.l #8, %d7 /* d7 = B.D. */ | ||
206 | lsr.l #8, %d3 /* d3 = .A.C */ | ||
207 | or.l %d7, %d3 /* d3 = BADC */ | ||
208 | |||
209 | move.l %d3, %d4 | ||
210 | lsl.l %d1, %d2 | ||
211 | lsr.l #8, %d3 | ||
212 | or.l %d3, %d2 /* combine old low byte with new top 3 bytes */ | ||
213 | move.l %d2, (%a0)+ /* store as long */ | ||
214 | move.l %d4, %d2 | ||
215 | cmp.l %a0, %a1 /* run up to last long bound */ | ||
216 | bhi.b .r_loop_u_l2 | ||
217 | |||
218 | .r_end_u_l2: | ||
219 | addq.l #2, %a1 /* back to final end address */ | ||
220 | cmp.l %a0, %a1 /* one word left? */ | ||
221 | bls.b .r_end_u_w2 | ||
222 | |||
223 | swap %d2 /* move old word to upper 16 bits */ | ||
224 | move.w (%a2), %d2 /* load final word */ | ||
225 | |||
226 | /* byte swap d2 */ | ||
227 | move.l %a3, %d7 /* d7 = 0x00FF00FF */ | ||
228 | and.l %d2, %d7 /* d7 = .B.D */ | ||
229 | eor.l %d7, %d2 /* d2 = A.C. */ | ||
230 | lsl.l #8, %d7 /* d7 = B.D. */ | ||
231 | lsr.l #8, %d2 /* d2 = .A.C */ | ||
232 | or.l %d7, %d2 /* d2 = BADC */ | ||
233 | |||
234 | move.l %d2, %d3 | ||
235 | lsr.l #8, %d3 | ||
236 | move.w %d3, (%a0)+ /* write bytes 2 and 3 as word */ | ||
237 | |||
238 | .r_end_u_w2: | ||
239 | move.b %d2, (%a0)+ /* store final byte */ | ||
240 | bra.w .r_exit | ||
241 | |||
242 | /* 16-bit aligned */ | ||
243 | .r_aligned: | ||
244 | btst.l #1, %d0 /* longword aligned? */ | ||
245 | beq.b .r_end_a_w1 /* yes, skip leading word handling */ | ||
246 | |||
247 | /* copy initial word */ | ||
248 | /* initial word has to be swapped */ | ||
249 | move.w (%a2), %d7 | ||
250 | move.b %d7, (%a0)+ | ||
251 | lsr.l #8, %d7 | ||
252 | move.b %d7, (%a0)+ | ||
253 | |||
254 | .r_end_a_w1: | ||
255 | moveq.l #12, %d0 | ||
256 | add.l %a0, %d0 | ||
257 | and.l #0xFFFFFFF0,%d0 /* d0 == first line bound */ | ||
258 | cmp.l %a0, %d0 /* any leading longwords? */ | ||
259 | bls.b .r_end_a_l1 /* no: skip loop */ | ||
260 | |||
261 | .r_loop_a_l1: | ||
262 | move.w (%a2), %d1 /* load first word */ | ||
263 | swap %d1 /* move it to upper 16 bits */ | ||
264 | move.w (%a2), %d1 /* load second word */ | ||
265 | |||
266 | /* byte swap d1 */ | ||
267 | move.l %a3, %d7 /* d7 = 0x00FF00FF */ | ||
268 | and.l %d1, %d7 /* d7 = .B.D */ | ||
269 | eor.l %d7, %d1 /* d1 = A.C. */ | ||
270 | lsl.l #8, %d7 /* d7 = B.D. */ | ||
271 | lsr.l #8, %d1 /* d1 = .A.C */ | ||
272 | or.l %d7, %d1 /* d1 = BADC */ | ||
273 | |||
274 | move.l %d1, (%a0)+ /* store as long */ | ||
275 | cmp.l %a0, %d0 /* run up to first line bound */ | ||
276 | bhi.b .r_loop_a_l1 | ||
277 | |||
278 | .r_end_a_l1: | ||
279 | lea.l (-14, %a1), %a1 /* adjust end addr. to 16 bytes/pass */ | ||
280 | |||
281 | .r_loop_a_line: | ||
282 | move.w (%a2), %d0 /* load 1st word */ | ||
283 | swap %d0 /* move it to upper 16 bits */ | ||
284 | move.w (%a2), %d0 /* load 2nd word */ | ||
285 | |||
286 | /* byte swap d0 */ | ||
287 | move.l %a3, %d7 /* d7 = 0x00FF00FF */ | ||
288 | and.l %d0, %d7 /* d7 = .B.D */ | ||
289 | eor.l %d7, %d0 /* d0 = A.C. */ | ||
290 | lsl.l #8, %d7 /* d7 = B.D. */ | ||
291 | lsr.l #8, %d0 /* d0 = .A.C */ | ||
292 | or.l %d7, %d0 /* d0 = BADC */ | ||
293 | |||
294 | move.w (%a2), %d1 /* load 3rd word */ | ||
295 | swap %d1 /* move it to upper 16 bits */ | ||
296 | move.w (%a2), %d1 /* load 4th word */ | ||
297 | |||
298 | /* byte swap d1 */ | ||
299 | move.l %a3, %d7 /* d7 = 0x00FF00FF */ | ||
300 | and.l %d1, %d7 /* d7 = .B.D */ | ||
301 | eor.l %d7, %d1 /* d1 = A.C. */ | ||
302 | lsl.l #8, %d7 /* d7 = B.D. */ | ||
303 | lsr.l #8, %d1 /* d1 = .A.C */ | ||
304 | or.l %d7, %d1 /* d1 = BADC */ | ||
305 | |||
306 | move.w (%a2), %d2 /* load 5th word */ | ||
307 | swap %d2 /* move it to upper 16 bits */ | ||
308 | move.w (%a2), %d2 /* load 6th word */ | ||
309 | |||
310 | /* byte swap d2 */ | ||
311 | move.l %a3, %d7 /* d7 = 0x00FF00FF */ | ||
312 | and.l %d2, %d7 /* d7 = .B.D */ | ||
313 | eor.l %d7, %d2 /* d2 = A.C. */ | ||
314 | lsl.l #8, %d7 /* d7 = B.D. */ | ||
315 | lsr.l #8, %d2 /* d2 = .A.C */ | ||
316 | or.l %d7, %d2 /* d2 = BADC */ | ||
317 | |||
318 | move.w (%a2), %d3 /* load 7th word */ | ||
319 | swap %d3 /* move it to upper 16 bits */ | ||
320 | move.w (%a2), %d3 /* load 8th word */ | ||
321 | |||
322 | /* byte swap d3 */ | ||
323 | move.l %a3, %d7 /* d7 = 0x00FF00FF */ | ||
324 | and.l %d3, %d7 /* d7 = .B.D */ | ||
325 | eor.l %d7, %d3 /* d3 = A.C. */ | ||
326 | lsl.l #8, %d7 /* d7 = B.D. */ | ||
327 | lsr.l #8, %d3 /* d3 = .A.C */ | ||
328 | or.l %d7, %d3 /* d3 = BADC */ | ||
329 | |||
330 | movem.l %d0-%d3, (%a0) /* store line */ | ||
331 | lea.l (16, %a0), %a0 | ||
332 | cmp.l %a0, %a1 /* run up to last line bound */ | ||
333 | bhi.b .r_loop_a_line | ||
334 | |||
335 | lea.l (12, %a1), %a1 /* readjust for longword loop */ | ||
336 | cmp.l %a0, %a1 /* any trailing longwords? */ | ||
337 | bls.b .r_end_a_l2 /* no: skip loop */ | ||
338 | |||
339 | .r_loop_a_l2: | ||
340 | move.w (%a2), %d1 /* read first word */ | ||
341 | swap %d1 /* move it to upper 16 bits */ | ||
342 | move.w (%a2), %d1 /* read second word */ | ||
343 | |||
344 | /* byte swap d1 */ | ||
345 | move.l %a3, %d7 /* d7 = 0x00FF00FF */ | ||
346 | and.l %d1, %d7 /* d7 = .B.D */ | ||
347 | eor.l %d7, %d1 /* d1 = A.C. */ | ||
348 | lsl.l #8, %d7 /* d7 = B.D. */ | ||
349 | lsr.l #8, %d1 /* d1 = .A.C */ | ||
350 | or.l %d7, %d1 /* d1 = BADC */ | ||
351 | |||
352 | move.l %d1, (%a0)+ /* store as long */ | ||
353 | cmp.l %a0, %a1 /* run up to last long bound */ | ||
354 | bhi.b .r_loop_a_l2 | ||
355 | |||
356 | .r_end_a_l2: | ||
357 | addq.l #2, %a1 /* back to final end address */ | ||
358 | cmp.l %a0, %a1 /* one word left? */ | ||
359 | bls.b .r_end_a_w2 | ||
360 | |||
361 | /* copy final word */ | ||
362 | /* final word has to be swapped */ | ||
363 | move.w (%a2), %d7 | ||
364 | move.b %d7, (%a0)+ | ||
365 | lsr.l #8, %d7 | ||
366 | move.b %d7, (%a0)+ | ||
367 | |||
368 | .r_end_a_w2: | ||
369 | |||
370 | .r_exit: | ||
371 | movem.l (%sp), %d2-%d7/%a2-%a3 | ||
372 | lea.l (32, %sp), %sp | ||
373 | rts | ||
374 | |||
375 | .r_end: | ||
376 | .size copy_read_sectors,.r_end-copy_read_sectors | ||
377 | |||
378 | .align 2 | ||
379 | .global copy_write_sectors | ||
380 | .type copy_write_sectors,@function | ||
381 | |||
382 | #if 0 | ||
383 | /* Write a number of words to the ATA data port | ||
384 | * | ||
385 | * Utilises line bursts, assumes there is at least one full line to copy. | ||
386 | * | ||
387 | * Arguments: | ||
388 | * (4,%sp) - buffer address | ||
389 | * (8,%sp) - word count | ||
390 | * | ||
391 | * Register usage: | ||
392 | * %a0 - current address | ||
393 | * %a1 - end address | ||
394 | * %a2 - ata port | ||
395 | * %d0 - scratch | ||
396 | * %d1 - shift count | ||
397 | * %d2-%d6 - read buffers | ||
398 | * | ||
399 | * %d7 - swap scrach | ||
400 | * %a3 - swap mask | ||
401 | */ | ||
402 | |||
403 | copy_write_sectors: | ||
404 | lea.l (-32, %sp), %sp | ||
405 | movem.l %d2-%d7/%a2-%a3, (%sp) | ||
406 | movem.l (36, %sp), %a0-%a1 | ||
407 | add.l %a1, %a1 | ||
408 | add.l %a0, %a1 | ||
409 | lea.l .ata_port, %a2 | ||
410 | lea.l .swapmask, %a3 | ||
411 | |||
412 | move.l %a0, %d0 | ||
413 | btst.l #0, %d0 /* 16-bit aligned? */ | ||
414 | beq .w_aligned /* yes, do word copy */ | ||
415 | |||
416 | /* not 16-bit aligned */ | ||
417 | subq.l #1, %a1 /* last byte is done unconditionally */ | ||
418 | moveq.l #24, %d1 /* preload shift count */ | ||
419 | |||
420 | move.b (%a0)+, %d2 | ||
421 | |||
422 | btst.l #1, %d0 /* longword aligned? (testing old d0 value!) */ | ||
423 | bne.b .w_end_u_w1 /* yes, skip leading word handling */ | ||
424 | |||
425 | swap %d2 | ||
426 | move.w (%a0)+, %d2 | ||
427 | move.l %d2, %d3 | ||
428 | lsr.l #8, %d3 | ||
429 | |||
430 | /* low word of %d3 has to be byte swaped */ | ||
431 | move.l %a3, %d7 /* d7 = 0x00FF00FF */ | ||
432 | and.l %d3, %d7 /* d7 = .B.D */ | ||
433 | eor.l %d7, %d3 /* d3 = A.C. */ | ||
434 | lsl.l #8, %d7 /* d7 = B.D. */ | ||
435 | lsr.l #8, %d3 /* d3 = .A.C */ | ||
436 | or.l %d7, %d3 /* d3 = BADC */ | ||
437 | |||
438 | move.w %d3, (%a2) | ||
439 | |||
440 | .w_end_u_w1: | ||
441 | moveq.l #12, %d0 | ||
442 | add.l %a0, %d0 | ||
443 | and.l #0xFFFFFFF0,%d0 /* d0 == first line bound */ | ||
444 | cmp.l %a0, %d0 /* any leading longwords? */ | ||
445 | bls.b .w_end_u_l1 /* no: skip loop */ | ||
446 | |||
447 | .w_loop_u_l1: | ||
448 | move.l (%a0)+, %d3 | ||
449 | move.l %d3, %d4 | ||
450 | lsl.l %d1, %d2 | ||
451 | lsr.l #8, %d3 | ||
452 | or.l %d3, %d2 | ||
453 | |||
454 | /* byte swap d2 */ | ||
455 | move.l %a3, %d7 /* d7 = 0x00FF00FF */ | ||
456 | and.l %d2, %d7 /* d7 = .B.D */ | ||
457 | eor.l %d7, %d2 /* d2 = A.C. */ | ||
458 | lsl.l #8, %d7 /* d7 = B.D. */ | ||
459 | lsr.l #8, %d2 /* d2 = .A.C */ | ||
460 | or.l %d7, %d2 /* d2 = BADC */ | ||
461 | |||
462 | swap %d2 | ||
463 | move.w %d2, (%a2) | ||
464 | swap %d2 | ||
465 | move.w %d2, (%a2) | ||
466 | move.l %d4, %d2 | ||
467 | cmp.l %a0, %d0 /* run up to first line bound */ | ||
468 | bhi.b .w_loop_u_l1 | ||
469 | |||
470 | .w_end_u_l1: | ||
471 | lea.l (-14, %a1), %a1 /* adjust end addr. to 16 bytes/pass */ | ||
472 | |||
473 | .w_loop_u_line: | ||
474 | movem.l (%a0), %d3-%d6 | ||
475 | lea.l (16, %a0), %a0 | ||
476 | move.l %d3, %d0 | ||
477 | lsl.l %d1, %d2 | ||
478 | lsr.l #8, %d0 | ||
479 | or.l %d0, %d2 | ||
480 | |||
481 | /* byte swap d2 */ | ||
482 | move.l %a3, %d7 /* d7 = 0x00FF00FF */ | ||
483 | and.l %d2, %d7 /* d7 = .B.D */ | ||
484 | eor.l %d7, %d2 /* d2 = A.C. */ | ||
485 | lsl.l #8, %d7 /* d7 = B.D. */ | ||
486 | lsr.l #8, %d2 /* d2 = .A.C */ | ||
487 | or.l %d7, %d2 /* d2 = BADC */ | ||
488 | |||
489 | swap %d2 | ||
490 | move.w %d2, (%a2) | ||
491 | swap %d2 | ||
492 | move.w %d2, (%a2) | ||
493 | move.l %d4, %d0 | ||
494 | lsl.l %d1, %d3 | ||
495 | lsr.l #8, %d0 | ||
496 | or.l %d0, %d3 | ||
497 | |||
498 | /* byte swap d3 */ | ||
499 | move.l %a3, %d7 /* d7 = 0x00FF00FF */ | ||
500 | and.l %d3, %d7 /* d7 = .B.D */ | ||
501 | eor.l %d7, %d3 /* d3 = A.C. */ | ||
502 | lsl.l #8, %d7 /* d7 = B.D. */ | ||
503 | lsr.l #8, %d3 /* d3 = .A.C */ | ||
504 | or.l %d7, %d3 /* d3 = BADC */ | ||
505 | |||
506 | swap %d3 | ||
507 | move.w %d3, (%a2) | ||
508 | swap %d3 | ||
509 | move.w %d3, (%a2) | ||
510 | move.l %d5, %d0 | ||
511 | lsl.l %d1, %d4 | ||
512 | lsr.l #8, %d0 | ||
513 | or.l %d0, %d4 | ||
514 | |||
515 | /* byte swap d4 */ | ||
516 | move.l %a3, %d7 /* d7 = 0x00FF00FF */ | ||
517 | and.l %d4, %d7 /* d7 = .B.D */ | ||
518 | eor.l %d7, %d4 /* d4 = A.C. */ | ||
519 | lsl.l #8, %d7 /* d7 = B.D. */ | ||
520 | lsr.l #8, %d4 /* d4 = .A.C */ | ||
521 | or.l %d7, %d4 /* d4 = BADC */ | ||
522 | |||
523 | swap %d4 | ||
524 | move.w %d4, (%a2) | ||
525 | swap %d4 | ||
526 | move.w %d4, (%a2) | ||
527 | move.l %d6, %d0 | ||
528 | lsl.l %d1, %d5 | ||
529 | lsr.l #8, %d0 | ||
530 | or.l %d0, %d5 | ||
531 | |||
532 | /* byte swap d5 */ | ||
533 | move.l %a3, %d7 /* d7 = 0x00FF00FF */ | ||
534 | and.l %d5, %d7 /* d7 = .B.D */ | ||
535 | eor.l %d7, %d5 /* d5 = A.C. */ | ||
536 | lsl.l #8, %d7 /* d7 = B.D. */ | ||
537 | lsr.l #8, %d5 /* d5 = .A.C */ | ||
538 | or.l %d7, %d5 /* d5 = BADC */ | ||
539 | |||
540 | swap %d5 | ||
541 | move.w %d5, (%a2) | ||
542 | swap %d5 | ||
543 | move.w %d5, (%a2) | ||
544 | move.l %d6, %d2 | ||
545 | cmp.l %a0, %a1 /* run up to last line bound */ | ||
546 | bhi.b .w_loop_u_line | ||
547 | |||
548 | lea.l (12, %a1), %a1 /* readjust for longword loop */ | ||
549 | cmp.l %a0, %a1 /* any trailing longwords? */ | ||
550 | bls.b .w_end_u_l2 /* no: skip loop */ | ||
551 | |||
552 | .w_loop_u_l2: | ||
553 | move.l (%a0)+, %d3 | ||
554 | move.l %d3, %d4 | ||
555 | lsl.l %d1, %d2 | ||
556 | lsr.l #8, %d3 | ||
557 | or.l %d3, %d2 | ||
558 | |||
559 | /* byte swap d2 */ | ||
560 | move.l %a3, %d7 /* d7 = 0x00FF00FF */ | ||
561 | and.l %d2, %d7 /* d7 = .B.D */ | ||
562 | eor.l %d7, %d2 /* d2 = A.C. */ | ||
563 | lsl.l #8, %d7 /* d7 = B.D. */ | ||
564 | lsr.l #8, %d2 /* d2 = .A.C */ | ||
565 | or.l %d7, %d2 /* d2 = BADC */ | ||
566 | |||
567 | swap %d2 | ||
568 | move.w %d2, (%a2) | ||
569 | swap %d2 | ||
570 | move.w %d2, (%a2) | ||
571 | move.l %d4, %d2 | ||
572 | cmp.l %a0, %a1 /* run up to first line bound */ | ||
573 | bhi.b .w_loop_u_l2 | ||
574 | |||
575 | .w_end_u_l2: | ||
576 | addq.l #2, %a1 /* back to final end address */ | ||
577 | cmp.l %a0, %a1 /* one word left? */ | ||
578 | bls.b .w_end_u_w2 | ||
579 | |||
580 | swap %d2 | ||
581 | move.w (%a0)+, %d2 | ||
582 | move.l %d2, %d3 | ||
583 | lsr.l #8, %d3 | ||
584 | |||
585 | /* byte swap d3 */ | ||
586 | move.l %a3, %d7 /* d7 = 0x00FF00FF */ | ||
587 | and.l %d3, %d7 /* d7 = .B.D */ | ||
588 | eor.l %d7, %d3 /* d3 = A.C. */ | ||
589 | lsl.l #8, %d7 /* d7 = B.D. */ | ||
590 | lsr.l #8, %d3 /* d3 = .A.C */ | ||
591 | or.l %d7, %d3 /* d3 = BADC */ | ||
592 | |||
593 | move.w %d3, (%a2) | ||
594 | |||
595 | .w_end_u_w2: | ||
596 | lsl.l #8, %d2 | ||
597 | move.b (%a0)+, %d2 | ||
598 | |||
599 | /* byte swap d2 */ | ||
600 | move.l %a3, %d7 /* d7 = 0x00FF00FF */ | ||
601 | and.l %d2, %d7 /* d7 = .B.D */ | ||
602 | eor.l %d7, %d2 /* d2 = A.C. */ | ||
603 | lsl.l #8, %d7 /* d7 = B.D. */ | ||
604 | lsr.l #8, %d2 /* d2 = .A.C */ | ||
605 | or.l %d7, %d2 /* d2 = BADC */ | ||
606 | |||
607 | move.w %d2, (%a2) | ||
608 | bra.w .w_exit | ||
609 | |||
610 | /* 16-bit aligned */ | ||
611 | .w_aligned: | ||
612 | btst.l #1, %d0 | ||
613 | beq.b .w_end_a_w1 | ||
614 | |||
615 | /* this has to be byte swaped */ | ||
616 | /* copy initial word */ | ||
617 | move.w (%a0)+, %d1 | ||
618 | |||
619 | /* byte swap d1 */ | ||
620 | move.l %a3, %d7 /* d7 = $00FF00FF */ | ||
621 | and.l %d1, %d7 /* d7 = .B.D */ | ||
622 | eor.l %d7, %d1 /* d1 = A.C. */ | ||
623 | lsl.l #8, %d7 /* d7 = B.D. */ | ||
624 | lsr.l #8, %d1 /* d1 = .A.C */ | ||
625 | or.l %d7, %d1 /* d1 = BADC */ | ||
626 | |||
627 | move.w %d1, (%a2) | ||
628 | |||
629 | |||
630 | .w_end_a_w1: | ||
631 | moveq.l #12, %d0 | ||
632 | add.l %a0, %d0 | ||
633 | and.l #0xFFFFFFF0,%d0 /* d0 == first line bound */ | ||
634 | cmp.l %a0, %d0 /* any leading longwords? */ | ||
635 | bls.b .w_end_a_l1 /* no: skip loop */ | ||
636 | |||
637 | .w_loop_a_l1: | ||
638 | move.l (%a0)+, %d1 | ||
639 | |||
640 | /* byte swap d1 */ | ||
641 | move.l %a3, %d7 /* d7 = 0x00FF00FF */ | ||
642 | and.l %d1, %d7 /* d7 = .B.D */ | ||
643 | eor.l %d7, %d1 /* d1 = A.C. */ | ||
644 | lsl.l #8, %d7 /* d7 = B.D. */ | ||
645 | lsr.l #8, %d1 /* d1 = .A.C */ | ||
646 | or.l %d7, %d1 /* d1 = BADC */ | ||
647 | |||
648 | swap %d1 | ||
649 | move.w %d1, (%a2) | ||
650 | swap %d1 | ||
651 | move.w %d1, (%a2) | ||
652 | cmp.l %a0, %d0 /* run up to first line bound */ | ||
653 | bhi.b .w_loop_a_l1 | ||
654 | |||
655 | .w_end_a_l1: | ||
656 | lea.l (-14, %a1), %a1 /* adjust end addr. to 16 bytes/pass */ | ||
657 | |||
658 | .w_loop_a_line: | ||
659 | movem.l (%a0), %d0-%d3 | ||
660 | |||
661 | /* byte swap d0-d3 */ | ||
662 | move.l %a3, %d7 /* d7 = 0x00FF00FF */ | ||
663 | and.l %d0, %d7 /* d7 = .B.D */ | ||
664 | eor.l %d7, %d0 /* d0 = A.C. */ | ||
665 | lsl.l #8, %d7 /* d7 = B.D. */ | ||
666 | lsr.l #8, %d0 /* d0 = .A.C */ | ||
667 | or.l %d7, %d0 /* d0 = BADC */ | ||
668 | |||
669 | move.l %a3, %d7 /* d7 = 0x00FF00FF */ | ||
670 | and.l %d1, %d7 /* d7 = .B.D */ | ||
671 | eor.l %d7, %d1 /* d1 = A.C. */ | ||
672 | lsl.l #8, %d7 /* d7 = B.D. */ | ||
673 | lsr.l #8, %d1 /* d1 = .A.C */ | ||
674 | or.l %d7, %d1 /* d1 = BADC */ | ||
675 | |||
676 | move.l %a3, %d7 /* d7 = 0x00FF00FF */ | ||
677 | and.l %d2, %d7 /* d7 = .B.D */ | ||
678 | eor.l %d7, %d2 /* d2 = A.C. */ | ||
679 | lsl.l #8, %d7 /* d7 = B.D. */ | ||
680 | lsr.l #8, %d2 /* d2 = .A.C */ | ||
681 | or.l %d7, %d2 /* d2 = BADC */ | ||
682 | |||
683 | move.l %a3, %d7 /* d7 = 0x00FF00FF */ | ||
684 | and.l %d3, %d7 /* d7 = .B.D */ | ||
685 | eor.l %d7, %d3 /* d3 = A.C. */ | ||
686 | lsl.l #8, %d7 /* d7 = B.D. */ | ||
687 | lsr.l #8, %d3 /* d3 = .A.C */ | ||
688 | or.l %d7, %d3 /* d3 = BADC */ | ||
689 | |||
690 | lea.l (16, %a0), %a0 | ||
691 | swap %d0 | ||
692 | move.w %d0, (%a2) | ||
693 | swap %d0 | ||
694 | move.w %d0, (%a2) | ||
695 | swap %d1 | ||
696 | move.w %d1, (%a2) | ||
697 | swap %d1 | ||
698 | move.w %d1, (%a2) | ||
699 | swap %d2 | ||
700 | move.w %d2, (%a2) | ||
701 | swap %d2 | ||
702 | move.w %d2, (%a2) | ||
703 | swap %d3 | ||
704 | move.w %d3, (%a2) | ||
705 | swap %d3 | ||
706 | move.w %d3, (%a2) | ||
707 | cmp.l %a0, %a1 /* run up to last line bound */ | ||
708 | bhi.b .w_loop_a_line | ||
709 | |||
710 | lea.l (12, %a1), %a1 /* readjust for longword loop */ | ||
711 | cmp.l %a0, %a1 /* any trailing longwords? */ | ||
712 | bls.b .w_end_a_l2 /* no: skip loop */ | ||
713 | |||
714 | .w_loop_a_l2: | ||
715 | move.l (%a0)+, %d1 | ||
716 | |||
717 | /* byte swap d1 */ | ||
718 | move.l %a3, %d7 /* d7 = 0x00FF00FF */ | ||
719 | and.l %d1, %d7 /* d7 = .B.D */ | ||
720 | eor.l %d7, %d1 /* d1 = A.C. */ | ||
721 | lsl.l #8, %d7 /* d7 = B.D. */ | ||
722 | lsr.l #8, %d1 /* d1 = .A.C */ | ||
723 | or.l %d7, %d1 /* d1 = BADC */ | ||
724 | |||
725 | swap %d1 | ||
726 | move.w %d1, (%a2) | ||
727 | swap %d1 | ||
728 | move.w %d1, (%a2) | ||
729 | cmp.l %a0, %a1 /* run up to first line bound */ | ||
730 | bhi.b .w_loop_a_l2 | ||
731 | |||
732 | .w_end_a_l2: | ||
733 | addq.l #2, %a1 /* back to final end address */ | ||
734 | cmp.l %a0, %a1 /* one word left? */ | ||
735 | bls.b .w_end_a_w2 | ||
736 | |||
737 | /* this has to be byte swaped */ | ||
738 | /* copy final word */ | ||
739 | move.w (%a0)+, %d0 | ||
740 | move.l %a3, %d7 | ||
741 | and.l %d0, %d7 | ||
742 | eor.l %d7, %d0 | ||
743 | lsl.l #8, %d7 | ||
744 | lsr.l #8, %d0 | ||
745 | or.l %d7, %d0 | ||
746 | move.w %d0, (%a2) | ||
747 | |||
748 | .w_end_a_w2: | ||
749 | |||
750 | .w_exit: | ||
751 | movem.l (%sp), %d2-%d7/%a2-%a3 | ||
752 | lea.l (32, %sp), %sp | ||
753 | rts | ||
754 | |||
755 | .w_end: | ||
756 | .size copy_write_sectors,.w_end-copy_write_sectors | ||
757 | #endif | ||