diff options
Diffstat (limited to 'firmware/target')
-rw-r--r-- | firmware/target/coldfire/ata-as-coldfire.S | 230 |
1 files changed, 224 insertions, 6 deletions
diff --git a/firmware/target/coldfire/ata-as-coldfire.S b/firmware/target/coldfire/ata-as-coldfire.S index 28993c1769..e7e0ee19c0 100644 --- a/firmware/target/coldfire/ata-as-coldfire.S +++ b/firmware/target/coldfire/ata-as-coldfire.S | |||
@@ -8,6 +8,7 @@ | |||
8 | * $Id$ | 8 | * $Id$ |
9 | * | 9 | * |
10 | * Copyright (C) 2006 by Jens Arnold | 10 | * Copyright (C) 2006 by Jens Arnold |
11 | * Copyright (C) 2010 by Marcin Bukat (byte swaps) | ||
11 | * | 12 | * |
12 | * This program is free software; you can redistribute it and/or | 13 | * This program is free software; you can redistribute it and/or |
13 | * modify it under the terms of the GNU General Public License | 14 | * modify it under the terms of the GNU General Public License |
@@ -19,9 +20,21 @@ | |||
19 | * | 20 | * |
20 | ****************************************************************************/ | 21 | ****************************************************************************/ |
21 | 22 | ||
22 | .section .icode,"ax",@progbits | 23 | #include "config.h" |
24 | |||
25 | .macro SWAP_BYTES dn | ||
26 | move.l %a3, %d7 | d7 = 0x00FF00FF | ||
27 | and.l \dn, %d7 | d7 = .B.D | ||
28 | eor.l %d7, \dn | dn = A.C. | ||
29 | lsl.l #8, %d7 | d7 = B.D. | ||
30 | lsr.l #8, \dn | d3 = .A.C | ||
31 | or.l %d7, \dn | dn = BADC | ||
32 | .endm | ||
23 | 33 | ||
24 | .equ .ata_port, 0x20000020 | 34 | .equ .ata_port, 0x20000020 |
35 | .equ .swapmask, 0x00FF00FF | ||
36 | |||
37 | .section .icode,"ax",@progbits | ||
25 | 38 | ||
26 | .align 2 | 39 | .align 2 |
27 | .global copy_read_sectors | 40 | .global copy_read_sectors |
@@ -42,12 +55,24 @@ | |||
42 | * %d0 - scratch | 55 | * %d0 - scratch |
43 | * %d1 - shift count | 56 | * %d1 - shift count |
44 | * %d2-%d6 - read buffers | 57 | * %d2-%d6 - read buffers |
58 | * | ||
59 | * #ifdef SWAP_WORDS | ||
60 | * %d7 - byte swap scrach | ||
61 | * %a3 - byte swap mask | ||
62 | * #endif | ||
45 | */ | 63 | */ |
46 | 64 | ||
47 | copy_read_sectors: | 65 | copy_read_sectors: |
66 | #ifdef SWAP_WORDS | ||
67 | lea.l (-32, %sp), %sp | ||
68 | movem.l %d2-%d7/%a2-%a3, (%sp) | ||
69 | movem.l (36, %sp), %a0-%a1 | ||
70 | lea.l .swapmask, %a3 | ||
71 | #else | ||
48 | lea.l (-24, %sp), %sp | 72 | lea.l (-24, %sp), %sp |
49 | movem.l %d2-%d6/%a2, (%sp) | 73 | movem.l %d2-%d6/%a2, (%sp) |
50 | movem.l (28, %sp), %a0-%a1 | 74 | movem.l (28, %sp), %a0-%a1 |
75 | #endif | ||
51 | add.l %a1, %a1 | 76 | add.l %a1, %a1 |
52 | add.l %a0, %a1 | 77 | add.l %a0, %a1 |
53 | lea.l .ata_port, %a2 | 78 | lea.l .ata_port, %a2 |
@@ -61,18 +86,30 @@ copy_read_sectors: | |||
61 | moveq.l #24, %d1 /* preload shift count */ | 86 | moveq.l #24, %d1 /* preload shift count */ |
62 | 87 | ||
63 | move.w (%a2), %d2 /* load initial word */ | 88 | move.w (%a2), %d2 /* load initial word */ |
89 | #ifdef SWAP_WORDS | ||
90 | move.b %d2, (%a0)+ | ||
91 | lsr.l #8, %d2 | ||
92 | #else | ||
64 | move.l %d2, %d3 | 93 | move.l %d2, %d3 |
65 | lsr.l #8, %d3 | 94 | lsr.l #8, %d3 |
66 | move.b %d3, (%a0)+ /* write high byte of it, aligns dest addr */ | 95 | move.b %d3, (%a0)+ /* write high byte of it, aligns dest addr */ |
67 | 96 | #endif | |
68 | btst.l #1, %d0 /* longword aligned? (testing old d0 value!) */ | 97 | btst.l #1, %d0 /* longword aligned? (testing old d0 value!) */ |
69 | bne.b .r_end_u_w1 /* yes, skip leading word handling */ | 98 | bne.b .r_end_u_w1 /* yes, skip leading word handling */ |
70 | 99 | #ifdef SWAP_WORDS | |
100 | move.w (%a2), %d3 | ||
101 | lsl.l #8, %d2 | ||
102 | move.b %d3, %d2 | ||
103 | move.w %d2, (%a0)+ /* write bytes 2 and 3 as word */ | ||
104 | move.l %d3, %d2 | ||
105 | lsr.l #8, %d2 | ||
106 | #else | ||
71 | swap %d2 /* move initial word up */ | 107 | swap %d2 /* move initial word up */ |
72 | move.w (%a2), %d2 /* combine with second word */ | 108 | move.w (%a2), %d2 /* combine with second word */ |
73 | move.l %d2, %d3 | 109 | move.l %d2, %d3 |
74 | lsr.l #8, %d3 | 110 | lsr.l #8, %d3 |
75 | move.w %d3, (%a0)+ /* write bytes 2 and 3 as word */ | 111 | move.w %d3, (%a0)+ /* write bytes 2 and 3 as word */ |
112 | #endif | ||
76 | 113 | ||
77 | .r_end_u_w1: | 114 | .r_end_u_w1: |
78 | moveq.l #12, %d0 | 115 | moveq.l #12, %d0 |
@@ -85,6 +122,11 @@ copy_read_sectors: | |||
85 | move.w (%a2), %d3 /* load first word */ | 122 | move.w (%a2), %d3 /* load first word */ |
86 | swap %d3 /* move to upper 16 bit */ | 123 | swap %d3 /* move to upper 16 bit */ |
87 | move.w (%a2), %d3 /* load second word */ | 124 | move.w (%a2), %d3 /* load second word */ |
125 | |||
126 | #ifdef SWAP_WORDS | ||
127 | SWAP_BYTES %d3 | ||
128 | #endif | ||
129 | |||
88 | move.l %d3, %d4 | 130 | move.l %d3, %d4 |
89 | lsl.l %d1, %d2 | 131 | lsl.l %d1, %d2 |
90 | lsr.l #8, %d3 | 132 | lsr.l #8, %d3 |
@@ -101,6 +143,11 @@ copy_read_sectors: | |||
101 | move.w (%a2), %d3 /* load 1st word */ | 143 | move.w (%a2), %d3 /* load 1st word */ |
102 | swap %d3 /* move to upper 16 bit */ | 144 | swap %d3 /* move to upper 16 bit */ |
103 | move.w (%a2), %d3 /* load 2nd word */ | 145 | move.w (%a2), %d3 /* load 2nd word */ |
146 | |||
147 | #ifdef SWAP_WORDS | ||
148 | SWAP_BYTES %d3 | ||
149 | #endif | ||
150 | |||
104 | move.l %d3, %d0 | 151 | move.l %d3, %d0 |
105 | lsl.l %d1, %d2 | 152 | lsl.l %d1, %d2 |
106 | lsr.l #8, %d0 | 153 | lsr.l #8, %d0 |
@@ -108,6 +155,11 @@ copy_read_sectors: | |||
108 | move.w (%a2), %d4 /* load 3rd word */ | 155 | move.w (%a2), %d4 /* load 3rd word */ |
109 | swap %d4 /* move to upper 16 bit */ | 156 | swap %d4 /* move to upper 16 bit */ |
110 | move.w (%a2), %d4 /* load 4th word */ | 157 | move.w (%a2), %d4 /* load 4th word */ |
158 | |||
159 | #ifdef SWAP_WORDS | ||
160 | SWAP_BYTES %d4 | ||
161 | #endif | ||
162 | |||
111 | move.l %d4, %d0 | 163 | move.l %d4, %d0 |
112 | lsl.l %d1, %d3 | 164 | lsl.l %d1, %d3 |
113 | lsr.l #8, %d0 | 165 | lsr.l #8, %d0 |
@@ -115,6 +167,11 @@ copy_read_sectors: | |||
115 | move.w (%a2), %d5 /* load 5th word */ | 167 | move.w (%a2), %d5 /* load 5th word */ |
116 | swap %d5 /* move to upper 16 bit */ | 168 | swap %d5 /* move to upper 16 bit */ |
117 | move.w (%a2), %d5 /* load 6th word */ | 169 | move.w (%a2), %d5 /* load 6th word */ |
170 | |||
171 | #ifdef SWAP_WORDS | ||
172 | SWAP_BYTES %d5 | ||
173 | #endif | ||
174 | |||
118 | move.l %d5, %d0 | 175 | move.l %d5, %d0 |
119 | lsl.l %d1, %d4 | 176 | lsl.l %d1, %d4 |
120 | lsr.l #8, %d0 | 177 | lsr.l #8, %d0 |
@@ -122,6 +179,11 @@ copy_read_sectors: | |||
122 | move.w (%a2), %d6 /* load 7th word */ | 179 | move.w (%a2), %d6 /* load 7th word */ |
123 | swap %d6 /* move to upper 16 bit */ | 180 | swap %d6 /* move to upper 16 bit */ |
124 | move.w (%a2), %d6 /* load 8th word */ | 181 | move.w (%a2), %d6 /* load 8th word */ |
182 | |||
183 | #ifdef SWAP_WORDS | ||
184 | SWAP_BYTES %d6 | ||
185 | #endif | ||
186 | |||
125 | move.l %d6, %d0 | 187 | move.l %d6, %d0 |
126 | lsl.l %d1, %d5 | 188 | lsl.l %d1, %d5 |
127 | lsr.l #8, %d0 | 189 | lsr.l #8, %d0 |
@@ -140,6 +202,11 @@ copy_read_sectors: | |||
140 | move.w (%a2), %d3 /* load first word */ | 202 | move.w (%a2), %d3 /* load first word */ |
141 | swap %d3 /* move to upper 16 bit */ | 203 | swap %d3 /* move to upper 16 bit */ |
142 | move.w (%a2), %d3 /* load second word */ | 204 | move.w (%a2), %d3 /* load second word */ |
205 | |||
206 | #ifdef SWAP_WORDS | ||
207 | SWAP_BYTES %d3 | ||
208 | #endif | ||
209 | |||
143 | move.l %d3, %d4 | 210 | move.l %d3, %d4 |
144 | lsl.l %d1, %d2 | 211 | lsl.l %d1, %d2 |
145 | lsr.l #8, %d3 | 212 | lsr.l #8, %d3 |
@@ -152,23 +219,40 @@ copy_read_sectors: | |||
152 | .r_end_u_l2: | 219 | .r_end_u_l2: |
153 | blo.b .r_end_u_w2 /* one word left? */ | 220 | blo.b .r_end_u_w2 /* one word left? */ |
154 | 221 | ||
222 | #ifdef SWAP_WORDS | ||
223 | move.w (%a2), %d3 | ||
224 | lsl.l #8, %d2 | ||
225 | move.b %d3, %d2 | ||
226 | move.w %d2, (%a0)+ /* write bytes 2 and 3 as word */ | ||
227 | move.l %d3, %d2 | ||
228 | lsr.l #8, %d2 | ||
229 | .r_end_u_w2: | ||
230 | move.b %d2, (%a0)+ /* store final byte */ | ||
231 | bra.w .r_exit | ||
232 | #else | ||
155 | swap %d2 /* move old word to upper 16 bits */ | 233 | swap %d2 /* move old word to upper 16 bits */ |
156 | move.w (%a2), %d2 /* load final word */ | 234 | move.w (%a2), %d2 /* load final word */ |
157 | move.l %d2, %d3 | 235 | move.l %d2, %d3 |
158 | lsr.l #8, %d3 | 236 | lsr.l #8, %d3 |
159 | move.w %d3, (%a0)+ /* write bytes 2 and 3 as word */ | 237 | move.w %d3, (%a0)+ /* write bytes 2 and 3 as word */ |
160 | |||
161 | .r_end_u_w2: | 238 | .r_end_u_w2: |
162 | move.b %d2, (%a0)+ /* store final byte */ | 239 | move.b %d2, (%a0)+ /* store final byte */ |
163 | bra.b .r_exit | 240 | bra.b .r_exit |
241 | #endif | ||
164 | 242 | ||
165 | /* 16-bit aligned */ | 243 | /* 16-bit aligned */ |
166 | .r_aligned: | 244 | .r_aligned: |
167 | btst.l #1, %d0 /* longword aligned? */ | 245 | btst.l #1, %d0 /* longword aligned? */ |
168 | beq.b .r_end_a_w1 /* yes, skip leading word handling */ | 246 | beq.b .r_end_a_w1 /* yes, skip leading word handling */ |
169 | 247 | ||
248 | #ifdef SWAP_WORDS | ||
249 | move.w (%a2), %d7 /* copy initial word after byte swap */ | ||
250 | move.b %d7, (%a0)+ | ||
251 | lsr.l #8, %d7 | ||
252 | move.b %d7, (%a0)+ | ||
253 | #else | ||
170 | move.w (%a2), (%a0)+ /* copy initial word */ | 254 | move.w (%a2), (%a0)+ /* copy initial word */ |
171 | 255 | #endif | |
172 | .r_end_a_w1: | 256 | .r_end_a_w1: |
173 | moveq.l #12, %d0 | 257 | moveq.l #12, %d0 |
174 | add.l %a0, %d0 | 258 | add.l %a0, %d0 |
@@ -180,6 +264,11 @@ copy_read_sectors: | |||
180 | move.w (%a2), %d1 /* load first word */ | 264 | move.w (%a2), %d1 /* load first word */ |
181 | swap %d1 /* move it to upper 16 bits */ | 265 | swap %d1 /* move it to upper 16 bits */ |
182 | move.w (%a2), %d1 /* load second word */ | 266 | move.w (%a2), %d1 /* load second word */ |
267 | |||
268 | #ifdef SWAP_WORDS | ||
269 | SWAP_BYTES %d1 | ||
270 | #endif | ||
271 | |||
183 | move.l %d1, (%a0)+ /* store as long */ | 272 | move.l %d1, (%a0)+ /* store as long */ |
184 | cmp.l %a0, %d0 /* run up to first line bound */ | 273 | cmp.l %a0, %d0 /* run up to first line bound */ |
185 | bhi.b .r_loop_a_l1 | 274 | bhi.b .r_loop_a_l1 |
@@ -200,6 +289,14 @@ copy_read_sectors: | |||
200 | move.w (%a2), %d3 /* load 7th word */ | 289 | move.w (%a2), %d3 /* load 7th word */ |
201 | swap %d3 /* move it to upper 16 bits */ | 290 | swap %d3 /* move it to upper 16 bits */ |
202 | move.w (%a2), %d3 /* load 8th word */ | 291 | move.w (%a2), %d3 /* load 8th word */ |
292 | |||
293 | #ifdef SWAP_WORDS | ||
294 | SWAP_BYTES %d0 | ||
295 | SWAP_BYTES %d1 | ||
296 | SWAP_BYTES %d2 | ||
297 | SWAP_BYTES %d3 | ||
298 | #endif | ||
299 | |||
203 | movem.l %d0-%d3, (%a0) /* store line */ | 300 | movem.l %d0-%d3, (%a0) /* store line */ |
204 | lea.l (16, %a0), %a0 | 301 | lea.l (16, %a0), %a0 |
205 | cmp.l %a0, %a1 /* run up to last line bound */ | 302 | cmp.l %a0, %a1 /* run up to last line bound */ |
@@ -213,6 +310,11 @@ copy_read_sectors: | |||
213 | move.w (%a2), %d1 /* read first word */ | 310 | move.w (%a2), %d1 /* read first word */ |
214 | swap %d1 /* move it to upper 16 bits */ | 311 | swap %d1 /* move it to upper 16 bits */ |
215 | move.w (%a2), %d1 /* read second word */ | 312 | move.w (%a2), %d1 /* read second word */ |
313 | |||
314 | #ifdef SWAP_WORDS | ||
315 | SWAP_BYTES %d1 | ||
316 | #endif | ||
317 | |||
216 | move.l %d1, (%a0)+ /* store as long */ | 318 | move.l %d1, (%a0)+ /* store as long */ |
217 | cmp.l %a0, %a1 /* run up to last long bound */ | 319 | cmp.l %a0, %a1 /* run up to last long bound */ |
218 | bhi.b .r_loop_a_l2 | 320 | bhi.b .r_loop_a_l2 |
@@ -220,13 +322,24 @@ copy_read_sectors: | |||
220 | .r_end_a_l2: | 322 | .r_end_a_l2: |
221 | blo.b .r_end_a_w2 /* one word left? */ | 323 | blo.b .r_end_a_w2 /* one word left? */ |
222 | 324 | ||
325 | #ifdef SWAP_WORDS | ||
326 | move.w (%a2), %d7 /* copy final word after byte swap */ | ||
327 | move.b %d7, (%a0)+ | ||
328 | lsr.l #8, %d7 | ||
329 | move.b %d7, (%a0)+ | ||
330 | #else | ||
223 | move.w (%a2), (%a0)+ /* copy final word */ | 331 | move.w (%a2), (%a0)+ /* copy final word */ |
224 | 332 | #endif | |
225 | .r_end_a_w2: | 333 | .r_end_a_w2: |
226 | 334 | ||
227 | .r_exit: | 335 | .r_exit: |
336 | #ifdef SWAP_WORDS | ||
337 | movem.l (%sp), %d2-%d7/%a2-%a3 | ||
338 | lea.l (32, %sp), %sp | ||
339 | #else | ||
228 | movem.l (%sp), %d2-%d6/%a2 | 340 | movem.l (%sp), %d2-%d6/%a2 |
229 | lea.l (24, %sp), %sp | 341 | lea.l (24, %sp), %sp |
342 | #endif | ||
230 | rts | 343 | rts |
231 | 344 | ||
232 | .r_end: | 345 | .r_end: |
@@ -251,12 +364,24 @@ copy_read_sectors: | |||
251 | * %d0 - scratch | 364 | * %d0 - scratch |
252 | * %d1 - shift count | 365 | * %d1 - shift count |
253 | * %d2-%d6 - read buffers | 366 | * %d2-%d6 - read buffers |
367 | * | ||
368 | * #ifdef SWAP_WORDS | ||
369 | * %d7 - swap scrach | ||
370 | * %a3 - swap mask | ||
371 | * #endif | ||
254 | */ | 372 | */ |
255 | 373 | ||
256 | copy_write_sectors: | 374 | copy_write_sectors: |
375 | #ifdef SWAP_WORDS | ||
376 | lea.l (-32, %sp), %sp | ||
377 | movem.l %d2-%d7/%a2-%a3, (%sp) | ||
378 | movem.l (36, %sp), %a0-%a1 | ||
379 | lea.l .swapmask, %a3 | ||
380 | #else | ||
257 | lea.l (-24, %sp), %sp | 381 | lea.l (-24, %sp), %sp |
258 | movem.l %d2-%d6/%a2, (%sp) | 382 | movem.l %d2-%d6/%a2, (%sp) |
259 | movem.l (28, %sp), %a0-%a1 | 383 | movem.l (28, %sp), %a0-%a1 |
384 | #endif | ||
260 | add.l %a1, %a1 | 385 | add.l %a1, %a1 |
261 | add.l %a0, %a1 | 386 | add.l %a0, %a1 |
262 | lea.l .ata_port, %a2 | 387 | lea.l .ata_port, %a2 |
@@ -278,6 +403,14 @@ copy_write_sectors: | |||
278 | move.w (%a0)+, %d2 | 403 | move.w (%a0)+, %d2 |
279 | move.l %d2, %d3 | 404 | move.l %d2, %d3 |
280 | lsr.l #8, %d3 | 405 | lsr.l #8, %d3 |
406 | |||
407 | #ifdef SWAP_WORDS | ||
408 | move.l %d3, %d7 /* byte swap low word of %d3 */ | ||
409 | lsr.l #8, %d7 | ||
410 | lsl.l #8, %d3 | ||
411 | move.b %d7, %d3 | ||
412 | #endif | ||
413 | |||
281 | move.w %d3, (%a2) | 414 | move.w %d3, (%a2) |
282 | 415 | ||
283 | .w_end_u_w1: | 416 | .w_end_u_w1: |
@@ -293,6 +426,11 @@ copy_write_sectors: | |||
293 | lsl.l %d1, %d2 | 426 | lsl.l %d1, %d2 |
294 | lsr.l #8, %d3 | 427 | lsr.l #8, %d3 |
295 | or.l %d3, %d2 | 428 | or.l %d3, %d2 |
429 | |||
430 | #ifdef SWAP_WORDS | ||
431 | SWAP_BYTES %d2 | ||
432 | #endif | ||
433 | |||
296 | swap %d2 | 434 | swap %d2 |
297 | move.w %d2, (%a2) | 435 | move.w %d2, (%a2) |
298 | swap %d2 | 436 | swap %d2 |
@@ -311,6 +449,11 @@ copy_write_sectors: | |||
311 | lsl.l %d1, %d2 | 449 | lsl.l %d1, %d2 |
312 | lsr.l #8, %d0 | 450 | lsr.l #8, %d0 |
313 | or.l %d0, %d2 | 451 | or.l %d0, %d2 |
452 | |||
453 | #ifdef SWAP_WORDS | ||
454 | SWAP_BYTES %d2 | ||
455 | #endif | ||
456 | |||
314 | swap %d2 | 457 | swap %d2 |
315 | move.w %d2, (%a2) | 458 | move.w %d2, (%a2) |
316 | swap %d2 | 459 | swap %d2 |
@@ -319,6 +462,11 @@ copy_write_sectors: | |||
319 | lsl.l %d1, %d3 | 462 | lsl.l %d1, %d3 |
320 | lsr.l #8, %d0 | 463 | lsr.l #8, %d0 |
321 | or.l %d0, %d3 | 464 | or.l %d0, %d3 |
465 | |||
466 | #ifdef SWAP_WORDS | ||
467 | SWAP_BYTES %d3 | ||
468 | #endif | ||
469 | |||
322 | swap %d3 | 470 | swap %d3 |
323 | move.w %d3, (%a2) | 471 | move.w %d3, (%a2) |
324 | swap %d3 | 472 | swap %d3 |
@@ -327,6 +475,11 @@ copy_write_sectors: | |||
327 | lsl.l %d1, %d4 | 475 | lsl.l %d1, %d4 |
328 | lsr.l #8, %d0 | 476 | lsr.l #8, %d0 |
329 | or.l %d0, %d4 | 477 | or.l %d0, %d4 |
478 | |||
479 | #ifdef SWAP_WORDS | ||
480 | SWAP_BYTES %d4 | ||
481 | #endif | ||
482 | |||
330 | swap %d4 | 483 | swap %d4 |
331 | move.w %d4, (%a2) | 484 | move.w %d4, (%a2) |
332 | swap %d4 | 485 | swap %d4 |
@@ -335,6 +488,11 @@ copy_write_sectors: | |||
335 | lsl.l %d1, %d5 | 488 | lsl.l %d1, %d5 |
336 | lsr.l #8, %d0 | 489 | lsr.l #8, %d0 |
337 | or.l %d0, %d5 | 490 | or.l %d0, %d5 |
491 | |||
492 | #ifdef SWAP_WORDS | ||
493 | SWAP_BYTES %d5 | ||
494 | #endif | ||
495 | |||
338 | swap %d5 | 496 | swap %d5 |
339 | move.w %d5, (%a2) | 497 | move.w %d5, (%a2) |
340 | swap %d5 | 498 | swap %d5 |
@@ -353,6 +511,11 @@ copy_write_sectors: | |||
353 | lsl.l %d1, %d2 | 511 | lsl.l %d1, %d2 |
354 | lsr.l #8, %d3 | 512 | lsr.l #8, %d3 |
355 | or.l %d3, %d2 | 513 | or.l %d3, %d2 |
514 | |||
515 | #ifdef SWAP_WORDS | ||
516 | SWAP_BYTES %d2 | ||
517 | #endif | ||
518 | |||
356 | swap %d2 | 519 | swap %d2 |
357 | move.w %d2, (%a2) | 520 | move.w %d2, (%a2) |
358 | swap %d2 | 521 | swap %d2 |
@@ -368,20 +531,43 @@ copy_write_sectors: | |||
368 | move.w (%a0)+, %d2 | 531 | move.w (%a0)+, %d2 |
369 | move.l %d2, %d3 | 532 | move.l %d2, %d3 |
370 | lsr.l #8, %d3 | 533 | lsr.l #8, %d3 |
534 | |||
535 | #ifdef SWAP_WORDS | ||
536 | SWAP_BYTES %d3 | ||
537 | #endif | ||
538 | |||
371 | move.w %d3, (%a2) | 539 | move.w %d3, (%a2) |
372 | 540 | ||
373 | .w_end_u_w2: | 541 | .w_end_u_w2: |
542 | #ifdef SWAP_WORDS | ||
543 | move.l %d2, %d7 | ||
544 | move.b (%a0)+, %d2 | ||
545 | lsl.l #8, %d2 | ||
546 | move.b %d7, %d2 | ||
547 | move.w %d2, (%a2) | ||
548 | bra.w .w_exit | ||
549 | #else | ||
374 | lsl.l #8, %d2 | 550 | lsl.l #8, %d2 |
375 | move.b (%a0)+, %d2 | 551 | move.b (%a0)+, %d2 |
376 | move.w %d2, (%a2) | 552 | move.w %d2, (%a2) |
377 | bra.b .w_exit | 553 | bra.b .w_exit |
554 | #endif | ||
378 | 555 | ||
379 | /* 16-bit aligned */ | 556 | /* 16-bit aligned */ |
380 | .w_aligned: | 557 | .w_aligned: |
381 | btst.l #1, %d0 | 558 | btst.l #1, %d0 |
382 | beq.b .w_end_a_w1 | 559 | beq.b .w_end_a_w1 |
383 | 560 | ||
561 | #ifdef SWAP_WORDS | ||
562 | move.w (%a0)+, %d1 /* copy initial word bytes swaped */ | ||
563 | move.l %d1, %d7 | ||
564 | lsl.l #8, %d1 | ||
565 | lsr.l #8, %d7 | ||
566 | move.b %d7, %d1 | ||
567 | move.w %d1, (%a2) | ||
568 | #else | ||
384 | move.w (%a0)+, (%a2) /* copy initial word */ | 569 | move.w (%a0)+, (%a2) /* copy initial word */ |
570 | #endif | ||
385 | 571 | ||
386 | .w_end_a_w1: | 572 | .w_end_a_w1: |
387 | moveq.l #12, %d0 | 573 | moveq.l #12, %d0 |
@@ -392,6 +578,11 @@ copy_write_sectors: | |||
392 | 578 | ||
393 | .w_loop_a_l1: | 579 | .w_loop_a_l1: |
394 | move.l (%a0)+, %d1 | 580 | move.l (%a0)+, %d1 |
581 | |||
582 | #ifdef SWAP_WORDS | ||
583 | SWAP_BYTES %d1 | ||
584 | #endif | ||
585 | |||
395 | swap %d1 | 586 | swap %d1 |
396 | move.w %d1, (%a2) | 587 | move.w %d1, (%a2) |
397 | swap %d1 | 588 | swap %d1 |
@@ -405,6 +596,14 @@ copy_write_sectors: | |||
405 | .w_loop_a_line: | 596 | .w_loop_a_line: |
406 | movem.l (%a0), %d0-%d3 | 597 | movem.l (%a0), %d0-%d3 |
407 | lea.l (16, %a0), %a0 | 598 | lea.l (16, %a0), %a0 |
599 | |||
600 | #ifdef SWAP_WORDS | ||
601 | SWAP_BYTES %d0 | ||
602 | SWAP_BYTES %d1 | ||
603 | SWAP_BYTES %d2 | ||
604 | SWAP_BYTES %d3 | ||
605 | #endif | ||
606 | |||
408 | swap %d0 | 607 | swap %d0 |
409 | move.w %d0, (%a2) | 608 | move.w %d0, (%a2) |
410 | swap %d0 | 609 | swap %d0 |
@@ -430,6 +629,11 @@ copy_write_sectors: | |||
430 | 629 | ||
431 | .w_loop_a_l2: | 630 | .w_loop_a_l2: |
432 | move.l (%a0)+, %d1 | 631 | move.l (%a0)+, %d1 |
632 | |||
633 | #ifdef SWAP_WORDS | ||
634 | SWAP_BYTES %d1 | ||
635 | #endif | ||
636 | |||
433 | swap %d1 | 637 | swap %d1 |
434 | move.w %d1, (%a2) | 638 | move.w %d1, (%a2) |
435 | swap %d1 | 639 | swap %d1 |
@@ -440,13 +644,27 @@ copy_write_sectors: | |||
440 | .w_end_a_l2: | 644 | .w_end_a_l2: |
441 | blo.b .w_end_a_w2 /* one word left? */ | 645 | blo.b .w_end_a_w2 /* one word left? */ |
442 | 646 | ||
647 | #ifdef SWAP_WORDS | ||
648 | move.w (%a0)+, %d0 /* copy final word after byte swap */ | ||
649 | move.l %d0, %d7 | ||
650 | lsl.l #8, %d0 | ||
651 | lsr.l #8, %d7 | ||
652 | move.b %d7, %d0 | ||
653 | move.w %d0, (%a2) | ||
654 | #else | ||
443 | move.w (%a0)+, (%a2) /* copy final word */ | 655 | move.w (%a0)+, (%a2) /* copy final word */ |
656 | #endif | ||
444 | 657 | ||
445 | .w_end_a_w2: | 658 | .w_end_a_w2: |
446 | 659 | ||
447 | .w_exit: | 660 | .w_exit: |
661 | #ifdef SWAP_WORDS | ||
662 | movem.l (%sp), %d2-%d7/%a2-%a3 | ||
663 | lea.l (32, %sp), %sp | ||
664 | #else | ||
448 | movem.l (%sp), %d2-%d6/%a2 | 665 | movem.l (%sp), %d2-%d6/%a2 |
449 | lea.l (24, %sp), %sp | 666 | lea.l (24, %sp), %sp |
667 | #endif | ||
450 | rts | 668 | rts |
451 | 669 | ||
452 | .w_end: | 670 | .w_end: |