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, udma_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         udma_mask = 0;
 626         if (id[ATA_ID_FIELD_VALID] & (1 << 2))
 627                 udma_mask = id[ATA_ID_UDMA_MODES] & 0xff;
 628
 629         if (ata_dev->class == ATA_DEV_ATA) {
 630                 if (ata_id_is_cfa(id)) {
 631                         if (id[162] & 1)
 632                                 printf("supports DRM functions and may "
 633                                         "not be fully accessable.\n");
 634                         sprintf(revbuf, "%s", "CFA");
 635                 } else {
 636                         if (ata_id_has_tpm(id))
 637                                 printf("supports DRM functions and may "
 638                                                 "not be fully accessable.\n");
 639                 }
 640
 641                 ata_dev->n_sectors = ata_id_n_sectors((u16*)id);
 642
 643                 if (ata_dev->id[59] & 0x100)
 644                         ata_dev->multi_count = ata_dev->id[59] & 0xff;
 645
 646                 if (ata_id_has_lba(id)) {
 647                         char ncq_desc[20];
 648
 649                         ata_dev->flags |= ATA_DFLAG_LBA;
 650                         if (ata_id_has_lba48(id)) {
 651                                 ata_dev->flags |= ATA_DFLAG_LBA48;
 652
 653                                 if (ata_dev->n_sectors >= (1UL << 28) &&
 654                                         ata_id_has_flush_ext(id))
 655                                         ata_dev->flags |= ATA_DFLAG_FLUSH_EXT;
 656                         }
 657                         if (!ata_id_has_ncq(ata_dev->id))
 658                                 ncq_desc[0] = '\0';
 659
 660                         if (ata_dev->horkage & ATA_HORKAGE_NONCQ)
 661                                 sprintf(ncq_desc, "%s", "NCQ (not used)");
 662
 663                         if (ap.flags & ATA_FLAG_NCQ)
 664                                 ata_dev->flags |= ATA_DFLAG_NCQ;
 665                 }
 666                 ata_dev->cdb_len = 16;
 667         }
 668         ata_dev->max_sectors = ATA_MAX_SECTORS;
 669         if (ata_dev->flags & ATA_DFLAG_LBA48)
 670                 ata_dev->max_sectors = ATA_MAX_SECTORS_LBA48;
 671
 672         if (!(ata_dev->horkage & ATA_HORKAGE_IPM)) {
 673                 if (ata_id_has_hipm(ata_dev->id))
 674                         ata_dev->flags |= ATA_DFLAG_HIPM;
 675                 if (ata_id_has_dipm(ata_dev->id))
 676                         ata_dev->flags |= ATA_DFLAG_DIPM;
 677         }
 678
 679         if ((ap.cbl == ATA_CBL_SATA) && (!ata_id_is_sata(ata_dev->id))) {
 680                 ata_dev->udma_mask &= ATA_UDMA5;
 681                 ata_dev->max_sectors = ATA_MAX_SECTORS;
 682         }
 683
 684         if (ata_dev->horkage & ATA_HORKAGE_DIAGNOSTIC) {
 685                 printf("Drive reports diagnostics failure."
 686                                 "This may indicate a drive\n");
 687                 printf("fault or invalid emulation."
 688                                 "Contact drive vendor for information.\n");
 689         }
 690
 691         rc = check_sata_dev_state();
 692
 693         ata_id_c_string(ata_dev->id,
 694                         (unsigned char *)sata_dev_desc[dev].revision,
 695                          ATA_ID_FW_REV, sizeof(sata_dev_desc[dev].revision));
 696         ata_id_c_string(ata_dev->id,
 697                         (unsigned char *)sata_dev_desc[dev].vendor,
 698                          ATA_ID_PROD, sizeof(sata_dev_desc[dev].vendor));
 699         ata_id_c_string(ata_dev->id,
 700                         (unsigned char *)sata_dev_desc[dev].product,
 701                          ATA_ID_SERNO, sizeof(sata_dev_desc[dev].product));
 702
 703         sata_dev_desc[dev].lba = (u32) ata_dev->n_sectors;
 704
 705 #ifdef CONFIG_LBA48
 706         if (ata_dev->id[83] & (1 << 10)) {
 707                 sata_dev_desc[dev].lba48 = 1;
 708         } else {
 709                 sata_dev_desc[dev].lba48 = 0;
 710         }
 711 #endif
 712
 713         return 0;
 714 }
 715
 716 static u8 ata_busy_wait(struct ata_port *ap,
 717                 unsigned int bits,unsigned int max)
 718 {
 719         u8 status;
 720
 721         do {
 722                 udelay(10);
 723                 status = ata_check_status(ap);
 724                 max--;
 725         } while (status != 0xff && (status & bits) && (max > 0));
 726
 727         return status;
 728 }
 729
 730 static int ata_dev_read_id(struct ata_device *dev, unsigned int *p_class,
 731                 unsigned int flags, u16 *id)
 732 {
 733         struct ata_port *ap = pap;
 734         unsigned int class = *p_class;
 735         struct ata_taskfile tf;
 736         unsigned int err_mask = 0;
 737         const char *reason;
 738         int may_fallback = 1, tried_spinup = 0;
 739         u8 status;
 740         int rc;
 741
 742         status = ata_busy_wait(ap, ATA_BUSY, 30000);
 743         if (status & ATA_BUSY) {
 744                 printf("BSY = 0 check. timeout.\n");
 745                 rc = FALSE;
 746                 return rc;
 747         }
 748
 749         ata_dev_select(ap, dev->devno, 1, 1);
 750
 751 retry:
 752         memset(&tf, 0, sizeof(tf));
 753         ap->print_id = 1;
 754         ap->flags &= ~ATA_FLAG_DISABLED;
 755         tf.ctl = ap->ctl;
 756         tf.device = ATA_DEVICE_OBS;
 757         tf.command = ATA_CMD_ID_ATA;
 758         tf.protocol = ATA_PROT_PIO;
 759
 760         /* Some devices choke if TF registers contain garbage.  Make
 761          * sure those are properly initialized.
 762          */
 763         tf.flags |= ATA_TFLAG_ISADDR | ATA_TFLAG_DEVICE;
 764
 765         /* Device presence detection is unreliable on some
 766          * controllers.  Always poll IDENTIFY if available.
 767          */
 768         tf.flags |= ATA_TFLAG_POLLING;
 769
 770         temp_n_block = 1;
 771
 772         err_mask = ata_exec_internal(dev, &tf, NULL, DMA_FROM_DEVICE,
 773                                         sizeof(id[0]) * ATA_ID_WORDS, 0);
 774
 775         if (err_mask) {
 776                 if (err_mask & AC_ERR_NODEV_HINT) {
 777                         printf("NODEV after polling detection\n");
 778                         return -ENOENT;
 779                 }
 780
 781                 if ((err_mask == AC_ERR_DEV) && (tf.feature & ATA_ABORTED)) {
 782                         /* Device or controller might have reported
 783                          * the wrong device class.  Give a shot at the
 784                          * other IDENTIFY if the current one is
 785                          * aborted by the device.
 786                          */
 787                         if (may_fallback) {
 788                                 may_fallback = 0;
 789
 790                                 if (class == ATA_DEV_ATA) {
 791                                         class = ATA_DEV_ATAPI;
 792                                 } else {
 793                                         class = ATA_DEV_ATA;
 794                                 }
 795                                 goto retry;
 796                         }
 797                         /* Control reaches here iff the device aborted
 798                          * both flavors of IDENTIFYs which happens
 799                          * sometimes with phantom devices.
 800                          */
 801                         printf("both IDENTIFYs aborted, assuming NODEV\n");
 802                         return -ENOENT;
 803                 }
 804                 rc = -EIO;
 805                 reason = "I/O error";
 806                 goto err_out;
 807         }
 808
 809         /* Falling back doesn't make sense if ID data was read
 810          * successfully at least once.
 811          */
 812         may_fallback = 0;
 813
 814         unsigned int id_cnt;
 815
 816         for (id_cnt = 0; id_cnt < ATA_ID_WORDS; id_cnt++)
 817                 id[id_cnt] = le16_to_cpu(id[id_cnt]);
 818
 819
 820         rc = -EINVAL;
 821         reason = "device reports invalid type";
 822
 823         if (class == ATA_DEV_ATA) {
 824                 if (!ata_id_is_ata(id) && !ata_id_is_cfa(id))
 825                         goto err_out;
 826         } else {
 827                 if (ata_id_is_ata(id))
 828                         goto err_out;
 829         }
 830         if (!tried_spinup && (id[2] == 0x37c8 || id[2] == 0x738c)) {
 831                 tried_spinup = 1;
 832                 /*
 833                  * Drive powered-up in standby mode, and requires a specific
 834                  * SET_FEATURES spin-up subcommand before it will accept
 835                  * anything other than the original IDENTIFY command.
 836                  */
 837                 err_mask = ata_dev_set_feature(dev, SETFEATURES_SPINUP, 0);
 838                 if (err_mask && id[2] != 0x738c) {
 839                         rc = -EIO;
 840                         reason = "SPINUP failed";
 841                         goto err_out;
 842                 }
 843                 /*
 844                  * If the drive initially returned incomplete IDENTIFY info,
 845                  * we now must reissue the IDENTIFY command.
 846                  */
 847                 if (id[2] == 0x37c8)
 848                         goto retry;
 849         }
 850
 851         if ((flags & ATA_READID_POSTRESET) && class == ATA_DEV_ATA) {
 852                 /*
 853                  * The exact sequence expected by certain pre-ATA4 drives is:
 854                  * SRST RESET
 855                  * IDENTIFY (optional in early ATA)
 856                  * INITIALIZE DEVICE PARAMETERS (later IDE and ATA)
 857                  * anything else..
 858                  * Some drives were very specific about that exact sequence.
 859                  *
 860                  * Note that ATA4 says lba is mandatory so the second check
 861                  * shoud never trigger.
 862                  */
 863                 if (ata_id_major_version(id) < 4 || !ata_id_has_lba(id)) {
 864                         err_mask = ata_dev_init_params(dev, id[3], id[6]);
 865                         if (err_mask) {
 866                                 rc = -EIO;
 867                                 reason = "INIT_DEV_PARAMS failed";
 868                                 goto err_out;
 869                         }
 870
 871                         /* current CHS translation info (id[53-58]) might be
 872                          * changed. reread the identify device info.
 873                          */
 874                         flags &= ~ATA_READID_POSTRESET;
 875                         goto retry;
 876                 }
 877         }
 878
 879         *p_class = class;
 880         return 0;
 881
 882 err_out:
 883         printf("failed to READ ID (%s, err_mask=0x%x)\n", reason, err_mask);
 884         return rc;
 885 }
 886
 887 static u8 ata_wait_idle(struct ata_port *ap)
 888 {
 889         u8 status = ata_busy_wait(ap, ATA_BUSY | ATA_DRQ, 1000);
 890         return status;
 891 }
 892
 893 static void ata_dev_select(struct ata_port *ap, unsigned int device,
 894                 unsigned int wait, unsigned int can_sleep)
 895 {
 896         if (wait)
 897                 ata_wait_idle(ap);
 898
 899         ata_std_dev_select(ap, device);
 900
 901         if (wait)
 902                 ata_wait_idle(ap);
 903 }
 904
 905 static void ata_std_dev_select(struct ata_port *ap, unsigned int device)
 906 {
 907         u8 tmp;
 908
 909         if (device == 0) {
 910                 tmp = ATA_DEVICE_OBS;
 911         } else {
 912                 tmp = ATA_DEVICE_OBS | ATA_DEV1;
 913         }
 914
 915         writeb(tmp, ap->ioaddr.device_addr);
 916
 917         readb(ap->ioaddr.altstatus_addr);
 918
 919         udelay(1);
 920 }
 921
 922 static int waiting_for_reg_state(volatile u8 *offset,
 923                                 int timeout_msec,
 924                                 u32 sign)
 925 {
 926         int i;
 927         u32 status;
 928
 929         for (i = 0; i < timeout_msec; i++) {
 930                 status = readl(offset);
 931                 if ((status & sign) != 0)
 932                         break;
 933                 msleep(1);
 934         }
 935
 936         return (i < timeout_msec) ? 0 : -1;
 937 }
 938
 939 static void ata_qc_reinit(struct ata_queued_cmd *qc)
 940 {
 941         qc->dma_dir = DMA_NONE;
 942         qc->flags = 0;
 943         qc->nbytes = qc->extrabytes = qc->curbytes = 0;
 944         qc->n_elem = 0;
 945         qc->err_mask = 0;
 946         qc->sect_size = ATA_SECT_SIZE;
 947         qc->nbytes = ATA_SECT_SIZE * temp_n_block;
 948
 949         memset(&qc->tf, 0, sizeof(qc->tf));
 950         qc->tf.ctl = 0;
 951         qc->tf.device = ATA_DEVICE_OBS;
 952
 953         qc->result_tf.command = ATA_DRDY;
 954         qc->result_tf.feature = 0;
 955 }
 956
 957 struct ata_queued_cmd *__ata_qc_from_tag(struct ata_port *ap,
 958                                         unsigned int tag)
 959 {
 960         if (tag < ATA_MAX_QUEUE)
 961                 return &ap->qcmd[tag];
 962         return NULL;
 963 }
 964
 965 static void __ata_port_freeze(struct ata_port *ap)
 966 {
 967         printf("set port freeze.\n");
 968         ap->pflags |= ATA_PFLAG_FROZEN;
 969 }
 970
 971 static int ata_port_freeze(struct ata_port *ap)
 972 {
 973         __ata_port_freeze(ap);
 974         return 0;
 975 }
 976
 977 unsigned ata_exec_internal(struct ata_device *dev,
 978                         struct ata_taskfile *tf, const u8 *cdb,
 979                         int dma_dir, unsigned int buflen,
 980                         unsigned long timeout)
 981 {
 982         struct ata_link *link = dev->link;
 983         struct ata_port *ap = pap;
 984         struct ata_queued_cmd *qc;
 985         unsigned int tag, preempted_tag;
 986         u32 preempted_sactive, preempted_qc_active;
 987         int preempted_nr_active_links;
 988         unsigned int err_mask;
 989         int rc = 0;
 990         u8 status;
 991
 992         status = ata_busy_wait(ap, ATA_BUSY, 300000);
 993         if (status & ATA_BUSY) {
 994                 printf("BSY = 0 check. timeout.\n");
 995                 rc = FALSE;
 996                 return rc;
 997         }
 998
 999         if (ap->pflags & ATA_PFLAG_FROZEN)
1000                 return AC_ERR_SYSTEM;
1001
1002         tag = ATA_TAG_INTERNAL;
1003
1004         if (test_and_set_bit(tag, &ap->qc_allocated)) {
1005                 rc = FALSE;
1006                 return rc;
1007         }
1008
1009         qc = __ata_qc_from_tag(ap, tag);
1010         qc->tag = tag;
1011         qc->ap = ap;
1012         qc->dev = dev;
1013
1014         ata_qc_reinit(qc);
1015
1016         preempted_tag = link->active_tag;
1017         preempted_sactive = link->sactive;
1018         preempted_qc_active = ap->qc_active;
1019         preempted_nr_active_links = ap->nr_active_links;
1020         link->active_tag = ATA_TAG_POISON;
1021         link->sactive = 0;
1022         ap->qc_active = 0;
1023         ap->nr_active_links = 0;
1024
1025         qc->tf = *tf;
1026         if (cdb)
1027                 memcpy(qc->cdb, cdb, ATAPI_CDB_LEN);
1028         qc->flags |= ATA_QCFLAG_RESULT_TF;
1029         qc->dma_dir = dma_dir;
1030         qc->private_data = 0;
1031
1032         ata_qc_issue(qc);
1033
1034         if (!timeout)
1035                 timeout = ata_probe_timeout * 1000 / HZ;
1036
1037         status = ata_busy_wait(ap, ATA_BUSY, 30000);
1038         if (status & ATA_BUSY) {
1039                 printf("BSY = 0 check. timeout.\n");
1040                 printf("altstatus = 0x%x.\n", status);
1041                 qc->err_mask |= AC_ERR_OTHER;
1042                 return qc->err_mask;
1043         }
1044
1045         if (waiting_for_reg_state(ap->ioaddr.altstatus_addr, 1000, 0x8)) {
1046                 u8 status = 0;
1047                 u8 errorStatus = 0;
1048
1049                 status = readb(ap->ioaddr.altstatus_addr);
1050                 if ((status & 0x01) != 0) {
1051                         errorStatus = readb(ap->ioaddr.feature_addr);
1052                         if (errorStatus == 0x04 &&
1053                                 qc->tf.command == ATA_CMD_PIO_READ_EXT){
1054                                 printf("Hard Disk doesn't support LBA48\n");
1055                                 dev_state = SATA_ERROR;
1056                                 qc->err_mask |= AC_ERR_OTHER;
1057                                 return qc->err_mask;
1058                         }
1059                 }
1060                 qc->err_mask |= AC_ERR_OTHER;
1061                 return qc->err_mask;
1062         }
1063
1064         status = ata_busy_wait(ap, ATA_BUSY, 10);
1065         if (status & ATA_BUSY) {
1066                 printf("BSY = 0 check. timeout.\n");
1067                 qc->err_mask |= AC_ERR_OTHER;
1068                 return qc->err_mask;
1069         }
1070
1071         ata_pio_task(ap);
1072
1073         if (!rc) {
1074                 if (qc->flags & ATA_QCFLAG_ACTIVE) {
1075                         qc->err_mask |= AC_ERR_TIMEOUT;
1076                         ata_port_freeze(ap);
1077                 }
1078         }
1079
1080         if (qc->flags & ATA_QCFLAG_FAILED) {
1081                 if (qc->result_tf.command & (ATA_ERR | ATA_DF))
1082                         qc->err_mask |= AC_ERR_DEV;
1083
1084                 if (!qc->err_mask)
1085                         qc->err_mask |= AC_ERR_OTHER;
1086
1087                 if (qc->err_mask & ~AC_ERR_OTHER)
1088                         qc->err_mask &= ~AC_ERR_OTHER;
1089         }
1090
1091         *tf = qc->result_tf;
1092         err_mask = qc->err_mask;
1093         ata_qc_free(qc);
1094         link->active_tag = preempted_tag;
1095         link->sactive = preempted_sactive;
1096         ap->qc_active = preempted_qc_active;
1097         ap->nr_active_links = preempted_nr_active_links;
1098
1099         if (ap->flags & ATA_FLAG_DISABLED) {
1100                 err_mask |= AC_ERR_SYSTEM;
1101                 ap->flags &= ~ATA_FLAG_DISABLED;
1102         }
1103
1104         return err_mask;
1105 }
1106
1107 static void ata_qc_issue(struct ata_queued_cmd *qc)
1108 {
1109         struct ata_port *ap = qc->ap;
1110         struct ata_link *link = qc->dev->link;
1111         u8 prot = qc->tf.protocol;
1112
1113         if (ata_is_ncq(prot)) {
1114                 if (!link->sactive)
1115                         ap->nr_active_links++;
1116                 link->sactive |= 1 << qc->tag;
1117         } else {
1118                 ap->nr_active_links++;
1119                 link->active_tag = qc->tag;
1120         }
1121
1122         qc->flags |= ATA_QCFLAG_ACTIVE;
1123         ap->qc_active |= 1 << qc->tag;
1124
1125         if (qc->dev->flags & ATA_DFLAG_SLEEPING) {
1126                 msleep(1);
1127                 return;
1128         }
1129
1130         qc->err_mask |= ata_qc_issue_prot(qc);
1131         if (qc->err_mask)
1132                 goto err;
1133
1134         return;
1135 err:
1136         ata_qc_complete(qc);
1137 }
1138
1139 static unsigned int ata_qc_issue_prot(struct ata_queued_cmd *qc)
1140 {
1141         struct ata_port *ap = qc->ap;
1142
1143         if (ap->flags & ATA_FLAG_PIO_POLLING) {
1144                 switch (qc->tf.protocol) {
1145                 case ATA_PROT_PIO:
1146                 case ATA_PROT_NODATA:
1147                 case ATAPI_PROT_PIO:
1148                 case ATAPI_PROT_NODATA:
1149                         qc->tf.flags |= ATA_TFLAG_POLLING;
1150                         break;
1151                 default:
1152                         break;
1153                 }
1154         }
1155
1156         ata_dev_select(ap, qc->dev->devno, 1, 0);
1157
1158         switch (qc->tf.protocol) {
1159         case ATA_PROT_PIO:
1160                 if (qc->tf.flags & ATA_TFLAG_POLLING)
1161                         qc->tf.ctl |= ATA_NIEN;
1162
1163                 ata_tf_to_host(ap, &qc->tf);
1164
1165                 ap->hsm_task_state = HSM_ST;
1166
1167                 if (qc->tf.flags & ATA_TFLAG_POLLING)
1168                         ata_pio_queue_task(ap, qc, 0);
1169
1170                 break;
1171
1172         default:
1173                 return AC_ERR_SYSTEM;
1174         }
1175
1176         return 0;
1177 }
1178
1179 static void ata_tf_to_host(struct ata_port *ap,
1180                         const struct ata_taskfile *tf)
1181 {
1182         ata_tf_load(ap, tf);
1183         ata_exec_command(ap, tf);
1184 }
1185
1186 static void ata_tf_load(struct ata_port *ap,
1187                         const struct ata_taskfile *tf)
1188 {
1189         struct ata_ioports *ioaddr = &ap->ioaddr;
1190         unsigned int is_addr = tf->flags & ATA_TFLAG_ISADDR;
1191
1192         if (tf->ctl != ap->last_ctl) {
1193                 if (ioaddr->ctl_addr)
1194                         writeb(tf->ctl, ioaddr->ctl_addr);
1195                 ap->last_ctl = tf->ctl;
1196                 ata_wait_idle(ap);
1197         }
1198
1199         if (is_addr && (tf->flags & ATA_TFLAG_LBA48)) {
1200                 writeb(tf->hob_feature, ioaddr->feature_addr);
1201                 writeb(tf->hob_nsect, ioaddr->nsect_addr);
1202                 writeb(tf->hob_lbal, ioaddr->lbal_addr);
1203                 writeb(tf->hob_lbam, ioaddr->lbam_addr);
1204                 writeb(tf->hob_lbah, ioaddr->lbah_addr);
1205         }
1206
1207         if (is_addr) {
1208                 writeb(tf->feature, ioaddr->feature_addr);
1209                 writeb(tf->nsect, ioaddr->nsect_addr);
1210                 writeb(tf->lbal, ioaddr->lbal_addr);
1211                 writeb(tf->lbam, ioaddr->lbam_addr);
1212                 writeb(tf->lbah, ioaddr->lbah_addr);
1213         }
1214
1215         if (tf->flags & ATA_TFLAG_DEVICE)
1216                 writeb(tf->device, ioaddr->device_addr);
1217
1218         ata_wait_idle(ap);
1219 }
1220
1221 static void ata_exec_command(struct ata_port *ap,
1222                         const struct ata_taskfile *tf)
1223 {
1224         writeb(tf->command, ap->ioaddr.command_addr);
1225
1226         readb(ap->ioaddr.altstatus_addr);
1227
1228         udelay(1);
1229 }
1230
1231 static void ata_pio_queue_task(struct ata_port *ap,
1232                         void *data,unsigned long delay)
1233 {
1234         ap->port_task_data = data;
1235 }
1236
1237 static unsigned int ac_err_mask(u8 status)
1238 {
1239         if (status & (ATA_BUSY | ATA_DRQ))
1240                 return AC_ERR_HSM;
1241         if (status & (ATA_ERR | ATA_DF))
1242                 return AC_ERR_DEV;
1243         return 0;
1244 }
1245
1246 static unsigned int __ac_err_mask(u8 status)
1247 {
1248         unsigned int mask = ac_err_mask(status);
1249         if (mask == 0)
1250                 return AC_ERR_OTHER;
1251         return mask;
1252 }
1253
1254 static void ata_pio_task(struct ata_port *arg_ap)
1255 {
1256         struct ata_port *ap = arg_ap;
1257         struct ata_queued_cmd *qc = ap->port_task_data;
1258         u8 status;
1259         int poll_next;
1260
1261 fsm_start:
1262         /*
1263          * This is purely heuristic.  This is a fast path.
1264          * Sometimes when we enter, BSY will be cleared in
1265          * a chk-status or two.  If not, the drive is probably seeking
1266          * or something.  Snooze for a couple msecs, then
1267          * chk-status again.  If still busy, queue delayed work.
1268          */
1269         status = ata_busy_wait(ap, ATA_BUSY, 5);
1270         if (status & ATA_BUSY) {
1271                 msleep(2);
1272                 status = ata_busy_wait(ap, ATA_BUSY, 10);
1273                 if (status & ATA_BUSY) {
1274                         ata_pio_queue_task(ap, qc, ATA_SHORT_PAUSE);
1275                         return;
1276                 }
1277         }
1278
1279         poll_next = ata_hsm_move(ap, qc, status, 1);
1280
1281         /* another command or interrupt handler
1282          * may be running at this point.
1283          */
1284         if (poll_next)
1285                 goto fsm_start;
1286 }
1287
1288 static int ata_hsm_move(struct ata_port *ap, struct ata_queued_cmd *qc,
1289                         u8 status, int in_wq)
1290 {
1291         int poll_next;
1292
1293 fsm_start:
1294         switch (ap->hsm_task_state) {
1295         case HSM_ST_FIRST:
1296                 poll_next = (qc->tf.flags & ATA_TFLAG_POLLING);
1297
1298                 if ((status & ATA_DRQ) == 0) {
1299                         if (status & (ATA_ERR | ATA_DF)) {
1300                                 qc->err_mask |= AC_ERR_DEV;
1301                         } else {
1302                                 qc->err_mask |= AC_ERR_HSM;
1303                         }
1304                         ap->hsm_task_state = HSM_ST_ERR;
1305                         goto fsm_start;
1306                 }
1307
1308                 /* Device should not ask for data transfer (DRQ=1)
1309                  * when it finds something wrong.
1310                  * We ignore DRQ here and stop the HSM by
1311                  * changing hsm_task_state to HSM_ST_ERR and
1312                  * let the EH abort the command or reset the device.
1313                  */
1314                 if (status & (ATA_ERR | ATA_DF)) {
1315                         if (!(qc->dev->horkage & ATA_HORKAGE_STUCK_ERR)) {
1316                                 printf("DRQ=1 with device error, "
1317                                         "dev_stat 0x%X\n", status);
1318                                 qc->err_mask |= AC_ERR_HSM;
1319                                 ap->hsm_task_state = HSM_ST_ERR;
1320                                 goto fsm_start;
1321                         }
1322                 }
1323
1324                 if (qc->tf.protocol == ATA_PROT_PIO) {
1325                         /* PIO data out protocol.
1326                          * send first data block.
1327                          */
1328                         /* ata_pio_sectors() might change the state
1329                          * to HSM_ST_LAST. so, the state is changed here
1330                          * before ata_pio_sectors().
1331                          */
1332                         ap->hsm_task_state = HSM_ST;
1333                         ata_pio_sectors(qc);
1334                 } else {
1335                         printf("protocol is not ATA_PROT_PIO \n");
1336                 }
1337                 break;
1338
1339         case HSM_ST:
1340                 if ((status & ATA_DRQ) == 0) {
1341                         if (status & (ATA_ERR | ATA_DF)) {
1342                                 qc->err_mask |= AC_ERR_DEV;
1343                         } else {
1344                                 /* HSM violation. Let EH handle this.
1345                                  * Phantom devices also trigger this
1346                                  * condition.  Mark hint.
1347                                  */
1348                                 qc->err_mask |= AC_ERR_HSM | AC_ERR_NODEV_HINT;
1349                         }
1350
1351                         ap->hsm_task_state = HSM_ST_ERR;
1352                         goto fsm_start;
1353                 }
1354                 /* For PIO reads, some devices may ask for
1355                  * data transfer (DRQ=1) alone with ERR=1.
1356                  * We respect DRQ here and transfer one
1357                  * block of junk data before changing the
1358                  * hsm_task_state to HSM_ST_ERR.
1359                  *
1360                  * For PIO writes, ERR=1 DRQ=1 doesn't make
1361                  * sense since the data block has been
1362                  * transferred to the device.
1363                  */
1364                 if (status & (ATA_ERR | ATA_DF)) {
1365                         qc->err_mask |= AC_ERR_DEV;
1366
1367                         if (!(qc->tf.flags & ATA_TFLAG_WRITE)) {
1368                                 ata_pio_sectors(qc);
1369                                 status = ata_wait_idle(ap);
1370                         }
1371
1372                         if (status & (ATA_BUSY | ATA_DRQ))
1373                                 qc->err_mask |= AC_ERR_HSM;
1374
1375                         /* ata_pio_sectors() might change the
1376                          * state to HSM_ST_LAST. so, the state
1377                          * is changed after ata_pio_sectors().
1378                          */
1379                         ap->hsm_task_state = HSM_ST_ERR;
1380                         goto fsm_start;
1381                 }
1382
1383                 ata_pio_sectors(qc);
1384                 if (ap->hsm_task_state == HSM_ST_LAST &&
1385                         (!(qc->tf.flags & ATA_TFLAG_WRITE))) {
1386                         status = ata_wait_idle(ap);
1387                         goto fsm_start;
1388                 }
1389
1390                 poll_next = 1;
1391                 break;
1392
1393         case HSM_ST_LAST:
1394                 if (!ata_ok(status)) {
1395                         qc->err_mask |= __ac_err_mask(status);
1396                         ap->hsm_task_state = HSM_ST_ERR;
1397                         goto fsm_start;
1398                 }
1399
1400                 ap->hsm_task_state = HSM_ST_IDLE;
1401
1402                 ata_hsm_qc_complete(qc, in_wq);
1403
1404                 poll_next = 0;
1405                 break;
1406
1407         case HSM_ST_ERR:
1408                 /* make sure qc->err_mask is available to
1409                  * know what's wrong and recover
1410                  */
1411                 ap->hsm_task_state = HSM_ST_IDLE;
1412
1413                 ata_hsm_qc_complete(qc, in_wq);
1414
1415                 poll_next = 0;
1416                 break;
1417         default:
1418                 poll_next = 0;
1419         }
1420
1421         return poll_next;
1422 }
1423
1424 static void ata_pio_sectors(struct ata_queued_cmd *qc)
1425 {
1426         struct ata_port *ap;
1427         ap = pap;
1428         qc->pdata = ap->pdata;
1429
1430         ata_pio_sector(qc);
1431
1432         readb(qc->ap->ioaddr.altstatus_addr);
1433         udelay(1);
1434 }
1435
1436 static void ata_pio_sector(struct ata_queued_cmd *qc)
1437 {
1438         int do_write = (qc->tf.flags & ATA_TFLAG_WRITE);
1439         struct ata_port *ap = qc->ap;
1440         unsigned int offset;
1441         unsigned char *buf;
1442         char temp_data_buf[512];
1443
1444         if (qc->curbytes == qc->nbytes - qc->sect_size)
1445                 ap->hsm_task_state = HSM_ST_LAST;
1446
1447         offset = qc->curbytes;
1448
1449         switch (qc->tf.command) {
1450         case ATA_CMD_ID_ATA:
1451                 buf = (unsigned char *)&ata_device.id[0];
1452                 break;
1453         case ATA_CMD_PIO_READ_EXT:
1454         case ATA_CMD_PIO_READ:
1455         case ATA_CMD_PIO_WRITE_EXT:
1456         case ATA_CMD_PIO_WRITE:
1457                 buf = qc->pdata + offset;
1458                 break;
1459         default:
1460                 buf = (unsigned char *)&temp_data_buf[0];
1461         }
1462
1463         ata_mmio_data_xfer(qc->dev, buf, qc->sect_size, do_write);
1464
1465         qc->curbytes += qc->sect_size;
1466
1467 }
1468
1469 static void ata_mmio_data_xfer(struct ata_device *dev, unsigned char *buf,
1470                                 unsigned int buflen, int do_write)
1471 {
1472         struct ata_port *ap = pap;
1473         void __iomem *data_addr = ap->ioaddr.data_addr;
1474         unsigned int words = buflen >> 1;
1475         u16 *buf16 = (u16 *)buf;
1476         unsigned int i = 0;
1477
1478         udelay(100);
1479         if (do_write) {
1480                 for (i = 0; i < words; i++)
1481                         writew(le16_to_cpu(buf16[i]), data_addr);
1482         } else {
1483                 for (i = 0; i < words; i++)
1484                         buf16[i] = cpu_to_le16(readw(data_addr));
1485         }
1486
1487         if (buflen & 0x01) {
1488                 __le16 align_buf[1] = { 0 };
1489                 unsigned char *trailing_buf = buf + buflen - 1;
1490
1491                 if (do_write) {
1492                         memcpy(align_buf, trailing_buf, 1);
1493                         writew(le16_to_cpu(align_buf[0]), data_addr);
1494                 } else {
1495                         align_buf[0] = cpu_to_le16(readw(data_addr));
1496                         memcpy(trailing_buf, align_buf, 1);
1497                 }
1498         }
1499 }
1500
1501 static void ata_hsm_qc_complete(struct ata_queued_cmd *qc, int in_wq)
1502 {
1503         struct ata_port *ap = qc->ap;
1504
1505         if (in_wq) {
1506                 /* EH might have kicked in while host lock is
1507                  * released.
1508                  */
1509                 qc = &ap->qcmd[qc->tag];
1510                 if (qc) {
1511                         if (!(qc->err_mask & AC_ERR_HSM)) {
1512                                 ata_irq_on(ap);
1513                                 ata_qc_complete(qc);
1514                         } else {
1515                                 ata_port_freeze(ap);
1516                         }
1517                 }
1518         } else {
1519                 if (!(qc->err_mask & AC_ERR_HSM)) {
1520                         ata_qc_complete(qc);
1521                 } else {
1522                         ata_port_freeze(ap);
1523                 }
1524         }
1525 }
1526
1527 static u8 ata_irq_on(struct ata_port *ap)
1528 {
1529         struct ata_ioports *ioaddr = &ap->ioaddr;
1530         u8 tmp;
1531
1532         ap->ctl &= ~ATA_NIEN;
1533         ap->last_ctl = ap->ctl;
1534
1535         if (ioaddr->ctl_addr)
1536                 writeb(ap->ctl, ioaddr->ctl_addr);
1537
1538         tmp = ata_wait_idle(ap);
1539
1540         return tmp;
1541 }
1542
1543 static unsigned int ata_tag_internal(unsigned int tag)
1544 {
1545         return tag == ATA_MAX_QUEUE - 1;
1546 }
1547
1548 static void ata_qc_complete(struct ata_queued_cmd *qc)
1549 {
1550         struct ata_device *dev = qc->dev;
1551         if (qc->err_mask)
1552                 qc->flags |= ATA_QCFLAG_FAILED;
1553
1554         if (qc->flags & ATA_QCFLAG_FAILED) {
1555                 if (!ata_tag_internal(qc->tag)) {
1556                         fill_result_tf(qc);
1557                         return;
1558                 }
1559         }
1560         if (qc->flags & ATA_QCFLAG_RESULT_TF)
1561                 fill_result_tf(qc);
1562
1563         /* Some commands need post-processing after successful
1564          * completion.
1565          */
1566         switch (qc->tf.command) {
1567         case ATA_CMD_SET_FEATURES:
1568                 if (qc->tf.feature != SETFEATURES_WC_ON &&
1569                                 qc->tf.feature != SETFEATURES_WC_OFF)
1570                         break;
1571         case ATA_CMD_INIT_DEV_PARAMS:
1572         case ATA_CMD_SET_MULTI:
1573                 break;
1574
1575         case ATA_CMD_SLEEP:
1576                 dev->flags |= ATA_DFLAG_SLEEPING;
1577                 break;
1578         }
1579
1580         __ata_qc_complete(qc);
1581 }
1582
1583 static void fill_result_tf(struct ata_queued_cmd *qc)
1584 {
1585         struct ata_port *ap = qc->ap;
1586
1587         qc->result_tf.flags = qc->tf.flags;
1588         ata_tf_read(ap, &qc->result_tf);
1589 }
1590
1591 static void ata_tf_read(struct ata_port *ap, struct ata_taskfile *tf)
1592 {
1593         struct ata_ioports *ioaddr = &ap->ioaddr;
1594
1595         tf->command = ata_check_status(ap);
1596         tf->feature = readb(ioaddr->error_addr);
1597         tf->nsect = readb(ioaddr->nsect_addr);
1598         tf->lbal = readb(ioaddr->lbal_addr);
1599         tf->lbam = readb(ioaddr->lbam_addr);
1600         tf->lbah = readb(ioaddr->lbah_addr);
1601         tf->device = readb(ioaddr->device_addr);
1602
1603         if (tf->flags & ATA_TFLAG_LBA48) {
1604                 if (ioaddr->ctl_addr) {
1605                         writeb(tf->ctl | ATA_HOB, ioaddr->ctl_addr);
1606
1607                         tf->hob_feature = readb(ioaddr->error_addr);
1608                         tf->hob_nsect = readb(ioaddr->nsect_addr);
1609                         tf->hob_lbal = readb(ioaddr->lbal_addr);
1610                         tf->hob_lbam = readb(ioaddr->lbam_addr);
1611                         tf->hob_lbah = readb(ioaddr->lbah_addr);
1612
1613                         writeb(tf->ctl, ioaddr->ctl_addr);
1614                         ap->last_ctl = tf->ctl;
1615                 } else {
1616                         printf("sata_dwc warnning register read.\n");
1617                 }
1618         }
1619 }
1620
1621 static void __ata_qc_complete(struct ata_queued_cmd *qc)
1622 {
1623         struct ata_port *ap = qc->ap;
1624         struct ata_link *link = qc->dev->link;
1625
1626         link->active_tag = ATA_TAG_POISON;
1627         ap->nr_active_links--;
1628
1629         if (qc->flags & ATA_QCFLAG_CLEAR_EXCL && ap->excl_link == link)
1630                 ap->excl_link = NULL;
1631
1632         qc->flags &= ~ATA_QCFLAG_ACTIVE;
1633         ap->qc_active &= ~(1 << qc->tag);
1634 }
1635
1636 static void ata_qc_free(struct ata_queued_cmd *qc)
1637 {
1638         struct ata_port *ap = qc->ap;
1639         unsigned int tag;
1640         qc->flags = 0;
1641         tag = qc->tag;
1642         if (tag < ATA_MAX_QUEUE) {
1643                 qc->tag = ATA_TAG_POISON;
1644                 clear_bit(tag, &ap->qc_allocated);
1645         }
1646 }
1647
1648 static int check_sata_dev_state(void)
1649 {
1650         unsigned long datalen;
1651         unsigned char *pdata;
1652         int ret = 0;
1653         int i = 0;
1654         char temp_data_buf[512];
1655
1656         while (1) {
1657                 udelay(10000);
1658
1659                 pdata = (unsigned char*)&temp_data_buf[0];
1660                 datalen = 512;
1661
1662                 ret = ata_dev_read_sectors(pdata, datalen, 0, 1);
1663
1664                 if (ret == TRUE)
1665                         break;
1666
1667                 i++;
1668                 if (i > (ATA_RESET_TIME * 100)) {
1669                         printf("** TimeOUT **\n");
1670                         dev_state = SATA_NODEVICE;
1671                         return FALSE;
1672                 }
1673
1674                 if ((i >= 100) && ((i % 100) == 0))
1675                         printf(".");
1676         }
1677
1678         dev_state = SATA_READY;
1679
1680         return TRUE;
1681 }
1682
1683 static unsigned int ata_dev_set_feature(struct ata_device *dev,
1684                                 u8 enable, u8 feature)
1685 {
1686         struct ata_taskfile tf;
1687         struct ata_port *ap;
1688         ap = pap;
1689         unsigned int err_mask;
1690
1691         memset(&tf, 0, sizeof(tf));
1692         tf.ctl = ap->ctl;
1693
1694         tf.device = ATA_DEVICE_OBS;
1695         tf.command = ATA_CMD_SET_FEATURES;
1696         tf.feature = enable;
1697         tf.flags |= ATA_TFLAG_ISADDR | ATA_TFLAG_DEVICE;
1698         tf.protocol = ATA_PROT_NODATA;
1699         tf.nsect = feature;
1700
1701         err_mask = ata_exec_internal(dev, &tf, NULL, DMA_NONE, 0, 0);
1702
1703         return err_mask;
1704 }
1705
1706 static unsigned int ata_dev_init_params(struct ata_device *dev,
1707                                 u16 heads, u16 sectors)
1708 {
1709         struct ata_taskfile tf;
1710         struct ata_port *ap;
1711         ap = pap;
1712         unsigned int err_mask;
1713
1714         if (sectors < 1 || sectors > 255 || heads < 1 || heads > 16)
1715                 return AC_ERR_INVALID;
1716
1717         memset(&tf, 0, sizeof(tf));
1718         tf.ctl = ap->ctl;
1719         tf.device = ATA_DEVICE_OBS;
1720         tf.command = ATA_CMD_INIT_DEV_PARAMS;
1721         tf.flags |= ATA_TFLAG_ISADDR | ATA_TFLAG_DEVICE;
1722         tf.protocol = ATA_PROT_NODATA;
1723         tf.nsect = sectors;
1724         tf.device |= (heads - 1) & 0x0f;
1725
1726         err_mask = ata_exec_internal(dev, &tf, NULL, DMA_NONE, 0, 0);
1727
1728         if (err_mask == AC_ERR_DEV && (tf.feature & ATA_ABORTED))
1729                 err_mask = 0;
1730
1731         return err_mask;
1732 }
1733
1734 #if defined(CONFIG_SATA_DWC) && !defined(CONFIG_LBA48)
1735 #define SATA_MAX_READ_BLK 0xFF
1736 #else
1737 #define SATA_MAX_READ_BLK 0xFFFF
1738 #endif
1739
1740 ulong sata_read(int device, ulong blknr, lbaint_t blkcnt, void *buffer)
1741 {
1742         ulong start,blks, buf_addr;
1743         unsigned short smallblks;
1744         unsigned long datalen;
1745         unsigned char *pdata;
1746         device &= 0xff;
1747
1748         u32 block = 0;
1749         u32 n_block = 0;
1750
1751         if (dev_state != SATA_READY)
1752                 return 0;
1753
1754         buf_addr = (unsigned long)buffer;
1755         start = blknr;
1756         blks = blkcnt;
1757         do {
1758                 pdata = (unsigned char *)buf_addr;
1759                 if (blks > SATA_MAX_READ_BLK) {
1760                         datalen = sata_dev_desc[device].blksz * SATA_MAX_READ_BLK;
1761                         smallblks = SATA_MAX_READ_BLK;
1762
1763                         block = (u32)start;
1764                         n_block = (u32)smallblks;
1765
1766                         start += SATA_MAX_READ_BLK;
1767                         blks -= SATA_MAX_READ_BLK;
1768                 } else {
1769                         datalen = sata_dev_desc[device].blksz * SATA_MAX_READ_BLK;
1770                         datalen = sata_dev_desc[device].blksz * blks;
1771                         smallblks = (unsigned short)blks;
1772
1773                         block = (u32)start;
1774                         n_block = (u32)smallblks;
1775
1776                         start += blks;
1777                         blks = 0;
1778                 }
1779
1780                 if (ata_dev_read_sectors(pdata, datalen, block, n_block) != TRUE) {
1781                         printf("sata_dwc : Hard disk read error.\n");
1782                         blkcnt -= blks;
1783                         break;
1784                 }
1785                 buf_addr += datalen;
1786         } while (blks != 0);
1787
1788         return (blkcnt);
1789 }
1790
1791 static int ata_dev_read_sectors(unsigned char *pdata, unsigned long datalen,
1792                                                 u32 block, u32 n_block)
1793 {
1794         struct ata_port *ap = pap;
1795         struct ata_device *dev = &ata_device;
1796         struct ata_taskfile tf;
1797         unsigned int class = ATA_DEV_ATA;
1798         unsigned int err_mask = 0;
1799         const char *reason;
1800         int may_fallback = 1;
1801
1802         if (dev_state == SATA_ERROR)
1803                 return FALSE;
1804
1805         ata_dev_select(ap, dev->devno, 1, 1);
1806
1807 retry:
1808         memset(&tf, 0, sizeof(tf));
1809         tf.ctl = ap->ctl;
1810         ap->print_id = 1;
1811         ap->flags &= ~ATA_FLAG_DISABLED;
1812
1813         ap->pdata = pdata;
1814
1815         tf.device = ATA_DEVICE_OBS;
1816
1817         temp_n_block = n_block;
1818
1819 #ifdef CONFIG_LBA48
1820         tf.command = ATA_CMD_PIO_READ_EXT;
1821         tf.flags |= ATA_TFLAG_LBA | ATA_TFLAG_LBA48;
1822
1823         tf.hob_feature = 31;
1824         tf.feature = 31;
1825         tf.hob_nsect = (n_block >> 8) & 0xff;
1826         tf.nsect = n_block & 0xff;
1827
1828         tf.hob_lbah = 0x0;
1829         tf.hob_lbam = 0x0;
1830         tf.hob_lbal = (block >> 24) & 0xff;
1831         tf.lbah = (block >> 16) & 0xff;
1832         tf.lbam = (block >> 8) & 0xff;
1833         tf.lbal = block & 0xff;
1834
1835         tf.device = 1 << 6;
1836         if (tf.flags & ATA_TFLAG_FUA)
1837                 tf.device |= 1 << 7;
1838 #else
1839         tf.command = ATA_CMD_PIO_READ;
1840         tf.flags |= ATA_TFLAG_LBA ;
1841
1842         tf.feature = 31;
1843         tf.nsect = n_block & 0xff;
1844
1845         tf.lbah = (block >> 16) & 0xff;
1846         tf.lbam = (block >> 8) & 0xff;
1847         tf.lbal = block & 0xff;
1848
1849         tf.device = (block >> 24) & 0xf;
1850
1851         tf.device |= 1 << 6;
1852         if (tf.flags & ATA_TFLAG_FUA)
1853                 tf.device |= 1 << 7;
1854
1855 #endif
1856
1857         tf.protocol = ATA_PROT_PIO;
1858
1859         /* Some devices choke if TF registers contain garbage.  Make
1860          * sure those are properly initialized.
1861          */
1862         tf.flags |= ATA_TFLAG_ISADDR | ATA_TFLAG_DEVICE;
1863         tf.flags |= ATA_TFLAG_POLLING;
1864
1865         err_mask = ata_exec_internal(dev, &tf, NULL, DMA_FROM_DEVICE, 0, 0);
1866
1867         if (err_mask) {
1868                 if (err_mask & AC_ERR_NODEV_HINT) {
1869                         printf("READ_SECTORS NODEV after polling detection\n");
1870                         return -ENOENT;
1871                 }
1872
1873                 if ((err_mask == AC_ERR_DEV) && (tf.feature & ATA_ABORTED)) {
1874                         /* Device or controller might have reported
1875                          * the wrong device class.  Give a shot at the
1876                          * other IDENTIFY if the current one is
1877                          * aborted by the device.
1878                          */
1879                         if (may_fallback) {
1880                                 may_fallback = 0;
1881
1882                                 if (class == ATA_DEV_ATA) {
1883                                         class = ATA_DEV_ATAPI;
1884                                 } else {
1885                                         class = ATA_DEV_ATA;
1886                                 }
1887                                 goto retry;
1888                         }
1889                         /* Control reaches here iff the device aborted
1890                          * both flavors of IDENTIFYs which happens
1891                          * sometimes with phantom devices.
1892                          */
1893                         printf("both IDENTIFYs aborted, assuming NODEV\n");
1894                         return -ENOENT;
1895                 }
1896
1897                 reason = "I/O error";
1898                 goto err_out;
1899         }
1900
1901         return TRUE;
1902
1903 err_out:
1904         printf("failed to READ SECTORS (%s, err_mask=0x%x)\n", reason, err_mask);
1905         return FALSE;
1906 }
1907
1908 #if defined(CONFIG_SATA_DWC) && !defined(CONFIG_LBA48)
1909 #define SATA_MAX_WRITE_BLK 0xFF
1910 #else
1911 #define SATA_MAX_WRITE_BLK 0xFFFF
1912 #endif
1913
1914 ulong sata_write(int device, ulong blknr, lbaint_t blkcnt, void *buffer)
1915 {
1916         ulong start,blks, buf_addr;
1917         unsigned short smallblks;
1918         unsigned long datalen;
1919         unsigned char *pdata;
1920         device &= 0xff;
1921
1922
1923         u32 block = 0;
1924         u32 n_block = 0;
1925
1926         if (dev_state != SATA_READY)
1927                 return 0;
1928
1929         buf_addr = (unsigned long)buffer;
1930         start = blknr;
1931         blks = blkcnt;
1932         do {
1933                 pdata = (unsigned char *)buf_addr;
1934                 if (blks > SATA_MAX_WRITE_BLK) {
1935                         datalen = sata_dev_desc[device].blksz * SATA_MAX_WRITE_BLK;
1936                         smallblks = SATA_MAX_WRITE_BLK;
1937
1938                         block = (u32)start;
1939                         n_block = (u32)smallblks;
1940
1941                         start += SATA_MAX_WRITE_BLK;
1942                         blks -= SATA_MAX_WRITE_BLK;
1943                 } else {
1944                         datalen = sata_dev_desc[device].blksz * blks;
1945                         smallblks = (unsigned short)blks;
1946
1947                         block = (u32)start;
1948                         n_block = (u32)smallblks;
1949
1950                         start += blks;
1951                         blks = 0;
1952                 }
1953
1954                 if (ata_dev_write_sectors(pdata, datalen, block, n_block) != TRUE) {
1955                         printf("sata_dwc : Hard disk read error.\n");
1956                         blkcnt -= blks;
1957                         break;
1958                 }
1959                 buf_addr += datalen;
1960         } while (blks != 0);
1961
1962         return (blkcnt);
1963 }
1964
1965 static int ata_dev_write_sectors(unsigned char* pdata, unsigned long datalen,
1966                                                 u32 block, u32 n_block)
1967 {
1968         struct ata_port *ap = pap;
1969         struct ata_device *dev = &ata_device;
1970         struct ata_taskfile tf;
1971         unsigned int class = ATA_DEV_ATA;
1972         unsigned int err_mask = 0;
1973         const char *reason;
1974         int may_fallback = 1;
1975
1976         if (dev_state == SATA_ERROR)
1977                 return FALSE;
1978
1979         ata_dev_select(ap, dev->devno, 1, 1);
1980
1981 retry:
1982         memset(&tf, 0, sizeof(tf));
1983         tf.ctl = ap->ctl;
1984         ap->print_id = 1;
1985         ap->flags &= ~ATA_FLAG_DISABLED;
1986
1987         ap->pdata = pdata;
1988
1989         tf.device = ATA_DEVICE_OBS;
1990
1991         temp_n_block = n_block;
1992
1993
1994 #ifdef CONFIG_LBA48
1995         tf.command = ATA_CMD_PIO_WRITE_EXT;
1996         tf.flags |= ATA_TFLAG_LBA | ATA_TFLAG_LBA48 | ATA_TFLAG_WRITE;
1997
1998         tf.hob_feature = 31;
1999         tf.feature = 31;
2000         tf.hob_nsect = (n_block >> 8) & 0xff;
2001         tf.nsect = n_block & 0xff;
2002
2003         tf.hob_lbah = 0x0;
2004         tf.hob_lbam = 0x0;
2005         tf.hob_lbal = (block >> 24) & 0xff;
2006         tf.lbah = (block >> 16) & 0xff;
2007         tf.lbam = (block >> 8) & 0xff;
2008         tf.lbal = block & 0xff;
2009
2010         tf.device = 1 << 6;
2011         if (tf.flags & ATA_TFLAG_FUA)
2012                 tf.device |= 1 << 7;
2013 #else
2014         tf.command = ATA_CMD_PIO_WRITE;
2015         tf.flags |= ATA_TFLAG_LBA | ATA_TFLAG_WRITE;
2016
2017         tf.feature = 31;
2018         tf.nsect = n_block & 0xff;
2019
2020         tf.lbah = (block >> 16) & 0xff;
2021         tf.lbam = (block >> 8) & 0xff;
2022         tf.lbal = block & 0xff;
2023
2024         tf.device = (block >> 24) & 0xf;
2025
2026         tf.device |= 1 << 6;
2027         if (tf.flags & ATA_TFLAG_FUA)
2028                 tf.device |= 1 << 7;
2029
2030 #endif
2031
2032         tf.protocol = ATA_PROT_PIO;
2033
2034         /* Some devices choke if TF registers contain garbage.  Make
2035          * sure those are properly initialized.
2036          */
2037         tf.flags |= ATA_TFLAG_ISADDR | ATA_TFLAG_DEVICE;
2038         tf.flags |= ATA_TFLAG_POLLING;
2039
2040         err_mask = ata_exec_internal(dev, &tf, NULL, DMA_FROM_DEVICE, 0, 0);
2041
2042         if (err_mask) {
2043                 if (err_mask & AC_ERR_NODEV_HINT) {
2044                         printf("READ_SECTORS NODEV after polling detection\n");
2045                         return -ENOENT;
2046                 }
2047
2048                 if ((err_mask == AC_ERR_DEV) && (tf.feature & ATA_ABORTED)) {
2049                         /* Device or controller might have reported
2050                          * the wrong device class.  Give a shot at the
2051                          * other IDENTIFY if the current one is
2052                          * aborted by the device.
2053                          */
2054                         if (may_fallback) {
2055                                 may_fallback = 0;
2056
2057                                 if (class == ATA_DEV_ATA) {
2058                                         class = ATA_DEV_ATAPI;
2059                                 } else {
2060                                         class = ATA_DEV_ATA;
2061                                 }
2062                                 goto retry;
2063                         }
2064                         /* Control reaches here iff the device aborted
2065                          * both flavors of IDENTIFYs which happens
2066                          * sometimes with phantom devices.
2067                          */
2068                         printf("both IDENTIFYs aborted, assuming NODEV\n");
2069                         return -ENOENT;
2070                 }
2071
2072                 reason = "I/O error";
2073                 goto err_out;
2074         }
2075
2076         return TRUE;
2077
2078 err_out:
2079         printf("failed to WRITE SECTORS (%s, err_mask=0x%x)\n", reason, err_mask);
2080         return FALSE;
2081 }