libata: improve ata_bus_probe failure handling

index e63c1ff..8def7a5 100644
@@ -1856,7 +1856,8 @@ static void ata_set_mode(struct ata_port
 		if (!ata_dev_present(dev))
 			continue;
 
-		if (ata_dev_set_mode(ap, dev))
+		rc = ata_dev_set_mode(ap, dev);
+		if (rc)
 			goto err_out;
 	}
 

index 8def7a5..c7ba523 100644
@@ -1349,13 +1349,13 @@ err_out_nosup:
  *	PCI/etc. bus probe sem.
  *
  *	RETURNS:
- *	Zero on success, non-zero on error.
+ *	Zero on success, negative errno otherwise.
  */
 
 static int ata_bus_probe(struct ata_port *ap)
 {
 	unsigned int classes[ATA_MAX_DEVICES];
-	unsigned int i, rc, found = 0;
+	int i, rc, found = 0;
 
 	ata_port_probe(ap);
 

@@ -1421,7 +1421,7 @@ static int ata_bus_probe(struct ata_port
 
 err_out_disable:
 	ap->ops->port_disable(ap);
-	return -1;
+	return -ENODEV;
 }
 
 /**

index c7ba523..6a336ba 100644
@@ -397,6 +397,18 @@ static const char *ata_mode_string(unsig
 	return "<n/a>";
 }
 
+static const char *sata_spd_string(unsigned int spd)
+{
+	static const char * const spd_str[] = {
+		"1.5 Gbps",
+		"3.0 Gbps",
+	};
+
+	if (spd == 0 || (spd - 1) >= ARRAY_SIZE(spd_str))
+		return "<unknown>";
+	return spd_str[spd - 1];
+}
+
 static void ata_dev_disable(struct ata_port *ap, struct ata_device *dev)
 {
 	if (ata_dev_present(dev)) {

@@ -1452,7 +1464,6 @@ void ata_port_probe(struct ata_port *ap)
 static void sata_print_link_status(struct ata_port *ap)
 {
 	u32 sstatus, tmp;
-	const char *speed;
 
 	if (!ap->ops->scr_read)
 		return;

@@ -1461,14 +1472,8 @@ static void sata_print_link_status(struc
 
 	if (sata_dev_present(ap)) {
 		tmp = (sstatus >> 4) & 0xf;
-		if (tmp & (1 << 0))
-			speed = "1.5";
-		else if (tmp & (1 << 1))
-			speed = "3.0";
-		else
-			speed = "<unknown>";
-		printk(KERN_INFO "ata%u: SATA link up %s Gbps (SStatus %X)\n",
-		       ap->id, speed, sstatus);
+		printk(KERN_INFO "ata%u: SATA link up %s (SStatus %X)\n",
+		       ap->id, sata_spd_string(tmp), sstatus);
 	} else {
 		printk(KERN_INFO "ata%u: SATA link down (SStatus %X)\n",
 		       ap->id, sstatus);

index 6a336ba..559abe4 100644
@@ -2371,16 +2371,16 @@ int ata_std_probe_reset(struct ata_port 
 				     ata_std_postreset, classes);
 }
 
-static int do_probe_reset(struct ata_port *ap, ata_reset_fn_t reset,
-			  ata_postreset_fn_t postreset,
-			  unsigned int *classes)
+static int ata_do_reset(struct ata_port *ap,
+			ata_reset_fn_t reset, ata_postreset_fn_t postreset,
+			int verbose, unsigned int *classes)
 {
 	int i, rc;
 
 	for (i = 0; i < ATA_MAX_DEVICES; i++)
 		classes[i] = ATA_DEV_UNKNOWN;
 
-	rc = reset(ap, 0, classes);
+	rc = reset(ap, verbose, classes);
 	if (rc)
 		return rc;
 

@@ -2400,7 +2400,7 @@ static int do_probe_reset(struct ata_por
 	if (postreset)
 		postreset(ap, classes);
 
-	return classes[0] != ATA_DEV_UNKNOWN ? 0 : -ENODEV;
+	return 0;
 }
 
 /**

@@ -2445,21 +2445,24 @@ int ata_drive_probe_reset(struct ata_por
 		probeinit(ap);
 
 	if (softreset) {
-		rc = do_probe_reset(ap, softreset, postreset, classes);
-		if (rc == 0)
-			return 0;
+		rc = ata_do_reset(ap, softreset, postreset, 0, classes);
+		if (rc == 0 && classes[0] != ATA_DEV_UNKNOWN)
+			goto done;
 	}
 
 	if (!hardreset)
-		return rc;
+		goto done;
 
-	rc = do_probe_reset(ap, hardreset, postreset, classes);
-	if (rc == 0 || rc != -ENODEV)
-		return rc;
+	rc = ata_do_reset(ap, hardreset, postreset, 0, classes);
+	if (rc || classes[0] != ATA_DEV_UNKNOWN)
+		goto done;
 
 	if (softreset)
-		rc = do_probe_reset(ap, softreset, postreset, classes);
+		rc = ata_do_reset(ap, softreset, postreset, 0, classes);
 
+ done:
+	if (rc == 0 && classes[0] == ATA_DEV_UNKNOWN)
+		rc = -ENODEV;
 	return rc;
 }
 

index 559abe4..c10c550 100644
@@ -411,7 +411,7 @@ static const char *sata_spd_string(unsig
 
 static void ata_dev_disable(struct ata_port *ap, struct ata_device *dev)
 {
-	if (ata_dev_present(dev)) {
+	if (ata_dev_enabled(dev)) {
 		printk(KERN_WARNING "ata%u: dev %u disabled\n",
 		       ap->id, dev->devno);
 		dev->class++;

@@ -1222,7 +1222,7 @@ static int ata_dev_configure(struct ata_
 	unsigned int xfer_mask;
 	int i, rc;
 
-	if (!ata_dev_present(dev)) {
+	if (!ata_dev_enabled(dev)) {
 		DPRINTK("ENTER/EXIT (host %u, dev %u) -- nodev\n",
 			ap->id, dev->devno);
 		return 0;

@@ -1401,7 +1401,7 @@ static int ata_bus_probe(struct ata_port
 
 		dev->class = classes[i];
 
-		if (!ata_dev_present(dev))
+		if (!ata_dev_enabled(dev))
 			continue;
 
 		WARN_ON(dev->id != NULL);

@@ -1565,7 +1565,7 @@ void sata_phy_reset(struct ata_port *ap)
 struct ata_device *ata_dev_pair(struct ata_port *ap, struct ata_device *adev)
 {
 	struct ata_device *pair = &ap->device[1 - adev->devno];
-	if (!ata_dev_present(pair))
+	if (!ata_dev_enabled(pair))
 		return NULL;
 	return pair;
 }

@@ -1778,7 +1778,7 @@ static int ata_host_set_pio(struct ata_p
 	for (i = 0; i < ATA_MAX_DEVICES; i++) {
 		struct ata_device *dev = &ap->device[i];
 
-		if (!ata_dev_present(dev))
+		if (!ata_dev_enabled(dev))
 			continue;
 
 		if (!dev->pio_mode) {

@@ -1802,7 +1802,7 @@ static void ata_host_set_dma(struct ata_
 	for (i = 0; i < ATA_MAX_DEVICES; i++) {
 		struct ata_device *dev = &ap->device[i];
 
-		if (!ata_dev_present(dev) || !dev->dma_mode)
+		if (!ata_dev_enabled(dev) || !dev->dma_mode)
 			continue;
 
 		dev->xfer_mode = dev->dma_mode;

@@ -1830,7 +1830,7 @@ static void ata_set_mode(struct ata_port
 		struct ata_device *dev = &ap->device[i];
 		unsigned int pio_mask, dma_mask;
 
-		if (!ata_dev_present(dev))
+		if (!ata_dev_enabled(dev))
 			continue;
 
 		ata_dev_xfermask(ap, dev);

@@ -1858,7 +1858,7 @@ static void ata_set_mode(struct ata_port
 	for (i = 0; i < ATA_MAX_DEVICES; i++) {
 		struct ata_device *dev = &ap->device[i];
 
-		if (!ata_dev_present(dev))
+		if (!ata_dev_enabled(dev))
 			continue;
 
 		rc = ata_dev_set_mode(ap, dev);

@@ -2550,7 +2550,7 @@ int ata_dev_revalidate(struct ata_port *
 	u16 *id;
 	int rc;
 
-	if (!ata_dev_present(dev))
+	if (!ata_dev_enabled(dev))
 		return -ENODEV;
 
 	class = dev->class;

@@ -2679,7 +2679,7 @@ static void ata_dev_xfermask(struct ata_
 	/* FIXME: Use port-wide xfermask for now */
 	for (i = 0; i < ATA_MAX_DEVICES; i++) {
 		struct ata_device *d = &ap->device[i];
-		if (!ata_dev_present(d))
+		if (!ata_dev_enabled(d))
 			continue;
 		xfer_mask &= ata_pack_xfermask(d->pio_mask, d->mwdma_mask,
 					       d->udma_mask);

@@ -4299,7 +4299,7 @@ int ata_device_resume(struct ata_port *a
 		ap->flags &= ~ATA_FLAG_SUSPENDED;
 		ata_set_mode(ap);
 	}
-	if (!ata_dev_present(dev))
+	if (!ata_dev_enabled(dev))
 		return 0;
 	if (dev->class == ATA_DEV_ATA)
 		ata_start_drive(ap, dev);

@@ -4317,7 +4317,7 @@ int ata_device_resume(struct ata_port *a
  */
 int ata_device_suspend(struct ata_port *ap, struct ata_device *dev, pm_message_t state)
 {
-	if (!ata_dev_present(dev))
+	if (!ata_dev_enabled(dev))
 		return 0;
 	if (dev->class == ATA_DEV_ATA)
 		ata_flush_cache(ap, dev);

index 53f5b0d..c1a4b29 100644
@@ -2349,7 +2349,7 @@ ata_scsi_find_dev(struct ata_port *ap, c
 		     (scsidev->lun != 0)))
 		return NULL;
 
-	if (unlikely(!ata_dev_present(dev)))
+	if (unlikely(!ata_dev_enabled(dev)))
 		return NULL;
 
 	if (!atapi_enabled || (ap->flags & ATA_FLAG_NO_ATAPI)) {

@@ -2743,7 +2743,7 @@ void ata_scsi_scan_host(struct ata_port 
 	for (i = 0; i < ATA_MAX_DEVICES; i++) {
 		dev = &ap->device[i];
 
-		if (ata_dev_present(dev))
+		if (ata_dev_enabled(dev))
 			scsi_scan_target(&ap->host->shost_gendev, 0, i, 0, 0);
 	}
 }

index fa901fd..0f7d334 100644
@@ -1991,7 +1991,7 @@ comreset_retry:
 	tf.nsect = readb((void __iomem *) ap->ioaddr.nsect_addr);
 
 	dev->class = ata_dev_classify(&tf);
-	if (!ata_dev_present(dev)) {
+	if (!ata_dev_enabled(dev)) {
 		VPRINTK("Port disabled post-sig: No device present.\n");
 		ata_port_disable(ap);
 	}

index 18c296c..d6c7086 100644
@@ -264,7 +264,7 @@ static void sil_post_set_mode (struct at
 
 	for (i = 0; i < 2; i++) {
 		dev = &ap->device[i];
-		if (!ata_dev_present(dev))
+		if (!ata_dev_enabled(dev))
 			dev_mode[i] = 0;	/* PIO0/1/2 */
 		else if (dev->flags & ATA_DFLAG_PIO)
 			dev_mode[i] = 1;	/* PIO3/4 */

index 0d61357..c6883ba 100644
@@ -672,14 +672,24 @@ static inline unsigned int ata_tag_valid
 	return (tag < ATA_MAX_QUEUE) ? 1 : 0;
 }
 
-static inline unsigned int ata_class_present(unsigned int class)
+static inline unsigned int ata_class_enabled(unsigned int class)
 {
 	return class == ATA_DEV_ATA || class == ATA_DEV_ATAPI;
 }
 
-static inline unsigned int ata_dev_present(const struct ata_device *dev)
+static inline unsigned int ata_class_disabled(unsigned int class)
 {
-	return ata_class_present(dev->class);
+	return class == ATA_DEV_ATA_UNSUP || class == ATA_DEV_ATAPI_UNSUP;
+}
+
+static inline unsigned int ata_dev_enabled(const struct ata_device *dev)
+{
+	return ata_class_enabled(dev->class);
+}
+
+static inline unsigned int ata_dev_disabled(const struct ata_device *dev)
+{
+	return ata_class_disabled(dev->class);
 }
 
 static inline u8 ata_chk_status(struct ata_port *ap)

index c10c550..913e378 100644
@@ -1823,7 +1823,7 @@ static void ata_host_set_dma(struct ata_
  */
 static void ata_set_mode(struct ata_port *ap)
 {
-	int i, rc, used_dma = 0;
+	int i, rc, used_dma = 0, found = 0;
 
 	/* step 1: calculate xfer_mask */
 	for (i = 0; i < ATA_MAX_DEVICES; i++) {

@@ -1842,9 +1842,12 @@ static void ata_set_mode(struct ata_port
 		dev->pio_mode = ata_xfer_mask2mode(pio_mask);
 		dev->dma_mode = ata_xfer_mask2mode(dma_mask);
 
+		found = 1;
 		if (dev->dma_mode)
 			used_dma = 1;
 	}
+	if (!found)
+		return;
 
 	/* step 2: always set host PIO timings */
 	rc = ata_host_set_pio(ap);

index 913e378..f04561a 100644
@@ -1771,47 +1771,6 @@ static int ata_dev_set_mode(struct ata_p
 	return 0;
 }
 
-static int ata_host_set_pio(struct ata_port *ap)
-{
-	int i;
-
-	for (i = 0; i < ATA_MAX_DEVICES; i++) {
-		struct ata_device *dev = &ap->device[i];
-
-		if (!ata_dev_enabled(dev))
-			continue;
-
-		if (!dev->pio_mode) {
-			printk(KERN_WARNING "ata%u: no PIO support for device %d.\n", ap->id, i);
-			return -1;
-		}
-
-		dev->xfer_mode = dev->pio_mode;
-		dev->xfer_shift = ATA_SHIFT_PIO;
-		if (ap->ops->set_piomode)
-			ap->ops->set_piomode(ap, dev);
-	}
-
-	return 0;
-}
-
-static void ata_host_set_dma(struct ata_port *ap)
-{
-	int i;
-
-	for (i = 0; i < ATA_MAX_DEVICES; i++) {
-		struct ata_device *dev = &ap->device[i];
-
-		if (!ata_dev_enabled(dev) || !dev->dma_mode)
-			continue;
-
-		dev->xfer_mode = dev->dma_mode;
-		dev->xfer_shift = ata_xfer_mode2shift(dev->dma_mode);
-		if (ap->ops->set_dmamode)
-			ap->ops->set_dmamode(ap, dev);
-	}
-}
-
 /**
  *	ata_set_mode - Program timings and issue SET FEATURES - XFER
  *	@ap: port on which timings will be programmed

@@ -1823,20 +1782,20 @@ static void ata_host_set_dma(struct ata_
  */
 static void ata_set_mode(struct ata_port *ap)
 {
+	struct ata_device *dev;
 	int i, rc, used_dma = 0, found = 0;
 
 	/* step 1: calculate xfer_mask */
 	for (i = 0; i < ATA_MAX_DEVICES; i++) {
-		struct ata_device *dev = &ap->device[i];
 		unsigned int pio_mask, dma_mask;
 
+		dev = &ap->device[i];
+
 		if (!ata_dev_enabled(dev))
 			continue;
 
 		ata_dev_xfermask(ap, dev);
 
-		/* TODO: let LLDD filter dev->*_mask here */
-
 		pio_mask = ata_pack_xfermask(dev->pio_mask, 0, 0);
 		dma_mask = ata_pack_xfermask(0, dev->mwdma_mask, dev->udma_mask);
 		dev->pio_mode = ata_xfer_mask2mode(pio_mask);

@@ -1850,16 +1809,40 @@ static void ata_set_mode(struct ata_port
 		return;
 
 	/* step 2: always set host PIO timings */
-	rc = ata_host_set_pio(ap);
-	if (rc)
-		goto err_out;
+	for (i = 0; i < ATA_MAX_DEVICES; i++) {
+		dev = &ap->device[i];
+		if (!ata_dev_enabled(dev))
+			continue;
+
+		if (!dev->pio_mode) {
+			printk(KERN_WARNING "ata%u: dev %u no PIO support\n",
+			       ap->id, dev->devno);
+			rc = -EINVAL;
+			goto err_out;
+		}
+
+		dev->xfer_mode = dev->pio_mode;
+		dev->xfer_shift = ATA_SHIFT_PIO;
+		if (ap->ops->set_piomode)
+			ap->ops->set_piomode(ap, dev);
+	}
 
 	/* step 3: set host DMA timings */
-	ata_host_set_dma(ap);
+	for (i = 0; i < ATA_MAX_DEVICES; i++) {
+		dev = &ap->device[i];
+
+		if (!ata_dev_enabled(dev) || !dev->dma_mode)
+			continue;
+
+		dev->xfer_mode = dev->dma_mode;
+		dev->xfer_shift = ata_xfer_mode2shift(dev->dma_mode);
+		if (ap->ops->set_dmamode)
+			ap->ops->set_dmamode(ap, dev);
+	}
 
 	/* step 4: update devices' xfer mode */
 	for (i = 0; i < ATA_MAX_DEVICES; i++) {
-		struct ata_device *dev = &ap->device[i];
+		dev = &ap->device[i];
 
 		if (!ata_dev_enabled(dev))
 			continue;

@@ -1869,17 +1852,13 @@ static void ata_set_mode(struct ata_port
 			goto err_out;
 	}
 
-	/*
-	 *	Record simplex status. If we selected DMA then the other
-	 *	host channels are not permitted to do so.
+	/* Record simplex status. If we selected DMA then the other
+	 * host channels are not permitted to do so.
 	 */
-
 	if (used_dma && (ap->host_set->flags & ATA_HOST_SIMPLEX))
 		ap->host_set->simplex_claimed = 1;
 
-	/*
-	 *	Chip specific finalisation
-	 */
+	/* step5: chip specific finalisation */
 	if (ap->ops->post_set_mode)
 		ap->ops->post_set_mode(ap);
 

index f04561a..061b0b6 100644
@@ -65,7 +65,7 @@ static unsigned int ata_dev_init_params(
 					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
 {
 	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
 
 	/* 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
 		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_p
 /**
  *	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
 			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
 			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
 
 		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
 	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_po
 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;

index 061b0b6..5dee649 100644
@@ -65,6 +65,8 @@ static unsigned int ata_dev_init_params(
 					struct ata_device *dev,
 					u16 heads,
 					u16 sectors);
+static int ata_down_xfermask_limit(struct ata_port *ap, struct ata_device *dev,
+				   int force_pio0);
 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);

@@ -1744,6 +1746,56 @@ int ata_timing_compute(struct ata_device
 	return 0;
 }
 
+/**
+ *	ata_down_xfermask_limit - adjust dev xfer masks downward
+ *	@ap: Port associated with device @dev
+ *	@dev: Device to adjust xfer masks
+ *	@force_pio0: Force PIO0
+ *
+ *	Adjust xfer masks of @dev downward.  Note that this function
+ *	does not apply the change.  Invoking ata_set_mode() afterwards
+ *	will apply the limit.
+ *
+ *	LOCKING:
+ *	Inherited from caller.
+ *
+ *	RETURNS:
+ *	0 on success, negative errno on failure
+ */
+static int ata_down_xfermask_limit(struct ata_port *ap, struct ata_device *dev,
+				   int force_pio0)
+{
+	unsigned long xfer_mask;
+	int highbit;
+
+	xfer_mask = ata_pack_xfermask(dev->pio_mask, dev->mwdma_mask,
+				      dev->udma_mask);
+
+	if (!xfer_mask)
+		goto fail;
+	/* don't gear down to MWDMA from UDMA, go directly to PIO */
+	if (xfer_mask & ATA_MASK_UDMA)
+		xfer_mask &= ~ATA_MASK_MWDMA;
+
+	highbit = fls(xfer_mask) - 1;
+	xfer_mask &= ~(1 << highbit);
+	if (force_pio0)
+		xfer_mask &= 1 << ATA_SHIFT_PIO;
+	if (!xfer_mask)
+		goto fail;
+
+	ata_unpack_xfermask(xfer_mask, &dev->pio_mask, &dev->mwdma_mask,
+			    &dev->udma_mask);
+
+	printk(KERN_WARNING "ata%u: dev %u limiting speed to %s\n",
+	       ap->id, dev->devno, ata_mode_string(xfer_mask));
+
+	return 0;
+
+ fail:
+	return -EINVAL;
+}
+
 static int ata_dev_set_mode(struct ata_port *ap, struct ata_device *dev)
 {
 	unsigned int err_mask;

index 5dee649..283994d 100644
@@ -65,6 +65,7 @@ static unsigned int ata_dev_init_params(
 					struct ata_device *dev,
 					u16 heads,
 					u16 sectors);
+static int ata_down_sata_spd_limit(struct ata_port *ap, struct ata_device *dev);
 static int ata_down_xfermask_limit(struct ata_port *ap, struct ata_device *dev,
 				   int force_pio0);
 static int ata_set_mode(struct ata_port *ap, struct ata_device **r_failed_dev);

@@ -1598,6 +1599,131 @@ void ata_port_disable(struct ata_port *a
 	ap->flags |= ATA_FLAG_PORT_DISABLED;
 }
 
+/**
+ *	ata_down_sata_spd_limit - adjust SATA spd limit downward
+ *	@ap: Port associated with device @dev
+ *	@dev: Device to adjust SATA spd limit
+ *
+ *	Adjust SATA spd limit of @dev downward.  Note that this
+ *	function only adjusts the limit.  The change must be applied
+ *	using ata_set_sata_spd().
+ *
+ *	LOCKING:
+ *	Inherited from caller.
+ *
+ *	RETURNS:
+ *	0 on success, negative errno on failure
+ */
+static int ata_down_sata_spd_limit(struct ata_port *ap, struct ata_device *dev)
+{
+	u32 spd, mask;
+	int highbit;
+
+	if (ap->cbl != ATA_CBL_SATA || !ap->ops->scr_read)
+		return -EOPNOTSUPP;
+
+	mask = dev->sata_spd_limit;
+	if (mask <= 1)
+		return -EINVAL;
+	highbit = fls(mask) - 1;
+	mask &= ~(1 << highbit);
+
+	spd = (scr_read(ap, SCR_STATUS) >> 4) & 0xf;
+	if (spd <= 1)
+		return -EINVAL;
+	spd--;
+	mask &= (1 << spd) - 1;
+	if (!mask)
+		return -EINVAL;
+
+	dev->sata_spd_limit = mask;
+
+	printk(KERN_WARNING "ata%u: dev %u limiting SATA link speed to %s\n",
+	       ap->id, dev->devno, sata_spd_string(fls(mask)));
+
+	return 0;
+}
+
+static int __ata_set_sata_spd_needed(struct ata_port *ap,
+				     struct ata_device *dev, u32 *scontrol)
+{
+	u32 spd, limit;
+
+	if (!ata_dev_enabled(dev))
+		return 0;
+
+	if (dev->sata_spd_limit == UINT_MAX)
+		limit = 0;
+	else
+		limit = fls(dev->sata_spd_limit);
+
+	spd = (*scontrol >> 4) & 0xf;
+	*scontrol = (*scontrol & ~0xf0) | ((limit & 0xf) << 4);
+
+	return spd != limit;
+}
+
+/**
+ *	ata_set_sata_spd_needed - is SATA spd configuration needed
+ *	@ap: Port associated with device @dev
+ *	@dev: Device in question
+ *
+ *	Test whether the spd limit in SControl matches
+ *	@dev->sata_spd_limit.  This function is used to determine
+ *	whether hardreset is necessary to apply SATA spd
+ *	configuration.
+ *
+ *	LOCKING:
+ *	Inherited from caller.
+ *
+ *	RETURNS:
+ *	1 if SATA spd configuration is needed, 0 otherwise.
+ */
+static int ata_set_sata_spd_needed(struct ata_port *ap, struct ata_device *dev)
+{
+	u32 scontrol;
+
+	if (!ata_dev_enabled(dev) ||
+	    ap->cbl != ATA_CBL_SATA || !ap->ops->scr_read)
+		return 0;
+
+	scontrol = scr_read(ap, SCR_CONTROL);
+
+	return __ata_set_sata_spd_needed(ap, dev, &scontrol);
+}
+
+/**
+ *	ata_set_sata_spd - set SATA spd according to spd limit
+ *	@ap: Port associated with device @dev
+ *	@dev: Device to set SATA spd
+ *
+ *	Set SATA spd of @ap according to sata_spd_limit.
+ *
+ *	LOCKING:
+ *	Inherited from caller.
+ *
+ *	RETURNS:
+ *	0 if spd doesn't need to be changed, 1 if spd has been
+ *	changed.  -EOPNOTSUPP if SCR registers are inaccessible.
+ */
+static int ata_set_sata_spd(struct ata_port *ap, struct ata_device *dev)
+{
+	u32 scontrol;
+
+	if (ap->cbl != ATA_CBL_SATA || !ap->ops->scr_read)
+		return -EOPNOTSUPP;
+
+	if (!ata_dev_enabled(dev))
+		return 0;
+
+	scontrol = scr_read(ap, SCR_CONTROL);
+	if (!__ata_set_sata_spd_needed(ap, dev, &scontrol))
+		return 0;
+
+	scr_write(ap, SCR_CONTROL, scontrol);
+	return 1;
+}
+
 /*
  * This mode timing computation functionality is ported over from
  * drivers/ide/ide-timing.h and was originally written by Vojtech Pavlik

@@ -2215,7 +2341,14 @@ static int sata_phy_resume(struct ata_po
 void ata_std_probeinit(struct ata_port *ap)
 {
 	if ((ap->flags & ATA_FLAG_SATA) && ap->ops->scr_read) {
+		u32 spd;
+
 		sata_phy_resume(ap);
+
+		spd = (scr_read(ap, SCR_CONTROL) & 0xf0) >> 4;
+		if (spd)
+			ap->device[0].sata_spd_limit &= (1 << spd) - 1;
+
 		if (sata_dev_present(ap))
 			ata_busy_sleep(ap, ATA_TMOUT_BOOT_QUICK, ATA_TMOUT_BOOT);
 	}

@@ -4512,6 +4645,7 @@ static void ata_host_init(struct ata_por
 		dev->pio_mask = UINT_MAX;
 		dev->mwdma_mask = UINT_MAX;
 		dev->udma_mask = UINT_MAX;
+		dev->sata_spd_limit = UINT_MAX;
 	}
 
 #ifdef ATA_IRQ_TRAP

index c6883ba..d720f5b 100644
@@ -369,6 +369,9 @@ struct ata_device {
 	unsigned int		mwdma_mask;
 	unsigned int		udma_mask;
 
+	/* SATA PHY speed limit */
+	unsigned int		sata_spd_limit;
+
 	/* for CHS addressing */
 	u16			cylinders;	/* Number of cylinders */
 	u16			heads;		/* Number of heads */

index 283994d..16095d8 100644
@@ -2310,9 +2310,11 @@ err_out:
 static int sata_phy_resume(struct ata_port *ap)
 {
 	unsigned long timeout = jiffies + (HZ * 5);
-	u32 sstatus;
+	u32 scontrol, sstatus;
 
-	scr_write_flush(ap, SCR_CONTROL, 0x300);
+	scontrol = scr_read(ap, SCR_CONTROL);
+	scontrol = (scontrol & 0x0f0) | 0x300;
+	scr_write_flush(ap, SCR_CONTROL, scontrol);
 
 	/* Wait for phy to become ready, if necessary. */
 	do {

@@ -2432,10 +2434,14 @@ int ata_std_softreset(struct ata_port *a
  */
 int sata_std_hardreset(struct ata_port *ap, int verbose, unsigned int *class)
 {
+	u32 scontrol;
+
 	DPRINTK("ENTER\n");
 
 	/* Issue phy wake/reset */
-	scr_write_flush(ap, SCR_CONTROL, 0x301);
+	scontrol = scr_read(ap, SCR_CONTROL);
+	scontrol = (scontrol & 0x0f0) | 0x301;
+	scr_write_flush(ap, SCR_CONTROL, scontrol);
 
 	/*
 	 * Couldn't find anything in SATA I/II specs, but AHCI-1.1

index 16095d8..30ee203 100644
@@ -2629,7 +2629,7 @@ int ata_drive_probe_reset(struct ata_por
 	if (probeinit)
 		probeinit(ap);
 
-	if (softreset) {
+	if (softreset && !ata_set_sata_spd_needed(ap, &ap->device[0])) {
 		rc = ata_do_reset(ap, softreset, postreset, 0, classes);
 		if (rc == 0 && classes[0] != ATA_DEV_UNKNOWN)
 			goto done;

@@ -2638,9 +2638,18 @@ int ata_drive_probe_reset(struct ata_por
 	if (!hardreset)
 		goto done;
 
-	rc = ata_do_reset(ap, hardreset, postreset, 0, classes);
-	if (rc || classes[0] != ATA_DEV_UNKNOWN)
-		goto done;
+	while (1) {
+		ata_set_sata_spd(ap, &ap->device[0]);
+		rc = ata_do_reset(ap, hardreset, postreset, 0, classes);
+		if (rc == 0) {
+			if (classes[0] != ATA_DEV_UNKNOWN)
+				goto done;
+			break;
+		}
+
+		if (ata_down_sata_spd_limit(ap, &ap->device[0]))
+			goto done;
+	}
 
 	if (softreset)
 		rc = ata_do_reset(ap, softreset, postreset, 0, classes);

index 30ee203..bd9ca3b 100644
@@ -2633,6 +2633,7 @@ int ata_drive_probe_reset(struct ata_por
 		rc = ata_do_reset(ap, softreset, postreset, 0, classes);
 		if (rc == 0 && classes[0] != ATA_DEV_UNKNOWN)
 			goto done;
+		ssleep(1);
 	}
 
 	if (!hardreset)

@@ -2649,6 +2650,7 @@ int ata_drive_probe_reset(struct ata_por
 
 		if (ata_down_sata_spd_limit(ap, &ap->device[0]))
 			goto done;
+		ssleep(1);
 	}
 
 	if (softreset)

index bd9ca3b..ac2ca31 100644
@@ -1370,11 +1370,18 @@ err_out_nosup:
 static int ata_bus_probe(struct ata_port *ap)
 {
 	unsigned int classes[ATA_MAX_DEVICES];
-	int i, rc, found = 0;
+	int tries[ATA_MAX_DEVICES];
+	int i, rc, down_xfermask;
 	struct ata_device *dev;
 
 	ata_port_probe(ap);
 
+	for (i = 0; i < ATA_MAX_DEVICES; i++)
+		tries[i] = ATA_PROBE_MAX_TRIES;
+
+ retry:
+	down_xfermask = 0;
+
 	/* reset and determine device classes */
 	for (i = 0; i < ATA_MAX_DEVICES; i++)
 		classes[i] = ATA_DEV_UNKNOWN;

@@ -1404,21 +1411,23 @@ static int ata_bus_probe(struct ata_port
 		dev = &ap->device[i];
 		dev->class = classes[i];
 
-		if (!ata_dev_enabled(dev))
-			continue;
-
-		WARN_ON(dev->id != NULL);
-		if (ata_dev_read_id(ap, dev, &dev->class, 1, &dev->id)) {
-			dev->class = ATA_DEV_NONE;
-			continue;
+		if (!tries[i]) {
+			ata_down_xfermask_limit(ap, dev, 1);
+			ata_dev_disable(ap, dev);
 		}
 
-		if (ata_dev_configure(ap, dev, 1)) {
-			ata_dev_disable(ap, dev);
+		if (!ata_dev_enabled(dev))
 			continue;
-		}
 
-		found = 1;
+		kfree(dev->id);
+		dev->id = NULL;
+		rc = ata_dev_read_id(ap, dev, &dev->class, 1, &dev->id);
+		if (rc)
+			goto fail;
+
+		rc = ata_dev_configure(ap, dev, 1);
+		if (rc)
+			goto fail;
 	}
 
 	/* configure transfer mode */

@@ -1427,12 +1436,18 @@ static int ata_bus_probe(struct ata_port
 		 * return error code and failing device on failure as
 		 * ata_set_mode() does.
 		 */
-		if (found)
-			ap->ops->set_mode(ap);
+		for (i = 0; i < ATA_MAX_DEVICES; i++)
+			if (ata_dev_enabled(&ap->device[i])) {
+				ap->ops->set_mode(ap);
+				break;
+			}
 		rc = 0;
 	} else {
-		while (ata_set_mode(ap, &dev))
-			ata_dev_disable(ap, dev);
+		rc = ata_set_mode(ap, &dev);
+		if (rc) {
+			down_xfermask = 1;
+			goto fail;
+		}
 	}
 
 	for (i = 0; i < ATA_MAX_DEVICES; i++)

@@ -1443,6 +1458,24 @@ static int ata_bus_probe(struct ata_port
 	ata_port_disable(ap);
 	ap->ops->port_disable(ap);
 	return -ENODEV;
+
+ fail:
+	switch (rc) {
+	case -EINVAL:
+	case -ENODEV:
+		tries[dev->devno] = 0;
+		break;
+	case -EIO:
+		ata_down_sata_spd_limit(ap, dev);
+		/* fall through */
+	default:
+		tries[dev->devno]--;
+		if (down_xfermask &&
+		    ata_down_xfermask_limit(ap, dev, tries[dev->devno] == 1))
+			tries[dev->devno] = 0;
+	}
+
+	goto retry;
 }
 
 /**

index d720f5b..c246a15 100644
@@ -211,6 +211,9 @@ enum {
 	/* Masks for port functions */
 	ATA_PORT_PRIMARY	= (1 << 0),
 	ATA_PORT_SECONDARY	= (1 << 1),
+
+	/* how hard are we gonna try to probe/recover devices */
+	ATA_PROBE_MAX_TRIES	= 3,
 };
 
 enum hsm_task_states {

index ac2ca31..0d72792 100644
@@ -2902,23 +2902,34 @@ static void ata_dev_xfermask(struct ata_
 	unsigned long xfer_mask;
 	int i;
 
-	xfer_mask = ata_pack_xfermask(ap->pio_mask, ap->mwdma_mask,
-				      ap->udma_mask);
+	xfer_mask = ata_pack_xfermask(ap->pio_mask,
+				      ap->mwdma_mask, ap->udma_mask);
+
+	/* Apply cable rule here.  Don't apply it early because when
+	 * we handle hot plug the cable type can itself change.
+	 */
+	if (ap->cbl == ATA_CBL_PATA40)
+		xfer_mask &= ~(0xF8 << ATA_SHIFT_UDMA);
 
 	/* FIXME: Use port-wide xfermask for now */
 	for (i = 0; i < ATA_MAX_DEVICES; i++) {
 		struct ata_device *d = &ap->device[i];
-		if (!ata_dev_enabled(d))
+
+		if (!ata_dev_enabled(d) && !ata_dev_disabled(d))
+			continue;
+
+		if (ata_dev_disabled(d)) {
+			/* to avoid violating device selection timing */
+			xfer_mask &= ata_pack_xfermask(d->pio_mask,
+						       UINT_MAX, UINT_MAX);
 			continue;
-		xfer_mask &= ata_pack_xfermask(d->pio_mask, d->mwdma_mask,
-					       d->udma_mask);
+		}
+
+		xfer_mask &= ata_pack_xfermask(d->pio_mask,
+					       d->mwdma_mask, d->udma_mask);
 		xfer_mask &= ata_id_xfermask(d->id);
 		if (ata_dma_blacklisted(d))
 			xfer_mask &= ~(ATA_MASK_MWDMA | ATA_MASK_UDMA);
-		/* Apply cable rule here. Don't apply it early because when
-		   we handle hot plug the cable type can itself change */
-		if (ap->cbl == ATA_CBL_PATA40)
-			xfer_mask &= ~(0xF8 << ATA_SHIFT_UDMA);
 	}
 
 	if (ata_dma_blacklisted(dev))

@@ -2929,11 +2940,12 @@ static void ata_dev_xfermask(struct ata_
 		if (hs->simplex_claimed)
 			xfer_mask &= ~(ATA_MASK_MWDMA | ATA_MASK_UDMA);
 	}
+
 	if (ap->ops->mode_filter)
 		xfer_mask = ap->ops->mode_filter(ap, dev, xfer_mask);
 
-	ata_unpack_xfermask(xfer_mask, &dev->pio_mask, &dev->mwdma_mask,
-			    &dev->udma_mask);
+	ata_unpack_xfermask(xfer_mask, &dev->pio_mask,
+			    &dev->mwdma_mask, &dev->udma_mask);
 }
 
 /**