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 @@ * $Id$ * * Copyright (C) 2006 by Jens Arnold + * Copyright (C) 2010 by Marcin Bukat (byte swaps) * * This program is free software; you can redistribute it and/or * modify it under the terms of the GNU General Public License @@ -19,9 +20,21 @@ * ****************************************************************************/ - .section .icode,"ax",@progbits +#include "config.h" + + .macro SWAP_BYTES dn + move.l %a3, %d7 | d7 = 0x00FF00FF + and.l \dn, %d7 | d7 = .B.D + eor.l %d7, \dn | dn = A.C. + lsl.l #8, %d7 | d7 = B.D. + lsr.l #8, \dn | d3 = .A.C + or.l %d7, \dn | dn = BADC + .endm .equ .ata_port, 0x20000020 + .equ .swapmask, 0x00FF00FF + + .section .icode,"ax",@progbits .align 2 .global copy_read_sectors @@ -42,12 +55,24 @@ * %d0 - scratch * %d1 - shift count * %d2-%d6 - read buffers + * + * #ifdef SWAP_WORDS + * %d7 - byte swap scrach + * %a3 - byte swap mask + * #endif */ copy_read_sectors: +#ifdef SWAP_WORDS + lea.l (-32, %sp), %sp + movem.l %d2-%d7/%a2-%a3, (%sp) + movem.l (36, %sp), %a0-%a1 + lea.l .swapmask, %a3 +#else lea.l (-24, %sp), %sp movem.l %d2-%d6/%a2, (%sp) movem.l (28, %sp), %a0-%a1 +#endif add.l %a1, %a1 add.l %a0, %a1 lea.l .ata_port, %a2 @@ -61,18 +86,30 @@ copy_read_sectors: moveq.l #24, %d1 /* preload shift count */ move.w (%a2), %d2 /* load initial word */ +#ifdef SWAP_WORDS + move.b %d2, (%a0)+ + lsr.l #8, %d2 +#else move.l %d2, %d3 lsr.l #8, %d3 move.b %d3, (%a0)+ /* write high byte of it, aligns dest addr */ - +#endif btst.l #1, %d0 /* longword aligned? (testing old d0 value!) */ bne.b .r_end_u_w1 /* yes, skip leading word handling */ - +#ifdef SWAP_WORDS + move.w (%a2), %d3 + lsl.l #8, %d2 + move.b %d3, %d2 + move.w %d2, (%a0)+ /* write bytes 2 and 3 as word */ + move.l %d3, %d2 + lsr.l #8, %d2 +#else swap %d2 /* move initial word up */ move.w (%a2), %d2 /* combine with second word */ move.l %d2, %d3 lsr.l #8, %d3 move.w %d3, (%a0)+ /* write bytes 2 and 3 as word */ +#endif .r_end_u_w1: moveq.l #12, %d0 @@ -85,6 +122,11 @@ copy_read_sectors: move.w (%a2), %d3 /* load first word */ swap %d3 /* move to upper 16 bit */ move.w (%a2), %d3 /* load second word */ + +#ifdef SWAP_WORDS + SWAP_BYTES %d3 +#endif + move.l %d3, %d4 lsl.l %d1, %d2 lsr.l #8, %d3 @@ -101,6 +143,11 @@ copy_read_sectors: move.w (%a2), %d3 /* load 1st word */ swap %d3 /* move to upper 16 bit */ move.w (%a2), %d3 /* load 2nd word */ + +#ifdef SWAP_WORDS + SWAP_BYTES %d3 +#endif + move.l %d3, %d0 lsl.l %d1, %d2 lsr.l #8, %d0 @@ -108,6 +155,11 @@ copy_read_sectors: move.w (%a2), %d4 /* load 3rd word */ swap %d4 /* move to upper 16 bit */ move.w (%a2), %d4 /* load 4th word */ + +#ifdef SWAP_WORDS + SWAP_BYTES %d4 +#endif + move.l %d4, %d0 lsl.l %d1, %d3 lsr.l #8, %d0 @@ -115,6 +167,11 @@ copy_read_sectors: move.w (%a2), %d5 /* load 5th word */ swap %d5 /* move to upper 16 bit */ move.w (%a2), %d5 /* load 6th word */ + +#ifdef SWAP_WORDS + SWAP_BYTES %d5 +#endif + move.l %d5, %d0 lsl.l %d1, %d4 lsr.l #8, %d0 @@ -122,6 +179,11 @@ copy_read_sectors: move.w (%a2), %d6 /* load 7th word */ swap %d6 /* move to upper 16 bit */ move.w (%a2), %d6 /* load 8th word */ + +#ifdef SWAP_WORDS + SWAP_BYTES %d6 +#endif + move.l %d6, %d0 lsl.l %d1, %d5 lsr.l #8, %d0 @@ -140,6 +202,11 @@ copy_read_sectors: move.w (%a2), %d3 /* load first word */ swap %d3 /* move to upper 16 bit */ move.w (%a2), %d3 /* load second word */ + +#ifdef SWAP_WORDS + SWAP_BYTES %d3 +#endif + move.l %d3, %d4 lsl.l %d1, %d2 lsr.l #8, %d3 @@ -152,23 +219,40 @@ copy_read_sectors: .r_end_u_l2: blo.b .r_end_u_w2 /* one word left? */ +#ifdef SWAP_WORDS + move.w (%a2), %d3 + lsl.l #8, %d2 + move.b %d3, %d2 + move.w %d2, (%a0)+ /* write bytes 2 and 3 as word */ + move.l %d3, %d2 + lsr.l #8, %d2 +.r_end_u_w2: + move.b %d2, (%a0)+ /* store final byte */ + bra.w .r_exit +#else swap %d2 /* move old word to upper 16 bits */ move.w (%a2), %d2 /* load final word */ move.l %d2, %d3 lsr.l #8, %d3 move.w %d3, (%a0)+ /* write bytes 2 and 3 as word */ - .r_end_u_w2: move.b %d2, (%a0)+ /* store final byte */ bra.b .r_exit +#endif /* 16-bit aligned */ .r_aligned: btst.l #1, %d0 /* longword aligned? */ beq.b .r_end_a_w1 /* yes, skip leading word handling */ +#ifdef SWAP_WORDS + move.w (%a2), %d7 /* copy initial word after byte swap */ + move.b %d7, (%a0)+ + lsr.l #8, %d7 + move.b %d7, (%a0)+ +#else move.w (%a2), (%a0)+ /* copy initial word */ - +#endif .r_end_a_w1: moveq.l #12, %d0 add.l %a0, %d0 @@ -180,6 +264,11 @@ copy_read_sectors: move.w (%a2), %d1 /* load first word */ swap %d1 /* move it to upper 16 bits */ move.w (%a2), %d1 /* load second word */ + +#ifdef SWAP_WORDS + SWAP_BYTES %d1 +#endif + move.l %d1, (%a0)+ /* store as long */ cmp.l %a0, %d0 /* run up to first line bound */ bhi.b .r_loop_a_l1 @@ -200,6 +289,14 @@ copy_read_sectors: move.w (%a2), %d3 /* load 7th word */ swap %d3 /* move it to upper 16 bits */ move.w (%a2), %d3 /* load 8th word */ + +#ifdef SWAP_WORDS + SWAP_BYTES %d0 + SWAP_BYTES %d1 + SWAP_BYTES %d2 + SWAP_BYTES %d3 +#endif + movem.l %d0-%d3, (%a0) /* store line */ lea.l (16, %a0), %a0 cmp.l %a0, %a1 /* run up to last line bound */ @@ -213,6 +310,11 @@ copy_read_sectors: move.w (%a2), %d1 /* read first word */ swap %d1 /* move it to upper 16 bits */ move.w (%a2), %d1 /* read second word */ + +#ifdef SWAP_WORDS + SWAP_BYTES %d1 +#endif + move.l %d1, (%a0)+ /* store as long */ cmp.l %a0, %a1 /* run up to last long bound */ bhi.b .r_loop_a_l2 @@ -220,13 +322,24 @@ copy_read_sectors: .r_end_a_l2: blo.b .r_end_a_w2 /* one word left? */ +#ifdef SWAP_WORDS + move.w (%a2), %d7 /* copy final word after byte swap */ + move.b %d7, (%a0)+ + lsr.l #8, %d7 + move.b %d7, (%a0)+ +#else move.w (%a2), (%a0)+ /* copy final word */ - +#endif .r_end_a_w2: .r_exit: +#ifdef SWAP_WORDS + movem.l (%sp), %d2-%d7/%a2-%a3 + lea.l (32, %sp), %sp +#else movem.l (%sp), %d2-%d6/%a2 lea.l (24, %sp), %sp +#endif rts .r_end: @@ -251,12 +364,24 @@ copy_read_sectors: * %d0 - scratch * %d1 - shift count * %d2-%d6 - read buffers + * + * #ifdef SWAP_WORDS + * %d7 - swap scrach + * %a3 - swap mask + * #endif */ copy_write_sectors: +#ifdef SWAP_WORDS + lea.l (-32, %sp), %sp + movem.l %d2-%d7/%a2-%a3, (%sp) + movem.l (36, %sp), %a0-%a1 + lea.l .swapmask, %a3 +#else lea.l (-24, %sp), %sp movem.l %d2-%d6/%a2, (%sp) movem.l (28, %sp), %a0-%a1 +#endif add.l %a1, %a1 add.l %a0, %a1 lea.l .ata_port, %a2 @@ -278,6 +403,14 @@ copy_write_sectors: move.w (%a0)+, %d2 move.l %d2, %d3 lsr.l #8, %d3 + +#ifdef SWAP_WORDS + move.l %d3, %d7 /* byte swap low word of %d3 */ + lsr.l #8, %d7 + lsl.l #8, %d3 + move.b %d7, %d3 +#endif + move.w %d3, (%a2) .w_end_u_w1: @@ -293,6 +426,11 @@ copy_write_sectors: lsl.l %d1, %d2 lsr.l #8, %d3 or.l %d3, %d2 + +#ifdef SWAP_WORDS + SWAP_BYTES %d2 +#endif + swap %d2 move.w %d2, (%a2) swap %d2 @@ -311,6 +449,11 @@ copy_write_sectors: lsl.l %d1, %d2 lsr.l #8, %d0 or.l %d0, %d2 + +#ifdef SWAP_WORDS + SWAP_BYTES %d2 +#endif + swap %d2 move.w %d2, (%a2) swap %d2 @@ -319,6 +462,11 @@ copy_write_sectors: lsl.l %d1, %d3 lsr.l #8, %d0 or.l %d0, %d3 + +#ifdef SWAP_WORDS + SWAP_BYTES %d3 +#endif + swap %d3 move.w %d3, (%a2) swap %d3 @@ -327,6 +475,11 @@ copy_write_sectors: lsl.l %d1, %d4 lsr.l #8, %d0 or.l %d0, %d4 + +#ifdef SWAP_WORDS + SWAP_BYTES %d4 +#endif + swap %d4 move.w %d4, (%a2) swap %d4 @@ -335,6 +488,11 @@ copy_write_sectors: lsl.l %d1, %d5 lsr.l #8, %d0 or.l %d0, %d5 + +#ifdef SWAP_WORDS + SWAP_BYTES %d5 +#endif + swap %d5 move.w %d5, (%a2) swap %d5 @@ -353,6 +511,11 @@ copy_write_sectors: lsl.l %d1, %d2 lsr.l #8, %d3 or.l %d3, %d2 + +#ifdef SWAP_WORDS + SWAP_BYTES %d2 +#endif + swap %d2 move.w %d2, (%a2) swap %d2 @@ -368,20 +531,43 @@ copy_write_sectors: move.w (%a0)+, %d2 move.l %d2, %d3 lsr.l #8, %d3 + +#ifdef SWAP_WORDS + SWAP_BYTES %d3 +#endif + move.w %d3, (%a2) .w_end_u_w2: +#ifdef SWAP_WORDS + move.l %d2, %d7 + move.b (%a0)+, %d2 + lsl.l #8, %d2 + move.b %d7, %d2 + move.w %d2, (%a2) + bra.w .w_exit +#else lsl.l #8, %d2 move.b (%a0)+, %d2 move.w %d2, (%a2) bra.b .w_exit +#endif /* 16-bit aligned */ .w_aligned: btst.l #1, %d0 beq.b .w_end_a_w1 +#ifdef SWAP_WORDS + move.w (%a0)+, %d1 /* copy initial word bytes swaped */ + move.l %d1, %d7 + lsl.l #8, %d1 + lsr.l #8, %d7 + move.b %d7, %d1 + move.w %d1, (%a2) +#else move.w (%a0)+, (%a2) /* copy initial word */ +#endif .w_end_a_w1: moveq.l #12, %d0 @@ -392,6 +578,11 @@ copy_write_sectors: .w_loop_a_l1: move.l (%a0)+, %d1 + +#ifdef SWAP_WORDS + SWAP_BYTES %d1 +#endif + swap %d1 move.w %d1, (%a2) swap %d1 @@ -405,6 +596,14 @@ copy_write_sectors: .w_loop_a_line: movem.l (%a0), %d0-%d3 lea.l (16, %a0), %a0 + +#ifdef SWAP_WORDS + SWAP_BYTES %d0 + SWAP_BYTES %d1 + SWAP_BYTES %d2 + SWAP_BYTES %d3 +#endif + swap %d0 move.w %d0, (%a2) swap %d0 @@ -430,6 +629,11 @@ copy_write_sectors: .w_loop_a_l2: move.l (%a0)+, %d1 + +#ifdef SWAP_WORDS + SWAP_BYTES %d1 +#endif + swap %d1 move.w %d1, (%a2) swap %d1 @@ -440,13 +644,27 @@ copy_write_sectors: .w_end_a_l2: blo.b .w_end_a_w2 /* one word left? */ +#ifdef SWAP_WORDS + move.w (%a0)+, %d0 /* copy final word after byte swap */ + move.l %d0, %d7 + lsl.l #8, %d0 + lsr.l #8, %d7 + move.b %d7, %d0 + move.w %d0, (%a2) +#else move.w (%a0)+, (%a2) /* copy final word */ +#endif .w_end_a_w2: .w_exit: +#ifdef SWAP_WORDS + movem.l (%sp), %d2-%d7/%a2-%a3 + lea.l (32, %sp), %sp +#else movem.l (%sp), %d2-%d6/%a2 lea.l (24, %sp), %sp +#endif rts .w_end: