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

   1 /***************************************************************************
   2  *             __________               __   ___.
   3  *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
   4  *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
   5  *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
   6  *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
   7  *                     \/            \/     \/    \/            \/
   8  * $Id$
   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
  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
  47 copy_read_sectors:
  48     lea.l   (-24, %sp), %sp
  49     movem.l %d2-%d6/%a2, (%sp)
  50     movem.l (28, %sp), %a0-%a1
  51     add.l   %a1, %a1
  52     add.l   %a0, %a1
  53     lea.l   .ata_port, %a2
  54
  55     move.l  %a0, %d0
  56     btst.l  #0, %d0         /* 16-bit aligned? */
  57     jeq     .r_aligned      /* yes, do word copy */
  58
  59     /* not 16-bit aligned */
  60     subq.l  #1, %a1         /* last byte is done unconditionally */
  61     moveq.l #24, %d1        /* preload shift count */
  62
  63     move.w  (%a2), %d2      /* load initial word */
  64     move.l  %d2, %d3
  65     lsr.l   #8, %d3
  66     move.b  %d3, (%a0)+     /* write high byte of it, aligns dest addr */
  67
  68     btst.l  #1, %d0         /* longword aligned? (testing old d0 value!) */
  69     bne.b   .r_end_u_w1     /* yes, skip leading word handling */
  70
  71     swap    %d2             /* move initial word up */
  72     move.w  (%a2), %d2      /* combine with second word */
  73     move.l  %d2, %d3
  74     lsr.l   #8, %d3
  75     move.w  %d3, (%a0)+     /* write bytes 2 and 3 as word */
  76
  77 .r_end_u_w1:
  78     moveq.l #12, %d0
  79     add.l   %a0, %d0
  80     and.l   #0xFFFFFFF0,%d0 /* d0 == first line bound */
  81     cmp.l   %a0, %d0        /* any leading longwords? */
  82     bls.b   .r_end_u_l1     /* no: skip loop */
  83
  84 .r_loop_u_l1:
  85     move.w  (%a2), %d3      /* load first word */
  86     swap    %d3             /* move to upper 16 bit */
  87     move.w  (%a2), %d3      /* load second word */
  88     move.l  %d3, %d4
  89     lsl.l   %d1, %d2
  90     lsr.l   #8, %d3
  91     or.l    %d3, %d2        /* combine old low byte with new top 3 bytes */
  92     move.l  %d2, (%a0)+     /* store as long */
  93     move.l  %d4, %d2
  94     cmp.l   %a0, %d0        /* run up to first line bound */
  95     bhi.b   .r_loop_u_l1
  96
  97 .r_end_u_l1:
  98     lea.l   (-14, %a1), %a1 /* adjust end addr. to 16 bytes/pass */
  99
 100 .r_loop_u_line:
 101     move.w  (%a2), %d3      /* load 1st word */
 102     swap    %d3             /* move to upper 16 bit */
 103     move.w  (%a2), %d3      /* load 2nd word */
 104     move.l  %d3, %d0
 105     lsl.l   %d1, %d2
 106     lsr.l   #8, %d0
 107     or.l    %d0, %d2        /* combine old low byte with new top 3 bytes */
 108     move.w  (%a2), %d4      /* load 3rd word */
 109     swap    %d4             /* move to upper 16 bit */
 110     move.w  (%a2), %d4      /* load 4th word */
 111     move.l  %d4, %d0
 112     lsl.l   %d1, %d3
 113     lsr.l   #8, %d0
 114     or.l    %d0, %d3        /* combine old low byte with new top 3 bytes */
 115     move.w  (%a2), %d5      /* load 5th word */
 116     swap    %d5             /* move to upper 16 bit */
 117     move.w  (%a2), %d5      /* load 6th word */
 118     move.l  %d5, %d0
 119     lsl.l   %d1, %d4
 120     lsr.l   #8, %d0
 121     or.l    %d0, %d4        /* combine old low byte with new top 3 bytes */
 122     move.w  (%a2), %d6      /* load 7th word */
 123     swap    %d6             /* move to upper 16 bit */
 124     move.w  (%a2), %d6      /* load 8th word */
 125     move.l  %d6, %d0
 126     lsl.l   %d1, %d5
 127     lsr.l   #8, %d0
 128     or.l    %d0, %d5        /* combine old low byte with new top 3 bytes */
 129     movem.l %d2-%d5, (%a0)  /* store line */
 130     lea.l   (16, %a0), %a0
 131     move.l  %d6, %d2
 132     cmp.l   %a0, %a1        /* run up to last line bound */
 133     bhi.b   .r_loop_u_line
 134
 135     lea.l   (12, %a1), %a1  /* readjust for longword loop */
 136     cmp.l   %a0, %a1        /* any trailing longwords? */
 137     bls.b   .r_end_u_l2     /* no: skip loop */
 138
 139 .r_loop_u_l2:
 140     move.w  (%a2), %d3      /* load first word */
 141     swap    %d3             /* move to upper 16 bit */
 142     move.w  (%a2), %d3      /* load second word */
 143     move.l  %d3, %d4
 144     lsl.l   %d1, %d2
 145     lsr.l   #8, %d3
 146     or.l    %d3, %d2        /* combine old low byte with new top 3 bytes */
 147     move.l  %d2, (%a0)+     /* store as long */
 148     move.l  %d4, %d2
 149     cmp.l   %a0, %a1        /* run up to last long bound */
 150     bhi.b   .r_loop_u_l2
 151
 152 .r_end_u_l2:
 153     addq.l  #2, %a1         /* back to final end address */
 154     cmp.l   %a0, %a1        /* one word left? */
 155     bls.b   .r_end_u_w2
 156
 157     swap    %d2             /* move old word to upper 16 bits */
 158     move.w  (%a2), %d2      /* load final word */
 159     move.l  %d2, %d3
 160     lsr.l   #8, %d3
 161     move.w  %d3, (%a0)+     /* write bytes 2 and 3 as word */
 162
 163 .r_end_u_w2:
 164     move.b  %d2, (%a0)+     /* store final byte */
 165     bra.b   .r_exit
 166
 167     /* 16-bit aligned */
 168 .r_aligned:
 169     btst.l  #1, %d0         /* longword aligned? */
 170     beq.b   .r_end_a_w1     /* yes, skip leading word handling */
 171
 172     move.w  (%a2), (%a0)+   /* copy initial word */
 173
 174 .r_end_a_w1:
 175     moveq.l #12, %d0
 176     add.l   %a0, %d0
 177     and.l   #0xFFFFFFF0,%d0 /* d0 == first line bound */
 178     cmp.l   %a0, %d0        /* any leading longwords? */
 179     bls.b   .r_end_a_l1     /* no: skip loop */
 180
 181 .r_loop_a_l1:
 182     move.w  (%a2), %d1      /* load first word */
 183     swap    %d1             /* move it to upper 16 bits */
 184     move.w  (%a2), %d1      /* load second word */
 185     move.l  %d1, (%a0)+     /* store as long */
 186     cmp.l   %a0, %d0        /* run up to first line bound */
 187     bhi.b   .r_loop_a_l1
 188
 189 .r_end_a_l1:
 190     lea.l   (-14, %a1), %a1 /* adjust end addr. to 16 bytes/pass */
 191
 192 .r_loop_a_line:
 193     move.w  (%a2), %d0      /* load 1st word */
 194     swap    %d0             /* move it to upper 16 bits */
 195     move.w  (%a2), %d0      /* load 2nd word */
 196     move.w  (%a2), %d1      /* load 3rd word */
 197     swap    %d1             /* move it to upper 16 bits */
 198     move.w  (%a2), %d1      /* load 4th word */
 199     move.w  (%a2), %d2      /* load 5th word */
 200     swap    %d2             /* move it to upper 16 bits */
 201     move.w  (%a2), %d2      /* load 6th word */
 202     move.w  (%a2), %d3      /* load 7th word */
 203     swap    %d3             /* move it to upper 16 bits */
 204     move.w  (%a2), %d3      /* load 8th word */
 205     movem.l %d0-%d3, (%a0)  /* store line */
 206     lea.l   (16, %a0), %a0
 207     cmp.l   %a0, %a1        /* run up to last line bound */
 208     bhi.b   .r_loop_a_line
 209
 210     lea.l   (12, %a1), %a1  /* readjust for longword loop */
 211     cmp.l   %a0, %a1        /* any trailing longwords? */
 212     bls.b   .r_end_a_l2     /* no: skip loop */
 213
 214 .r_loop_a_l2:
 215     move.w  (%a2), %d1      /* read first word */
 216     swap    %d1             /* move it to upper 16 bits */
 217     move.w  (%a2), %d1      /* read second word */
 218     move.l  %d1, (%a0)+     /* store as long */
 219     cmp.l   %a0, %a1        /* run up to last long bound */
 220     bhi.b   .r_loop_a_l2
 221
 222 .r_end_a_l2:
 223     addq.l  #2, %a1         /* back to final end address */
 224     cmp.l   %a0, %a1        /* one word left? */
 225     bls.b   .r_end_a_w2
 226
 227     move.w  (%a2), (%a0)+   /* copy final word */
 228
 229 .r_end_a_w2:
 230
 231 .r_exit:
 232     movem.l (%sp), %d2-%d6/%a2
 233     lea.l   (24, %sp), %sp
 234     rts
 235
 236 .r_end:
 237     .size   copy_read_sectors,.r_end-copy_read_sectors
 238
 239     .align  2
 240     .global copy_write_sectors
 241     .type   copy_write_sectors,@function
 242
 243 /* Write a number of words to the ATA data port
 244  *
 245  * Utilises line bursts, assumes there is at least one full line to copy.
 246  *
 247  * Arguments:
 248  *   (4,%sp) - buffer address
 249  *   (8,%sp) - word count
 250  *
 251  * Register usage:
 252  *   %a0 - current address
 253  *   %a1 - end address
 254  *   %a2 - ata port
 255  *   %d0 - scratch
 256  *   %d1 - shift count
 257  *   %d2-%d6 - read buffers
 258  */
 259
 260 copy_write_sectors:
 261     lea.l   (-24, %sp), %sp
 262     movem.l %d2-%d6/%a2, (%sp)
 263     movem.l (28, %sp), %a0-%a1
 264     add.l   %a1, %a1
 265     add.l   %a0, %a1
 266     lea.l   .ata_port, %a2
 267
 268     move.l  %a0, %d0
 269     btst.l  #0, %d0         /* 16-bit aligned? */
 270     jeq     .w_aligned      /* yes, do word copy */
 271
 272     /* not 16-bit aligned */
 273     subq.l  #1, %a1         /* last byte is done unconditionally */
 274     moveq.l #24, %d1        /* preload shift count */
 275
 276     move.b  (%a0)+, %d2
 277
 278     btst.l  #1, %d0         /* longword aligned? (testing old d0 value!) */
 279     bne.b   .w_end_u_w1     /* yes, skip leading word handling */
 280
 281     swap    %d2
 282     move.w  (%a0)+, %d2
 283     move.l  %d2, %d3
 284     lsr.l   #8, %d3
 285     move.w  %d3, (%a2)
 286
 287 .w_end_u_w1:
 288     moveq.l #12, %d0
 289     add.l   %a0, %d0
 290     and.l   #0xFFFFFFF0,%d0 /* d0 == first line bound */
 291     cmp.l   %a0, %d0        /* any leading longwords? */
 292     bls.b   .w_end_u_l1     /* no: skip loop */
 293
 294 .w_loop_u_l1:
 295     move.l  (%a0)+, %d3
 296     move.l  %d3, %d4
 297     lsl.l   %d1, %d2
 298     lsr.l   #8, %d3
 299     or.l    %d3, %d2
 300     swap    %d2
 301     move.w  %d2, (%a2)
 302     swap    %d2
 303     move.w  %d2, (%a2)
 304     move.l  %d4, %d2
 305     cmp.l   %a0, %d0        /* run up to first line bound */
 306     bhi.b   .w_loop_u_l1
 307
 308 .w_end_u_l1:
 309     lea.l   (-14, %a1), %a1 /* adjust end addr. to 16 bytes/pass */
 310
 311 .w_loop_u_line:
 312     movem.l (%a0), %d3-%d6
 313     lea.l   (16, %a0), %a0
 314     move.l  %d3, %d0
 315     lsl.l   %d1, %d2
 316     lsr.l   #8, %d0
 317     or.l    %d0, %d2
 318     swap    %d2
 319     move.w  %d2, (%a2)
 320     swap    %d2
 321     move.w  %d2, (%a2)
 322     move.l  %d4, %d0
 323     lsl.l   %d1, %d3
 324     lsr.l   #8, %d0
 325     or.l    %d0, %d3
 326     swap    %d3
 327     move.w  %d3, (%a2)
 328     swap    %d3
 329     move.w  %d3, (%a2)
 330     move.l  %d5, %d0
 331     lsl.l   %d1, %d4
 332     lsr.l   #8, %d0
 333     or.l    %d0, %d4
 334     swap    %d4
 335     move.w  %d4, (%a2)
 336     swap    %d4
 337     move.w  %d4, (%a2)
 338     move.l  %d6, %d0
 339     lsl.l   %d1, %d5
 340     lsr.l   #8, %d0
 341     or.l    %d0, %d5
 342     swap    %d5
 343     move.w  %d5, (%a2)
 344     swap    %d5
 345     move.w  %d5, (%a2)
 346     move.l  %d6, %d2
 347     cmp.l   %a0, %a1        /* run up to last line bound */
 348     bhi.b   .w_loop_u_line
 349
 350     lea.l   (12, %a1), %a1  /* readjust for longword loop */
 351     cmp.l   %a0, %a1        /* any trailing longwords? */
 352     bls.b   .w_end_u_l2     /* no: skip loop */
 353
 354 .w_loop_u_l2:
 355     move.l  (%a0)+, %d3
 356     move.l  %d3, %d4
 357     lsl.l   %d1, %d2
 358     lsr.l   #8, %d3
 359     or.l    %d3, %d2
 360     swap    %d2
 361     move.w  %d2, (%a2)
 362     swap    %d2
 363     move.w  %d2, (%a2)
 364     move.l  %d4, %d2
 365     cmp.l   %a0, %a1        /* run up to first line bound */
 366     bhi.b   .w_loop_u_l2
 367
 368 .w_end_u_l2:
 369     addq.l  #2, %a1         /* back to final end address */
 370     cmp.l   %a0, %a1        /* one word left? */
 371     bls.b   .w_end_u_w2
 372
 373     swap    %d2
 374     move.w  (%a0)+, %d2
 375     move.l  %d2, %d3
 376     lsr.l   #8, %d3
 377     move.w  %d3, (%a2)
 378
 379 .w_end_u_w2:
 380     lsl.l   #8, %d2
 381     move.b  (%a0)+, %d2
 382     move.w  %d2, (%a2)
 383     bra.b    .w_exit
 384
 385     /* 16-bit aligned */
 386 .w_aligned:
 387     btst.l  #1, %d0
 388     beq.b   .w_end_a_w1
 389
 390     move.w  (%a0)+, (%a2)   /* copy initial word */
 391
 392 .w_end_a_w1:
 393     moveq.l #12, %d0
 394     add.l   %a0, %d0
 395     and.l   #0xFFFFFFF0,%d0 /* d0 == first line bound */
 396     cmp.l   %a0, %d0        /* any leading longwords? */
 397     bls.b   .w_end_a_l1     /* no: skip loop */
 398
 399 .w_loop_a_l1:
 400     move.l  (%a0)+, %d1
 401     swap    %d1
 402     move.w  %d1, (%a2)
 403     swap    %d1
 404     move.w  %d1, (%a2)
 405     cmp.l   %a0, %d0        /* run up to first line bound */
 406     bhi.b   .w_loop_a_l1
 407
 408 .w_end_a_l1:
 409     lea.l   (-14, %a1), %a1 /* adjust end addr. to 16 bytes/pass */
 410
 411 .w_loop_a_line:
 412     movem.l (%a0), %d0-%d3
 413     lea.l   (16, %a0), %a0
 414     swap    %d0
 415     move.w  %d0, (%a2)
 416     swap    %d0
 417     move.w  %d0, (%a2)
 418     swap    %d1
 419     move.w  %d1, (%a2)
 420     swap    %d1
 421     move.w  %d1, (%a2)
 422     swap    %d2
 423     move.w  %d2, (%a2)
 424     swap    %d2
 425     move.w  %d2, (%a2)
 426     swap    %d3
 427     move.w  %d3, (%a2)
 428     swap    %d3
 429     move.w  %d3, (%a2)
 430     cmp.l   %a0, %a1        /* run up to last line bound */
 431     bhi.b   .w_loop_a_line
 432
 433     lea.l   (12, %a1), %a1  /* readjust for longword loop */
 434     cmp.l   %a0, %a1        /* any trailing longwords? */
 435     bls.b   .w_end_a_l2     /* no: skip loop */
 436
 437 .w_loop_a_l2:
 438     move.l  (%a0)+, %d1
 439     swap    %d1
 440     move.w  %d1, (%a2)
 441     swap    %d1
 442     move.w  %d1, (%a2)
 443     cmp.l   %a0, %a1        /* run up to first line bound */
 444     bhi.b   .w_loop_a_l2
 445
 446 .w_end_a_l2:
 447     addq.l  #2, %a1         /* back to final end address */
 448     cmp.l   %a0, %a1        /* one word left? */
 449     bls.b   .w_end_a_w2
 450
 451     move.w  (%a0)+, (%a2)   /* copy final word */
 452
 453 .w_end_a_w2:
 454
 455 .w_exit:
 456     movem.l (%sp), %d2-%d6/%a2
 457     lea.l   (24, %sp), %sp
 458     rts
 459
 460 .w_end:
 461     .size   copy_write_sectors,.w_end-copy_write_sectors