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