firmware/drivers/ata.c

   1 /***************************************************************************
   2  *             __________               __   ___.
   3  *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
   4  *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
   5  *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
   6  *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
   7  *                     \/            \/     \/    \/            \/
   8  * $Id$
   9  *
  10  * Copyright (C) 2002 by Alan Korr
  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 <stdbool.h>
  22 #include "ata.h"
  23 #include "kernel.h"
  24 #include "thread.h"
  25 #include "led.h"
  26 #include "cpu.h"
  27 #include "system.h"
  28 #include "debug.h"
  29 #include "panic.h"
  30 #include "usb.h"
  31 #include "power.h"
  32 #include "string.h"
  33 #include "ata_idle_notify.h"
  34 #include "ata-target.h"
  35 #include "storage.h"
  36
  37 #define SECTOR_SIZE     (512)
  38
  39 #define ATA_FEATURE     ATA_ERROR
  40
  41 #define ATA_STATUS      ATA_COMMAND
  42 #define ATA_ALT_STATUS  ATA_CONTROL
  43
  44 #define SELECT_DEVICE1  0x10
  45 #define SELECT_LBA      0x40
  46
  47 #define CONTROL_nIEN    0x02
  48 #define CONTROL_SRST    0x04
  49
  50 #define CMD_READ_SECTORS           0x20
  51 #define CMD_WRITE_SECTORS          0x30
  52 #define CMD_WRITE_SECTORS_EXT      0x34
  53 #define CMD_READ_MULTIPLE          0xC4
  54 #define CMD_READ_MULTIPLE_EXT      0x29
  55 #define CMD_WRITE_MULTIPLE         0xC5
  56 #define CMD_SET_MULTIPLE_MODE      0xC6
  57 #define CMD_STANDBY_IMMEDIATE      0xE0
  58 #define CMD_STANDBY                0xE2
  59 #define CMD_IDENTIFY               0xEC
  60 #define CMD_SLEEP                  0xE6
  61 #define CMD_SET_FEATURES           0xEF
  62 #define CMD_SECURITY_FREEZE_LOCK   0xF5
  63 #ifdef HAVE_ATA_DMA
  64 #define CMD_READ_DMA               0xC8
  65 #define CMD_READ_DMA_EXT           0x25
  66 #define CMD_WRITE_DMA              0xCA
  67 #define CMD_WRITE_DMA_EXT          0x35
  68 #endif
  69
  70 /* Should all be < 0x100 (which are reserved for control messages) */
  71 #define Q_SLEEP    0
  72 #define Q_CLOSE    1
  73
  74 #define READWRITE_TIMEOUT 5*HZ
  75
  76 #ifdef HAVE_ATA_POWER_OFF
  77 #define ATA_POWER_OFF_TIMEOUT 2*HZ
  78 #endif
  79
  80 #ifdef ATA_DRIVER_CLOSE
  81 static unsigned int ata_thread_id = 0;
  82 #endif
  83
  84 #if defined(MAX_PHYS_SECTOR_SIZE) && MEM == 64
  85 /* Hack - what's the deal with 5g? */
  86 struct ata_lock
  87 {
  88     struct thread_entry *thread;
  89     int count;
  90     volatile unsigned char locked;
  91     IF_COP( struct corelock cl; )
  92 };
  93
  94 static void ata_lock_init(struct ata_lock *l)
  95 {
  96     corelock_init(&l->cl);
  97     l->locked = 0;
  98     l->count = 0;
  99     l->thread = NULL;
 100 }
 101
 102 static void ata_lock_lock(struct ata_lock *l)
 103 {
 104     struct thread_entry * const current =
 105         thread_id_entry(THREAD_ID_CURRENT);
 106
 107     if (current == l->thread)
 108     {
 109         l->count++;
 110         return;
 111     }
 112
 113     corelock_lock(&l->cl);
 114
 115     IF_PRIO( current->skip_count = -1; )
 116
 117     while (l->locked != 0)
 118     {
 119         corelock_unlock(&l->cl);
 120         switch_thread();
 121         corelock_lock(&l->cl);
 122     }
 123
 124     l->locked = 1;
 125     l->thread = current;
 126     corelock_unlock(&l->cl);
 127 }
 128
 129 static void ata_lock_unlock(struct ata_lock *l)
 130 {
 131     if (l->count > 0)
 132     {
 133         l->count--;
 134         return;
 135     }
 136
 137     corelock_lock(&l->cl);
 138
 139     IF_PRIO( l->thread->skip_count = 0; )
 140
 141     l->thread = NULL;
 142     l->locked = 0;
 143
 144     corelock_unlock(&l->cl);
 145 }
 146
 147 #define mutex           ata_lock
 148 #define mutex_init      ata_lock_init
 149 #define mutex_lock      ata_lock_lock
 150 #define mutex_unlock    ata_lock_unlock
 151 #endif /* MAX_PHYS_SECTOR_SIZE */
 152
 153 #if defined(HAVE_USBSTACK) && defined(USE_ROCKBOX_USB)
 154 #define ALLOW_USB_SPINDOWN
 155 #endif
 156
 157 static struct mutex ata_mtx SHAREDBSS_ATTR;
 158 static int ata_device; /* device 0 (master) or 1 (slave) */
 159
 160 static int spinup_time = 0;
 161 #if (CONFIG_LED == LED_REAL)
 162 static bool ata_led_enabled = true;
 163 static bool ata_led_on = false;
 164 #endif
 165 static bool spinup = false;
 166 static bool sleeping = true;
 167 static bool poweroff = false;
 168 static long sleep_timeout = 5*HZ;
 169 #ifdef HAVE_LBA48
 170 static bool lba48 = false; /* set for 48 bit addressing */
 171 #endif
 172 static long ata_stack[(DEFAULT_STACK_SIZE*3)/sizeof(long)];
 173 static const char ata_thread_name[] = "ata";
 174 static struct event_queue ata_queue SHAREDBSS_ATTR;
 175 static bool initialized = false;
 176
 177 static long last_user_activity = -1;
 178 static long last_disk_activity = -1;
 179
 180 static unsigned long total_sectors;
 181 static int multisectors; /* number of supported multisectors */
 182 static unsigned short identify_info[SECTOR_SIZE/2];
 183
 184 #ifdef MAX_PHYS_SECTOR_SIZE
 185
 186 struct sector_cache_entry {
 187     bool inuse;
 188     unsigned long sectornum;  /* logical sector */
 189     unsigned char data[MAX_PHYS_SECTOR_SIZE];
 190 };
 191 /* buffer for reading and writing large physical sectors */
 192 #define NUMCACHES 2
 193 static struct sector_cache_entry sector_cache;
 194 static int phys_sector_mult = 1;
 195 #endif
 196
 197 #ifdef HAVE_ATA_DMA
 198 static int dma_mode = 0;
 199 #endif
 200
 201 static int ata_power_on(void);
 202 static int perform_soft_reset(void);
 203 static int set_multiple_mode(int sectors);
 204 static int set_features(void);
 205
 206 STATICIRAM ICODE_ATTR int wait_for_bsy(void)
 207 {
 208     long timeout = current_tick + HZ*30;
 209
 210     do
 211     {
 212         if (!(ATA_STATUS & STATUS_BSY))
 213             return 1;
 214         last_disk_activity = current_tick;
 215         yield();
 216     } while (TIME_BEFORE(current_tick, timeout));
 217
 218     return 0; /* timeout */
 219 }
 220
 221 STATICIRAM ICODE_ATTR int wait_for_rdy(void)
 222 {
 223     long timeout;
 224
 225     if (!wait_for_bsy())
 226         return 0;
 227
 228     timeout = current_tick + HZ*10;
 229
 230     do
 231     {
 232         if (ATA_ALT_STATUS & STATUS_RDY)
 233             return 1;
 234         last_disk_activity = current_tick;
 235         yield();
 236     } while (TIME_BEFORE(current_tick, timeout));
 237
 238     return 0; /* timeout */
 239 }
 240
 241 STATICIRAM ICODE_ATTR int wait_for_start_of_transfer(void)
 242 {
 243     if (!wait_for_bsy())
 244         return 0;
 245
 246     return (ATA_ALT_STATUS & (STATUS_BSY|STATUS_DRQ)) == STATUS_DRQ;
 247 }
 248
 249 STATICIRAM ICODE_ATTR int wait_for_end_of_transfer(void)
 250 {
 251     if (!wait_for_bsy())
 252         return 0;
 253     return (ATA_ALT_STATUS &
 254             (STATUS_BSY|STATUS_RDY|STATUS_DF|STATUS_DRQ|STATUS_ERR))
 255            == STATUS_RDY;
 256 }
 257
 258 #if (CONFIG_LED == LED_REAL)
 259 /* Conditionally block LED access for the ATA driver, so the LED can be
 260  * (mis)used for other purposes */
 261 static void ata_led(bool on)
 262 {
 263     ata_led_on = on;
 264     if (ata_led_enabled)
 265         led(ata_led_on);
 266 }
 267 #else
 268 #define ata_led(on) led(on)
 269 #endif
 270
 271 #ifndef ATA_OPTIMIZED_READING
 272 STATICIRAM ICODE_ATTR void copy_read_sectors(unsigned char* buf, int wordcount)
 273 {
 274     unsigned short tmp = 0;
 275
 276     if ( (unsigned long)buf & 1)
 277     {   /* not 16-bit aligned, copy byte by byte */
 278         unsigned char* bufend = buf + wordcount*2;
 279         do
 280         {
 281             tmp = ATA_DATA;
 282 #if defined(SWAP_WORDS) || defined(ROCKBOX_LITTLE_ENDIAN)
 283             *buf++ = tmp & 0xff; /* I assume big endian */
 284             *buf++ = tmp >> 8;   /*  and don't use the SWAB16 macro */
 285 #else
 286             *buf++ = tmp >> 8;
 287             *buf++ = tmp & 0xff;
 288 #endif
 289         } while (buf < bufend); /* tail loop is faster */
 290     }
 291     else
 292     {   /* 16-bit aligned, can do faster copy */
 293         unsigned short* wbuf = (unsigned short*)buf;
 294         unsigned short* wbufend = wbuf + wordcount;
 295         do
 296         {
 297 #ifdef SWAP_WORDS
 298             *wbuf = swap16(ATA_DATA);
 299 #else
 300             *wbuf = ATA_DATA;
 301 #endif
 302         } while (++wbuf < wbufend); /* tail loop is faster */
 303     }
 304 }
 305 #endif /* !ATA_OPTIMIZED_READING */
 306
 307 #ifndef ATA_OPTIMIZED_WRITING
 308 STATICIRAM ICODE_ATTR void copy_write_sectors(const unsigned char* buf,
 309                                               int wordcount)
 310 {
 311     if ( (unsigned long)buf & 1)
 312     {   /* not 16-bit aligned, copy byte by byte */
 313         unsigned short tmp = 0;
 314         const unsigned char* bufend = buf + wordcount*2;
 315         do
 316         {
 317 #if defined(SWAP_WORDS) || defined(ROCKBOX_LITTLE_ENDIAN)
 318             tmp = (unsigned short) *buf++;
 319             tmp |= (unsigned short) *buf++ << 8;
 320             SET_16BITREG(ATA_DATA, tmp);
 321 #else
 322             tmp = (unsigned short) *buf++ << 8;
 323             tmp |= (unsigned short) *buf++;
 324             SET_16BITREG(ATA_DATA, tmp);
 325 #endif
 326         } while (buf < bufend); /* tail loop is faster */
 327     }
 328     else
 329     {   /* 16-bit aligned, can do faster copy */
 330         unsigned short* wbuf = (unsigned short*)buf;
 331         unsigned short* wbufend = wbuf + wordcount;
 332         do
 333         {
 334 #ifdef SWAP_WORDS
 335             SET_16BITREG(ATA_DATA, swap16(*wbuf));
 336 #else
 337             SET_16BITREG(ATA_DATA, *wbuf);
 338 #endif
 339         } while (++wbuf < wbufend); /* tail loop is faster */
 340     }
 341 }
 342 #endif /* !ATA_OPTIMIZED_WRITING */
 343
 344 static int ata_transfer_sectors(unsigned long start,
 345                                 int incount,
 346                                 void* inbuf,
 347                                 int write)
 348 {
 349     int ret = 0;
 350     long timeout;
 351     int count;
 352     void* buf;
 353     long spinup_start;
 354 #ifdef HAVE_ATA_DMA
 355     bool usedma = false;
 356 #endif
 357
 358 #ifndef MAX_PHYS_SECTOR_SIZE
 359     mutex_lock(&ata_mtx);
 360 #endif
 361
 362     if (start + incount > total_sectors) {
 363         ret = -1;
 364         goto error;
 365     }
 366
 367     last_disk_activity = current_tick;
 368     spinup_start = current_tick;
 369
 370     ata_led(true);
 371
 372     if ( sleeping ) {
 373         spinup = true;
 374         if (poweroff) {
 375             if (ata_power_on()) {
 376                 ret = -2;
 377                 goto error;
 378             }
 379         }
 380         else {
 381             if (perform_soft_reset()) {
 382                 ret = -2;
 383                 goto error;
 384             }
 385         }
 386     }
 387
 388     timeout = current_tick + READWRITE_TIMEOUT;
 389
 390     SET_REG(ATA_SELECT, ata_device);
 391     if (!wait_for_rdy())
 392     {
 393         ret = -3;
 394         goto error;
 395     }
 396
 397  retry:
 398     buf = inbuf;
 399     count = incount;
 400     while (TIME_BEFORE(current_tick, timeout)) {
 401         ret = 0;
 402         last_disk_activity = current_tick;
 403
 404 #ifdef HAVE_ATA_DMA
 405         /* If DMA is supported and parameters are ok for DMA, use it */
 406         if (dma_mode && ata_dma_setup(inbuf, incount * SECTOR_SIZE, write))
 407             usedma = true;
 408 #endif
 409
 410 #ifdef HAVE_LBA48
 411         if (lba48)
 412         {
 413             SET_REG(ATA_NSECTOR, count >> 8);
 414             SET_REG(ATA_NSECTOR, count & 0xff);
 415             SET_REG(ATA_SECTOR, (start >> 24) & 0xff); /* 31:24 */
 416             SET_REG(ATA_SECTOR, start & 0xff); /* 7:0 */
 417             SET_REG(ATA_LCYL, 0); /* 39:32 */
 418             SET_REG(ATA_LCYL, (start >> 8) & 0xff); /* 15:8 */
 419             SET_REG(ATA_HCYL, 0); /* 47:40 */
 420             SET_REG(ATA_HCYL, (start >> 16) & 0xff); /* 23:16 */
 421             SET_REG(ATA_SELECT, SELECT_LBA | ata_device);
 422 #ifdef HAVE_ATA_DMA
 423             if (write)
 424                 SET_REG(ATA_COMMAND, usedma ? CMD_WRITE_DMA_EXT : CMD_WRITE_MULTIPLE_EXT);
 425             else
 426                 SET_REG(ATA_COMMAND, usedma ? CMD_READ_DMA_EXT : CMD_READ_MULTIPLE_EXT);
 427 #else
 428             SET_REG(ATA_COMMAND, write ? CMD_WRITE_MULTIPLE_EXT : CMD_READ_MULTIPLE_EXT);
 429 #endif
 430         }
 431         else
 432 #endif
 433         {
 434             SET_REG(ATA_NSECTOR, count & 0xff); /* 0 means 256 sectors */
 435             SET_REG(ATA_SECTOR, start & 0xff);
 436             SET_REG(ATA_LCYL, (start >> 8) & 0xff);
 437             SET_REG(ATA_HCYL, (start >> 16) & 0xff);
 438             SET_REG(ATA_SELECT, ((start >> 24) & 0xf) | SELECT_LBA | ata_device);
 439 #ifdef HAVE_ATA_DMA
 440             if (write)
 441                 SET_REG(ATA_COMMAND, usedma ? CMD_WRITE_DMA : CMD_WRITE_MULTIPLE);
 442             else
 443                 SET_REG(ATA_COMMAND, usedma ? CMD_READ_DMA : CMD_READ_MULTIPLE);
 444 #else
 445             SET_REG(ATA_COMMAND, write ? CMD_WRITE_MULTIPLE : CMD_READ_MULTIPLE);
 446 #endif
 447         }
 448
 449         /* wait at least 400ns between writing command and reading status */
 450         __asm__ volatile ("nop");
 451         __asm__ volatile ("nop");
 452         __asm__ volatile ("nop");
 453         __asm__ volatile ("nop");
 454         __asm__ volatile ("nop");
 455
 456 #ifdef HAVE_ATA_DMA
 457         if (usedma) {
 458             if (!ata_dma_finish())
 459                 ret = -7;
 460
 461             if (ret != 0) {
 462                 perform_soft_reset();
 463                 goto retry;
 464             }
 465
 466             if (spinup) {
 467                 spinup_time = current_tick - spinup_start;
 468                 spinup = false;
 469                 sleeping = false;
 470                 poweroff = false;
 471             }
 472         }
 473         else
 474 #endif /* HAVE_ATA_DMA */
 475         {
 476             while (count) {
 477                 int sectors;
 478                 int wordcount;
 479                 int status;
 480
 481                 if (!wait_for_start_of_transfer()) {
 482                     /* We have timed out waiting for RDY and/or DRQ, possibly
 483                        because the hard drive is shaking and has problems
 484                        reading the data. We have two options:
 485                        1) Wait some more
 486                        2) Perform a soft reset and try again.
 487
 488                        We choose alternative 2.
 489                     */
 490                     perform_soft_reset();
 491                     ret = -5;
 492                     goto retry;
 493                 }
 494
 495                 if (spinup) {
 496                     spinup_time = current_tick - spinup_start;
 497                     spinup = false;
 498                     sleeping = false;
 499                     poweroff = false;
 500                 }
 501
 502                 /* read the status register exactly once per loop */
 503                 status = ATA_STATUS;
 504
 505                 if (count >= multisectors )
 506                     sectors = multisectors;
 507                 else
 508                     sectors = count;
 509
 510                 wordcount = sectors * SECTOR_SIZE / 2;
 511
 512                 if (write)
 513                     copy_write_sectors(buf, wordcount);
 514                 else
 515                     copy_read_sectors(buf, wordcount);
 516
 517                 /*
 518                   "Device errors encountered during READ MULTIPLE commands
 519                   are posted at the beginning of the block or partial block
 520                   transfer, but the DRQ bit is still set to one and the data
 521                   transfer shall take place, including transfer of corrupted
 522                   data, if any."
 523                     -- ATA specification
 524                 */
 525                 if ( status & (STATUS_BSY | STATUS_ERR | STATUS_DF) ) {
 526                     perform_soft_reset();
 527                     ret = -6;
 528                     goto retry;
 529                 }
 530
 531                 buf += sectors * SECTOR_SIZE; /* Advance one chunk of sectors */
 532                 count -= sectors;
 533
 534                 last_disk_activity = current_tick;
 535             }
 536         }
 537
 538         if(!ret && !wait_for_end_of_transfer()) {
 539             perform_soft_reset();
 540             ret = -4;
 541             goto retry;
 542         }
 543         break;
 544     }
 545
 546   error:
 547     ata_led(false);
 548 #ifndef MAX_PHYS_SECTOR_SIZE
 549     mutex_unlock(&ata_mtx);
 550 #endif
 551
 552     return ret;
 553 }
 554
 555 #ifndef MAX_PHYS_SECTOR_SIZE
 556 int ata_read_sectors(IF_MD2(int drive,)
 557                      unsigned long start,
 558                      int incount,
 559                      void* inbuf)
 560 {
 561 #ifdef HAVE_MULTIDRIVE
 562     (void)drive; /* unused for now */
 563 #endif
 564
 565     return ata_transfer_sectors(start, incount, inbuf, false);
 566 }
 567 #endif
 568
 569 #ifndef MAX_PHYS_SECTOR_SIZE
 570 int ata_write_sectors(IF_MD2(int drive,)
 571                       unsigned long start,
 572                       int count,
 573                       const void* buf)
 574 {
 575 #ifdef HAVE_MULTIDRIVE
 576     (void)drive; /* unused for now */
 577 #endif
 578
 579     return ata_transfer_sectors(start, count, (void*)buf, true);
 580 }
 581 #endif
 582
 583 #ifdef MAX_PHYS_SECTOR_SIZE
 584 static int cache_sector(unsigned long sector)
 585 {
 586     int rc;
 587
 588     sector &= ~(phys_sector_mult - 1);
 589               /* round down to physical sector boundary */
 590
 591     /* check whether the sector is already cached */
 592     if (sector_cache.inuse && (sector_cache.sectornum == sector))
 593         return 0;
 594
 595     /* not found: read the sector */
 596     sector_cache.inuse = false;
 597     rc = ata_transfer_sectors(sector, phys_sector_mult, sector_cache.data, false);
 598     if (!rc)
 599     {
 600         sector_cache.sectornum = sector;
 601         sector_cache.inuse = true;
 602     }
 603     return rc;
 604 }
 605
 606 static inline int flush_current_sector(void)
 607 {
 608     return ata_transfer_sectors(sector_cache.sectornum, phys_sector_mult,
 609                                 sector_cache.data, true);
 610 }
 611
 612 int ata_read_sectors(IF_MD2(int drive,)
 613                      unsigned long start,
 614                      int incount,
 615                      void* inbuf)
 616 {
 617     int rc = 0;
 618     int offset;
 619
 620 #ifdef HAVE_MULTIDRIVE
 621     (void)drive; /* unused for now */
 622 #endif
 623     mutex_lock(&ata_mtx);
 624
 625     offset = start & (phys_sector_mult - 1);
 626
 627     if (offset) /* first partial sector */
 628     {
 629         int partcount = MIN(incount, phys_sector_mult - offset);
 630
 631         rc = cache_sector(start);
 632         if (rc)
 633         {
 634             rc = rc * 10 - 1;
 635             goto error;
 636         }
 637         memcpy(inbuf, sector_cache.data + offset * SECTOR_SIZE,
 638                partcount * SECTOR_SIZE);
 639
 640         start += partcount;
 641         inbuf += partcount * SECTOR_SIZE;
 642         incount -= partcount;
 643     }
 644     if (incount)
 645     {
 646         offset = incount & (phys_sector_mult - 1);
 647         incount -= offset;
 648
 649         if (incount)
 650         {
 651             rc = ata_transfer_sectors(start, incount, inbuf, false);
 652             if (rc)
 653             {
 654                 rc = rc * 10 - 2;
 655                 goto error;
 656             }
 657             start += incount;
 658             inbuf += incount * SECTOR_SIZE;
 659         }
 660         if (offset)
 661         {
 662             rc = cache_sector(start);
 663             if (rc)
 664             {
 665                 rc = rc * 10 - 3;
 666                 goto error;
 667             }
 668             memcpy(inbuf, sector_cache.data, offset * SECTOR_SIZE);
 669         }
 670     }
 671
 672   error:
 673     mutex_unlock(&ata_mtx);
 674
 675     return rc;
 676 }
 677
 678 int ata_write_sectors(IF_MD2(int drive,)
 679                       unsigned long start,
 680                       int count,
 681                       const void* buf)
 682 {
 683     int rc = 0;
 684     int offset;
 685
 686 #ifdef HAVE_MULTIDRIVE
 687     (void)drive; /* unused for now */
 688 #endif
 689     mutex_lock(&ata_mtx);
 690
 691     offset = start & (phys_sector_mult - 1);
 692
 693     if (offset) /* first partial sector */
 694     {
 695         int partcount = MIN(count, phys_sector_mult - offset);
 696
 697         rc = cache_sector(start);
 698         if (rc)
 699         {
 700             rc = rc * 10 - 1;
 701             goto error;
 702         }
 703         memcpy(sector_cache.data + offset * SECTOR_SIZE, buf,
 704                partcount * SECTOR_SIZE);
 705         rc = flush_current_sector();
 706         if (rc)
 707         {
 708             rc = rc * 10 - 2;
 709             goto error;
 710         }
 711         start += partcount;
 712         buf += partcount * SECTOR_SIZE;
 713         count -= partcount;
 714     }
 715     if (count)
 716     {
 717         offset = count & (phys_sector_mult - 1);
 718         count -= offset;
 719
 720         if (count)
 721         {
 722             rc = ata_transfer_sectors(start, count, (void*)buf, true);
 723             if (rc)
 724             {
 725                 rc = rc * 10 - 3;
 726                 goto error;
 727             }
 728             start += count;
 729             buf += count * SECTOR_SIZE;
 730         }
 731         if (offset)
 732         {
 733             rc = cache_sector(start);
 734             if (rc)
 735             {
 736                 rc = rc * 10 - 4;
 737                 goto error;
 738             }
 739             memcpy(sector_cache.data, buf, offset * SECTOR_SIZE);
 740             rc = flush_current_sector();
 741             if (rc)
 742             {
 743                 rc = rc * 10 - 5;
 744                 goto error;
 745             }
 746         }
 747     }
 748
 749   error:
 750     mutex_unlock(&ata_mtx);
 751
 752     return rc;
 753 }
 754 #endif /* MAX_PHYS_SECTOR_SIZE */
 755
 756 static int check_registers(void)
 757 {
 758     int i;
 759     if ( ATA_STATUS & STATUS_BSY )
 760             return -1;
 761
 762     for (i = 0; i<64; i++) {
 763         SET_REG(ATA_NSECTOR, WRITE_PATTERN1);
 764         SET_REG(ATA_SECTOR,  WRITE_PATTERN2);
 765         SET_REG(ATA_LCYL,    WRITE_PATTERN3);
 766         SET_REG(ATA_HCYL,    WRITE_PATTERN4);
 767
 768         if (((ATA_NSECTOR & READ_PATTERN1_MASK) == READ_PATTERN1) &&
 769             ((ATA_SECTOR & READ_PATTERN2_MASK) == READ_PATTERN2) &&
 770             ((ATA_LCYL & READ_PATTERN3_MASK) == READ_PATTERN3) &&
 771             ((ATA_HCYL & READ_PATTERN4_MASK) == READ_PATTERN4))
 772             return 0;
 773     }
 774     return -2;
 775 }
 776
 777 static int freeze_lock(void)
 778 {
 779     /* does the disk support Security Mode feature set? */
 780     if (identify_info[82] & 2)
 781     {
 782         SET_REG(ATA_SELECT, ata_device);
 783
 784         if (!wait_for_rdy())
 785             return -1;
 786
 787         SET_REG(ATA_COMMAND, CMD_SECURITY_FREEZE_LOCK);
 788
 789         if (!wait_for_rdy())
 790             return -2;
 791     }
 792
 793     return 0;
 794 }
 795
 796 void ata_spindown(int seconds)
 797 {
 798     sleep_timeout = seconds * HZ;
 799 }
 800
 801 bool ata_disk_is_active(void)
 802 {
 803     return !sleeping;
 804 }
 805
 806 static int ata_perform_sleep(void)
 807 {
 808     mutex_lock(&ata_mtx);
 809
 810     SET_REG(ATA_SELECT, ata_device);
 811
 812     if(!wait_for_rdy()) {
 813         DEBUGF("ata_perform_sleep() - not RDY\n");
 814         mutex_unlock(&ata_mtx);
 815         return -1;
 816     }
 817
 818     SET_REG(ATA_COMMAND, CMD_SLEEP);
 819
 820     if (!wait_for_rdy())
 821     {
 822         DEBUGF("ata_perform_sleep() - CMD failed\n");
 823         mutex_unlock(&ata_mtx);
 824         return -2;
 825     }
 826
 827     sleeping = true;
 828     mutex_unlock(&ata_mtx);
 829     return 0;
 830 }
 831
 832 void ata_sleep(void)
 833 {
 834     queue_post(&ata_queue, Q_SLEEP, 0);
 835 }
 836
 837 void ata_sleepnow(void)
 838 {
 839     if (!spinup && !sleeping && !ata_mtx.locked && initialized)
 840     {
 841         call_storage_idle_notifys(false);
 842         ata_perform_sleep();
 843     }
 844 }
 845
 846 void ata_spin(void)
 847 {
 848     last_user_activity = current_tick;
 849 }
 850
 851 static void ata_thread(void)
 852 {
 853     static long last_sleep = 0;
 854     struct queue_event ev;
 855     static long last_seen_mtx_unlock = 0;
 856 #ifdef ALLOW_USB_SPINDOWN
 857     static bool usb_mode = false;
 858 #endif
 859
 860     while (1) {
 861         queue_wait_w_tmo(&ata_queue, &ev, HZ/2);
 862
 863         switch ( ev.id ) {
 864             case SYS_TIMEOUT:
 865                 if (!spinup && !sleeping)
 866                 {
 867                     if (!ata_mtx.locked)
 868                     {
 869                         if (!last_seen_mtx_unlock)
 870                             last_seen_mtx_unlock = current_tick;
 871                         if (TIME_AFTER(current_tick, last_seen_mtx_unlock+(HZ*2)))
 872                         {
 873 #ifdef ALLOW_USB_SPINDOWN
 874                             if(!usb_mode)
 875 #endif
 876                             {
 877                                 call_storage_idle_notifys(false);
 878                             }
 879                             last_seen_mtx_unlock = 0;
 880                         }
 881                     }
 882                     if ( sleep_timeout &&
 883                          TIME_AFTER( current_tick,
 884                                     last_user_activity + sleep_timeout ) &&
 885                          TIME_AFTER( current_tick,
 886                                     last_disk_activity + sleep_timeout ) )
 887                     {
 888 #ifdef ALLOW_USB_SPINDOWN
 889                         if(!usb_mode)
 890 #endif
 891                         {
 892                             call_storage_idle_notifys(true);
 893                         }
 894                         ata_perform_sleep();
 895                         last_sleep = current_tick;
 896                     }
 897                 }
 898
 899 #ifdef HAVE_ATA_POWER_OFF
 900                 if ( !spinup && sleeping && !poweroff &&
 901                      TIME_AFTER( current_tick, last_sleep + ATA_POWER_OFF_TIMEOUT ))
 902                 {
 903                     mutex_lock(&ata_mtx);
 904                     ide_power_enable(false);
 905                     poweroff = true;
 906                     mutex_unlock(&ata_mtx);
 907                 }
 908 #endif
 909                 break;
 910
 911 #ifndef USB_NONE
 912             case SYS_USB_CONNECTED:
 913                 /* Tell the USB thread that we are safe */
 914                 DEBUGF("ata_thread got SYS_USB_CONNECTED\n");
 915 #ifdef ALLOW_USB_SPINDOWN
 916                 usb_mode = true;
 917                 usb_acknowledge(SYS_USB_CONNECTED_ACK);
 918                 /* There is no need to force ATA power on */
 919 #else
 920                 if (sleeping) {
 921                     mutex_lock(&ata_mtx);
 922                     ata_led(true);
 923                     if (poweroff) {
 924                         ata_power_on();
 925                         poweroff = false;
 926                     }
 927                     else {
 928                         perform_soft_reset();
 929                     }
 930                     sleeping = false;
 931                     ata_led(false);
 932                     mutex_unlock(&ata_mtx);
 933                 }
 934
 935                 /* Wait until the USB cable is extracted again */
 936                 usb_acknowledge(SYS_USB_CONNECTED_ACK);
 937                 usb_wait_for_disconnect(&ata_queue);
 938 #endif
 939                 break;
 940
 941 #ifdef ALLOW_USB_SPINDOWN
 942             case SYS_USB_DISCONNECTED:
 943                 /* Tell the USB thread that we are ready again */
 944                 DEBUGF("ata_thread got SYS_USB_DISCONNECTED\n");
 945                 usb_acknowledge(SYS_USB_DISCONNECTED_ACK);
 946                 usb_mode = false;
 947                 break;
 948 #endif
 949 #endif /* USB_NONE */
 950
 951             case Q_SLEEP:
 952 #ifdef ALLOW_USB_SPINDOWN
 953                 if(!usb_mode)
 954 #endif
 955                 {
 956                     call_storage_idle_notifys(false);
 957                 }
 958                 last_disk_activity = current_tick - sleep_timeout + (HZ/2);
 959                 break;
 960
 961 #ifdef ATA_DRIVER_CLOSE
 962             case Q_CLOSE:
 963                 return;
 964 #endif
 965         }
 966     }
 967 }
 968
 969 /* Hardware reset protocol as specified in chapter 9.1, ATA spec draft v5 */
 970 static int ata_hard_reset(void)
 971 {
 972     int ret;
 973
 974     mutex_lock(&ata_mtx);
 975
 976     ata_reset();
 977
 978     /* state HRR2 */
 979     SET_REG(ATA_SELECT, ata_device); /* select the right device */
 980     ret = wait_for_bsy();
 981
 982     /* Massage the return code so it is 0 on success and -1 on failure */
 983     ret = ret?0:-1;
 984
 985     mutex_unlock(&ata_mtx);
 986
 987     return ret;
 988 }
 989
 990 static int perform_soft_reset(void)
 991 {
 992 /* If this code is allowed to run on a Nano, the next reads from the flash will
 993  * time out, so we disable it. It shouldn't be necessary anyway, since the
 994  * ATA -> Flash interface automatically sleeps almost immediately after the
 995  * last command.
 996  */
 997     int ret;
 998     int retry_count;
 999
1000     SET_REG(ATA_SELECT, SELECT_LBA | ata_device );
1001     SET_REG(ATA_CONTROL, CONTROL_nIEN|CONTROL_SRST );
1002     sleep(1); /* >= 5us */
1003
1004 #ifdef HAVE_ATA_DMA
1005     /* DMA requires INTRQ be enabled */
1006     SET_REG(ATA_CONTROL, 0);
1007 #else
1008     SET_REG(ATA_CONTROL, CONTROL_nIEN);
1009 #endif
1010     sleep(1); /* >2ms */
1011
1012     /* This little sucker can take up to 30 seconds */
1013     retry_count = 8;
1014     do
1015     {
1016         ret = wait_for_rdy();
1017     } while(!ret && retry_count--);
1018
1019     if (!ret)
1020         return -1;
1021
1022     if (set_features())
1023         return -2;
1024
1025     if (set_multiple_mode(multisectors))
1026         return -3;
1027
1028     if (freeze_lock())
1029         return -4;
1030
1031     return 0;
1032 }
1033
1034 int ata_soft_reset(void)
1035 {
1036     int ret;
1037
1038     mutex_lock(&ata_mtx);
1039
1040     ret = perform_soft_reset();
1041
1042     mutex_unlock(&ata_mtx);
1043     return ret;
1044 }
1045
1046 static int ata_power_on(void)
1047 {
1048     int rc;
1049
1050     ide_power_enable(true);
1051     sleep(HZ/4); /* allow voltage to build up */
1052
1053     /* Accessing the PP IDE controller too early after powering up the disk
1054      * makes the core hang for a short time, causing an audio dropout. This
1055      * also depends on the disk; iPod Mini G2 needs at least HZ/5 to get rid
1056      * of the dropout. Since this time isn't additive (the wait_for_bsy() in
1057      * ata_hard_reset() will shortened by the same amount), it's a good idea
1058      * to do this on all HDD based targets. */
1059
1060     if( ata_hard_reset() )
1061         return -1;
1062
1063     rc = set_features();
1064     if (rc)
1065         return rc * 10 - 2;
1066
1067     if (set_multiple_mode(multisectors))
1068         return -3;
1069
1070     if (freeze_lock())
1071         return -4;
1072
1073     return 0;
1074 }
1075
1076 static int master_slave_detect(void)
1077 {
1078     /* master? */
1079     SET_REG(ATA_SELECT, 0);
1080     if ( ATA_STATUS & (STATUS_RDY|STATUS_BSY) ) {
1081         ata_device = 0;
1082         DEBUGF("Found master harddisk\n");
1083     }
1084     else {
1085         /* slave? */
1086         SET_REG(ATA_SELECT, SELECT_DEVICE1);
1087         if ( ATA_STATUS & (STATUS_RDY|STATUS_BSY) ) {
1088             ata_device = SELECT_DEVICE1;
1089             DEBUGF("Found slave harddisk\n");
1090         }
1091         else
1092             return -1;
1093     }
1094     return 0;
1095 }
1096
1097 static int identify(void)
1098 {
1099     int i;
1100
1101     SET_REG(ATA_SELECT, ata_device);
1102
1103     if(!wait_for_rdy()) {
1104         DEBUGF("identify() - not RDY\n");
1105         return -1;
1106     }
1107     SET_REG(ATA_COMMAND, CMD_IDENTIFY);
1108
1109     if (!wait_for_start_of_transfer())
1110     {
1111         DEBUGF("identify() - CMD failed\n");
1112         return -2;
1113     }
1114
1115     for (i=0; i<SECTOR_SIZE/2; i++) {
1116         /* the IDENTIFY words are already swapped, so we need to treat
1117            this info differently that normal sector data */
1118 #if defined(ROCKBOX_BIG_ENDIAN) && !defined(SWAP_WORDS)
1119         identify_info[i] = swap16(ATA_DATA);
1120 #else
1121         identify_info[i] = ATA_DATA;
1122 #endif
1123     }
1124
1125     return 0;
1126 }
1127
1128 static int set_multiple_mode(int sectors)
1129 {
1130     SET_REG(ATA_SELECT, ata_device);
1131
1132     if(!wait_for_rdy()) {
1133         DEBUGF("set_multiple_mode() - not RDY\n");
1134         return -1;
1135     }
1136
1137     SET_REG(ATA_NSECTOR, sectors);
1138     SET_REG(ATA_COMMAND, CMD_SET_MULTIPLE_MODE);
1139
1140     if (!wait_for_rdy())
1141     {
1142         DEBUGF("set_multiple_mode() - CMD failed\n");
1143         return -2;
1144     }
1145
1146     return 0;
1147 }
1148
1149 #ifdef HAVE_ATA_DMA
1150 static int get_best_mode(unsigned short identword, int max, int modetype)
1151 {
1152     unsigned short testbit = BIT_N(max);
1153
1154     while (1) {
1155         if (identword & testbit)
1156             return max | modetype;
1157         testbit >>= 1;
1158         if (!testbit)
1159             return 0;
1160         max--;
1161     }
1162 }
1163 #endif
1164
1165 static int set_features(void)
1166 {
1167     static struct {
1168         unsigned char id_word;
1169         unsigned char id_bit;
1170         unsigned char subcommand;
1171         unsigned char parameter;
1172     } features[] = {
1173         { 83, 14, 0x03, 0 },   /* force PIO mode */
1174         { 83, 3, 0x05, 0x80 }, /* adv. power management: lowest w/o standby */
1175         { 83, 9, 0x42, 0x80 }, /* acoustic management: lowest noise */
1176         { 82, 6, 0xaa, 0 },    /* enable read look-ahead */
1177 #ifdef HAVE_ATA_DMA
1178         { 0, 0, 0x03, 0 },     /* DMA mode */
1179 #endif
1180     };
1181     int i;
1182     int pio_mode = 2;
1183
1184     /* Find out the highest supported PIO mode */
1185     if(identify_info[64] & 2)
1186         pio_mode = 4;
1187     else
1188         if(identify_info[64] & 1)
1189             pio_mode = 3;
1190
1191     /* Update the table: set highest supported pio mode that we also support */
1192     features[0].parameter = 8 + pio_mode;
1193
1194 #ifdef HAVE_ATA_DMA
1195     if (identify_info[53] & (1<<2))
1196         /* Ultra DMA mode info present, find a mode */
1197         dma_mode = get_best_mode(identify_info[88], ATA_MAX_UDMA, 0x40);
1198
1199     if (!dma_mode) {
1200         /* No UDMA mode found, try to find a multi-word DMA mode */
1201         dma_mode = get_best_mode(identify_info[63], ATA_MAX_MWDMA, 0x20);
1202         features[4].id_word = 63;
1203     }
1204     else
1205         features[4].id_word = 88;
1206
1207     features[4].id_bit = dma_mode & 7;
1208     features[4].parameter = dma_mode;
1209 #endif /* HAVE_ATA_DMA */
1210
1211     SET_REG(ATA_SELECT, ata_device);
1212
1213     if (!wait_for_rdy()) {
1214         DEBUGF("set_features() - not RDY\n");
1215         return -1;
1216     }
1217
1218     for (i=0; i < (int)(sizeof(features)/sizeof(features[0])); i++) {
1219         if (identify_info[features[i].id_word] & BIT_N(features[i].id_bit)) {
1220             SET_REG(ATA_FEATURE, features[i].subcommand);
1221             SET_REG(ATA_NSECTOR, features[i].parameter);
1222             SET_REG(ATA_COMMAND, CMD_SET_FEATURES);
1223
1224             if (!wait_for_rdy()) {
1225                 DEBUGF("set_features() - CMD failed\n");
1226                 return -10 - i;
1227             }
1228
1229             if((ATA_ALT_STATUS & STATUS_ERR) && (i != 1)) {
1230                 /* some CF cards don't like advanced powermanagement
1231                    even if they mark it as supported - go figure... */
1232                 if(ATA_ERROR & ERROR_ABRT) {
1233                     return -20 - i;
1234                 }
1235             }
1236         }
1237     }
1238
1239 #ifdef ATA_SET_DEVICE_FEATURES
1240     ata_set_pio_timings(pio_mode);
1241 #endif
1242
1243 #ifdef HAVE_ATA_DMA
1244     ata_dma_set_mode(dma_mode);
1245 #endif
1246
1247     return 0;
1248 }
1249
1250 unsigned short* ata_get_identify(void)
1251 {
1252     return identify_info;
1253 }
1254
1255 static int init_and_check(bool hard_reset)
1256 {
1257     int rc;
1258
1259     if (hard_reset)
1260     {
1261         /* This should reset both master and slave, we don't yet know what's in */
1262         ata_device = 0;
1263         if (ata_hard_reset())
1264             return -1;
1265     }
1266
1267     rc = master_slave_detect();
1268     if (rc)
1269         return -10 + rc;
1270
1271     /* symptom fix: else check_registers() below may fail */
1272     if (hard_reset && !wait_for_bsy())
1273         return -20;
1274
1275     rc = check_registers();
1276     if (rc)
1277         return -30 + rc;
1278
1279     return 0;
1280 }
1281
1282 int ata_init(void)
1283 {
1284     int rc = 0;
1285     bool coldstart;
1286
1287     if ( !initialized ) {
1288         mutex_init(&ata_mtx);
1289         queue_init(&ata_queue, true);
1290     }
1291
1292     mutex_lock(&ata_mtx);
1293
1294     /* must be called before ata_device_init() */
1295     coldstart = ata_is_coldstart();
1296     ata_led(false);
1297     ata_device_init();
1298     sleeping = false;
1299     ata_enable(true);
1300 #ifdef MAX_PHYS_SECTOR_SIZE
1301     memset(&sector_cache, 0, sizeof(sector_cache));
1302 #endif
1303
1304     if ( !initialized ) {
1305         /* First call won't have multiple thread contention - this
1306          * may return at any point without having to unlock */
1307         mutex_unlock(&ata_mtx);
1308
1309         if (!ide_powered()) /* somebody has switched it off */
1310         {
1311             ide_power_enable(true);
1312             sleep(HZ/4); /* allow voltage to build up */
1313         }
1314
1315 #ifdef HAVE_ATA_DMA
1316         /* DMA requires INTRQ be enabled */
1317         SET_REG(ATA_CONTROL, 0);
1318 #endif
1319
1320         /* first try, hard reset at cold start only */
1321         rc = init_and_check(coldstart);
1322
1323         if (rc)
1324         {   /* failed? -> second try, always with hard reset */
1325             DEBUGF("ata: init failed, retrying...\n");
1326             rc  = init_and_check(true);
1327             if (rc)
1328                 return rc;
1329         }
1330
1331         rc = identify();
1332
1333         if (rc)
1334             return -40 + rc;
1335
1336         multisectors = identify_info[47] & 0xff;
1337         if (multisectors == 0) /* Invalid multisector info, try with 16 */
1338             multisectors = 16;
1339
1340         DEBUGF("ata: %d sectors per ata request\n",multisectors);
1341
1342 #ifdef MAX_PHYS_SECTOR_SIZE
1343         /* Find out the physical sector size */
1344         if((identify_info[106] & 0xe000) == 0x6000)
1345             phys_sector_mult = BIT_N(identify_info[106] & 0x000f);
1346         else
1347             phys_sector_mult = 1;
1348
1349         DEBUGF("ata: %d logical sectors per phys sector", phys_sector_mult);
1350
1351         if (phys_sector_mult > (MAX_PHYS_SECTOR_SIZE/SECTOR_SIZE))
1352             panicf("Unsupported physical sector size: %d",
1353                    phys_sector_mult * SECTOR_SIZE);
1354 #endif
1355
1356         total_sectors = identify_info[60] | (identify_info[61] << 16);
1357
1358 #ifdef HAVE_LBA48
1359         if (identify_info[83] & 0x0400       /* 48 bit address support */
1360             && total_sectors == 0x0FFFFFFF)  /* and disk size >= 128 GiB */
1361         {                                    /* (needs BigLBA addressing) */
1362             if (identify_info[102] || identify_info[103])
1363                 panicf("Unsupported disk size: >= 2^32 sectors");
1364
1365             total_sectors = identify_info[100] | (identify_info[101] << 16);
1366             lba48 = true; /* use BigLBA */
1367         }
1368 #endif
1369         rc = freeze_lock();
1370
1371         if (rc)
1372             return -50 + rc;
1373
1374         rc = set_features();
1375         if (rc)
1376             return -60 + rc;
1377
1378         mutex_lock(&ata_mtx); /* Balance unlock below */
1379
1380         last_disk_activity = current_tick;
1381 #ifdef ATA_DRIVER_CLOSE
1382         ata_thread_id =
1383 #endif
1384         create_thread(ata_thread, ata_stack,
1385                       sizeof(ata_stack), 0, ata_thread_name
1386                       IF_PRIO(, PRIORITY_USER_INTERFACE)
1387                       IF_COP(, CPU));
1388         initialized = true;
1389
1390     }
1391     rc = set_multiple_mode(multisectors);
1392     if (rc)
1393         rc = -70 + rc;
1394
1395     mutex_unlock(&ata_mtx);
1396     return rc;
1397 }
1398
1399 #ifdef ATA_DRIVER_CLOSE
1400 void ata_close(void)
1401 {
1402     unsigned int thread_id = ata_thread_id;
1403
1404     if (thread_id == 0)
1405         return;
1406
1407     ata_thread_id = 0;
1408
1409     queue_post(&ata_queue, Q_CLOSE, 0);
1410     thread_wait(thread_id);
1411 }
1412 #endif /* ATA_DRIVER_CLOSE */
1413
1414 #if (CONFIG_LED == LED_REAL)
1415 void ata_set_led_enabled(bool enabled)
1416 {
1417     ata_led_enabled = enabled;
1418     if (ata_led_enabled)
1419         led(ata_led_on);
1420     else
1421         led(false);
1422 }
1423 #endif
1424
1425 long ata_last_disk_activity(void)
1426 {
1427     return last_disk_activity;
1428 }
1429
1430 int ata_spinup_time(void)
1431 {
1432     return spinup_time;
1433 }
1434
1435 #ifdef STORAGE_GET_INFO
1436 void ata_get_info(IF_MD2(int drive,)struct storage_info *info)
1437 {
1438     unsigned short *src,*dest;
1439     static char vendor[8];
1440     static char product[16];
1441     static char revision[4];
1442 #ifdef HAVE_MULTIDRIVE
1443     (void)drive; /* unused for now */
1444 #endif
1445     int i;
1446     info->sector_size = SECTOR_SIZE;
1447     info->num_sectors= total_sectors;
1448
1449     src = (unsigned short*)&identify_info[27];
1450     dest = (unsigned short*)vendor;
1451     for (i=0;i<4;i++)
1452         dest[i] = htobe16(src[i]);
1453     info->vendor=vendor;
1454
1455     src = (unsigned short*)&identify_info[31];
1456     dest = (unsigned short*)product;
1457     for (i=0;i<8;i++)
1458         dest[i] = htobe16(src[i]);
1459     info->product=product;
1460
1461     src = (unsigned short*)&identify_info[23];
1462     dest = (unsigned short*)revision;
1463     for (i=0;i<2;i++)
1464         dest[i] = htobe16(src[i]);
1465     info->revision=revision;
1466 }
1467 #endif
1468
1469 #ifdef HAVE_ATA_DMA
1470 /* Returns last DMA mode as set by set_features() */
1471 int ata_get_dma_mode(void)
1472 {
1473     return dma_mode;
1474 }
1475
1476 /* Needed to allow updating while waiting for DMA to complete */
1477 void ata_keep_active(void)
1478 {
1479     last_disk_activity = current_tick;
1480 }
1481 #endif
1482
1483 #ifdef CONFIG_STORAGE_MULTI
1484 int ata_num_drives(int first_drive)
1485 {
1486     /* We don't care which logical drive number(s) we have been assigned */
1487     (void)first_drive;
1488
1489     return 1;
1490 }
1491 #endif