--- a/drivers/ata/pata_ixp4xx_cf.c
+++ b/drivers/ata/pata_ixp4xx_cf.c
@@ -24,17 +24,58 @@
 #include <scsi/scsi_host.h>
 
 #define DRV_NAME	"pata_ixp4xx_cf"
-#define DRV_VERSION	"0.2"
+#define DRV_VERSION	"0.3"
 
 static int ixp4xx_set_mode(struct ata_link *link, struct ata_device **error)
 {
 	struct ata_device *dev;
+	struct ixp4xx_pata_data *data = link->ap->host->dev->platform_data;
+	unsigned int pio_mask;
 
 	ata_link_for_each_dev(dev, link) {
+    if (dev->id[ATA_ID_FIELD_VALID] & (1 << 1)){
+      pio_mask = dev->id[ATA_ID_PIO_MODES] & 0x03;
+      if (pio_mask & (1 << 1)){
+        pio_mask = 4;
+      }else{
+        pio_mask = 3;
+      }
+    }else{
+      pio_mask = (dev->id[ATA_ID_OLD_PIO_MODES] >> 8);
+    }
+		switch (pio_mask){
+			case 0:
+				ata_dev_printk(dev, KERN_INFO, "configured for PIO0\n");
+				dev->pio_mode = XFER_PIO_0;
+				dev->xfer_mode = XFER_PIO_0;
+				*data->cs0_cfg = 0x8a473c03;
+			break;
+			case 1:
+				ata_dev_printk(dev, KERN_INFO, "configured for PIO1\n");
+				dev->pio_mode = XFER_PIO_1;
+				dev->xfer_mode = XFER_PIO_1;
+				*data->cs0_cfg = 0x86433c03;
+			break;
+			case 2:
+				ata_dev_printk(dev, KERN_INFO, "configured for PIO2\n");
+				dev->pio_mode = XFER_PIO_2;
+				dev->xfer_mode = XFER_PIO_2;
+				*data->cs0_cfg = 0x82413c03;
+			break;
+			case 3:
+				ata_dev_printk(dev, KERN_INFO, "configured for PIO3\n");
+				dev->pio_mode = XFER_PIO_3;
+				dev->xfer_mode = XFER_PIO_3;
+				*data->cs0_cfg = 0x80823c03;
+			break;
+			case 4:
+				ata_dev_printk(dev, KERN_INFO, "configured for PIO4\n");
+				dev->pio_mode = XFER_PIO_4;
+				dev->xfer_mode = XFER_PIO_4;
+				*data->cs0_cfg = 0x80403c03;
+			break;
+		}
 		if (ata_dev_enabled(dev)) {
-			ata_dev_printk(dev, KERN_INFO, "configured for PIO0\n");
-			dev->pio_mode = XFER_PIO_0;
-			dev->xfer_mode = XFER_PIO_0;
 			dev->xfer_shift = ATA_SHIFT_PIO;
 			dev->flags |= ATA_DFLAG_PIO;
 		}
@@ -48,6 +89,7 @@
 	unsigned int i;
 	unsigned int words = buflen >> 1;
 	u16 *buf16 = (u16 *) buf;
+	unsigned int pio_mask;
 	struct ata_port *ap = dev->link->ap;
 	void __iomem *mmio = ap->ioaddr.data_addr;
 	struct ixp4xx_pata_data *data = ap->host->dev->platform_data;
@@ -55,8 +97,34 @@
 	/* set the expansion bus in 16bit mode and restore
 	 * 8 bit mode after the transaction.
 	 */
-	*data->cs0_cfg &= ~(0x01);
-	udelay(100);
+	if (dev->id[ATA_ID_FIELD_VALID] & (1 << 1)){
+		pio_mask = dev->id[ATA_ID_PIO_MODES] & 0x03;
+		if (pio_mask & (1 << 1)){
+			pio_mask = 4;
+		}else{
+			pio_mask = 3;
+		}
+	}else{
+		pio_mask = (dev->id[ATA_ID_OLD_PIO_MODES] >> 8);
+	}
+	switch (pio_mask){
+		case 0:
+			*data->cs0_cfg = 0xa9643c42;
+		break;
+		case 1:
+			*data->cs0_cfg = 0x85033c42;
+		break;
+		case 2:
+			*data->cs0_cfg = 0x80b23c42;
+		break;
+		case 3:
+			*data->cs0_cfg = 0x80823c42;
+		break;
+		case 4:
+			*data->cs0_cfg = 0x80403c42;
+		break;
+	}
+	udelay(5);
 
 	/* Transfer multiple of 2 bytes */
 	if (rw == READ)
@@ -81,8 +149,24 @@
 		words++;
 	}
 
-	udelay(100);
-	*data->cs0_cfg |= 0x01;
+	udelay(5);
+	switch (pio_mask){
+		case 0:
+			*data->cs0_cfg = 0x8a473c03;
+		break;
+		case 1:
+			*data->cs0_cfg = 0x86433c03;
+		break;
+		case 2:
+			*data->cs0_cfg = 0x82413c03;
+		break;
+		case 3:
+			*data->cs0_cfg = 0x80823c03;
+		break;
+		case 4:
+			*data->cs0_cfg = 0x80403c03;
+		break;
+	}
 
 	return words << 1;
 }
