libata: add @disable_on_err to ata_set_mode()

index 7b1150d..92c7c53 100644
@@ -394,6 +394,15 @@ static const char *ata_mode_string(unsig
 	return "<n/a>";
 }
 
+static void ata_dev_disable(struct ata_port *ap, struct ata_device *dev)
+{
+	if (ata_dev_present(dev)) {
+		printk(KERN_WARNING "ata%u: dev %u disabled\n",
+		       ap->id, dev->devno);
+		dev->class++;
+	}
+}
+
 /**
  *	ata_pio_devchk - PATA device presence detection
  *	@ap: ATA channel to examine

index 92c7c53..b7595bf 100644
@@ -1373,8 +1373,6 @@ static int ata_dev_configure(struct ata_
 	return 0;
 
 err_out_nosup:
-	printk(KERN_WARNING "ata%u: dev %u not supported, ignoring\n",
-	       ap->id, dev->devno);
 	DPRINTK("EXIT, err\n");
 	return rc;
 }

@@ -1443,7 +1441,7 @@ static int ata_bus_probe(struct ata_port
 		}
 
 		if (ata_dev_configure(ap, dev, 1)) {
-			dev->class++;	/* disable device */
+			ata_dev_disable(ap, dev);
 			continue;
 		}
 

index b7595bf..6826181 100644
@@ -64,7 +64,8 @@
 static unsigned int ata_dev_init_params(struct ata_port *ap,
 					struct ata_device *dev);
 static void ata_set_mode(struct ata_port *ap);
-static void ata_dev_set_xfermode(struct ata_port *ap, struct ata_device *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);
 
 static unsigned int ata_unique_id = 1;

@@ -1754,20 +1755,28 @@ int ata_timing_compute(struct ata_device
 	return 0;
 }
 
