3 files changed, 31 insertions, 12 deletions

diff --git a/drivers/ata/libata-sff.c b/drivers/ata/libata-sff.c
index dee3c2c52562..e30c537cce32 100644
--- a/drivers/ata/libata-sff.c
+++ b/drivers/ata/libata-sff.c
@@ -1045,7 +1045,8 @@ static void ata_hsm_qc_complete(struct ata_queued_cmd *qc, int in_wq)
 int ata_sff_hsm_move(struct ata_port *ap, struct ata_queued_cmd *qc,
                      u8 status, int in_wq)
 {
-        struct ata_eh_info *ehi = &ap->link.eh_info;
+        struct ata_link *link = qc->dev->link;
+        struct ata_eh_info *ehi = &link->eh_info;
         unsigned long flags = 0;
         int poll_next;
 
@@ -1301,8 +1302,14 @@ fsm_start:
 }
 EXPORT_SYMBOL_GPL(ata_sff_hsm_move);
 
-void ata_sff_queue_pio_task(struct ata_port *ap, unsigned long delay)
+void ata_sff_queue_pio_task(struct ata_link *link, unsigned long delay)
 {
+        struct ata_port *ap = link->ap;
+
+        WARN_ON((ap->sff_pio_task_link != NULL) &&
+                (ap->sff_pio_task_link != link));
+        ap->sff_pio_task_link = link;
+
         /* may fail if ata_sff_flush_pio_task() in progress */
         queue_delayed_work(ata_sff_wq, &ap->sff_pio_task,
                            msecs_to_jiffies(delay));
@@ -1324,14 +1331,18 @@ static void ata_sff_pio_task(struct work_struct *work)
 {
         struct ata_port *ap =
                 container_of(work, struct ata_port, sff_pio_task.work);
+        struct ata_link *link = ap->sff_pio_task_link;
         struct ata_queued_cmd *qc;
         u8 status;
         int poll_next;
 
+        BUG_ON(ap->sff_pio_task_link == NULL); 
         /* qc can be NULL if timeout occurred */
-        qc = ata_qc_from_tag(ap, ap->link.active_tag);
-        if (!qc)
+        qc = ata_qc_from_tag(ap, link->active_tag);
+        if (!qc) {
+                ap->sff_pio_task_link = NULL;
                 return;
+        }
 
 fsm_start:
         WARN_ON_ONCE(ap->hsm_task_state == HSM_ST_IDLE);
@@ -1348,11 +1359,16 @@ fsm_start:
                 msleep(2);
                 status = ata_sff_busy_wait(ap, ATA_BUSY, 10);
                 if (status & ATA_BUSY) {
-                        ata_sff_queue_pio_task(ap, ATA_SHORT_PAUSE);
+                        ata_sff_queue_pio_task(link, ATA_SHORT_PAUSE);
                         return;
                 }
         }
 
+        /*
+         * hsm_move() may trigger another command to be processed.
+         * clean the link beforehand.
+         */
+        ap->sff_pio_task_link = NULL;
         /* move the HSM */
         poll_next = ata_sff_hsm_move(ap, qc, status, 1);
 
@@ -1379,6 +1395,7 @@ fsm_start:
 unsigned int ata_sff_qc_issue(struct ata_queued_cmd *qc)
 {
         struct ata_port *ap = qc->ap;
+        struct ata_link *link = qc->dev->link;
 
         /* Use polling pio if the LLD doesn't handle
          * interrupt driven pio and atapi CDB interrupt.
@@ -1399,7 +1416,7 @@ unsigned int ata_sff_qc_issue(struct ata_queued_cmd *qc)
                 ap->hsm_task_state = HSM_ST_LAST;
 
                 if (qc->tf.flags & ATA_TFLAG_POLLING)
-                        ata_sff_queue_pio_task(ap, 0);
+                        ata_sff_queue_pio_task(link, 0);
 
                 break;
 
@@ -1412,7 +1429,7 @@ unsigned int ata_sff_qc_issue(struct ata_queued_cmd *qc)
                 if (qc->tf.flags & ATA_TFLAG_WRITE) {
                         /* PIO data out protocol */
                         ap->hsm_task_state = HSM_ST_FIRST;
-                        ata_sff_queue_pio_task(ap, 0);
+                        ata_sff_queue_pio_task(link, 0);
 
                         /* always send first data block using the
                          * ata_sff_pio_task() codepath.
@@ -1422,7 +1439,7 @@ unsigned int ata_sff_qc_issue(struct ata_queued_cmd *qc)
                         ap->hsm_task_state = HSM_ST;
 
                         if (qc->tf.flags & ATA_TFLAG_POLLING)
-                                ata_sff_queue_pio_task(ap, 0);
+                                ata_sff_queue_pio_task(link, 0);
 
                         /* if polling, ata_sff_pio_task() handles the
                          * rest.  otherwise, interrupt handler takes
@@ -1444,7 +1461,7 @@ unsigned int ata_sff_qc_issue(struct ata_queued_cmd *qc)
                 /* send cdb by polling if no cdb interrupt */
                 if ((!(qc->dev->flags & ATA_DFLAG_CDB_INTR)) ||
                     (qc->tf.flags & ATA_TFLAG_POLLING))
-                        ata_sff_queue_pio_task(ap, 0);
+                        ata_sff_queue_pio_task(link, 0);
                 break;
 
         default:
@@ -2737,6 +2754,7 @@ EXPORT_SYMBOL_GPL(ata_bmdma_dumb_qc_prep);
 unsigned int ata_bmdma_qc_issue(struct ata_queued_cmd *qc)
 {
         struct ata_port *ap = qc->ap;
+        struct ata_link *link = qc->dev->link;
 
         /* defer PIO handling to sff_qc_issue */
         if (!ata_is_dma(qc->tf.protocol))
@@ -2765,7 +2783,7 @@ unsigned int ata_bmdma_qc_issue(struct ata_queued_cmd *qc)
 
                 /* send cdb by polling if no cdb interrupt */
                 if (!(qc->dev->flags & ATA_DFLAG_CDB_INTR))
-                        ata_sff_queue_pio_task(ap, 0);
+                        ata_sff_queue_pio_task(link, 0);
                 break;
 
         default:
diff --git a/drivers/ata/sata_mv.c b/drivers/ata/sata_mv.c
index 81982594a014..a9fd9709c262 100644
--- a/drivers/ata/sata_mv.c
+++ b/drivers/ata/sata_mv.c
@@ -2284,7 +2284,7 @@ static unsigned int mv_qc_issue_fis(struct ata_queued_cmd *qc)
         }
 
         if (qc->tf.flags & ATA_TFLAG_POLLING)
-                ata_sff_queue_pio_task(ap, 0);
+                ata_sff_queue_pio_task(link, 0);
         return 0;
 }
 
diff --git a/include/linux/libata.h b/include/linux/libata.h
index 7de282d8bedf..45fb2967b66d 100644
--- a/include/linux/libata.h
+++ b/include/linux/libata.h
@@ -724,6 +724,7 @@ struct ata_port {
         struct ata_ioports      ioaddr; /* ATA cmd/ctl/dma register blocks */
         u8                      ctl;    /* cache of ATA control register */
         u8                      last_ctl;       /* Cache last written value */
+        struct ata_link*        sff_pio_task_link; /* link currently used */
         struct delayed_work     sff_pio_task;
 #ifdef CONFIG_ATA_BMDMA
         struct ata_bmdma_prd    *bmdma_prd;     /* BMDMA SG list */
@@ -1595,7 +1596,7 @@ extern void ata_sff_irq_on(struct ata_port *ap);
 extern void ata_sff_irq_clear(struct ata_port *ap);
 extern int ata_sff_hsm_move(struct ata_port *ap, struct ata_queued_cmd *qc,
                             u8 status, int in_wq);
-extern void ata_sff_queue_pio_task(struct ata_port *ap, unsigned long delay);
+extern void ata_sff_queue_pio_task(struct ata_link *link, unsigned long delay);
 extern unsigned int ata_sff_qc_issue(struct ata_queued_cmd *qc);
 extern bool ata_sff_qc_fill_rtf(struct ata_queued_cmd *qc);
 extern unsigned int ata_sff_port_intr(struct ata_port *ap,