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