-static void ata_dev_set_mode(struct ata_port *ap, struct ata_device *dev)
+static int ata_dev_set_mode(struct ata_port *ap, struct ata_device *dev)
 {
-	if (!ata_dev_present(dev) || (ap->flags & ATA_FLAG_PORT_DISABLED))
-		return;
+	unsigned int err_mask;
+	int rc;
 
 	if (dev->xfer_shift == ATA_SHIFT_PIO)
 		dev->flags |= ATA_DFLAG_PIO;
 
-	ata_dev_set_xfermode(ap, dev);
+	err_mask = ata_dev_set_xfermode(ap, dev);
+	if (err_mask) {
+		printk(KERN_ERR
+		       "ata%u: failed to set xfermode (err_mask=0x%x)\n",
+		       ap->id, err_mask);
+		return -EIO;
+	}
 
-	if (ata_dev_revalidate(ap, dev, 0)) {
-		printk(KERN_ERR "ata%u: failed to revalidate after set "
-		       "xfermode, disabled\n", ap->id);
-		ata_port_disable(ap);
+	rc = ata_dev_revalidate(ap, dev, 0);
+	if (rc) {
+		printk(KERN_ERR
+		       "ata%u: failed to revalidate after set xfermode\n",
+		       ap->id);
+		return rc;
 	}
 
 	DPRINTK("xfer_shift=%u, xfer_mode=0x%x\n",

@@ -1776,6 +1785,7 @@ static void ata_dev_set_mode(struct ata_
 	printk(KERN_INFO "ata%u: dev %u configured for %s\n",
 	       ap->id, dev->devno,
 	       ata_mode_string(ata_xfer_mode2mask(dev->xfer_mode)));
+	return 0;
 }
 
 static int ata_host_set_pio(struct ata_port *ap)

@@ -1859,11 +1869,15 @@ static void ata_set_mode(struct ata_port
 	ata_host_set_dma(ap);
 
 	/* step 4: update devices' xfer mode */
-	for (i = 0; i < ATA_MAX_DEVICES; i++)
-		ata_dev_set_mode(ap, &ap->device[i]);
+	for (i = 0; i < ATA_MAX_DEVICES; i++) {
+		struct ata_device *dev = &ap->device[i];
 
-	if (ap->flags & ATA_FLAG_PORT_DISABLED)
-		return;
+		if (!ata_dev_present(dev))
+			continue;
+
+		if (ata_dev_set_mode(ap, dev))
+			goto err_out;
+	}
 
 	if (ap->ops->post_set_mode)
 		ap->ops->post_set_mode(ap);

@@ -2702,11 +2716,16 @@ static void ata_dev_xfermask(struct ata_
  *
  *	LOCKING:
  *	PCI/etc. bus probe sem.
+ *
+ *	RETURNS:
+ *	0 on success, AC_ERR_* mask otherwise.
  */
 
-static void ata_dev_set_xfermode(struct ata_port *ap, struct ata_device *dev)
+static unsigned int ata_dev_set_xfermode(struct ata_port *ap,
+					 struct ata_device *dev)
 {
 	struct ata_taskfile tf;
+	unsigned int err_mask;
 
 	/* set up set-features taskfile */
 	DPRINTK("set features - xfer mode\n");

@@ -2718,13 +2737,10 @@ static void ata_dev_set_xfermode(struct 
 	tf.protocol = ATA_PROT_NODATA;
 	tf.nsect = dev->xfer_mode;
 
-	if (ata_exec_internal(ap, dev, &tf, DMA_NONE, NULL, 0)) {
-		printk(KERN_ERR "ata%u: failed to set xfermode, disabled\n",
-		       ap->id);
-		ata_port_disable(ap);
-	}
+	err_mask = ata_exec_internal(ap, dev, &tf, DMA_NONE, NULL, 0);
 
-	DPRINTK("EXIT\n");
+	DPRINTK("EXIT, err_mask=%x\n", err_mask);
+	return err_mask;
 }
 
 /**

index 6826181..f620595 100644
@@ -63,7 +63,7 @@
 
 static unsigned int ata_dev_init_params(struct ata_port *ap,
 					struct ata_device *dev);
-static void ata_set_mode(struct ata_port *ap);
+static int ata_set_mode(struct ata_port *ap, int disable_on_err);
 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);

@@ -1449,16 +1449,16 @@ static int ata_bus_probe(struct ata_port
 		found = 1;
 	}
 
-	if (!found)
-		goto err_out_disable;
-
-	ata_set_mode(ap);
-	if (ap->flags & ATA_FLAG_PORT_DISABLED)
-		goto err_out_disable;
-
-	return 0;
+	/* configure transfer mode */
+	if (found) {
+		ata_set_mode(ap, 1);
+		for (i = 0; i < ATA_MAX_DEVICES; i++)
+			if (ata_dev_present(&ap->device[i]))
+				return 0;
+	}
 
-err_out_disable:
+	/* no device present, disable port */
+	ata_port_disable(ap);
 	ap->ops->port_disable(ap);
 	return -1;
 }

@@ -1788,7 +1788,7 @@ static int ata_dev_set_mode(struct ata_p
 	return 0;
 }
 
-static int ata_host_set_pio(struct ata_port *ap)
+static int ata_host_set_pio(struct ata_port *ap, int disable_on_err)
 {
 	int i;
 

@@ -1799,8 +1799,13 @@ static int ata_host_set_pio(struct ata_p
 			continue;
 
 		if (!dev->pio_mode) {
-			printk(KERN_WARNING "ata%u: no PIO support\n", ap->id);
-			return -1;
+			printk(KERN_WARNING "ata%u: dev %u no PIO support\n",
+			       ap->id, dev->devno);
+			if (disable_on_err) {
+				ata_dev_disable(ap, dev);
+				continue;
+			} else
+				return -EINVAL;
 		}
 
 		dev->xfer_mode = dev->pio_mode;

@@ -1832,13 +1837,19 @@ static void ata_host_set_dma(struct ata_
 /**
  *	ata_set_mode - Program timings and issue SET FEATURES - XFER
  *	@ap: port on which timings will be programmed
+ *	@disable_on_err: disable device on error
  *
- *	Set ATA device disk transfer mode (PIO3, UDMA6, etc.).
+ *	Set ATA device disk transfer mode (PIO3, UDMA6, etc.).  If
+ *	@disable_on_err is non-zero, devices which fail to configure
+ *	are taken offline and this function always succeeds.
  *
  *	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, int disable_on_err)
 {
 	int i, rc;
 

@@ -1861,9 +1872,9 @@ static void ata_set_mode(struct ata_port
 	}
 
 	/* step 2: always set host PIO timings */
-	rc = ata_host_set_pio(ap);
+	rc = ata_host_set_pio(ap, disable_on_err);
 	if (rc)
-		goto err_out;
+		return rc;
 
 	/* step 3: set host DMA timings */
 	ata_host_set_dma(ap);

@@ -1875,17 +1886,19 @@ static void ata_set_mode(struct ata_port
 		if (!ata_dev_present(dev))
 			continue;
 
-		if (ata_dev_set_mode(ap, dev))
-			goto err_out;
+		rc = ata_dev_set_mode(ap, dev);
+		if (rc) {
+			if (disable_on_err)
+				ata_dev_disable(ap, dev);
+			else
+				return rc;
+		}
 	}
 
 	if (ap->ops->post_set_mode)
 		ap->ops->post_set_mode(ap);
 
-	return;
-
-err_out:
-	ata_port_disable(ap);
+	return 0;
 }
 
 /**

@@ -4526,7 +4539,7 @@ int ata_device_resume(struct ata_port *a
 {
 	if (ap->flags & ATA_FLAG_SUSPENDED) {
 		ap->flags &= ~ATA_FLAG_SUSPENDED;
-		ata_set_mode(ap);
+		ata_set_mode(ap, 1);
 	}
 	if (!ata_dev_present(dev))
 		return 0;