git.sur5r.net Git - u-boot/blob - drivers/block/sata_dwc.c

   1 /*
   2  * sata_dwc.c
   3  *
   4  * Synopsys DesignWare Cores (DWC) SATA host driver
   5  *
   6  * Author: Mark Miesfeld <mmiesfeld@amcc.com>
   7  *
   8  * Ported from 2.6.19.2 to 2.6.25/26 by Stefan Roese <sr@denx.de>
   9  * Copyright 2008 DENX Software Engineering
  10  *
  11  * Based on versions provided by AMCC and Synopsys which are:
  12  *          Copyright 2006 Applied Micro Circuits Corporation
  13  *          COPYRIGHT (C) 2005  SYNOPSYS, INC.  ALL RIGHTS RESERVED
  14  *
  15  * This program is free software; you can redistribute
  16  * it and/or modify it under the terms of the GNU
  17  * General Public License as published by the
  18  * Free Software Foundation;  either version 2 of the  License,
  19  * or (at your option) any later version.
  20  *
  21  */
  22 /*
  23  * SATA support based on the chip canyonlands.
  24  *
  25  * 04-17-2009
  26  *              The local version of this driver for the canyonlands board
  27  *              does not use interrupts but polls the chip instead.
  28  */
  29
  30 #include <common.h>
  31 #include <command.h>
  32 #include <pci.h>
  33 #include <asm/processor.h>
  34 #include <asm/errno.h>
  35 #include <asm/io.h>
  36 #include <malloc.h>
  37 #include <ata.h>
  38 #include <linux/ctype.h>
  39
  40 #include "sata_dwc.h"
  41
  42 #define DMA_NUM_CHANS                   1
  43 #define DMA_NUM_CHAN_REGS               8
  44
  45 #define AHB_DMA_BRST_DFLT               16
  46
  47 struct dmareg {
  48         u32 low;
  49         u32 high;
  50 };
  51
  52 struct dma_chan_regs {
  53         struct dmareg sar;
  54         struct dmareg dar;
  55         struct dmareg llp;
  56         struct dmareg ctl;
  57         struct dmareg sstat;
  58         struct dmareg dstat;
  59         struct dmareg sstatar;
  60         struct dmareg dstatar;
  61         struct dmareg cfg;
  62         struct dmareg sgr;
  63         struct dmareg dsr;
  64 };
  65
  66 struct dma_interrupt_regs {
  67         struct dmareg tfr;
  68         struct dmareg block;
  69         struct dmareg srctran;
  70         struct dmareg dsttran;
  71         struct dmareg error;
  72 };
  73
  74 struct ahb_dma_regs {
  75         struct dma_chan_regs    chan_regs[DMA_NUM_CHAN_REGS];
  76         struct dma_interrupt_regs       interrupt_raw;
  77         struct dma_interrupt_regs       interrupt_status;
  78         struct dma_interrupt_regs       interrupt_mask;
  79         struct dma_interrupt_regs       interrupt_clear;
  80         struct dmareg                   statusInt;
  81         struct dmareg                   rq_srcreg;
  82         struct dmareg                   rq_dstreg;
  83         struct dmareg                   rq_sgl_srcreg;
  84         struct dmareg                   rq_sgl_dstreg;
  85         struct dmareg                   rq_lst_srcreg;
  86         struct dmareg                   rq_lst_dstreg;
  87         struct dmareg                   dma_cfg;
  88         struct dmareg                   dma_chan_en;
  89         struct dmareg                   dma_id;
  90         struct dmareg                   dma_test;
  91         struct dmareg                   res1;
  92         struct dmareg                   res2;
  93         /* DMA Comp Params
  94          * Param 6 = dma_param[0], Param 5 = dma_param[1],
  95          * Param 4 = dma_param[2] ...
  96          */
  97         struct dmareg                   dma_params[6];
  98 };
  99
 100 #define DMA_EN                  0x00000001
 101 #define DMA_DI                  0x00000000
 102 #define DMA_CHANNEL(ch)         (0x00000001 << (ch))
 103 #define DMA_ENABLE_CHAN(ch)     ((0x00000001 << (ch)) | \
 104                                 ((0x000000001 << (ch)) << 8))
 105 #define DMA_DISABLE_CHAN(ch)    (0x00000000 |   \
 106                                 ((0x000000001 << (ch)) << 8))
 107
 108 #define SATA_DWC_MAX_PORTS      1
 109 #define SATA_DWC_SCR_OFFSET     0x24
 110 #define SATA_DWC_REG_OFFSET     0x64
 111
 112 struct sata_dwc_regs {
 113         u32 fptagr;
 114         u32 fpbor;
 115         u32 fptcr;
 116         u32 dmacr;
 117         u32 dbtsr;
 118         u32 intpr;
 119         u32 intmr;
 120         u32 errmr;
 121         u32 llcr;
 122         u32 phycr;
 123         u32 physr;
 124         u32 rxbistpd;
 125         u32 rxbistpd1;
 126         u32 rxbistpd2;
 127         u32 txbistpd;
 128         u32 txbistpd1;
 129         u32 txbistpd2;
 130         u32 bistcr;
 131         u32 bistfctr;
 132         u32 bistsr;
 133         u32 bistdecr;
 134         u32 res[15];
 135         u32 testr;
 136         u32 versionr;
 137         u32 idr;
 138         u32 unimpl[192];
 139         u32 dmadr[256];
 140 };
 141
 142 #define SATA_DWC_TXFIFO_DEPTH           0x01FF
 143 #define SATA_DWC_RXFIFO_DEPTH           0x01FF
 144
 145 #define SATA_DWC_DBTSR_MWR(size)        ((size / 4) & SATA_DWC_TXFIFO_DEPTH)
 146 #define SATA_DWC_DBTSR_MRD(size)        (((size / 4) &  \
 147                                         SATA_DWC_RXFIFO_DEPTH) << 16)
 148 #define SATA_DWC_INTPR_DMAT             0x00000001
 149 #define SATA_DWC_INTPR_NEWFP            0x00000002
 150 #define SATA_DWC_INTPR_PMABRT           0x00000004
 151 #define SATA_DWC_INTPR_ERR              0x00000008
 152 #define SATA_DWC_INTPR_NEWBIST          0x00000010
 153 #define SATA_DWC_INTPR_IPF              0x10000000
 154 #define SATA_DWC_INTMR_DMATM            0x00000001
 155 #define SATA_DWC_INTMR_NEWFPM           0x00000002
 156 #define SATA_DWC_INTMR_PMABRTM          0x00000004
 157 #define SATA_DWC_INTMR_ERRM             0x00000008
 158 #define SATA_DWC_INTMR_NEWBISTM         0x00000010
 159
 160 #define SATA_DWC_DMACR_TMOD_TXCHEN      0x00000004
 161 #define SATA_DWC_DMACR_TXRXCH_CLEAR     SATA_DWC_DMACR_TMOD_TXCHEN
 162
 163 #define SATA_DWC_QCMD_MAX       32
 164
 165 #define SATA_DWC_SERROR_ERR_BITS        0x0FFF0F03
 166
 167 #define HSDEVP_FROM_AP(ap)      (struct sata_dwc_device_port*)  \
 168                                 (ap)->private_data
 169
 170 struct sata_dwc_device {
 171         struct device           *dev;
 172         struct ata_probe_ent    *pe;
 173         struct ata_host         *host;
 174         u8                      *reg_base;
 175         struct sata_dwc_regs    *sata_dwc_regs;
 176         int                     irq_dma;
 177 };
 178
 179 struct sata_dwc_device_port {
 180         struct sata_dwc_device  *hsdev;
 181         int                     cmd_issued[SATA_DWC_QCMD_MAX];
 182         u32                     dma_chan[SATA_DWC_QCMD_MAX];
 183         int                     dma_pending[SATA_DWC_QCMD_MAX];
 184 };
 185
 186 enum {
 187         SATA_DWC_CMD_ISSUED_NOT         = 0,
 188         SATA_DWC_CMD_ISSUED_PEND        = 1,
 189         SATA_DWC_CMD_ISSUED_EXEC        = 2,
 190         SATA_DWC_CMD_ISSUED_NODATA      = 3,
 191
 192         SATA_DWC_DMA_PENDING_NONE       = 0,
 193         SATA_DWC_DMA_PENDING_TX         = 1,
 194         SATA_DWC_DMA_PENDING_RX         = 2,
 195 };
 196
 197 #define msleep(a)       udelay(a * 1000)
 198 #define ssleep(a)       msleep(a * 1000)
 199
 200 static int ata_probe_timeout = (ATA_TMOUT_INTERNAL / 100);
 201
 202 enum sata_dev_state {
 203         SATA_INIT = 0,
 204         SATA_READY = 1,
 205         SATA_NODEVICE = 2,
 206         SATA_ERROR = 3,
 207 };
 208 enum sata_dev_state dev_state = SATA_INIT;
 209
 210 static struct ahb_dma_regs              *sata_dma_regs = 0;
 211 static struct ata_host                  *phost;
 212 static struct ata_port                  ap;
 213 static struct ata_port                  *pap = &ap;
 214 static struct ata_device                ata_device;
 215 static struct sata_dwc_device_port      dwc_devp;
 216
 217 static void     *scr_addr_sstatus;
 218 static u32      temp_n_block = 0;
 219
 220 static unsigned ata_exec_internal(struct ata_device *dev,
 221                         struct ata_taskfile *tf, const u8 *cdb,
 222                         int dma_dir, unsigned int buflen,
 223                         unsigned long timeout);
 224 static unsigned int ata_dev_set_feature(struct ata_device *dev,
 225                         u8 enable,u8 feature);
 226 static unsigned int ata_dev_init_params(struct ata_device *dev,
 227                         u16 heads, u16 sectors);
 228 static u8 ata_irq_on(struct ata_port *ap);
 229 static struct ata_queued_cmd *__ata_qc_from_tag(struct ata_port *ap,
 230                         unsigned int tag);
 231 static int ata_hsm_move(struct ata_port *ap, struct ata_queued_cmd *qc,
 232                         u8 status, int in_wq);
 233 static void ata_tf_to_host(struct ata_port *ap,
 234                         const struct ata_taskfile *tf);
 235 static void ata_exec_command(struct ata_port *ap,
 236                         const struct ata_taskfile *tf);
 237 static unsigned int ata_qc_issue_prot(struct ata_queued_cmd *qc);
 238 static u8 ata_check_altstatus(struct ata_port *ap);
 239 static u8 ata_check_status(struct ata_port *ap);
 240 static void ata_dev_select(struct ata_port *ap, unsigned int device,
 241                         unsigned int wait, unsigned int can_sleep);
 242 static void ata_qc_issue(struct ata_queued_cmd *qc);
 243 static void ata_tf_load(struct ata_port *ap,
 244                         const struct ata_taskfile *tf);
 245 static int ata_dev_read_sectors(unsigned char* pdata,
 246                         unsigned long datalen, u32 block, u32 n_block);
 247 static int ata_dev_write_sectors(unsigned char* pdata,
 248                         unsigned long datalen , u32 block, u32 n_block);
 249 static void ata_std_dev_select(struct ata_port *ap, unsigned int device);
 250 static void ata_qc_complete(struct ata_queued_cmd *qc);
 251 static void __ata_qc_complete(struct ata_queued_cmd *qc);
 252 static void fill_result_tf(struct ata_queued_cmd *qc);
 253 static void ata_tf_read(struct ata_port *ap, struct ata_taskfile *tf);
 254 static void ata_mmio_data_xfer(struct ata_device *dev,
 255                         unsigned char *buf,
 256                         unsigned int buflen,int do_write);
 257 static void ata_pio_task(struct ata_port *arg_ap);
 258 static void __ata_port_freeze(struct ata_port *ap);
 259 static int ata_port_freeze(struct ata_port *ap);
 260 static void ata_qc_free(struct ata_queued_cmd *qc);
 261 static void ata_pio_sectors(struct ata_queued_cmd *qc);
 262 static void ata_pio_sector(struct ata_queued_cmd *qc);
 263 static void ata_pio_queue_task(struct ata_port *ap,
 264                         void *data,unsigned long delay);
 265 static void ata_hsm_qc_complete(struct ata_queued_cmd *qc, int in_wq);
 266 static int sata_dwc_softreset(struct ata_port *ap);
 267 static int ata_dev_read_id(struct ata_device *dev, unsigned int *p_class,
 268                 unsigned int flags, u16 *id);
 269 static int check_sata_dev_state(void);
 270
 271 extern block_dev_desc_t sata_dev_desc[CONFIG_SYS_SATA_MAX_DEVICE];
 272
 273 static const struct ata_port_info sata_dwc_port_info[] = {
 274         {
 275                 .flags          = ATA_FLAG_SATA | ATA_FLAG_NO_LEGACY |
 276                                 ATA_FLAG_MMIO | ATA_FLAG_PIO_POLLING |
 277                                 ATA_FLAG_SRST | ATA_FLAG_NCQ,
 278                 .pio_mask       = 0x1f,
 279                 .mwdma_mask     = 0x07,
 280                 .udma_mask      = 0x7f,
 281         },
 282 };
 283
 284 int init_sata(int dev)
 285 {
 286         struct sata_dwc_device hsdev;
 287         struct ata_host host;
 288         struct ata_port_info pi = sata_dwc_port_info[0];
 289         struct ata_link *link;
 290         struct sata_dwc_device_port hsdevp = dwc_devp;
 291         u8 *base = 0;
 292         u8 *sata_dma_regs_addr = 0;
 293         u8 status;
 294         unsigned long base_addr = 0;
 295         int chan = 0;
 296         int rc;
 297         int i;
 298
 299         phost = &host;
 300
 301         base = (u8*)SATA_BASE_ADDR;
 302
 303         hsdev.sata_dwc_regs = (void *__iomem)(base + SATA_DWC_REG_OFFSET);
 304
 305         host.n_ports = SATA_DWC_MAX_PORTS;
 306
 307         for (i = 0; i < SATA_DWC_MAX_PORTS; i++) {
 308                 ap.pflags |= ATA_PFLAG_INITIALIZING;
 309                 ap.flags = ATA_FLAG_DISABLED;
 310                 ap.print_id = -1;
 311                 ap.ctl = ATA_DEVCTL_OBS;
 312                 ap.host = &host;
 313                 ap.last_ctl = 0xFF;
 314
 315                 link = &ap.link;
 316                 link->ap = &ap;
 317                 link->pmp = 0;
 318                 link->active_tag = ATA_TAG_POISON;
 319                 link->hw_sata_spd_limit = 0;
 320
 321                 ap.port_no = i;
 322                 host.ports[i] = &ap;
 323         }
 324
 325         ap.pio_mask = pi.pio_mask;
 326         ap.mwdma_mask = pi.mwdma_mask;
 327         ap.udma_mask = pi.udma_mask;
 328         ap.flags |= pi.flags;
 329         ap.link.flags |= pi.link_flags;
 330
 331         host.ports[0]->ioaddr.cmd_addr = base;
 332         host.ports[0]->ioaddr.scr_addr = base + SATA_DWC_SCR_OFFSET;
 333         scr_addr_sstatus = base + SATA_DWC_SCR_OFFSET;
 334
 335         base_addr = (unsigned long)base;
 336
 337         host.ports[0]->ioaddr.cmd_addr = (void *)base_addr + 0x00;
 338         host.ports[0]->ioaddr.data_addr = (void *)base_addr + 0x00;
 339
 340         host.ports[0]->ioaddr.error_addr = (void *)base_addr + 0x04;
 341         host.ports[0]->ioaddr.feature_addr = (void *)base_addr + 0x04;
 342
 343         host.ports[0]->ioaddr.nsect_addr = (void *)base_addr + 0x08;
 344
 345         host.ports[0]->ioaddr.lbal_addr = (void *)base_addr + 0x0c;
 346         host.ports[0]->ioaddr.lbam_addr = (void *)base_addr + 0x10;
 347         host.ports[0]->ioaddr.lbah_addr = (void *)base_addr + 0x14;
 348
 349         host.ports[0]->ioaddr.device_addr = (void *)base_addr + 0x18;
 350         host.ports[0]->ioaddr.command_addr = (void *)base_addr + 0x1c;
 351         host.ports[0]->ioaddr.status_addr = (void *)base_addr + 0x1c;
 352
 353         host.ports[0]->ioaddr.altstatus_addr = (void *)base_addr + 0x20;
 354         host.ports[0]->ioaddr.ctl_addr = (void *)base_addr + 0x20;
 355
 356         sata_dma_regs_addr = (u8*)SATA_DMA_REG_ADDR;
 357         sata_dma_regs = (void *__iomem)sata_dma_regs_addr;
 358
 359         status = ata_check_altstatus(&ap);
 360
 361         if (status == 0x7f) {
 362                 printf("Hard Disk not found.\n");
 363                 dev_state = SATA_NODEVICE;
 364                 rc = FALSE;
 365                 return rc;
 366         }
 367
 368         printf("Waiting for device...");
 369         i = 0;
 370         while (1) {
 371                 udelay(10000);
 372
 373                 status = ata_check_altstatus(&ap);
 374
 375                 if ((status & ATA_BUSY) == 0) {
 376                         printf("\n");
 377                         break;
 378                 }
 379
 380                 i++;
 381                 if (i > (ATA_RESET_TIME * 100)) {
 382                         printf("** TimeOUT **\n");
 383
 384                         dev_state = SATA_NODEVICE;
 385                         rc = FALSE;
 386                         return rc;
 387                 }
 388                 if ((i >= 100) && ((i % 100) == 0))
 389                         printf(".");
 390         }
 391
 392         rc = sata_dwc_softreset(&ap);
 393
 394         if (rc) {
 395                 printf("sata_dwc : error. soft reset failed\n");
 396                 return rc;
 397         }
 398
 399         for (chan = 0; chan < DMA_NUM_CHANS; chan++) {
 400                 out_le32(&(sata_dma_regs->interrupt_mask.error.low),
 401                                 DMA_DISABLE_CHAN(chan));
 402
 403                 out_le32(&(sata_dma_regs->interrupt_mask.tfr.low),
 404                                 DMA_DISABLE_CHAN(chan));
 405         }
 406
 407         out_le32(&(sata_dma_regs->dma_cfg.low), DMA_DI);
 408
 409         out_le32(&hsdev.sata_dwc_regs->intmr,
 410                 SATA_DWC_INTMR_ERRM |
 411                 SATA_DWC_INTMR_PMABRTM);
 412
 413         /* Unmask the error bits that should trigger
 414          * an error interrupt by setting the error mask register.
 415          */
 416         out_le32(&hsdev.sata_dwc_regs->errmr, SATA_DWC_SERROR_ERR_BITS);
 417
 418         hsdev.host = ap.host;
 419         memset(&hsdevp, 0, sizeof(hsdevp));
 420         hsdevp.hsdev = &hsdev;
 421
 422         for (i = 0; i < SATA_DWC_QCMD_MAX; i++)
 423                 hsdevp.cmd_issued[i] = SATA_DWC_CMD_ISSUED_NOT;
 424
 425         out_le32((void __iomem *)scr_addr_sstatus + 4,
 426                 in_le32((void __iomem *)scr_addr_sstatus + 4));
 427
 428         rc = 0;
 429         return rc;
 430 }
 431
 432 static u8 ata_check_altstatus(struct ata_port *ap)
 433 {
 434         u8 val = 0;
 435         val = readb(ap->ioaddr.altstatus_addr);
 436         return val;
 437 }
 438
 439 static int sata_dwc_softreset(struct ata_port *ap)
 440 {
 441         u8 nsect,lbal = 0;
 442         u8 tmp = 0;
 443         struct ata_ioports *ioaddr = &ap->ioaddr;
 444
 445         in_le32((void *)ap->ioaddr.scr_addr + (SCR_ERROR * 4));
 446
 447         writeb(0x55, ioaddr->nsect_addr);
 448         writeb(0xaa, ioaddr->lbal_addr);
 449         writeb(0xaa, ioaddr->nsect_addr);
 450         writeb(0x55, ioaddr->lbal_addr);
 451         writeb(0x55, ioaddr->nsect_addr);
 452         writeb(0xaa, ioaddr->lbal_addr);
 453
 454         nsect = readb(ioaddr->nsect_addr);
 455         lbal = readb(ioaddr->lbal_addr);
 456
 457         if ((nsect == 0x55) && (lbal == 0xaa)) {
 458                 printf("Device found\n");
 459         } else {
 460                 printf("No device found\n");
 461                 dev_state = SATA_NODEVICE;
 462                 return FALSE;
 463         }
 464
 465         tmp = ATA_DEVICE_OBS;
 466         writeb(tmp, ioaddr->device_addr);
 467         writeb(ap->ctl, ioaddr->ctl_addr);
 468
 469         udelay(200);
 470
 471         writeb(ap->ctl | ATA_SRST, ioaddr->ctl_addr);
 472
 473         udelay(200);
 474         writeb(ap->ctl, ioaddr->ctl_addr);
 475
 476         msleep(150);
 477         ata_check_status(ap);
 478
 479         msleep(50);
 480         ata_check_status(ap);
 481
 482         while (1) {
 483                 u8 status = ata_check_status(ap);
 484
 485                 if (!(status & ATA_BUSY))
 486                         break;
 487
 488                 printf("Hard Disk status is BUSY.\n");
 489                 msleep(50);
 490         }
 491
 492         tmp = ATA_DEVICE_OBS;
 493         writeb(tmp, ioaddr->device_addr);
 494
 495         nsect = readb(ioaddr->nsect_addr);
 496         lbal = readb(ioaddr->lbal_addr);
 497
 498         return 0;
 499 }
 500
 501 static u8 ata_check_status(struct ata_port *ap)
 502 {
 503         u8 val = 0;
 504         val = readb(ap->ioaddr.status_addr);
 505         return val;
 506 }
 507
 508 static int ata_id_has_hipm(const u16 *id)
 509 {
 510         u16 val = id[76];
 511
 512         if (val == 0 || val == 0xffff)
 513                 return -1;
 514
 515         return val & (1 << 9);
 516 }
 517
 518 static int ata_id_has_dipm(const u16 *id)
 519 {
 520         u16 val = id[78];
 521
 522         if (val == 0 || val == 0xffff)
 523                 return -1;
 524
 525         return val & (1 << 3);
 526 }
 527
 528 int scan_sata(int dev)
 529 {
 530         int i;
 531         int rc;
 532         u8 status;
 533         const u16 *id;
 534         struct ata_device *ata_dev = &ata_device;
 535         unsigned long pio_mask, mwdma_mask;
 536         char revbuf[7];
 537         u16 iobuf[ATA_SECTOR_WORDS];
 538
 539         memset(iobuf, 0, sizeof(iobuf));
 540
 541         if (dev_state == SATA_NODEVICE)
 542                 return 1;
 543
 544         printf("Waiting for device...");
 545         i = 0;
 546         while (1) {
 547                 udelay(10000);
 548
 549                 status = ata_check_altstatus(&ap);
 550
 551                 if ((status & ATA_BUSY) == 0) {
 552                         printf("\n");
 553                         break;
 554                 }
 555
 556                 i++;
 557                 if (i > (ATA_RESET_TIME * 100)) {
 558                         printf("** TimeOUT **\n");
 559
 560                         dev_state = SATA_NODEVICE;
 561                         return 1;
 562                 }
 563                 if ((i >= 100) && ((i % 100) == 0))
 564                         printf(".");
 565         }
 566
 567         udelay(1000);
 568
 569         rc = ata_dev_read_id(ata_dev, &ata_dev->class,
 570                         ATA_READID_POSTRESET,ata_dev->id);
 571         if (rc) {
 572                 printf("sata_dwc : error. failed sata scan\n");
 573                 return 1;
 574         }
 575
 576         /* SATA drives indicate we have a bridge. We don't know which
 577          * end of the link the bridge is which is a problem
 578          */
 579         if (ata_id_is_sata(ata_dev->id))
 580                 ap.cbl = ATA_CBL_SATA;
 581
 582         id = ata_dev->id;
 583
 584         ata_dev->flags &= ~ATA_DFLAG_CFG_MASK;
 585         ata_dev->max_sectors = 0;
 586         ata_dev->cdb_len = 0;
 587         ata_dev->n_sectors = 0;
 588         ata_dev->cylinders = 0;
 589         ata_dev->heads = 0;
 590         ata_dev->sectors = 0;
 591
 592         if (id[ATA_ID_FIELD_VALID] & (1 << 1)) {
 593                 pio_mask = id[ATA_ID_PIO_MODES] & 0x03;
 594                 pio_mask <<= 3;
 595                 pio_mask |= 0x7;
 596         } else {
 597                 /* If word 64 isn't valid then Word 51 high byte holds
 598                  * the PIO timing number for the maximum. Turn it into
 599                  * a mask.
 600                  */
 601                 u8 mode = (id[ATA_ID_OLD_PIO_MODES] >> 8) & 0xFF;
 602                 if (mode < 5) {
 603                         pio_mask = (2 << mode) - 1;
 604                 } else {
 605                         pio_mask = 1;
 606                 }
 607         }
 608
 609         mwdma_mask = id[ATA_ID_MWDMA_MODES] & 0x07;
 610
 611         if (ata_id_is_cfa(id)) {
 612                 int pio = id[163] & 0x7;
 613                 int dma = (id[163] >> 3) & 7;
 614
 615                 if (pio)
 616                         pio_mask |= (1 << 5);
 617                 if (pio > 1)
 618                         pio_mask |= (1 << 6);
 619                 if (dma)
 620                         mwdma_mask |= (1 << 3);
 621                 if (dma > 1)
 622                         mwdma_mask |= (1 << 4);
 623         }
 624
 625         if (ata_dev->class == ATA_DEV_ATA) {
 626                 if (ata_id_is_cfa(id)) {
 627                         if (id[162] & 1)
 628                                 printf("supports DRM functions and may "
 629                                         "not be fully accessable.\n");
 630                         sprintf(revbuf, "%s", "CFA");
 631                 } else {
 632                         if (ata_id_has_tpm(id))
 633                                 printf("supports DRM functions and may "
 634                                                 "not be fully accessable.\n");
 635                 }
 636
 637                 ata_dev->n_sectors = ata_id_n_sectors((u16*)id);
 638
 639                 if (ata_dev->id[59] & 0x100)
 640                         ata_dev->multi_count = ata_dev->id[59] & 0xff;
 641
 642                 if (ata_id_has_lba(id)) {
 643                         char ncq_desc[20];
 644
 645                         ata_dev->flags |= ATA_DFLAG_LBA;
 646                         if (ata_id_has_lba48(id)) {
 647                                 ata_dev->flags |= ATA_DFLAG_LBA48;
 648
 649                                 if (ata_dev->n_sectors >= (1UL << 28) &&
 650                                         ata_id_has_flush_ext(id))
 651                                         ata_dev->flags |= ATA_DFLAG_FLUSH_EXT;
 652                         }
 653                         if (!ata_id_has_ncq(ata_dev->id))
 654                                 ncq_desc[0] = '\0';
 655
 656                         if (ata_dev->horkage & ATA_HORKAGE_NONCQ)
 657                                 sprintf(ncq_desc, "%s", "NCQ (not used)");
 658
 659                         if (ap.flags & ATA_FLAG_NCQ)
 660                                 ata_dev->flags |= ATA_DFLAG_NCQ;
 661                 }
 662                 ata_dev->cdb_len = 16;
 663         }
 664         ata_dev->max_sectors = ATA_MAX_SECTORS;
 665         if (ata_dev->flags & ATA_DFLAG_LBA48)
 666                 ata_dev->max_sectors = ATA_MAX_SECTORS_LBA48;
 667
 668         if (!(ata_dev->horkage & ATA_HORKAGE_IPM)) {
 669                 if (ata_id_has_hipm(ata_dev->id))
 670                         ata_dev->flags |= ATA_DFLAG_HIPM;
 671                 if (ata_id_has_dipm(ata_dev->id))
 672                         ata_dev->flags |= ATA_DFLAG_DIPM;
 673         }
 674
 675         if ((ap.cbl == ATA_CBL_SATA) && (!ata_id_is_sata(ata_dev->id))) {
 676                 ata_dev->udma_mask &= ATA_UDMA5;
 677                 ata_dev->max_sectors = ATA_MAX_SECTORS;
 678         }
 679
 680         if (ata_dev->horkage & ATA_HORKAGE_DIAGNOSTIC) {
 681                 printf("Drive reports diagnostics failure."
 682                                 "This may indicate a drive\n");
 683                 printf("fault or invalid emulation."
 684                                 "Contact drive vendor for information.\n");
 685         }
 686
 687         rc = check_sata_dev_state();
 688
 689         ata_id_c_string(ata_dev->id,
 690                         (unsigned char *)sata_dev_desc[dev].revision,
 691                          ATA_ID_FW_REV, sizeof(sata_dev_desc[dev].revision));
 692         ata_id_c_string(ata_dev->id,
 693                         (unsigned char *)sata_dev_desc[dev].vendor,
 694                          ATA_ID_PROD, sizeof(sata_dev_desc[dev].vendor));
 695         ata_id_c_string(ata_dev->id,
 696                         (unsigned char *)sata_dev_desc[dev].product,
 697                          ATA_ID_SERNO, sizeof(sata_dev_desc[dev].product));
 698
 699         sata_dev_desc[dev].lba = (u32) ata_dev->n_sectors;
 700
 701 #ifdef CONFIG_LBA48
 702         if (ata_dev->id[83] & (1 << 10)) {
 703                 sata_dev_desc[dev].lba48 = 1;
 704         } else {
 705                 sata_dev_desc[dev].lba48 = 0;
 706         }
 707 #endif
 708
 709         return 0;
 710 }
 711
 712 static u8 ata_busy_wait(struct ata_port *ap,
 713                 unsigned int bits,unsigned int max)
 714 {
 715         u8 status;
 716
 717         do {
 718                 udelay(10);
 719                 status = ata_check_status(ap);
 720                 max--;
 721         } while (status != 0xff && (status & bits) && (max > 0));
 722
 723         return status;
 724 }
 725
 726 static int ata_dev_read_id(struct ata_device *dev, unsigned int *p_class,
 727                 unsigned int flags, u16 *id)
 728 {
 729         struct ata_port *ap = pap;
 730         unsigned int class = *p_class;
 731         struct ata_taskfile tf;
 732         unsigned int err_mask = 0;
 733         const char *reason;
 734         int may_fallback = 1, tried_spinup = 0;
 735         u8 status;
 736         int rc;
 737
 738         status = ata_busy_wait(ap, ATA_BUSY, 30000);
 739         if (status & ATA_BUSY) {
 740                 printf("BSY = 0 check. timeout.\n");
 741                 rc = FALSE;
 742                 return rc;
 743         }
 744
 745         ata_dev_select(ap, dev->devno, 1, 1);
 746
 747 retry:
 748         memset(&tf, 0, sizeof(tf));
 749         ap->print_id = 1;
 750         ap->flags &= ~ATA_FLAG_DISABLED;
 751         tf.ctl = ap->ctl;
 752         tf.device = ATA_DEVICE_OBS;
 753         tf.command = ATA_CMD_ID_ATA;
 754         tf.protocol = ATA_PROT_PIO;
 755
 756         /* Some devices choke if TF registers contain garbage.  Make
 757          * sure those are properly initialized.
 758          */
 759         tf.flags |= ATA_TFLAG_ISADDR | ATA_TFLAG_DEVICE;
 760
 761         /* Device presence detection is unreliable on some
 762          * controllers.  Always poll IDENTIFY if available.
 763          */
 764         tf.flags |= ATA_TFLAG_POLLING;
 765
 766         temp_n_block = 1;
 767
 768         err_mask = ata_exec_internal(dev, &tf, NULL, DMA_FROM_DEVICE,
 769                                         sizeof(id[0]) * ATA_ID_WORDS, 0);
 770
 771         if (err_mask) {
 772                 if (err_mask & AC_ERR_NODEV_HINT) {
 773                         printf("NODEV after polling detection\n");
 774                         return -ENOENT;
 775                 }
 776
 777                 if ((err_mask == AC_ERR_DEV) && (tf.feature & ATA_ABORTED)) {
 778                         /* Device or controller might have reported
 779                          * the wrong device class.  Give a shot at the
 780                          * other IDENTIFY if the current one is
 781                          * aborted by the device.
 782                          */
 783                         if (may_fallback) {
 784                                 may_fallback = 0;
 785
 786                                 if (class == ATA_DEV_ATA) {
 787                                         class = ATA_DEV_ATAPI;
 788                                 } else {
 789                                         class = ATA_DEV_ATA;
 790                                 }
 791                                 goto retry;
 792                         }
 793                         /* Control reaches here iff the device aborted
 794                          * both flavors of IDENTIFYs which happens
 795                          * sometimes with phantom devices.
 796                          */
 797                         printf("both IDENTIFYs aborted, assuming NODEV\n");
 798                         return -ENOENT;
 799                 }
 800                 rc = -EIO;
 801                 reason = "I/O error";
 802                 goto err_out;
 803         }
 804
 805         /* Falling back doesn't make sense if ID data was read
 806          * successfully at least once.
 807          */
 808         may_fallback = 0;
 809
 810         unsigned int id_cnt;
 811
 812         for (id_cnt = 0; id_cnt < ATA_ID_WORDS; id_cnt++)
 813                 id[id_cnt] = le16_to_cpu(id[id_cnt]);
 814
 815
 816         rc = -EINVAL;
 817         reason = "device reports invalid type";
 818
 819         if (class == ATA_DEV_ATA) {
 820                 if (!ata_id_is_ata(id) && !ata_id_is_cfa(id))
 821                         goto err_out;
 822         } else {
 823                 if (ata_id_is_ata(id))
 824                         goto err_out;
 825         }
 826         if (!tried_spinup && (id[2] == 0x37c8 || id[2] == 0x738c)) {
 827                 tried_spinup = 1;
 828                 /*
 829                  * Drive powered-up in standby mode, and requires a specific
 830                  * SET_FEATURES spin-up subcommand before it will accept
 831                  * anything other than the original IDENTIFY command.
 832                  */
 833                 err_mask = ata_dev_set_feature(dev, SETFEATURES_SPINUP, 0);
 834                 if (err_mask && id[2] != 0x738c) {
 835                         rc = -EIO;
 836                         reason = "SPINUP failed";
 837                         goto err_out;
 838                 }
 839                 /*
 840                  * If the drive initially returned incomplete IDENTIFY info,
 841                  * we now must reissue the IDENTIFY command.
 842                  */
 843                 if (id[2] == 0x37c8)
 844                         goto retry;
 845         }
 846
 847         if ((flags & ATA_READID_POSTRESET) && class == ATA_DEV_ATA) {
 848                 /*
 849                  * The exact sequence expected by certain pre-ATA4 drives is:
 850                  * SRST RESET
 851                  * IDENTIFY (optional in early ATA)
 852                  * INITIALIZE DEVICE PARAMETERS (later IDE and ATA)
 853                  * anything else..
 854                  * Some drives were very specific about that exact sequence.
 855                  *
 856                  * Note that ATA4 says lba is mandatory so the second check
 857                  * shoud never trigger.
 858                  */
 859                 if (ata_id_major_version(id) < 4 || !ata_id_has_lba(id)) {
 860                         err_mask = ata_dev_init_params(dev, id[3], id[6]);
 861                         if (err_mask) {
 862                                 rc = -EIO;
 863                                 reason = "INIT_DEV_PARAMS failed";
 864                                 goto err_out;
 865                         }
 866
 867                         /* current CHS translation info (id[53-58]) might be
 868                          * changed. reread the identify device info.
 869                          */
 870                         flags &= ~ATA_READID_POSTRESET;
 871                         goto retry;
 872                 }
 873         }
 874
 875         *p_class = class;
 876         return 0;
 877
 878 err_out:
 879         printf("failed to READ ID (%s, err_mask=0x%x)\n", reason, err_mask);
 880         return rc;
 881 }
 882
 883 static u8 ata_wait_idle(struct ata_port *ap)
 884 {
 885         u8 status = ata_busy_wait(ap, ATA_BUSY | ATA_DRQ, 1000);
 886         return status;
 887 }
 888
 889 static void ata_dev_select(struct ata_port *ap, unsigned int device,
 890                 unsigned int wait, unsigned int can_sleep)
 891 {
 892         if (wait)
 893                 ata_wait_idle(ap);
 894
 895         ata_std_dev_select(ap, device);
 896
 897         if (wait)
 898                 ata_wait_idle(ap);
 899 }
 900
 901 static void ata_std_dev_select(struct ata_port *ap, unsigned int device)
 902 {
 903         u8 tmp;
 904
 905         if (device == 0) {
 906                 tmp = ATA_DEVICE_OBS;
 907         } else {
 908                 tmp = ATA_DEVICE_OBS | ATA_DEV1;
 909         }
 910
 911         writeb(tmp, ap->ioaddr.device_addr);
 912
 913         readb(ap->ioaddr.altstatus_addr);
 914
 915         udelay(1);
 916 }
 917
 918 static int waiting_for_reg_state(volatile u8 *offset,
 919                                 int timeout_msec,
 920                                 u32 sign)
 921 {
 922         int i;
 923         u32 status;
 924
 925         for (i = 0; i < timeout_msec; i++) {
 926                 status = readl(offset);
 927                 if ((status & sign) != 0)
 928                         break;
 929                 msleep(1);
 930         }
 931
 932         return (i < timeout_msec) ? 0 : -1;
 933 }
 934
 935 static void ata_qc_reinit(struct ata_queued_cmd *qc)
 936 {
 937         qc->dma_dir = DMA_NONE;
 938         qc->flags = 0;
 939         qc->nbytes = qc->extrabytes = qc->curbytes = 0;
 940         qc->n_elem = 0;
 941         qc->err_mask = 0;
 942         qc->sect_size = ATA_SECT_SIZE;
 943         qc->nbytes = ATA_SECT_SIZE * temp_n_block;
 944
 945         memset(&qc->tf, 0, sizeof(qc->tf));
 946         qc->tf.ctl = 0;
 947         qc->tf.device = ATA_DEVICE_OBS;
 948
 949         qc->result_tf.command = ATA_DRDY;
 950         qc->result_tf.feature = 0;
 951 }
 952
 953 struct ata_queued_cmd *__ata_qc_from_tag(struct ata_port *ap,
 954                                         unsigned int tag)
 955 {
 956         if (tag < ATA_MAX_QUEUE)
 957                 return &ap->qcmd[tag];
 958         return NULL;
 959 }
 960
 961 static void __ata_port_freeze(struct ata_port *ap)
 962 {
 963         printf("set port freeze.\n");
 964         ap->pflags |= ATA_PFLAG_FROZEN;
 965 }
 966
 967 static int ata_port_freeze(struct ata_port *ap)
 968 {
 969         __ata_port_freeze(ap);
 970         return 0;
 971 }
 972
 973 unsigned ata_exec_internal(struct ata_device *dev,
 974                         struct ata_taskfile *tf, const u8 *cdb,
 975                         int dma_dir, unsigned int buflen,
 976                         unsigned long timeout)
 977 {
 978         struct ata_link *link = dev->link;
 979         struct ata_port *ap = pap;
 980         struct ata_queued_cmd *qc;
 981         unsigned int tag, preempted_tag;
 982         u32 preempted_sactive, preempted_qc_active;
 983         int preempted_nr_active_links;
 984         unsigned int err_mask;
 985         int rc = 0;
 986         u8 status;
 987
 988         status = ata_busy_wait(ap, ATA_BUSY, 300000);
 989         if (status & ATA_BUSY) {
 990                 printf("BSY = 0 check. timeout.\n");
 991                 rc = FALSE;
 992                 return rc;
 993         }
 994
 995         if (ap->pflags & ATA_PFLAG_FROZEN)
 996                 return AC_ERR_SYSTEM;
 997
 998         tag = ATA_TAG_INTERNAL;
 999
1000         if (test_and_set_bit(tag, &ap->qc_allocated)) {
1001                 rc = FALSE;
1002                 return rc;
1003         }
1004
1005         qc = __ata_qc_from_tag(ap, tag);
1006         qc->tag = tag;
1007         qc->ap = ap;
1008         qc->dev = dev;
1009
1010         ata_qc_reinit(qc);
1011
1012         preempted_tag = link->active_tag;
1013         preempted_sactive = link->sactive;
1014         preempted_qc_active = ap->qc_active;
1015         preempted_nr_active_links = ap->nr_active_links;
1016         link->active_tag = ATA_TAG_POISON;
1017         link->sactive = 0;
1018         ap->qc_active = 0;
1019         ap->nr_active_links = 0;
1020
1021         qc->tf = *tf;
1022         if (cdb)
1023                 memcpy(qc->cdb, cdb, ATAPI_CDB_LEN);
1024         qc->flags |= ATA_QCFLAG_RESULT_TF;
1025         qc->dma_dir = dma_dir;
1026         qc->private_data = 0;
1027
1028         ata_qc_issue(qc);
1029
1030         if (!timeout)
1031                 timeout = ata_probe_timeout * 1000 / HZ;
1032
1033         status = ata_busy_wait(ap, ATA_BUSY, 30000);
1034         if (status & ATA_BUSY) {
1035                 printf("BSY = 0 check. timeout.\n");
1036                 printf("altstatus = 0x%x.\n", status);
1037                 qc->err_mask |= AC_ERR_OTHER;
1038                 return qc->err_mask;
1039         }
1040
1041         if (waiting_for_reg_state(ap->ioaddr.altstatus_addr, 1000, 0x8)) {
1042                 u8 status = 0;
1043                 u8 errorStatus = 0;
1044
1045                 status = readb(ap->ioaddr.altstatus_addr);
1046                 if ((status & 0x01) != 0) {
1047                         errorStatus = readb(ap->ioaddr.feature_addr);
1048                         if (errorStatus == 0x04 &&
1049                                 qc->tf.command == ATA_CMD_PIO_READ_EXT){
1050                                 printf("Hard Disk doesn't support LBA48\n");
1051                                 dev_state = SATA_ERROR;
1052                                 qc->err_mask |= AC_ERR_OTHER;
1053                                 return qc->err_mask;
1054                         }
1055                 }
1056                 qc->err_mask |= AC_ERR_OTHER;
1057                 return qc->err_mask;
1058         }
1059
1060         status = ata_busy_wait(ap, ATA_BUSY, 10);
1061         if (status & ATA_BUSY) {
1062                 printf("BSY = 0 check. timeout.\n");
1063                 qc->err_mask |= AC_ERR_OTHER;
1064                 return qc->err_mask;
1065         }
1066
1067         ata_pio_task(ap);
1068
1069         if (!rc) {
1070                 if (qc->flags & ATA_QCFLAG_ACTIVE) {
1071                         qc->err_mask |= AC_ERR_TIMEOUT;
1072                         ata_port_freeze(ap);
1073                 }
1074         }
1075
1076         if (qc->flags & ATA_QCFLAG_FAILED) {
1077                 if (qc->result_tf.command & (ATA_ERR | ATA_DF))
1078                         qc->err_mask |= AC_ERR_DEV;
1079
1080                 if (!qc->err_mask)
1081                         qc->err_mask |= AC_ERR_OTHER;
1082
1083                 if (qc->err_mask & ~AC_ERR_OTHER)
1084                         qc->err_mask &= ~AC_ERR_OTHER;
1085         }
1086
1087         *tf = qc->result_tf;
1088         err_mask = qc->err_mask;
1089         ata_qc_free(qc);
1090         link->active_tag = preempted_tag;
1091         link->sactive = preempted_sactive;
1092         ap->qc_active = preempted_qc_active;
1093         ap->nr_active_links = preempted_nr_active_links;
1094
1095         if (ap->flags & ATA_FLAG_DISABLED) {
1096                 err_mask |= AC_ERR_SYSTEM;
1097                 ap->flags &= ~ATA_FLAG_DISABLED;
1098         }
1099
1100         return err_mask;
1101 }
1102
1103 static void ata_qc_issue(struct ata_queued_cmd *qc)
1104 {
1105         struct ata_port *ap = qc->ap;
1106         struct ata_link *link = qc->dev->link;
1107         u8 prot = qc->tf.protocol;
1108
1109         if (ata_is_ncq(prot)) {
1110                 if (!link->sactive)
1111                         ap->nr_active_links++;
1112                 link->sactive |= 1 << qc->tag;
1113         } else {
1114                 ap->nr_active_links++;
1115                 link->active_tag = qc->tag;
1116         }
1117
1118         qc->flags |= ATA_QCFLAG_ACTIVE;
1119         ap->qc_active |= 1 << qc->tag;
1120
1121         if (qc->dev->flags & ATA_DFLAG_SLEEPING) {
1122                 msleep(1);
1123                 return;
1124         }
1125
1126         qc->err_mask |= ata_qc_issue_prot(qc);
1127         if (qc->err_mask)
1128                 goto err;
1129
1130         return;
1131 err:
1132         ata_qc_complete(qc);
1133 }
1134
1135 static unsigned int ata_qc_issue_prot(struct ata_queued_cmd *qc)
1136 {
1137         struct ata_port *ap = qc->ap;
1138
1139         if (ap->flags & ATA_FLAG_PIO_POLLING) {
1140                 switch (qc->tf.protocol) {
1141                 case ATA_PROT_PIO:
1142                 case ATA_PROT_NODATA:
1143                 case ATAPI_PROT_PIO:
1144                 case ATAPI_PROT_NODATA:
1145                         qc->tf.flags |= ATA_TFLAG_POLLING;
1146                         break;
1147                 default:
1148                         break;
1149                 }
1150         }
1151
1152         ata_dev_select(ap, qc->dev->devno, 1, 0);
1153
1154         switch (qc->tf.protocol) {
1155         case ATA_PROT_PIO:
1156                 if (qc->tf.flags & ATA_TFLAG_POLLING)
1157                         qc->tf.ctl |= ATA_NIEN;
1158
1159                 ata_tf_to_host(ap, &qc->tf);
1160
1161                 ap->hsm_task_state = HSM_ST;
1162
1163                 if (qc->tf.flags & ATA_TFLAG_POLLING)
1164                         ata_pio_queue_task(ap, qc, 0);
1165
1166                 break;
1167
1168         default:
1169                 return AC_ERR_SYSTEM;
1170         }
1171
1172         return 0;
1173 }
1174
1175 static void ata_tf_to_host(struct ata_port *ap,
1176                         const struct ata_taskfile *tf)
1177 {
1178         ata_tf_load(ap, tf);
1179         ata_exec_command(ap, tf);
1180 }
1181
1182 static void ata_tf_load(struct ata_port *ap,
1183                         const struct ata_taskfile *tf)
1184 {
1185         struct ata_ioports *ioaddr = &ap->ioaddr;
1186         unsigned int is_addr = tf->flags & ATA_TFLAG_ISADDR;
1187
1188         if (tf->ctl != ap->last_ctl) {
1189                 if (ioaddr->ctl_addr)
1190                         writeb(tf->ctl, ioaddr->ctl_addr);
1191                 ap->last_ctl = tf->ctl;
1192                 ata_wait_idle(ap);
1193         }
1194
1195         if (is_addr && (tf->flags & ATA_TFLAG_LBA48)) {
1196                 writeb(tf->hob_feature, ioaddr->feature_addr);
1197                 writeb(tf->hob_nsect, ioaddr->nsect_addr);
1198                 writeb(tf->hob_lbal, ioaddr->lbal_addr);
1199                 writeb(tf->hob_lbam, ioaddr->lbam_addr);
1200                 writeb(tf->hob_lbah, ioaddr->lbah_addr);
1201         }
1202
1203         if (is_addr) {
1204                 writeb(tf->feature, ioaddr->feature_addr);
1205                 writeb(tf->nsect, ioaddr->nsect_addr);
1206                 writeb(tf->lbal, ioaddr->lbal_addr);
1207                 writeb(tf->lbam, ioaddr->lbam_addr);
1208                 writeb(tf->lbah, ioaddr->lbah_addr);
1209         }
1210
1211         if (tf->flags & ATA_TFLAG_DEVICE)
1212                 writeb(tf->device, ioaddr->device_addr);
1213
1214         ata_wait_idle(ap);
1215 }
1216
1217 static void ata_exec_command(struct ata_port *ap,
1218                         const struct ata_taskfile *tf)
1219 {
1220         writeb(tf->command, ap->ioaddr.command_addr);
1221
1222         readb(ap->ioaddr.altstatus_addr);
1223
1224         udelay(1);
1225 }
1226
1227 static void ata_pio_queue_task(struct ata_port *ap,
1228                         void *data,unsigned long delay)
1229 {
1230         ap->port_task_data = data;
1231 }
1232
1233 static unsigned int ac_err_mask(u8 status)
1234 {
1235         if (status & (ATA_BUSY | ATA_DRQ))
1236                 return AC_ERR_HSM;
1237         if (status & (ATA_ERR | ATA_DF))
1238                 return AC_ERR_DEV;
1239         return 0;
1240 }
1241
1242 static unsigned int __ac_err_mask(u8 status)
1243 {
1244         unsigned int mask = ac_err_mask(status);
1245         if (mask == 0)
1246                 return AC_ERR_OTHER;
1247         return mask;
1248 }
1249
1250 static void ata_pio_task(struct ata_port *arg_ap)
1251 {
1252         struct ata_port *ap = arg_ap;
1253         struct ata_queued_cmd *qc = ap->port_task_data;
1254         u8 status;
1255         int poll_next;
1256
1257 fsm_start:
1258         /*
1259          * This is purely heuristic.  This is a fast path.
1260          * Sometimes when we enter, BSY will be cleared in
1261          * a chk-status or two.  If not, the drive is probably seeking
1262          * or something.  Snooze for a couple msecs, then
1263          * chk-status again.  If still busy, queue delayed work.
1264          */
1265         status = ata_busy_wait(ap, ATA_BUSY, 5);
1266         if (status & ATA_BUSY) {
1267                 msleep(2);
1268                 status = ata_busy_wait(ap, ATA_BUSY, 10);
1269                 if (status & ATA_BUSY) {
1270                         ata_pio_queue_task(ap, qc, ATA_SHORT_PAUSE);
1271                         return;
1272                 }
1273         }
1274
1275         poll_next = ata_hsm_move(ap, qc, status, 1);
1276
1277         /* another command or interrupt handler
1278          * may be running at this point.
1279          */
1280         if (poll_next)
1281                 goto fsm_start;
1282 }
1283
1284 static int ata_hsm_move(struct ata_port *ap, struct ata_queued_cmd *qc,
1285                         u8 status, int in_wq)
1286 {
1287         int poll_next;
1288
1289 fsm_start:
1290         switch (ap->hsm_task_state) {
1291         case HSM_ST_FIRST:
1292                 poll_next = (qc->tf.flags & ATA_TFLAG_POLLING);
1293
1294                 if ((status & ATA_DRQ) == 0) {
1295                         if (status & (ATA_ERR | ATA_DF)) {
1296                                 qc->err_mask |= AC_ERR_DEV;
1297                         } else {
1298                                 qc->err_mask |= AC_ERR_HSM;
1299                         }
1300                         ap->hsm_task_state = HSM_ST_ERR;
1301                         goto fsm_start;
1302                 }
1303
1304                 /* Device should not ask for data transfer (DRQ=1)
1305                  * when it finds something wrong.
1306                  * We ignore DRQ here and stop the HSM by
1307                  * changing hsm_task_state to HSM_ST_ERR and
1308                  * let the EH abort the command or reset the device.
1309                  */
1310                 if (status & (ATA_ERR | ATA_DF)) {
1311                         if (!(qc->dev->horkage & ATA_HORKAGE_STUCK_ERR)) {
1312                                 printf("DRQ=1 with device error, "
1313                                         "dev_stat 0x%X\n", status);
1314                                 qc->err_mask |= AC_ERR_HSM;
1315                                 ap->hsm_task_state = HSM_ST_ERR;
1316                                 goto fsm_start;
1317                         }
1318                 }
1319
1320                 if (qc->tf.protocol == ATA_PROT_PIO) {
1321                         /* PIO data out protocol.
1322                          * send first data block.
1323                          */
1324                         /* ata_pio_sectors() might change the state
1325                          * to HSM_ST_LAST. so, the state is changed here
1326                          * before ata_pio_sectors().
1327                          */
1328                         ap->hsm_task_state = HSM_ST;
1329                         ata_pio_sectors(qc);
1330                 } else {
1331                         printf("protocol is not ATA_PROT_PIO \n");
1332                 }
1333                 break;
1334
1335         case HSM_ST:
1336                 if ((status & ATA_DRQ) == 0) {
1337                         if (status & (ATA_ERR | ATA_DF)) {
1338                                 qc->err_mask |= AC_ERR_DEV;
1339                         } else {
1340                                 /* HSM violation. Let EH handle this.
1341                                  * Phantom devices also trigger this
1342                                  * condition.  Mark hint.
1343                                  */
1344                                 qc->err_mask |= AC_ERR_HSM | AC_ERR_NODEV_HINT;
1345                         }
1346
1347                         ap->hsm_task_state = HSM_ST_ERR;
1348                         goto fsm_start;
1349                 }
1350                 /* For PIO reads, some devices may ask for
1351                  * data transfer (DRQ=1) alone with ERR=1.
1352                  * We respect DRQ here and transfer one
1353                  * block of junk data before changing the
1354                  * hsm_task_state to HSM_ST_ERR.
1355                  *
1356                  * For PIO writes, ERR=1 DRQ=1 doesn't make
1357                  * sense since the data block has been
1358                  * transferred to the device.
1359                  */
1360                 if (status & (ATA_ERR | ATA_DF)) {
1361                         qc->err_mask |= AC_ERR_DEV;
1362
1363                         if (!(qc->tf.flags & ATA_TFLAG_WRITE)) {
1364                                 ata_pio_sectors(qc);
1365                                 status = ata_wait_idle(ap);
1366                         }
1367
1368                         if (status & (ATA_BUSY | ATA_DRQ))
1369                                 qc->err_mask |= AC_ERR_HSM;
1370
1371                         /* ata_pio_sectors() might change the
1372                          * state to HSM_ST_LAST. so, the state
1373                          * is changed after ata_pio_sectors().
1374                          */
1375                         ap->hsm_task_state = HSM_ST_ERR;
1376                         goto fsm_start;
1377                 }
1378
1379                 ata_pio_sectors(qc);
1380                 if (ap->hsm_task_state == HSM_ST_LAST &&
1381                         (!(qc->tf.flags & ATA_TFLAG_WRITE))) {
1382                         status = ata_wait_idle(ap);
1383                         goto fsm_start;
1384                 }
1385
1386                 poll_next = 1;
1387                 break;
1388
1389         case HSM_ST_LAST:
1390                 if (!ata_ok(status)) {
1391                         qc->err_mask |= __ac_err_mask(status);
1392                         ap->hsm_task_state = HSM_ST_ERR;
1393                         goto fsm_start;
1394                 }
1395
1396                 ap->hsm_task_state = HSM_ST_IDLE;
1397
1398                 ata_hsm_qc_complete(qc, in_wq);
1399
1400                 poll_next = 0;
1401                 break;
1402
1403         case HSM_ST_ERR:
1404                 /* make sure qc->err_mask is available to
1405                  * know what's wrong and recover
1406                  */
1407                 ap->hsm_task_state = HSM_ST_IDLE;
1408
1409                 ata_hsm_qc_complete(qc, in_wq);
1410
1411                 poll_next = 0;
1412                 break;
1413         default:
1414                 poll_next = 0;
1415         }
1416
1417         return poll_next;
1418 }
1419
1420 static void ata_pio_sectors(struct ata_queued_cmd *qc)
1421 {
1422         struct ata_port *ap;
1423         ap = pap;
1424         qc->pdata = ap->pdata;
1425
1426         ata_pio_sector(qc);
1427
1428         readb(qc->ap->ioaddr.altstatus_addr);
1429         udelay(1);
1430 }
1431
1432 static void ata_pio_sector(struct ata_queued_cmd *qc)
1433 {
1434         int do_write = (qc->tf.flags & ATA_TFLAG_WRITE);
1435         struct ata_port *ap = qc->ap;
1436         unsigned int offset;
1437         unsigned char *buf;
1438         char temp_data_buf[512];
1439
1440         if (qc->curbytes == qc->nbytes - qc->sect_size)
1441                 ap->hsm_task_state = HSM_ST_LAST;
1442
1443         offset = qc->curbytes;
1444
1445         switch (qc->tf.command) {
1446         case ATA_CMD_ID_ATA:
1447                 buf = (unsigned char *)&ata_device.id[0];
1448                 break;
1449         case ATA_CMD_PIO_READ_EXT:
1450         case ATA_CMD_PIO_READ:
1451         case ATA_CMD_PIO_WRITE_EXT:
1452         case ATA_CMD_PIO_WRITE:
1453                 buf = qc->pdata + offset;
1454                 break;
1455         default:
1456                 buf = (unsigned char *)&temp_data_buf[0];
1457         }
1458
1459         ata_mmio_data_xfer(qc->dev, buf, qc->sect_size, do_write);
1460
1461         qc->curbytes += qc->sect_size;
1462
1463 }
1464
1465 static void ata_mmio_data_xfer(struct ata_device *dev, unsigned char *buf,
1466                                 unsigned int buflen, int do_write)
1467 {
1468         struct ata_port *ap = pap;
1469         void __iomem *data_addr = ap->ioaddr.data_addr;
1470         unsigned int words = buflen >> 1;
1471         u16 *buf16 = (u16 *)buf;
1472         unsigned int i = 0;
1473
1474         udelay(100);
1475         if (do_write) {
1476                 for (i = 0; i < words; i++)
1477                         writew(le16_to_cpu(buf16[i]), data_addr);
1478         } else {
1479                 for (i = 0; i < words; i++)
1480                         buf16[i] = cpu_to_le16(readw(data_addr));
1481         }
1482
1483         if (buflen & 0x01) {
1484                 __le16 align_buf[1] = { 0 };
1485                 unsigned char *trailing_buf = buf + buflen - 1;
1486
1487                 if (do_write) {
1488                         memcpy(align_buf, trailing_buf, 1);
1489                         writew(le16_to_cpu(align_buf[0]), data_addr);
1490                 } else {
1491                         align_buf[0] = cpu_to_le16(readw(data_addr));
1492                         memcpy(trailing_buf, align_buf, 1);
1493                 }
1494         }
1495 }
1496
1497 static void ata_hsm_qc_complete(struct ata_queued_cmd *qc, int in_wq)
1498 {
1499         struct ata_port *ap = qc->ap;
1500
1501         if (in_wq) {
1502                 /* EH might have kicked in while host lock is
1503                  * released.
1504                  */
1505                 qc = &ap->qcmd[qc->tag];
1506                 if (qc) {
1507                         if (!(qc->err_mask & AC_ERR_HSM)) {
1508                                 ata_irq_on(ap);
1509                                 ata_qc_complete(qc);
1510                         } else {
1511                                 ata_port_freeze(ap);
1512                         }
1513                 }
1514         } else {
1515                 if (!(qc->err_mask & AC_ERR_HSM)) {
1516                         ata_qc_complete(qc);
1517                 } else {
1518                         ata_port_freeze(ap);
1519                 }
1520         }
1521 }
1522
1523 static u8 ata_irq_on(struct ata_port *ap)
1524 {
1525         struct ata_ioports *ioaddr = &ap->ioaddr;
1526         u8 tmp;
1527
1528         ap->ctl &= ~ATA_NIEN;
1529         ap->last_ctl = ap->ctl;
1530
1531         if (ioaddr->ctl_addr)
1532                 writeb(ap->ctl, ioaddr->ctl_addr);
1533
1534         tmp = ata_wait_idle(ap);
1535
1536         return tmp;
1537 }
1538
1539 static unsigned int ata_tag_internal(unsigned int tag)
1540 {
1541         return tag == ATA_MAX_QUEUE - 1;
1542 }
1543
1544 static void ata_qc_complete(struct ata_queued_cmd *qc)
1545 {
1546         struct ata_device *dev = qc->dev;
1547         if (qc->err_mask)
1548                 qc->flags |= ATA_QCFLAG_FAILED;
1549
1550         if (qc->flags & ATA_QCFLAG_FAILED) {
1551                 if (!ata_tag_internal(qc->tag)) {
1552                         fill_result_tf(qc);
1553                         return;
1554                 }
1555         }
1556         if (qc->flags & ATA_QCFLAG_RESULT_TF)
1557                 fill_result_tf(qc);
1558
1559         /* Some commands need post-processing after successful
1560          * completion.
1561          */
1562         switch (qc->tf.command) {
1563         case ATA_CMD_SET_FEATURES:
1564                 if (qc->tf.feature != SETFEATURES_WC_ON &&
1565                                 qc->tf.feature != SETFEATURES_WC_OFF)
1566                         break;
1567         case ATA_CMD_INIT_DEV_PARAMS:
1568         case ATA_CMD_SET_MULTI:
1569                 break;
1570
1571         case ATA_CMD_SLEEP:
1572                 dev->flags |= ATA_DFLAG_SLEEPING;
1573                 break;
1574         }
1575
1576         __ata_qc_complete(qc);
1577 }
1578
1579 static void fill_result_tf(struct ata_queued_cmd *qc)
1580 {
1581         struct ata_port *ap = qc->ap;
1582
1583         qc->result_tf.flags = qc->tf.flags;
1584         ata_tf_read(ap, &qc->result_tf);
1585 }
1586
1587 static void ata_tf_read(struct ata_port *ap, struct ata_taskfile *tf)
1588 {
1589         struct ata_ioports *ioaddr = &ap->ioaddr;
1590
1591         tf->command = ata_check_status(ap);
1592         tf->feature = readb(ioaddr->error_addr);
1593         tf->nsect = readb(ioaddr->nsect_addr);
1594         tf->lbal = readb(ioaddr->lbal_addr);
1595         tf->lbam = readb(ioaddr->lbam_addr);
1596         tf->lbah = readb(ioaddr->lbah_addr);
1597         tf->device = readb(ioaddr->device_addr);
1598
1599         if (tf->flags & ATA_TFLAG_LBA48) {
1600                 if (ioaddr->ctl_addr) {
1601                         writeb(tf->ctl | ATA_HOB, ioaddr->ctl_addr);
1602
1603                         tf->hob_feature = readb(ioaddr->error_addr);
1604                         tf->hob_nsect = readb(ioaddr->nsect_addr);
1605                         tf->hob_lbal = readb(ioaddr->lbal_addr);
1606                         tf->hob_lbam = readb(ioaddr->lbam_addr);
1607                         tf->hob_lbah = readb(ioaddr->lbah_addr);
1608
1609                         writeb(tf->ctl, ioaddr->ctl_addr);
1610                         ap->last_ctl = tf->ctl;
1611                 } else {
1612                         printf("sata_dwc warnning register read.\n");
1613                 }
1614         }
1615 }
1616
1617 static void __ata_qc_complete(struct ata_queued_cmd *qc)
1618 {
1619         struct ata_port *ap = qc->ap;
1620         struct ata_link *link = qc->dev->link;
1621
1622         link->active_tag = ATA_TAG_POISON;
1623         ap->nr_active_links--;
1624
1625         if (qc->flags & ATA_QCFLAG_CLEAR_EXCL && ap->excl_link == link)
1626                 ap->excl_link = NULL;
1627
1628         qc->flags &= ~ATA_QCFLAG_ACTIVE;
1629         ap->qc_active &= ~(1 << qc->tag);
1630 }
1631
1632 static void ata_qc_free(struct ata_queued_cmd *qc)
1633 {
1634         struct ata_port *ap = qc->ap;
1635         unsigned int tag;
1636         qc->flags = 0;
1637         tag = qc->tag;
1638         if (tag < ATA_MAX_QUEUE) {
1639                 qc->tag = ATA_TAG_POISON;
1640                 clear_bit(tag, &ap->qc_allocated);
1641         }
1642 }
1643
1644 static int check_sata_dev_state(void)
1645 {
1646         unsigned long datalen;
1647         unsigned char *pdata;
1648         int ret = 0;
1649         int i = 0;
1650         char temp_data_buf[512];
1651
1652         while (1) {
1653                 udelay(10000);
1654
1655                 pdata = (unsigned char*)&temp_data_buf[0];
1656                 datalen = 512;
1657
1658                 ret = ata_dev_read_sectors(pdata, datalen, 0, 1);
1659
1660                 if (ret == TRUE)
1661                         break;
1662
1663                 i++;
1664                 if (i > (ATA_RESET_TIME * 100)) {
1665                         printf("** TimeOUT **\n");
1666                         dev_state = SATA_NODEVICE;
1667                         return FALSE;
1668                 }
1669
1670                 if ((i >= 100) && ((i % 100) == 0))
1671                         printf(".");
1672         }
1673
1674         dev_state = SATA_READY;
1675
1676         return TRUE;
1677 }
1678
1679 static unsigned int ata_dev_set_feature(struct ata_device *dev,
1680                                 u8 enable, u8 feature)
1681 {
1682         struct ata_taskfile tf;
1683         struct ata_port *ap;
1684         ap = pap;
1685         unsigned int err_mask;
1686
1687         memset(&tf, 0, sizeof(tf));
1688         tf.ctl = ap->ctl;
1689
1690         tf.device = ATA_DEVICE_OBS;
1691         tf.command = ATA_CMD_SET_FEATURES;
1692         tf.feature = enable;
1693         tf.flags |= ATA_TFLAG_ISADDR | ATA_TFLAG_DEVICE;
1694         tf.protocol = ATA_PROT_NODATA;
1695         tf.nsect = feature;
1696
1697         err_mask = ata_exec_internal(dev, &tf, NULL, DMA_NONE, 0, 0);
1698
1699         return err_mask;
1700 }
1701
1702 static unsigned int ata_dev_init_params(struct ata_device *dev,
1703                                 u16 heads, u16 sectors)
1704 {
1705         struct ata_taskfile tf;
1706         struct ata_port *ap;
1707         ap = pap;
1708         unsigned int err_mask;
1709
1710         if (sectors < 1 || sectors > 255 || heads < 1 || heads > 16)
1711                 return AC_ERR_INVALID;
1712
1713         memset(&tf, 0, sizeof(tf));
1714         tf.ctl = ap->ctl;
1715         tf.device = ATA_DEVICE_OBS;
1716         tf.command = ATA_CMD_INIT_DEV_PARAMS;
1717         tf.flags |= ATA_TFLAG_ISADDR | ATA_TFLAG_DEVICE;
1718         tf.protocol = ATA_PROT_NODATA;
1719         tf.nsect = sectors;
1720         tf.device |= (heads - 1) & 0x0f;
1721
1722         err_mask = ata_exec_internal(dev, &tf, NULL, DMA_NONE, 0, 0);
1723
1724         if (err_mask == AC_ERR_DEV && (tf.feature & ATA_ABORTED))
1725                 err_mask = 0;
1726
1727         return err_mask;
1728 }
1729
1730 #if defined(CONFIG_SATA_DWC) && !defined(CONFIG_LBA48)
1731 #define SATA_MAX_READ_BLK 0xFF
1732 #else
1733 #define SATA_MAX_READ_BLK 0xFFFF
1734 #endif
1735
1736 ulong sata_read(int device, ulong blknr, lbaint_t blkcnt, void *buffer)
1737 {
1738         ulong start,blks, buf_addr;
1739         unsigned short smallblks;
1740         unsigned long datalen;
1741         unsigned char *pdata;
1742         device &= 0xff;
1743
1744         u32 block = 0;
1745         u32 n_block = 0;
1746
1747         if (dev_state != SATA_READY)
1748                 return 0;
1749
1750         buf_addr = (unsigned long)buffer;
1751         start = blknr;
1752         blks = blkcnt;
1753         do {
1754                 pdata = (unsigned char *)buf_addr;
1755                 if (blks > SATA_MAX_READ_BLK) {
1756                         datalen = sata_dev_desc[device].blksz * SATA_MAX_READ_BLK;
1757                         smallblks = SATA_MAX_READ_BLK;
1758
1759                         block = (u32)start;
1760                         n_block = (u32)smallblks;
1761
1762                         start += SATA_MAX_READ_BLK;
1763                         blks -= SATA_MAX_READ_BLK;
1764                 } else {
1765                         datalen = sata_dev_desc[device].blksz * SATA_MAX_READ_BLK;
1766                         datalen = sata_dev_desc[device].blksz * blks;
1767                         smallblks = (unsigned short)blks;
1768
1769                         block = (u32)start;
1770                         n_block = (u32)smallblks;
1771
1772                         start += blks;
1773                         blks = 0;
1774                 }
1775
1776                 if (ata_dev_read_sectors(pdata, datalen, block, n_block) != TRUE) {
1777                         printf("sata_dwc : Hard disk read error.\n");
1778                         blkcnt -= blks;
1779                         break;
1780                 }
1781                 buf_addr += datalen;
1782         } while (blks != 0);
1783
1784         return (blkcnt);
1785 }
1786
1787 static int ata_dev_read_sectors(unsigned char *pdata, unsigned long datalen,
1788                                                 u32 block, u32 n_block)
1789 {
1790         struct ata_port *ap = pap;
1791         struct ata_device *dev = &ata_device;
1792         struct ata_taskfile tf;
1793         unsigned int class = ATA_DEV_ATA;
1794         unsigned int err_mask = 0;
1795         const char *reason;
1796         int may_fallback = 1;
1797
1798         if (dev_state == SATA_ERROR)
1799                 return FALSE;
1800
1801         ata_dev_select(ap, dev->devno, 1, 1);
1802
1803 retry:
1804         memset(&tf, 0, sizeof(tf));
1805         tf.ctl = ap->ctl;
1806         ap->print_id = 1;
1807         ap->flags &= ~ATA_FLAG_DISABLED;
1808
1809         ap->pdata = pdata;
1810
1811         tf.device = ATA_DEVICE_OBS;
1812
1813         temp_n_block = n_block;
1814
1815 #ifdef CONFIG_LBA48
1816         tf.command = ATA_CMD_PIO_READ_EXT;
1817         tf.flags |= ATA_TFLAG_LBA | ATA_TFLAG_LBA48;
1818
1819         tf.hob_feature = 31;
1820         tf.feature = 31;
1821         tf.hob_nsect = (n_block >> 8) & 0xff;
1822         tf.nsect = n_block & 0xff;
1823
1824         tf.hob_lbah = 0x0;
1825         tf.hob_lbam = 0x0;
1826         tf.hob_lbal = (block >> 24) & 0xff;
1827         tf.lbah = (block >> 16) & 0xff;
1828         tf.lbam = (block >> 8) & 0xff;
1829         tf.lbal = block & 0xff;
1830
1831         tf.device = 1 << 6;
1832         if (tf.flags & ATA_TFLAG_FUA)
1833                 tf.device |= 1 << 7;
1834 #else
1835         tf.command = ATA_CMD_PIO_READ;
1836         tf.flags |= ATA_TFLAG_LBA ;
1837
1838         tf.feature = 31;
1839         tf.nsect = n_block & 0xff;
1840
1841         tf.lbah = (block >> 16) & 0xff;
1842         tf.lbam = (block >> 8) & 0xff;
1843         tf.lbal = block & 0xff;
1844
1845         tf.device = (block >> 24) & 0xf;
1846
1847         tf.device |= 1 << 6;
1848         if (tf.flags & ATA_TFLAG_FUA)
1849                 tf.device |= 1 << 7;
1850
1851 #endif
1852
1853         tf.protocol = ATA_PROT_PIO;
1854
1855         /* Some devices choke if TF registers contain garbage.  Make
1856          * sure those are properly initialized.
1857          */
1858         tf.flags |= ATA_TFLAG_ISADDR | ATA_TFLAG_DEVICE;
1859         tf.flags |= ATA_TFLAG_POLLING;
1860
1861         err_mask = ata_exec_internal(dev, &tf, NULL, DMA_FROM_DEVICE, 0, 0);
1862
1863         if (err_mask) {
1864                 if (err_mask & AC_ERR_NODEV_HINT) {
1865                         printf("READ_SECTORS NODEV after polling detection\n");
1866                         return -ENOENT;
1867                 }
1868
1869                 if ((err_mask == AC_ERR_DEV) && (tf.feature & ATA_ABORTED)) {
1870                         /* Device or controller might have reported
1871                          * the wrong device class.  Give a shot at the
1872                          * other IDENTIFY if the current one is
1873                          * aborted by the device.
1874                          */
1875                         if (may_fallback) {
1876                                 may_fallback = 0;
1877
1878                                 if (class == ATA_DEV_ATA) {
1879                                         class = ATA_DEV_ATAPI;
1880                                 } else {
1881                                         class = ATA_DEV_ATA;
1882                                 }
1883                                 goto retry;
1884                         }
1885                         /* Control reaches here iff the device aborted
1886                          * both flavors of IDENTIFYs which happens
1887                          * sometimes with phantom devices.
1888                          */
1889                         printf("both IDENTIFYs aborted, assuming NODEV\n");
1890                         return -ENOENT;
1891                 }
1892
1893                 reason = "I/O error";
1894                 goto err_out;
1895         }
1896
1897         return TRUE;
1898
1899 err_out:
1900         printf("failed to READ SECTORS (%s, err_mask=0x%x)\n", reason, err_mask);
1901         return FALSE;
1902 }
1903
1904 #if defined(CONFIG_SATA_DWC) && !defined(CONFIG_LBA48)
1905 #define SATA_MAX_WRITE_BLK 0xFF
1906 #else
1907 #define SATA_MAX_WRITE_BLK 0xFFFF
1908 #endif
1909
1910 ulong sata_write(int device, ulong blknr, lbaint_t blkcnt, void *buffer)
1911 {
1912         ulong start,blks, buf_addr;
1913         unsigned short smallblks;
1914         unsigned long datalen;
1915         unsigned char *pdata;
1916         device &= 0xff;
1917
1918
1919         u32 block = 0;
1920         u32 n_block = 0;
1921
1922         if (dev_state != SATA_READY)
1923                 return 0;
1924
1925         buf_addr = (unsigned long)buffer;
1926         start = blknr;
1927         blks = blkcnt;
1928         do {
1929                 pdata = (unsigned char *)buf_addr;
1930                 if (blks > SATA_MAX_WRITE_BLK) {
1931                         datalen = sata_dev_desc[device].blksz * SATA_MAX_WRITE_BLK;
1932                         smallblks = SATA_MAX_WRITE_BLK;
1933
1934                         block = (u32)start;
1935                         n_block = (u32)smallblks;
1936
1937                         start += SATA_MAX_WRITE_BLK;
1938                         blks -= SATA_MAX_WRITE_BLK;
1939                 } else {
1940                         datalen = sata_dev_desc[device].blksz * blks;
1941                         smallblks = (unsigned short)blks;
1942
1943                         block = (u32)start;
1944                         n_block = (u32)smallblks;
1945
1946                         start += blks;
1947                         blks = 0;
1948                 }
1949
1950                 if (ata_dev_write_sectors(pdata, datalen, block, n_block) != TRUE) {
1951                         printf("sata_dwc : Hard disk read error.\n");
1952                         blkcnt -= blks;
1953                         break;
1954                 }
1955                 buf_addr += datalen;
1956         } while (blks != 0);
1957
1958         return (blkcnt);
1959 }
1960
1961 static int ata_dev_write_sectors(unsigned char* pdata, unsigned long datalen,
1962                                                 u32 block, u32 n_block)
1963 {
1964         struct ata_port *ap = pap;
1965         struct ata_device *dev = &ata_device;
1966         struct ata_taskfile tf;
1967         unsigned int class = ATA_DEV_ATA;
1968         unsigned int err_mask = 0;
1969         const char *reason;
1970         int may_fallback = 1;
1971
1972         if (dev_state == SATA_ERROR)
1973                 return FALSE;
1974
1975         ata_dev_select(ap, dev->devno, 1, 1);
1976
1977 retry:
1978         memset(&tf, 0, sizeof(tf));
1979         tf.ctl = ap->ctl;
1980         ap->print_id = 1;
1981         ap->flags &= ~ATA_FLAG_DISABLED;
1982
1983         ap->pdata = pdata;
1984
1985         tf.device = ATA_DEVICE_OBS;
1986
1987         temp_n_block = n_block;
1988
1989
1990 #ifdef CONFIG_LBA48
1991         tf.command = ATA_CMD_PIO_WRITE_EXT;
1992         tf.flags |= ATA_TFLAG_LBA | ATA_TFLAG_LBA48 | ATA_TFLAG_WRITE;
1993
1994         tf.hob_feature = 31;
1995         tf.feature = 31;
1996         tf.hob_nsect = (n_block >> 8) & 0xff;
1997         tf.nsect = n_block & 0xff;
1998
1999         tf.hob_lbah = 0x0;
2000         tf.hob_lbam = 0x0;
2001         tf.hob_lbal = (block >> 24) & 0xff;
2002         tf.lbah = (block >> 16) & 0xff;
2003         tf.lbam = (block >> 8) & 0xff;
2004         tf.lbal = block & 0xff;
2005
2006         tf.device = 1 << 6;
2007         if (tf.flags & ATA_TFLAG_FUA)
2008                 tf.device |= 1 << 7;
2009 #else
2010         tf.command = ATA_CMD_PIO_WRITE;
2011         tf.flags |= ATA_TFLAG_LBA | ATA_TFLAG_WRITE;
2012
2013         tf.feature = 31;
2014         tf.nsect = n_block & 0xff;
2015
2016         tf.lbah = (block >> 16) & 0xff;
2017         tf.lbam = (block >> 8) & 0xff;
2018         tf.lbal = block & 0xff;
2019
2020         tf.device = (block >> 24) & 0xf;
2021
2022         tf.device |= 1 << 6;
2023         if (tf.flags & ATA_TFLAG_FUA)
2024                 tf.device |= 1 << 7;
2025
2026 #endif
2027
2028         tf.protocol = ATA_PROT_PIO;
2029
2030         /* Some devices choke if TF registers contain garbage.  Make
2031          * sure those are properly initialized.
2032          */
2033         tf.flags |= ATA_TFLAG_ISADDR | ATA_TFLAG_DEVICE;
2034         tf.flags |= ATA_TFLAG_POLLING;
2035
2036         err_mask = ata_exec_internal(dev, &tf, NULL, DMA_FROM_DEVICE, 0, 0);
2037
2038         if (err_mask) {
2039                 if (err_mask & AC_ERR_NODEV_HINT) {
2040                         printf("READ_SECTORS NODEV after polling detection\n");
2041                         return -ENOENT;
2042                 }
2043
2044                 if ((err_mask == AC_ERR_DEV) && (tf.feature & ATA_ABORTED)) {
2045                         /* Device or controller might have reported
2046                          * the wrong device class.  Give a shot at the
2047                          * other IDENTIFY if the current one is
2048                          * aborted by the device.
2049                          */
2050                         if (may_fallback) {
2051                                 may_fallback = 0;
2052
2053                                 if (class == ATA_DEV_ATA) {
2054                                         class = ATA_DEV_ATAPI;
2055                                 } else {
2056                                         class = ATA_DEV_ATA;
2057                                 }
2058                                 goto retry;
2059                         }
2060                         /* Control reaches here iff the device aborted
2061                          * both flavors of IDENTIFYs which happens
2062                          * sometimes with phantom devices.
2063                          */
2064                         printf("both IDENTIFYs aborted, assuming NODEV\n");
2065                         return -ENOENT;
2066                 }
2067
2068                 reason = "I/O error";
2069                 goto err_out;
2070         }
2071
2072         return TRUE;
2073
2074 err_out:
2075         printf("failed to WRITE SECTORS (%s, err_mask=0x%x)\n", reason, err_mask);
2076         return FALSE;
2077 }