1 files changed, 39 insertions, 2 deletions

diff --git a/drivers/ata/sata_nv.c b/drivers/ata/sata_nv.c
index 19817b376161..aea005d5663c 100644
--- a/drivers/ata/sata_nv.c
+++ b/drivers/ata/sata_nv.c
@@ -512,14 +512,38 @@ static void nv_adma_register_mode(struct ata_port *ap)
 {
         struct nv_adma_port_priv *pp = ap->private_data;
         void __iomem *mmio = pp->ctl_block;
-        u16 tmp;
+        u16 tmp, status;
+        int count = 0;
 
         if (pp->flags & NV_ADMA_PORT_REGISTER_MODE)
                 return;
 
+        status = readw(mmio + NV_ADMA_STAT);
+        while(!(status & NV_ADMA_STAT_IDLE) && count < 20) {
+                ndelay(50);
+                status = readw(mmio + NV_ADMA_STAT);
+                count++;
+        }
+        if(count == 20)
+                ata_port_printk(ap, KERN_WARNING,
+                        "timeout waiting for ADMA IDLE, stat=0x%hx\n",
+                        status);
+
         tmp = readw(mmio + NV_ADMA_CTL);
         writew(tmp & ~NV_ADMA_CTL_GO, mmio + NV_ADMA_CTL);
 
+        count = 0;
+        status = readw(mmio + NV_ADMA_STAT);
+        while(!(status & NV_ADMA_STAT_LEGACY) && count < 20) {
+                ndelay(50);
+                status = readw(mmio + NV_ADMA_STAT);
+                count++;
+        }
+        if(count == 20)
+                ata_port_printk(ap, KERN_WARNING,
+                         "timeout waiting for ADMA LEGACY, stat=0x%hx\n",
+                         status);
+
         pp->flags |= NV_ADMA_PORT_REGISTER_MODE;
 }
 
@@ -527,7 +551,8 @@ static void nv_adma_mode(struct ata_port *ap)
 {
         struct nv_adma_port_priv *pp = ap->private_data;
         void __iomem *mmio = pp->ctl_block;
-        u16 tmp;
+        u16 tmp, status;
+        int count = 0;
 
         if (!(pp->flags & NV_ADMA_PORT_REGISTER_MODE))
                 return;
@@ -537,6 +562,18 @@ static void nv_adma_mode(struct ata_port *ap)
         tmp = readw(mmio + NV_ADMA_CTL);
         writew(tmp | NV_ADMA_CTL_GO, mmio + NV_ADMA_CTL);
 
+        status = readw(mmio + NV_ADMA_STAT);
+        while(((status & NV_ADMA_STAT_LEGACY) ||
+              !(status & NV_ADMA_STAT_IDLE)) && count < 20) {
+                ndelay(50);
+                status = readw(mmio + NV_ADMA_STAT);
+                count++;
+        }
+        if(count == 20)
+                ata_port_printk(ap, KERN_WARNING,
+                        "timeout waiting for ADMA LEGACY clear and IDLE, stat=0x%hx\n",
+                        status);
+
         pp->flags &= ~NV_ADMA_PORT_REGISTER_MODE;
 }