firmware/target/arm/imx31/ata-imx31.c

   1 /***************************************************************************
   2  *             __________               __   ___.
   3  *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
   4  *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
   5  *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
   6  *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
   7  *                     \/            \/     \/    \/            \/
   8  * $Id$
   9  *
  10  * Copyright (C) 2007 by Will Robertson
  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 #include "config.h"
  22 #include "cpu.h"
  23 #include "kernel.h"
  24 #include "thread.h"
  25 #include "system.h"
  26 #include "power.h"
  27 #include "panic.h"
  28 #include "ata.h"
  29 #include "ata-defines.h"
  30 #include "ata-target.h"
  31 #include "ccm-imx31.h"
  32 #ifdef HAVE_ATA_DMA
  33 #include "sdma-imx31.h"
  34 #include "mmu-imx31.h"
  35 #endif
  36
  37 /* PIO modes timing info */
  38 static const struct ata_pio_timings
  39 {
  40     uint16_t time_2w;       /* t2 during write */
  41     uint16_t time_2r;       /* t2 during read */
  42     uint8_t  time_ax;       /* tA */
  43     uint8_t  time_1;        /* t1 */
  44     uint8_t  time_4;        /* t4 */
  45     uint8_t  time_9;        /* t9 */
  46 } pio_timings[5] =
  47 {
  48     [0] =  /* PIO mode 0 */
  49     {
  50         .time_1 = 70,
  51         .time_2w = 290,
  52         .time_2r = 290,
  53         .time_ax = 35,
  54         .time_4 = 30,
  55         .time_9 = 20
  56     },
  57     [1] = /* PIO mode 1 */
  58     {
  59         .time_1 = 50,
  60         .time_2w = 290,
  61         .time_2r = 290,
  62         .time_ax = 35,
  63         .time_4 = 20,
  64         .time_9 = 15
  65     },
  66     [2] = /* PIO mode 2 */
  67     {
  68         .time_1 = 30,
  69         .time_2w = 290,
  70         .time_2r = 290,
  71         .time_ax = 35,
  72         .time_4 = 15,
  73         .time_9 = 10
  74     },
  75     [3] = /* PIO mode 3 */
  76     {
  77         .time_1 = 30,
  78         .time_2w = 80,
  79         .time_2r = 80,
  80         .time_ax = 35,
  81         .time_4 = 10,
  82         .time_9 = 10
  83     },
  84     [4] = /* PIO mode 4 */
  85     {
  86         .time_1 = 25,
  87         .time_2w = 70,
  88         .time_2r = 70,
  89         .time_ax = 35,
  90         .time_4 = 10,
  91         .time_9 = 10
  92     }
  93 };
  94
  95 /* Track first init */
  96 static bool initialized = false;
  97
  98 #ifdef HAVE_ATA_DMA
  99 /* One DMA channel for reads, the other for writes othewise one channel would
 100  * have to be reinitialized every time the direction changed. (Different
 101  * SDMA scripts are used for reading or writing) */
 102 #define ATA_DMA_CH_NUM_RD   3
 103 #define ATA_DMA_CH_NUM_WR   4
 104 /* Use default priority for these channels (1) - ATA isn't realtime urgent. */
 105 /* Maximum DMA size per buffer descriptor (32-byte aligned) */
 106 #define ATA_MAX_BD_SIZE     (65534 & ~31) /* 65504 */
 107
 108 /* Number of buffer descriptors required for a maximum sector count trasfer.
 109  * NOTE: Assumes LBA28 and 512-byte sectors! */
 110 #define ATA_BASE_BD_COUNT   ((256*512 + (ATA_MAX_BD_SIZE-1)) / ATA_MAX_BD_SIZE)
 111 #define ATA_BD_COUNT        (ATA_BASE_BD_COUNT + 2)
 112
 113 static const struct ata_mdma_timings
 114 {
 115     uint8_t time_m;         /* tM */
 116     uint8_t time_jn;        /* tH */
 117     uint8_t time_d;         /* tD */
 118     uint8_t time_k;         /* tKW */
 119 } mdma_timings[] =
 120 {
 121     [0] = /* MDMA mode 0 */
 122     {
 123         .time_m = 50,
 124         .time_jn = 20,
 125         .time_d = 215,
 126         .time_k = 215
 127     },
 128     [1] = /* MDMA mode 1 */
 129     {
 130         .time_m = 30,
 131         .time_jn = 15,
 132         .time_d = 80,
 133         .time_k = 50
 134     },
 135     [2] = /* MDMA mode 2 */
 136     {
 137         .time_m = 25,
 138         .time_jn = 10,
 139         .time_d = 70,
 140         .time_k = 25
 141     }
 142 };
 143
 144 static const struct ata_udma_timings
 145 {
 146     uint8_t time_ack;       /* tACK */
 147     uint8_t time_env;       /* tENV */
 148     uint8_t time_rpx;       /* tRP */
 149     uint8_t time_zah;       /* tZAH */
 150     uint8_t time_mlix;      /* tMLI */
 151     uint8_t time_dvh;       /* tDVH */
 152     uint8_t time_dzfs;      /* tDVS+tDVH? */
 153     uint8_t time_dvs;       /* tDVS */
 154     uint8_t time_cvh;       /* ?? */
 155     uint8_t time_ss;        /* tSS */
 156     uint8_t time_cyc;       /* tCYC */
 157 } udma_timings[] =
 158 {
 159     [0] = /* UDMA mode 0 */
 160     {
 161         .time_ack = 20,
 162         .time_env = 20,
 163         .time_rpx = 160,
 164         .time_zah = 20,
 165         .time_mlix = 20,
 166         .time_dvh = 6,
 167         .time_dzfs = 80,
 168         .time_dvs = 70,
 169         .time_cvh = 6,
 170         .time_ss = 50,
 171         .time_cyc = 114
 172     },
 173     [1] = /* UDMA mode 1 */
 174     {
 175         .time_ack = 20,
 176         .time_env = 20,
 177         .time_rpx = 125,
 178         .time_zah = 20,
 179         .time_mlix = 20,
 180         .time_dvh = 6,
 181         .time_dzfs = 63,
 182         .time_dvs = 48,
 183         .time_cvh = 6,
 184         .time_ss = 50,
 185         .time_cyc = 75
 186     },
 187     [2] = /* UDMA mode 2 */
 188     {
 189         .time_ack = 20,
 190         .time_env = 20,
 191         .time_rpx = 100,
 192         .time_zah = 20,
 193         .time_mlix = 20,
 194         .time_dvh = 6,
 195         .time_dzfs = 47,
 196         .time_dvs = 34,
 197         .time_cvh = 6,
 198         .time_ss = 50,
 199         .time_cyc = 55
 200     },
 201     [3] = /* UDMA mode 3 */
 202     {
 203         .time_ack = 20,
 204         .time_env = 20,
 205         .time_rpx = 100,
 206         .time_zah = 20,
 207         .time_mlix = 20,
 208         .time_dvh = 6,
 209         .time_dzfs = 35,
 210         .time_dvs = 20,
 211         .time_cvh = 6,
 212         .time_ss = 50,
 213         .time_cyc = 39
 214     },
 215     [4] = /* UDMA mode 4 */
 216     {
 217         .time_ack = 20,
 218         .time_env = 20,
 219         .time_rpx = 100,
 220         .time_zah = 20,
 221         .time_mlix = 20,
 222         .time_dvh = 6,
 223         .time_dzfs = 25,
 224         .time_dvs = 7,
 225         .time_cvh = 6,
 226         .time_ss = 50,
 227         .time_cyc = 25
 228     },
 229 #if 0
 230     [5] = /* UDMA mode 5 (bus clock 80MHz or higher only) */
 231     {
 232         .time_ack = 20,
 233         .time_env = 20,
 234         .time_rpx = 85,
 235         .time_zah = 20,
 236         .time_mlix = 20,
 237         .time_dvh = 6,
 238         .time_dzfs = 40,
 239         .time_dvs = 5,
 240         .time_cvh = 10,
 241         .time_ss = 50,
 242         .time_cyc = 17
 243     }
 244 #endif
 245 };
 246
 247 /** Threading **/
 248 /* Signal to tell thread when DMA is done */
 249 static struct wakeup ata_dma_wakeup;
 250
 251 /** SDMA **/
 252 /* Array of buffer descriptors for large transfers and alignnment */
 253 static struct buffer_descriptor ata_bda[ATA_BD_COUNT] NOCACHEBSS_ATTR;
 254 /* ATA channel descriptors */
 255 /* Read/write channels share buffer descriptors and callbacks */
 256 static void ata_dma_callback(void);
 257
 258 static struct channel_descriptor ata_cd_rd = /* read channel */
 259 {
 260     .bd_count = ATA_BD_COUNT,
 261     .callback = ata_dma_callback,
 262     .shp_addr = SDMA_PER_ADDR_ATA_RX,
 263     .wml      = SDMA_ATA_WML,
 264     .per_type = SDMA_PER_ATA,
 265     .tran_type = SDMA_TRAN_PER_2_EMI,
 266     .event_id1 = SDMA_REQ_ATA_TXFER_END,
 267     .event_id2 = SDMA_REQ_ATA_RX,
 268 };
 269
 270 static struct channel_descriptor ata_cd_wr = /* write channel */
 271 {
 272     .bd_count = ATA_BD_COUNT,
 273     .callback = ata_dma_callback,
 274     .shp_addr = SDMA_PER_ADDR_ATA_TX,
 275     .wml      = SDMA_ATA_WML,
 276     .per_type = SDMA_PER_ATA,
 277     .tran_type = SDMA_TRAN_EMI_2_PER,
 278     .event_id1 = SDMA_REQ_ATA_TXFER_END,
 279     .event_id2 = SDMA_REQ_ATA_TX,
 280 };
 281
 282 /* DMA channel to be started for transfer */
 283 static unsigned int current_channel = 0;
 284
 285 /** Buffers **/
 286 /* Scatter buffer for first and last 32 bytes of a non cache-aligned transfer
 287  * to cached RAM. */
 288 static uint32_t scatter_buffer[32/4*2] NOCACHEBSS_ATTR;
 289 /* Address of ends in destination buffer for unaligned reads - copied after
 290  * DMA completes. */
 291 static void *sb_dst[2] = { NULL, NULL };
 292
 293 /** Modes **/
 294 #define ATA_DMA_MWDMA    0x00000000             /* Using multiword DMA */
 295 #define ATA_DMA_UDMA     ATA_DMA_ULTRA_SELECTED /* Using Ultra DMA */
 296 #define ATA_DMA_PIO      0x80000000             /* Using PIO */
 297 #define ATA_DMA_DISABLED 0x80000001             /* DMA init error - use PIO */
 298 static unsigned long ata_dma_selected = ATA_DMA_PIO;
 299 #endif /* HAVE_ATA_DMA */
 300
 301 static unsigned int get_T(void)
 302 {
 303     /* T = ATA clock period in nanoseconds */
 304     return 1000 * 1000 * 1000 / ccm_get_ata_clk();
 305 }
 306
 307 static void ata_wait_for_idle(void)
 308 {
 309     while (!(ATA_INTERRUPT_PENDING & ATA_CONTROLLER_IDLE));
 310 }
 311
 312 /* Route the INTRQ to either the MCU or SDMA depending upon whether there is
 313  * a DMA transfer in progress. */
 314 static inline void ata_set_intrq(bool to_dma)
 315 {
 316      ATA_INTERRUPT_ENABLE =
 317         (ATA_INTERRUPT_ENABLE & ~(ATA_INTRQ1 | ATA_INTRQ2)) |
 318             (to_dma ? ATA_INTRQ1 : ATA_INTRQ2);
 319 }
 320
 321 /* Setup the timing for PIO mode */
 322 void ata_set_pio_timings(int mode)
 323 {
 324     const struct ata_pio_timings * const timings = &pio_timings[mode];
 325     unsigned int T = get_T();
 326
 327     ata_wait_for_idle();
 328
 329     ATA_TIME_1 = (timings->time_1 + T) / T;
 330     ATA_TIME_2W = (timings->time_2w + T) / T;
 331     ATA_TIME_2R = (timings->time_2r + T) / T;
 332     ATA_TIME_AX = (timings->time_ax + T) / T + 2; /* 1.5 + tAX */
 333     ATA_TIME_PIO_RDX = 1;
 334     ATA_TIME_4 = (timings->time_4 + T) / T;
 335     ATA_TIME_9 = (timings->time_9 + T) / T;
 336 }
 337
 338 void ata_reset(void)
 339 {
 340     /* Be sure we're not busy */
 341     ata_wait_for_idle();
 342
 343     ATA_INTF_CONTROL &= ~(ATA_ATA_RST | ATA_FIFO_RST);
 344     sleep(HZ/100);
 345     ATA_INTF_CONTROL = ATA_ATA_RST | ATA_FIFO_RST;
 346     sleep(HZ/100);
 347
 348     ata_wait_for_idle();
 349 }
 350
 351 void ata_enable(bool on)
 352 {
 353     /* Unconditionally clock module before writing regs */
 354     ccm_module_clock_gating(CG_ATA, CGM_ON_RUN_WAIT);
 355     ata_wait_for_idle();
 356
 357     if (on)
 358     {
 359         ATA_INTF_CONTROL = ATA_ATA_RST | ATA_FIFO_RST;
 360         sleep(HZ/100);
 361     }
 362     else
 363     {
 364         ATA_INTF_CONTROL &= ~(ATA_ATA_RST | ATA_FIFO_RST);
 365         sleep(HZ/100);
 366
 367         /* Disable off - unclock ATA module */
 368         ccm_module_clock_gating(CG_ATA, CGM_OFF);
 369     }
 370 }
 371
 372 bool ata_is_coldstart(void)
 373 {
 374     return true;
 375 }
 376
 377 #ifdef HAVE_ATA_DMA
 378 static void ata_set_mdma_timings(unsigned int mode)
 379 {
 380     const struct ata_mdma_timings * const timings = &mdma_timings[mode];
 381     unsigned int T = get_T();
 382
 383     ATA_TIME_M = (timings->time_m + T) / T;
 384     ATA_TIME_JN = (timings->time_jn + T) / T;
 385     ATA_TIME_D = (timings->time_d + T) / T;
 386     ATA_TIME_K = (timings->time_k + T) / T;
 387 }
 388
 389 static void ata_set_udma_timings(unsigned int mode)
 390 {
 391     const struct ata_udma_timings * const timings = &udma_timings[mode];
 392     unsigned int T = get_T();
 393
 394     ATA_TIME_ACK = (timings->time_ack + T) / T;
 395     ATA_TIME_ENV = (timings->time_env + T) / T;
 396     ATA_TIME_RPX = (timings->time_rpx + T) / T;
 397     ATA_TIME_ZAH = (timings->time_zah + T) / T;
 398     ATA_TIME_MLIX = (timings->time_mlix + T) / T;
 399     ATA_TIME_DVH = (timings->time_dvh + T) / T + 1;
 400     ATA_TIME_DZFS = (timings->time_dzfs + T) / T;
 401     ATA_TIME_DVS = (timings->time_dvs + T) / T;
 402     ATA_TIME_CVH = (timings->time_cvh + T) / T;
 403     ATA_TIME_SS = (timings->time_ss + T) / T;
 404     ATA_TIME_CYC = (timings->time_cyc + T) / T;
 405 }
 406
 407 void ata_dma_set_mode(unsigned char mode)
 408 {
 409     unsigned int modeidx = mode & 0x07;
 410     unsigned int dmamode = mode & 0xf8;
 411
 412     ata_wait_for_idle();
 413
 414     if (ata_dma_selected == ATA_DMA_DISABLED)
 415     {
 416         /* Configuration error - no DMA */
 417     }
 418     else if (dmamode == 0x40 && modeidx <= ATA_MAX_UDMA)
 419     {
 420         /* Using Ultra DMA */
 421         ata_set_udma_timings(dmamode);
 422         ata_dma_selected = ATA_DMA_UDMA;
 423     }
 424     else if (dmamode == 0x20 && modeidx <= ATA_MAX_MWDMA)
 425     {
 426         /* Using Multiword DMA */
 427         ata_set_mdma_timings(dmamode);
 428         ata_dma_selected = ATA_DMA_MWDMA;
 429     }
 430     else
 431     {
 432         /* Don't understand this - force PIO. */
 433         ata_dma_selected = ATA_DMA_PIO;
 434     }
 435 }
 436
 437 /* Called by SDMA when transfer is complete */
 438 static void ata_dma_callback(void)
 439 {
 440     /* Clear FIFO if not empty - shouldn't happen */
 441     while (ATA_FIFO_FILL != 0)
 442         ATA_FIFO_DATA_32;
 443
 444     /* Clear FIFO interrupts (the only ones that can be) */
 445     ATA_INTERRUPT_CLEAR = ATA_INTERRUPT_PENDING;
 446
 447     ata_set_intrq(false);           /* Return INTRQ to MCU */
 448     wakeup_signal(&ata_dma_wakeup); /* Signal waiting thread */
 449 }
 450
 451 bool ata_dma_setup(void *addr, unsigned long bytes, bool write)
 452 {
 453     struct buffer_descriptor *bd_p;
 454     unsigned char *buf;
 455
 456     if (UNLIKELY(bytes > ATA_BASE_BD_COUNT*ATA_MAX_BD_SIZE ||
 457                  (ata_dma_selected & ATA_DMA_PIO)))
 458     {
 459         /* Too much? Implies BD count should be reevaluated since this
 460          * shouldn't be reached based upon size. Otherwise we simply didn't
 461          * understand the DMA mode setup. Force PIO in both cases. */
 462         ATA_INTF_CONTROL = ATA_FIFO_RST | ATA_ATA_RST;
 463         yield();
 464         return false;
 465     }
 466
 467     bd_p = &ata_bda[0];
 468     buf = (unsigned char *)addr_virt_to_phys((unsigned long)addr);
 469     sb_dst[0] = NULL; /* Assume not needed */
 470
 471     if (write)
 472     {
 473         /* No cache alignment concerns */
 474         current_channel = ATA_DMA_CH_NUM_WR;
 475
 476         if (LIKELY(buf != addr))
 477         {
 478             /* addr is virtual */
 479             clean_dcache_range(addr, bytes);
 480         }
 481
 482         /* Setup ATA controller for DMA transmit */
 483         ATA_INTF_CONTROL = ATA_FIFO_RST | ATA_ATA_RST | ATA_FIFO_TX_EN |
 484             ATA_DMA_PENDING | ata_dma_selected | ATA_DMA_WRITE;
 485         ATA_FIFO_ALARM = SDMA_ATA_WML / 2;
 486     }
 487     else
 488     {
 489         current_channel = ATA_DMA_CH_NUM_RD;
 490
 491         /* Setup ATA controller for DMA receive */
 492         ATA_INTF_CONTROL = ATA_FIFO_RST | ATA_ATA_RST | ATA_FIFO_RCV_EN |
 493             ATA_DMA_PENDING | ata_dma_selected;
 494         ATA_FIFO_ALARM = SDMA_ATA_WML / 2;
 495
 496         if (LIKELY(buf != addr))
 497         {
 498             /* addr is virtual */
 499             dump_dcache_range(addr, bytes);
 500
 501             if ((unsigned long)addr & 31)
 502             {
 503                 /* Not cache aligned, must use scatter buffers for first and
 504                  * last 32 bytes. */
 505                 unsigned char *bufstart = buf;
 506
 507                 sb_dst[0] = addr;
 508                 bd_p->buf_addr = scatter_buffer;
 509                 bd_p->mode.count = 32;
 510                 bd_p->mode.status = BD_DONE | BD_CONT;
 511
 512                 buf += 32;
 513                 bytes -= 32;
 514                 bd_p++;
 515
 516                 while (bytes > ATA_MAX_BD_SIZE)
 517                 {
 518                     bd_p->buf_addr = buf;
 519                     bd_p->mode.count = ATA_MAX_BD_SIZE;
 520                     bd_p->mode.status = BD_DONE | BD_CONT;
 521                     buf += ATA_MAX_BD_SIZE;
 522                     bytes -= ATA_MAX_BD_SIZE;
 523                     bd_p++;
 524                 }
 525
 526                 if (bytes > 32)
 527                 {
 528                     unsigned long size = bytes - 32;
 529                     bd_p->buf_addr = buf;
 530                     bd_p->mode.count = size;
 531                     bd_p->mode.status = BD_DONE | BD_CONT;
 532                     buf += size;
 533                     bd_p++;
 534                 }
 535
 536                 /* There will be exactly 32 bytes left */
 537
 538                 /* Final buffer - wrap to base bd, interrupt */
 539                 sb_dst[1] = addr + (buf - bufstart);
 540                 bd_p->buf_addr = &scatter_buffer[32/4];
 541                 bd_p->mode.count = 32;
 542                 bd_p->mode.status = BD_DONE | BD_WRAP | BD_INTR;
 543
 544                 return true;
 545             }
 546         }
 547     }
 548
 549     /* Setup buffer descriptors for both cache-aligned reads and all write
 550      * operations. */
 551     while (bytes > ATA_MAX_BD_SIZE)
 552     {
 553         bd_p->buf_addr = buf;
 554         bd_p->mode.count = ATA_MAX_BD_SIZE;
 555         bd_p->mode.status = BD_DONE | BD_CONT;
 556         buf += ATA_MAX_BD_SIZE;
 557         bytes -= ATA_MAX_BD_SIZE;
 558         bd_p++;
 559     }
 560
 561     /* Final buffer - wrap to base bd, interrupt */
 562     bd_p->buf_addr = buf;
 563     bd_p->mode.count = bytes;
 564     bd_p->mode.status = BD_DONE | BD_WRAP | BD_INTR;
 565
 566     return true;
 567 }
 568
 569 bool ata_dma_finish(void)
 570 {
 571     unsigned int channel = current_channel;
 572     long timeout = current_tick + HZ*10;
 573
 574     current_channel = 0;
 575
 576     ata_set_intrq(true);       /* Give INTRQ to DMA */
 577     sdma_channel_run(channel); /* Kick the channel to wait for events */
 578
 579     while (1)
 580     {
 581         int oldirq;
 582
 583         if (LIKELY(wakeup_wait(&ata_dma_wakeup, HZ/2) == OBJ_WAIT_SUCCEEDED))
 584             break;
 585
 586         ata_keep_active();
 587
 588         if (TIME_BEFORE(current_tick, timeout))
 589             continue;
 590
 591         /* Epic fail - timed out - maybe. */
 592         oldirq = disable_irq_save();
 593         ata_set_intrq(false);       /* Strip INTRQ from DMA */
 594         sdma_channel_stop(channel); /* Stop DMA */
 595         restore_irq(oldirq);
 596
 597         if (wakeup_wait(&ata_dma_wakeup, TIMEOUT_NOBLOCK) == OBJ_WAIT_SUCCEEDED)
 598             break; /* DMA really did finish after timeout */
 599
 600         sdma_channel_reset(channel); /* Reset everything + clear error */
 601         return false;
 602     }
 603
 604     if (sdma_channel_is_error(channel))
 605     {
 606         /* Channel error in one or more descriptors */
 607         sdma_channel_reset(channel); /* Reset everything + clear error */
 608         return false;
 609     }
 610
 611     if (sb_dst[0] != NULL)
 612     {
 613         /* NOTE: This requires that unaligned access support be enabled! */
 614         register void *sbs = scatter_buffer;
 615         register void *sbd0 = sb_dst[0];
 616         register void *sbd1 = sb_dst[1];
 617         asm volatile(
 618             "add    r0, %1, #32         \n" /* Prefetch at DMA-direct boundaries */
 619             "mcrr   p15, 2, r0, r0, c12 \n"
 620             "mcrr   p15, 2, %2, %2, c12 \n"
 621             "ldmia  %0!, { r0-r3 }      \n" /* Copy the 32-bytes to destination */
 622             "str    r0, [%1], #4        \n" /* stmia doesn't work unaligned */
 623             "str    r1, [%1], #4        \n"
 624             "str    r2, [%1], #4        \n"
 625             "str    r3, [%1], #4        \n"
 626             "ldmia  %0!, { r0-r3 }      \n"
 627             "str    r0, [%1], #4        \n"
 628             "str    r1, [%1], #4        \n"
 629             "str    r2, [%1], #4        \n"
 630             "str    r3, [%1]            \n"
 631             "ldmia  %0!, { r0-r3 }      \n" /* Copy the 32-bytes to destination */
 632             "str    r0, [%2], #4        \n" /* stmia doesn't work unaligned */
 633             "str    r1, [%2], #4        \n"
 634             "str    r2, [%2], #4        \n"
 635             "str    r3, [%2], #4        \n"
 636             "ldmia  %0!, { r0-r3 }      \n"
 637             "str    r0, [%2], #4        \n"
 638             "str    r1, [%2], #4        \n"
 639             "str    r2, [%2], #4        \n"
 640             "str    r3, [%2]            \n"
 641             : "+r"(sbs), "+r"(sbd0), "+r"(sbd1)
 642             :
 643             : "r0", "r1", "r2", "r3");
 644     }
 645
 646     return true;
 647 }
 648 #endif /* HAVE_ATA_DMA */
 649
 650 static int ata_wait_status(unsigned status, unsigned mask, int timeout)
 651 {
 652     long busy_timeout = usec_timer() + 2;
 653     long end_tick = current_tick + timeout;
 654
 655     while (1)
 656     {
 657         if ((ATA_DRIVE_STATUS & mask) == status)
 658             return 1;
 659
 660         if (!TIME_AFTER(usec_timer(), busy_timeout))
 661             continue;
 662
 663         ata_keep_active();
 664
 665         if (TIME_AFTER(current_tick, end_tick))
 666             break;
 667
 668         sleep(0);
 669         busy_timeout = usec_timer() + 2;
 670     }
 671
 672     return 0; /* timed out */
 673 }
 674
 675 int ata_wait_for_bsy(void)
 676 {
 677     /* BSY = 0 */
 678     return ata_wait_status(0, STATUS_BSY, 30*HZ);
 679 }
 680
 681 int ata_wait_for_rdy(void)
 682 {
 683     /* RDY = 1 && BSY = 0 */
 684     return ata_wait_status(STATUS_RDY, STATUS_RDY | STATUS_BSY, 40*HZ);
 685 }
 686
 687 void ata_device_init(void)
 688 {
 689     /* Make sure we're not in reset mode */
 690     ata_enable(true);
 691
 692     if (!initialized)
 693     {
 694         ATA_INTERRUPT_ENABLE = 0;
 695         ATA_INTERRUPT_CLEAR = ATA_INTERRUPT_PENDING;
 696     }
 697
 698     ata_set_intrq(false);
 699
 700     if (initialized)
 701         return;
 702
 703     /* All modes use same tOFF/tON */
 704     ATA_TIME_OFF = 3;
 705     ATA_TIME_ON = 3;
 706
 707     /* Setup mode 0 for all by default
 708      * Mode may be switched later once identify info is ready in which
 709      * case the main driver calls back */
 710     ata_set_pio_timings(0);
 711
 712 #ifdef HAVE_ATA_DMA
 713     ata_set_mdma_timings(0);
 714     ata_set_udma_timings(0);
 715
 716     ata_dma_selected = ATA_DMA_PIO;
 717
 718     /* Called for first time at startup */
 719     wakeup_init(&ata_dma_wakeup);
 720
 721     if (!sdma_channel_init(ATA_DMA_CH_NUM_RD, &ata_cd_rd, ata_bda) ||
 722         !sdma_channel_init(ATA_DMA_CH_NUM_WR, &ata_cd_wr, ata_bda))
 723     {
 724         /* Channel init error - disable DMA forever */
 725         ata_dma_selected = ATA_DMA_DISABLED;
 726     }
 727 #endif /* HAVE_ATA_DMA */
 728
 729     initialized = true;
 730 }