Fixup ahci suspend / resume

index a800fb5..cb2c9e5 100644
@@ -44,6 +44,7 @@
 #include <linux/device.h>
 #include <scsi/scsi_host.h>
 #include <scsi/scsi_cmnd.h>
+#include <scsi/scsi_device.h>
 #include <linux/libata.h>
 #include <asm/io.h>
 

@@ -186,15 +187,21 @@ static void ahci_scr_write (struct ata_p
 static int ahci_init_one (struct pci_dev *pdev, const struct pci_device_id *ent);
 static int ahci_qc_issue(struct ata_queued_cmd *qc);
 static irqreturn_t ahci_interrupt (int irq, void *dev_instance, struct pt_regs *regs);
+static void ahci_start_engine(struct ata_port *ap);
+static int ahci_stop_engine(struct ata_port *ap);
 static void ahci_phy_reset(struct ata_port *ap);
 static void ahci_irq_clear(struct ata_port *ap);
 static void ahci_eng_timeout(struct ata_port *ap);
 static int ahci_port_start(struct ata_port *ap);
+static void ahci_port_suspend(struct ata_port *ap);
+static void ahci_port_resume(struct ata_port *ap);
 static void ahci_port_stop(struct ata_port *ap);
 static void ahci_tf_read(struct ata_port *ap, struct ata_taskfile *tf);
 static void ahci_qc_prep(struct ata_queued_cmd *qc);
 static u8 ahci_check_status(struct ata_port *ap);
 static inline int ahci_host_intr(struct ata_port *ap, struct ata_queued_cmd *qc);
+static int ahci_scsi_device_suspend(struct scsi_device *sdev);
+static int ahci_scsi_device_resume(struct scsi_device *sdev);
 static void ahci_remove_one (struct pci_dev *pdev);
 
 static struct scsi_host_template ahci_sht = {

@@ -214,6 +221,8 @@ static struct scsi_host_template ahci_sh
 	.dma_boundary		= AHCI_DMA_BOUNDARY,
 	.slave_configure	= ata_scsi_slave_config,
 	.bios_param		= ata_std_bios_param,
+	.resume			= ahci_scsi_device_resume,
+	.suspend		= ahci_scsi_device_suspend,
 };
 
 static const struct ata_port_operations ahci_ops = {

@@ -299,6 +308,8 @@ static struct pci_driver ahci_pci_driver
 	.id_table		= ahci_pci_tbl,
 	.probe			= ahci_init_one,
 	.remove			= ahci_remove_one,
+	.suspend		= ata_pci_device_suspend,
+	.resume			= ata_pci_device_resume,
 };
 
 

@@ -315,10 +326,7 @@ static inline void __iomem *ahci_port_ba
 static int ahci_port_start(struct ata_port *ap)
 {
 	struct device *dev = ap->host_set->dev;
-	struct ahci_host_priv *hpriv = ap->host_set->private_data;
 	struct ahci_port_priv *pp;
-	void __iomem *mmio = ap->host_set->mmio_base;
-	void __iomem *port_mmio = ahci_port_base(mmio, ap->port_no);
 	void *mem;
 	dma_addr_t mem_dma;
 	int rc;

@@ -372,6 +380,22 @@ static int ahci_port_start(struct ata_po
 
 	ap->private_data = pp;
 
+	/*
+	 * Internal structures are initialized,
+	 * we can now do a simple resume()
+	 */
+	ahci_port_resume(ap);
+
+	return 0;
+}
+
+static void ahci_port_resume(struct ata_port *ap)
+{
+	void *mmio = ap->host_set->mmio_base;
+	void *port_mmio = ahci_port_base(mmio, ap->port_no);
+	struct ahci_host_priv *hpriv = ap->host_set->private_data;
+	struct ahci_port_priv *pp = ap->private_data;
+
 	if (hpriv->cap & HOST_CAP_64)
 		writel((pp->cmd_slot_dma >> 16) >> 16, port_mmio + PORT_LST_ADDR_HI);
 	writel(pp->cmd_slot_dma & 0xffffffff, port_mmio + PORT_LST_ADDR);

@@ -383,31 +407,49 @@ static int ahci_port_start(struct ata_po
 	readl(port_mmio + PORT_FIS_ADDR); /* flush */
 
 	writel(PORT_CMD_ICC_ACTIVE | PORT_CMD_FIS_RX |
-	       PORT_CMD_POWER_ON | PORT_CMD_SPIN_UP |
-	       PORT_CMD_START, port_mmio + PORT_CMD);
+	       PORT_CMD_POWER_ON | PORT_CMD_SPIN_UP,
+	       port_mmio + PORT_CMD);
 	readl(port_mmio + PORT_CMD); /* flush */
 
-	return 0;
+	ahci_start_engine(ap);
 }
 
-
-static void ahci_port_stop(struct ata_port *ap)
+static void ahci_port_suspend(struct ata_port *ap)
 {
-	struct device *dev = ap->host_set->dev;
-	struct ahci_port_priv *pp = ap->private_data;
-	void __iomem *mmio = ap->host_set->mmio_base;
-	void __iomem *port_mmio = ahci_port_base(mmio, ap->port_no);
+	void *mmio = ap->host_set->mmio_base;
+	void *port_mmio = ahci_port_base(mmio, ap->port_no);
 	u32 tmp;
+	int work;
 
+	ahci_stop_engine(ap);
+
+	/*
+	 * Disable FIS reception
+	 */
 	tmp = readl(port_mmio + PORT_CMD);
-	tmp &= ~(PORT_CMD_START | PORT_CMD_FIS_RX);
+	tmp &= ~(PORT_CMD_FIS_RX);
 	writel(tmp, port_mmio + PORT_CMD);
 	readl(port_mmio + PORT_CMD); /* flush */
 
-	/* spec says 500 msecs for each PORT_CMD_{START,FIS_RX} bit, so
-	 * this is slightly incorrect.
+	/*
+	 * Wait for HBA to acknowledge.
+	 * This could be as long as 500 msec
 	 */
-	msleep(500);
+	work = 1000;
+	while (work-- > 0) {
+		tmp = readl(port_mmio + PORT_CMD);
+		if ((tmp & PORT_CMD_FIS_ON) == 0)
+			break;
+		udelay(10);
+	}
+}
+
+static void ahci_port_stop(struct ata_port *ap)
+{
+	struct device *dev = ap->host_set->dev;
+	struct ahci_port_priv *pp = ap->private_data;
+
+	ahci_port_suspend(ap);
 
 	ap->private_data = NULL;
 	dma_free_coherent(dev, AHCI_PORT_PRIV_DMA_SZ,

@@ -457,11 +499,19 @@ static void ahci_phy_reset(struct ata_po
 	struct ata_device *dev = &ap->device[0];
 	u32 new_tmp, tmp;
 
+	ahci_stop_engine(ap);
+
 	__sata_phy_reset(ap);
 
+	/* clear SATA phy error, if any */
+	tmp = readl(port_mmio + PORT_SCR_ERR);
+	writel(tmp, port_mmio + PORT_SCR_ERR);
+
 	if (ap->flags & ATA_FLAG_PORT_DISABLED)
 		return;
 
+	ahci_start_engine(ap);
+
 	tmp = readl(port_mmio + PORT_SIG);
 	tf.lbah		= (tmp >> 24)	& 0xff;
 	tf.lbam		= (tmp >> 16)	& 0xff;

@@ -571,6 +621,65 @@ static void ahci_qc_prep(struct ata_queu
 	pp->cmd_slot[0].opts |= cpu_to_le32(n_elem << 16);
 }
 
+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;
+	int work;
+
+	tmp = readl(port_mmio + PORT_CMD);
+	/*
+	 * AHCI rev 1.1 section 10.3.1:
+	 * Software shall not set PxCMD.ST to ‘1’ until it verifies
+	 * that PxCMD.CR is ‘0’ and has set PxCMD.FRE to ‘1’.
+	 */
+	if ((tmp & PORT_CMD_FIS_RX) == 0)
+		printk(KERN_WARNING "ata%d: dma not running\n",ap->id);
+	/* 
+	 * wait for engine to become idle.
+	 */
+	work = 1000;
+	while (work-- > 0) {
+		tmp = readl(port_mmio + PORT_CMD);
+		if ((tmp & PORT_CMD_LIST_ON) == 0)
+			break;
+		udelay(10);
+	}
+	
+	/*
+	 * Start DMA
+	 */
+	tmp |= PORT_CMD_START;
+	writel(tmp, port_mmio + PORT_CMD);
+	readl(port_mmio + PORT_CMD); /* flush */
+}
+
+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 become idle
+	 */
+	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_restart_port(struct ata_port *ap, u32 irq_stat)
 {
 	void __iomem *mmio = ap->host_set->mmio_base;

@@ -592,20 +701,7 @@ static void ahci_restart_port(struct ata
 			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);

@@ -624,10 +720,7 @@ static void ahci_restart_port(struct ata
 	}
 
 	/* 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)

@@ -787,6 +880,30 @@ static int ahci_qc_issue(struct ata_queu
 	return 0;
 }
 
+int ahci_scsi_device_suspend(struct scsi_device *sdev)
+{
+	struct ata_port *ap = (struct ata_port *) &sdev->host->hostdata[0];
+	struct ata_device *dev = &ap->device[sdev->id];
+	int rc;
+
+	rc = ata_device_suspend(ap, dev);
+
+	if (!rc)
+		ahci_port_suspend(ap);
+
+	return rc;
+}
+
+int ahci_scsi_device_resume(struct scsi_device *sdev)
+{
+	struct ata_port *ap = (struct ata_port *) &sdev->host->hostdata[0];
+	struct ata_device *dev = &ap->device[sdev->id];
+
+	ahci_port_resume(ap);
+
+	return ata_device_resume(ap, dev);
+}
+
 static void ahci_setup_port(struct ata_ioports *port, unsigned long base,
 			    unsigned int port_idx)
 {

index 1c2ab3d..e0dc933 100644
@@ -30,6 +30,12 @@
  * http://www.intel.com/technology/serialata/pdf/rev1_0.pdf
  * http://www.intel.com/technology/serialata/pdf/rev1_1.pdf
  *
+ * TODO:
+ * - Error handling; some routines might fail (eg port_suspend / _resume)
+ *   upon which a port reset should be done
+ * - Proper handling of additional features: cold-presence detection,
+ *   staggered spin-up, power modes. Currently we're calling those commands
+ *   as we pleas.
  */
 
 #include <linux/kernel.h>

@@ -44,6 +50,7 @@
 #include <linux/device.h>
 #include <scsi/scsi_host.h>
 #include <scsi/scsi_cmnd.h>
+#include <scsi/scsi_device.h>
 #include <linux/libata.h>
 #include <asm/io.h>
 

@@ -194,11 +201,21 @@ static int ahci_probe_reset(struct ata_p
 static void ahci_irq_clear(struct ata_port *ap);
 static void ahci_eng_timeout(struct ata_port *ap);
 static int ahci_port_start(struct ata_port *ap);
+static int ahci_port_resume(struct ata_port *ap);
+static int ahci_port_suspend(struct ata_port *ap);
 static void ahci_port_stop(struct ata_port *ap);
+static int ahci_start_engine(void __iomem *port_mmio);
+static int ahci_stop_engine(void __iomem *port_mmio);
+static void ahci_start_fis_rx(void __iomem *port_mmio, 
+			      struct ahci_port_priv *pp,
+			      struct ahci_host_priv *hpriv);
+static int ahci_stop_fis_rx(void __iomem *port_mmio);
 static void ahci_tf_read(struct ata_port *ap, struct ata_taskfile *tf);
 static void ahci_qc_prep(struct ata_queued_cmd *qc);
 static u8 ahci_check_status(struct ata_port *ap);
 static inline int ahci_host_intr(struct ata_port *ap, struct ata_queued_cmd *qc);
+static int ahci_scsi_device_suspend(struct scsi_device *sdev);
+static int ahci_scsi_device_resume(struct scsi_device *sdev);
 static void ahci_remove_one (struct pci_dev *pdev);
 
 static struct scsi_host_template ahci_sht = {

@@ -218,6 +235,8 @@ static struct scsi_host_template ahci_sh
 	.dma_boundary		= AHCI_DMA_BOUNDARY,
 	.slave_configure	= ata_scsi_slave_config,
 	.bios_param		= ata_std_bios_param,
+	.resume			= ahci_scsi_device_resume,
+	.suspend		= ahci_scsi_device_suspend,
 };
 
 static const struct ata_port_operations ahci_ops = {

@@ -302,6 +321,8 @@ static struct pci_driver ahci_pci_driver
 	.id_table		= ahci_pci_tbl,
 	.probe			= ahci_init_one,
 	.remove			= ahci_remove_one,
+	.suspend		= ata_pci_device_suspend,
+	.resume			= ata_pci_device_resume,       
 };
 
 

@@ -375,42 +396,24 @@ static int ahci_port_start(struct ata_po
 
 	ap->private_data = pp;
 
-	if (hpriv->cap & HOST_CAP_64)
-		writel((pp->cmd_slot_dma >> 16) >> 16, port_mmio + PORT_LST_ADDR_HI);
-	writel(pp->cmd_slot_dma & 0xffffffff, port_mmio + PORT_LST_ADDR);
-	readl(port_mmio + PORT_LST_ADDR); /* flush */
-
-	if (hpriv->cap & HOST_CAP_64)
-		writel((pp->rx_fis_dma >> 16) >> 16, port_mmio + PORT_FIS_ADDR_HI);
-	writel(pp->rx_fis_dma & 0xffffffff, port_mmio + PORT_FIS_ADDR);
-	readl(port_mmio + PORT_FIS_ADDR); /* flush */
-
-	writel(PORT_CMD_ICC_ACTIVE | PORT_CMD_FIS_RX |
-	       PORT_CMD_POWER_ON | PORT_CMD_SPIN_UP |
-	       PORT_CMD_START, port_mmio + PORT_CMD);
-	readl(port_mmio + PORT_CMD); /* flush */
-
-	return 0;
+	/*
+	 * Driver is setup, now initialize the HBA
+	 */
+	ahci_start_fis_rx(port_mmio, pp, hpriv);
+	
+	rc = ahci_start_engine(port_mmio);
+	if (rc)
+		printk(KERN_WARNING "ata%u: could not start DMA engine\n", 
+		       ap->id);
+	return rc;
 }
 
-
 static void ahci_port_stop(struct ata_port *ap)
 {
 	struct device *dev = ap->host_set->dev;
 	struct ahci_port_priv *pp = ap->private_data;
-	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 | PORT_CMD_FIS_RX);
-	writel(tmp, port_mmio + PORT_CMD);
-	readl(port_mmio + PORT_CMD); /* flush */
-
-	/* spec says 500 msecs for each PORT_CMD_{START,FIS_RX} bit, so
-	 * this is slightly incorrect.
-	 */
-	msleep(500);
+	ahci_port_suspend(ap);
 
 	ap->private_data = NULL;
 	dma_free_coherent(dev, AHCI_PORT_PRIV_DMA_SZ,

@@ -419,6 +422,45 @@ static void ahci_port_stop(struct ata_po
 	kfree(pp);
 }
 
+static int ahci_port_resume(struct ata_port *ap)
+{
+	void __iomem *mmio = ap->host_set->mmio_base;
+	void __iomem *port_mmio = ahci_port_base(mmio, ap->port_no);
+	struct ahci_host_priv *hpriv = ap->host_set->private_data;
+	struct ahci_port_priv *pp = ap->private_data;
+
+	/*
+	 * Enable FIS reception
+	 */
+	ahci_start_fis_rx(port_mmio, pp, hpriv);
+
+	/*
+	 * Enable DMA
+	 */
+	return ahci_start_engine(port_mmio);
+}
+
+static int ahci_port_suspend(struct ata_port *ap)
+{
+	void __iomem *mmio = ap->host_set->mmio_base;
+	void __iomem *port_mmio = ahci_port_base(mmio, ap->port_no);
+	int rc;
+
+	/*
+	 * Disable DMA
+	 */
+	rc = ahci_stop_engine(port_mmio);
+	if (rc) {
+		printk(KERN_WARNING "ata%u: DMA engine busy\n", ap->id);
+		return rc;
+	}
+
+	/*
+	 * Disable FIS reception
+	 */
+	return ahci_stop_fis_rx(port_mmio);
+}
+
 static u32 ahci_scr_read (struct ata_port *ap, unsigned int sc_reg_in)
 {
 	unsigned int sc_reg;

@@ -453,19 +495,23 @@ static void ahci_scr_write (struct ata_p
 	writel(val, (void __iomem *) ap->ioaddr.scr_addr + (sc_reg * 4));
 }
 
-static int ahci_stop_engine(struct ata_port *ap)
+static int ahci_stop_engine(void __iomem *port_mmio)
 {
-	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);
+	/* Check if the HBA is idle */
+	if ((tmp & (PORT_CMD_START | PORT_CMD_LIST_ON)) == 0)
+		return 0;
+
+	/* Setting HBA to idle */
 	tmp &= ~PORT_CMD_START;
 	writel(tmp, port_mmio + PORT_CMD);
 
-	/* wait for engine to stop.  TODO: this could be
-	 * as long as 500 msec
+	/* 
+	 * wait for engine to stop.
+	 * This could be as long as 500 msec
 	 */
 	work = 1000;
 	while (work-- > 0) {

@@ -478,16 +524,120 @@ static int ahci_stop_engine(struct ata_p
 	return -EIO;
 }
 
-static void ahci_start_engine(struct ata_port *ap)
+static int ahci_start_engine(void __iomem *port_mmio)
 {
-	void __iomem *mmio = ap->host_set->mmio_base;
-	void __iomem *port_mmio = ahci_port_base(mmio, ap->port_no);
 	u32 tmp;
+	int work = 1000;
 
+	/*
+	 * Get current status
+	 */
 	tmp = readl(port_mmio + PORT_CMD);
+
+	/*
+	 * AHCI rev 1.1 section 10.3.1:
+	 * Software shall not set PxCMD.ST to ‘1’ until it verifies
+	 * that PxCMD.CR is ‘0’ and has set PxCMD.FRE to ‘1’.
+	 */
+	if ((tmp & PORT_CMD_FIS_RX) == 0) {
+		return -EIO;
+	}
+
+	/* 
+	 * wait for DMA engine to become idle.
+	 */
+	if (tmp & PORT_CMD_LIST_ON) {
+		while (work-- > 0) {
+			tmp = readl(port_mmio + PORT_CMD);
+			if ((tmp & PORT_CMD_LIST_ON) == 0)
+				break;
+			udelay(10);
+		}
+	}
+
+	if (!work) {
+		/*
+		 * We need to do a port reset / HBA reset here.
+		 */
+		return -EIO;
+	}
+
+	/*
+	 * Start DMA
+	 */
 	tmp |= PORT_CMD_START;
 	writel(tmp, port_mmio + PORT_CMD);
 	readl(port_mmio + PORT_CMD); /* flush */
+
+	return 0;
+}
+
+static int ahci_stop_fis_rx(void __iomem *port_mmio)
+{
+	u32 tmp;
+	int work = 1000;
+
+	/*
+	 * Get current status
+	 */
+	tmp = readl(port_mmio + PORT_CMD);
+
+	/*
+	 * Disable FIS reception
+	 */
+	if (tmp & PORT_CMD_FIS_RX) {
+		tmp &= ~(PORT_CMD_FIS_RX);
+		writel(tmp, port_mmio + PORT_CMD);
+		tmp = readl(port_mmio + PORT_CMD); /* flush */
+	}
+
+	/*
+	 * Wait for HBA to become idle.
+	 * This could be as long as 500 msec
+	 */
+	if (tmp & PORT_CMD_FIS_ON) {
+		work = 1000;
+		while (work-- > 0) {
+			tmp = readl(port_mmio + PORT_CMD);
+			if ((tmp & PORT_CMD_FIS_ON) == 0)
+				return 0;
+			udelay(10);
+		}
+	}
+	
+	return -EIO;
+}
+
+static void ahci_start_fis_rx(void __iomem *port_mmio, 
+			      struct ahci_port_priv *pp,
+			      struct ahci_host_priv *hpriv)
+{
+	/*
+	 * Enable FIS reception
+	 */
+	if (hpriv->cap & HOST_CAP_64)
+		writel((pp->cmd_slot_dma >> 16) >> 16, port_mmio + PORT_LST_ADDR_HI);
+	writel(pp->cmd_slot_dma & 0xffffffff, port_mmio + PORT_LST_ADDR);
+	readl(port_mmio + PORT_LST_ADDR); /* flush */
+
+	if (hpriv->cap & HOST_CAP_64)
+		writel((pp->rx_fis_dma >> 16) >> 16, port_mmio + PORT_FIS_ADDR_HI);
+	writel(pp->rx_fis_dma & 0xffffffff, port_mmio + PORT_FIS_ADDR);
+	readl(port_mmio + PORT_FIS_ADDR); /* flush */
+
+	/*
+	 * This is wrong. We should only activate
+	 * FIX_RX here; everything else should be handled
+	 * separately.
+	 * Some bits might not even be settable here
+	 * as they depend on the respective feature to be
+	 * implemented (Staggered Spin-up, 
+	 * Cold-presence detection etc.)
+	 */
+	writel(PORT_CMD_ICC_ACTIVE | PORT_CMD_FIS_RX |
+	       PORT_CMD_POWER_ON | PORT_CMD_SPIN_UP,
+	       port_mmio + PORT_CMD);
+	readl(port_mmio + PORT_CMD); /* flush */
 }
 
 static unsigned int ahci_dev_classify(struct ata_port *ap)

@@ -515,13 +665,16 @@ static void ahci_fill_cmd_slot(struct ah
 
 static int ahci_hardreset(struct ata_port *ap, int verbose, unsigned int *class)
 {
+	void __iomem *mmio = ap->host_set->mmio_base;
+	void __iomem *port_mmio = ahci_port_base(mmio, ap->port_no);
 	int rc;
 
 	DPRINTK("ENTER\n");
 
-	ahci_stop_engine(ap);
+	ahci_stop_engine(port_mmio);
 	rc = sata_std_hardreset(ap, verbose, class);
-	ahci_start_engine(ap);
+	if (!rc)
+		rc = ahci_start_engine(port_mmio);
 
 	if (rc == 0)
 		*class = ahci_dev_classify(ap);

@@ -641,6 +794,7 @@ static void ahci_restart_port(struct ata
 	void __iomem *mmio = ap->host_set->mmio_base;
 	void __iomem *port_mmio = ahci_port_base(mmio, ap->port_no);
 	u32 tmp;
+	int rc;
 
 	if ((ap->device[0].class != ATA_DEV_ATAPI) ||
 	    ((irq_stat & PORT_IRQ_TF_ERR) == 0))

@@ -656,7 +810,7 @@ static void ahci_restart_port(struct ata
 			readl(port_mmio + PORT_SCR_ERR));
 
 	/* stop DMA */
-	ahci_stop_engine(ap);
+	rc = ahci_stop_engine(port_mmio);
 
 	/* clear SATA phy error, if any */
 	tmp = readl(port_mmio + PORT_SCR_ERR);

@@ -666,7 +820,7 @@ static void ahci_restart_port(struct ata
 	 * if so, issue COMRESET
 	 */
 	tmp = readl(port_mmio + PORT_TFDATA);
-	if (tmp & (ATA_BUSY | ATA_DRQ)) {
+	if (rc || (tmp & (ATA_BUSY | ATA_DRQ))) {
 		writel(0x301, port_mmio + PORT_SCR_CTL);
 		readl(port_mmio + PORT_SCR_CTL); /* flush */
 		udelay(10);

@@ -675,7 +829,7 @@ static void ahci_restart_port(struct ata
 	}
 
 	/* re-start DMA */
-	ahci_start_engine(ap);
+	rc = ahci_start_engine(port_mmio);
 }
 
 static void ahci_eng_timeout(struct ata_port *ap)

@@ -823,6 +977,31 @@ static unsigned int ahci_qc_issue(struct
 	return 0;
 }
 
+int ahci_scsi_device_suspend(struct scsi_device *sdev)
+{
+	struct ata_port *ap = (struct ata_port *) &sdev->host->hostdata[0];
+	struct ata_device *dev = &ap->device[sdev->id];
+	int rc;
+
+	rc = ata_device_suspend(ap, dev);
+
+	if (!rc) {
+		rc = ahci_port_suspend(ap);
+	}
+
+	return rc;
+}
+
+int ahci_scsi_device_resume(struct scsi_device *sdev)
+{
+	struct ata_port *ap = (struct ata_port *) &sdev->host->hostdata[0];
+	struct ata_device *dev = &ap->device[sdev->id];
+
+	ahci_port_resume(ap);
+
+	return ata_device_resume(ap, dev);
+}
+
 static void ahci_setup_port(struct ata_ioports *port, unsigned long base,
 			    unsigned int port_idx)
 {

@@ -930,23 +1109,18 @@ static int ahci_host_init(struct ata_pro
 				(unsigned long) mmio, i);
 
 		/* make sure port is not active */
-		tmp = readl(port_mmio + PORT_CMD);
-		VPRINTK("PORT_CMD 0x%x\n", tmp);
-		if (tmp & (PORT_CMD_LIST_ON | PORT_CMD_FIS_ON |
-			   PORT_CMD_FIS_RX | PORT_CMD_START)) {
-			tmp &= ~(PORT_CMD_LIST_ON | PORT_CMD_FIS_ON |
-				 PORT_CMD_FIS_RX | PORT_CMD_START);
-			writel(tmp, port_mmio + PORT_CMD);
-			readl(port_mmio + PORT_CMD); /* flush */
-
-			/* spec says 500 msecs for each bit, so
-			 * this is slightly incorrect.
-			 */
-			msleep(500);
-		}
+		ahci_stop_engine(port_mmio);
+		ahci_stop_fis_rx(port_mmio);
+
+		/*
+		 * TODO: port / HBA reset if the above fails
+		 */
 
 		writel(PORT_CMD_SPIN_UP, port_mmio + PORT_CMD);
 
+		/*
+		 * Wait for communications link to establish
+		 */
 		j = 0;
 		while (j < 100) {
 			msleep(10);