1 files changed, 40 insertions, 26 deletions

diff --git a/drivers/scsi/libata-core.c b/drivers/scsi/libata-core.c
index f04561abf6d8..061b0b6544cc 100644
--- a/drivers/scsi/libata-core.c
+++ b/drivers/scsi/libata-core.c
@@ -65,7 +65,7 @@ static unsigned int ata_dev_init_params(struct ata_port *ap,
                                         struct ata_device *dev,
                                         u16 heads,
                                         u16 sectors);
-static void ata_set_mode(struct ata_port *ap);
+static int ata_set_mode(struct ata_port *ap, struct ata_device **r_failed_dev);
 static unsigned int ata_dev_set_xfermode(struct ata_port *ap,
                                          struct ata_device *dev);
 static void ata_dev_xfermask(struct ata_port *ap, struct ata_device *dev);
@@ -1368,6 +1368,7 @@ static int ata_bus_probe(struct ata_port *ap)
 {
         unsigned int classes[ATA_MAX_DEVICES];
         int i, rc, found = 0;
+        struct ata_device *dev;
 
         ata_port_probe(ap);
 
@@ -1397,8 +1398,7 @@ static int ata_bus_probe(struct ata_port *ap)
 
         /* read IDENTIFY page and configure devices */
         for (i = 0; i < ATA_MAX_DEVICES; i++) {
-                struct ata_device *dev = &ap->device[i];
-
+                dev = &ap->device[i];
                 dev->class = classes[i];
 
                 if (!ata_dev_enabled(dev))
@@ -1418,20 +1418,26 @@ static int ata_bus_probe(struct ata_port *ap)
                 found = 1;
         }
 
-        if (!found)
-                goto err_out_disable;
-
-        if (ap->ops->set_mode)
-                ap->ops->set_mode(ap);
-        else
-                ata_set_mode(ap);
-
-        if (ap->flags & ATA_FLAG_PORT_DISABLED)
-                goto err_out_disable;
+        /* configure transfer mode */
+        if (ap->ops->set_mode) {
+                /* FIXME: make ->set_mode handle no device case and
+                 * return error code and failing device on failure as
+                 * ata_set_mode() does.
+                 */
+                if (found)
+                        ap->ops->set_mode(ap);
+                rc = 0;
+        } else {
+                while (ata_set_mode(ap, &dev))
+                        ata_dev_disable(ap, dev);
+        }
 
-        return 0;
+        for (i = 0; i < ATA_MAX_DEVICES; i++)
+                if (ata_dev_enabled(&ap->device[i]))
+                        return 0;
 
-err_out_disable:
+        /* no device present, disable port */
+        ata_port_disable(ap);
         ap->ops->port_disable(ap);
         return -ENODEV;
 }
@@ -1774,16 +1780,22 @@ static int ata_dev_set_mode(struct ata_port *ap, struct ata_device *dev)
 /**
  *      ata_set_mode - Program timings and issue SET FEATURES - XFER
  *      @ap: port on which timings will be programmed
+ *      @r_failed_dev: out paramter for failed device
  *
- *      Set ATA device disk transfer mode (PIO3, UDMA6, etc.).
+ *      Set ATA device disk transfer mode (PIO3, UDMA6, etc.).  If
+ *      ata_set_mode() fails, pointer to the failing device is
+ *      returned in @r_failed_dev.
  *
  *      LOCKING:
  *      PCI/etc. bus probe sem.
+ *
+ *      RETURNS:
+ *      0 on success, negative errno otherwise
  */
-static void ata_set_mode(struct ata_port *ap)
+static int ata_set_mode(struct ata_port *ap, struct ata_device **r_failed_dev)
 {
         struct ata_device *dev;
-        int i, rc, used_dma = 0, found = 0;
+        int i, rc = 0, used_dma = 0, found = 0;
 
         /* step 1: calculate xfer_mask */
         for (i = 0; i < ATA_MAX_DEVICES; i++) {
@@ -1806,7 +1818,7 @@ static void ata_set_mode(struct ata_port *ap)
                         used_dma = 1;
         }
         if (!found)
-                return;
+                goto out;
 
         /* step 2: always set host PIO timings */
         for (i = 0; i < ATA_MAX_DEVICES; i++) {
@@ -1818,7 +1830,7 @@ static void ata_set_mode(struct ata_port *ap)
                         printk(KERN_WARNING "ata%u: dev %u no PIO support\n",
                                ap->id, dev->devno);
                         rc = -EINVAL;
-                        goto err_out;
+                        goto out;
                 }
 
                 dev->xfer_mode = dev->pio_mode;
@@ -1849,7 +1861,7 @@ static void ata_set_mode(struct ata_port *ap)
 
                 rc = ata_dev_set_mode(ap, dev);
                 if (rc)
-                        goto err_out;
+                        goto out;
         }
 
         /* Record simplex status. If we selected DMA then the other
@@ -1862,10 +1874,10 @@ static void ata_set_mode(struct ata_port *ap)
         if (ap->ops->post_set_mode)
                 ap->ops->post_set_mode(ap);
 
-        return;
-
-err_out:
-        ata_port_disable(ap);
+ out:
+        if (rc)
+                *r_failed_dev = dev;
+        return rc;
 }
 
 /**
@@ -4278,8 +4290,10 @@ static int ata_start_drive(struct ata_port *ap, struct ata_device *dev)
 int ata_device_resume(struct ata_port *ap, struct ata_device *dev)
 {
         if (ap->flags & ATA_FLAG_SUSPENDED) {
+                struct ata_device *failed_dev;
                 ap->flags &= ~ATA_FLAG_SUSPENDED;
-                ata_set_mode(ap);
+                while (ata_set_mode(ap, &failed_dev))
+                        ata_dev_disable(ap, failed_dev);
         }
         if (!ata_dev_enabled(dev))
                 return 0;