1 files changed, 55 insertions, 27 deletions

diff --git a/drivers/scsi/ahci.c b/drivers/scsi/ahci.c
index 19bd346951dd..30676b0eb366 100644
--- a/drivers/scsi/ahci.c
+++ b/drivers/scsi/ahci.c
@@ -446,10 +446,61 @@ static void ahci_scr_write (struct ata_port *ap, unsigned int sc_reg_in,
         writel(val, (void __iomem *) ap->ioaddr.scr_addr + (sc_reg * 4));
 }
 
-static void ahci_phy_reset(struct ata_port *ap)
+static int ahci_stop_engine(struct ata_port *ap)
+{
+        void __iomem *mmio = ap->host_set->mmio_base;
+        void __iomem *port_mmio = ahci_port_base(mmio, ap->port_no);
+        int work;
+        u32 tmp;
+
+        tmp = readl(port_mmio + PORT_CMD);
+        tmp &= ~PORT_CMD_START;
+        writel(tmp, port_mmio + PORT_CMD);
+
+        /* wait for engine to stop.  TODO: this could be
+         * as long as 500 msec
+         */
+        work = 1000;
+        while (work-- > 0) {
+                tmp = readl(port_mmio + PORT_CMD);
+                if ((tmp & PORT_CMD_LIST_ON) == 0)
+                        return 0;
+                udelay(10);
+        }
+
+        return -EIO;
+}
+
+static void ahci_start_engine(struct ata_port *ap)
+{
+        void __iomem *mmio = ap->host_set->mmio_base;
+        void __iomem *port_mmio = ahci_port_base(mmio, ap->port_no);
+        u32 tmp;
+
+        tmp = readl(port_mmio + PORT_CMD);
+        tmp |= PORT_CMD_START;
+        writel(tmp, port_mmio + PORT_CMD);
+        readl(port_mmio + PORT_CMD); /* flush */
+}
+
+static unsigned int ahci_dev_classify(struct ata_port *ap)
 {
         void __iomem *port_mmio = (void __iomem *) ap->ioaddr.cmd_addr;
         struct ata_taskfile tf;
+        u32 tmp;
+
+        tmp = readl(port_mmio + PORT_SIG);
+        tf.lbah         = (tmp >> 24)   & 0xff;
+        tf.lbam         = (tmp >> 16)   & 0xff;
+        tf.lbal         = (tmp >> 8)    & 0xff;
+        tf.nsect        = (tmp)         & 0xff;
+
+        return ata_dev_classify(&tf);
+}
+
+static void ahci_phy_reset(struct ata_port *ap)
+{
+        void __iomem *port_mmio = (void __iomem *) ap->ioaddr.cmd_addr;
         struct ata_device *dev = &ap->device[0];
         u32 new_tmp, tmp;
 
@@ -458,13 +509,7 @@ static void ahci_phy_reset(struct ata_port *ap)
         if (ap->flags & ATA_FLAG_PORT_DISABLED)
                 return;
 
-        tmp = readl(port_mmio + PORT_SIG);
-        tf.lbah         = (tmp >> 24)   & 0xff;
-        tf.lbam         = (tmp >> 16)   & 0xff;
-        tf.lbal         = (tmp >> 8)    & 0xff;
-        tf.nsect        = (tmp)         & 0xff;
-
-        dev->class = ata_dev_classify(&tf);
+        dev->class = ahci_dev_classify(ap);
         if (!ata_dev_present(dev)) {
                 ata_port_disable(ap);
                 return;
@@ -572,7 +617,6 @@ static void ahci_restart_port(struct ata_port *ap, u32 irq_stat)
         void __iomem *mmio = ap->host_set->mmio_base;
         void __iomem *port_mmio = ahci_port_base(mmio, ap->port_no);
         u32 tmp;
-        int work;
 
         if ((ap->device[0].class != ATA_DEV_ATAPI) ||
             ((irq_stat & PORT_IRQ_TF_ERR) == 0))
@@ -588,20 +632,7 @@ static void ahci_restart_port(struct ata_port *ap, u32 irq_stat)
                         readl(port_mmio + PORT_SCR_ERR));
 
         /* stop DMA */
-        tmp = readl(port_mmio + PORT_CMD);
-        tmp &= ~PORT_CMD_START;
-        writel(tmp, port_mmio + PORT_CMD);
-
-        /* wait for engine to stop.  TODO: this could be
-         * as long as 500 msec
-         */
-        work = 1000;
-        while (work-- > 0) {
-                tmp = readl(port_mmio + PORT_CMD);
-                if ((tmp & PORT_CMD_LIST_ON) == 0)
-                        break;
-                udelay(10);
-        }
+        ahci_stop_engine(ap);
 
         /* clear SATA phy error, if any */
         tmp = readl(port_mmio + PORT_SCR_ERR);
@@ -620,10 +651,7 @@ static void ahci_restart_port(struct ata_port *ap, u32 irq_stat)
         }
 
         /* re-start DMA */
-        tmp = readl(port_mmio + PORT_CMD);
-        tmp |= PORT_CMD_START;
-        writel(tmp, port_mmio + PORT_CMD);
-        readl(port_mmio + PORT_CMD); /* flush */
+        ahci_start_engine(ap);
 }
 
 static void ahci_eng_timeout(struct ata_port *ap)