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