Integrate byte swap of ata word into ata-as-coldfire.S

git-svn-id: svn://svn.rockbox.org/rockbox/trunk@26105 a1c6a512-1295-4272-9138-f99709370657

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 @@
  * $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: