firmware/target/coldfire/mpio/ata-as-mpio.S

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