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