1 files changed, 19 insertions, 22 deletions
diff --git a/drivers/ide/ide-iops.c b/drivers/ide/ide-iops.c
index 5403e4a44be4..27bb70ddd459 100644
--- a/drivers/ide/ide-iops.c
+++ b/drivers/ide/ide-iops.c
@@ -27,21 +27,6 @@
 #include <asm/uaccess.h>
 #include <asm/io.h>
-void SELECT_DRIVE(ide_drive_t *drive)
-{
-        ide_hwif_t *hwif = drive->hwif;
-        const struct ide_port_ops *port_ops = hwif->port_ops;
-        struct ide_cmd cmd;
-        if (port_ops && port_ops->selectproc)
-                port_ops->selectproc(drive);
-        memset(&cmd, 0, sizeof(cmd));
-        cmd.tf_flags = IDE_TFLAG_OUT_DEVICE;
-        drive->hwif->tp_ops->tf_load(drive, &cmd);
-}
 void SELECT_MASK(ide_drive_t *drive, int mask)
 {
        const struct ide_port_ops *port_ops = drive->hwif->port_ops;
@@ -55,7 +40,7 @@ u8 ide_read_error(ide_drive_t *drive)
        struct ide_cmd cmd;
        memset(&cmd, 0, sizeof(cmd));
-        cmd.tf_flags = IDE_TFLAG_IN_FEATURE;
+        cmd.tf_flags = IDE_TFLAG_IN_ERROR;
        drive->hwif->tp_ops->tf_read(drive, &cmd);
@@ -306,6 +291,7 @@ int ide_driveid_update(ide_drive_t *drive)
        drive->id[ATA_ID_UDMA_MODES]  = id[ATA_ID_UDMA_MODES];
        drive->id[ATA_ID_MWDMA_MODES] = id[ATA_ID_MWDMA_MODES];
        drive->id[ATA_ID_SWDMA_MODES] = id[ATA_ID_SWDMA_MODES];
+        drive->id[ATA_ID_CFA_MODES]   = id[ATA_ID_CFA_MODES];
        /* anything more ? */
        kfree(id);
@@ -356,10 +342,10 @@ int ide_config_drive_speed(ide_drive_t *drive, u8 speed)
        disable_irq_nosync(hwif->irq);
        udelay(1);
-        SELECT_DRIVE(drive);
+        tp_ops->dev_select(drive);
        SELECT_MASK(drive, 1);
        udelay(1);
-        tp_ops->set_irq(hwif, 0);
+        tp_ops->write_devctl(hwif, ATA_NIEN | ATA_DEVCTL_OBS);
        memset(&cmd, 0, sizeof(cmd));
        cmd.tf_flags = IDE_TFLAG_OUT_FEATURE | IDE_TFLAG_OUT_NSECT;
@@ -371,7 +357,7 @@ int ide_config_drive_speed(ide_drive_t *drive, u8 speed)
        tp_ops->exec_command(hwif, ATA_CMD_SET_FEATURES);
        if (drive->quirk_list == 2)
-                tp_ops->set_irq(hwif, 1);
+                tp_ops->write_devctl(hwif, ATA_DEVCTL_OBS);
        error = __ide_wait_stat(drive, drive->ready_stat,
                                ATA_BUSY | ATA_DRQ | ATA_ERR,
@@ -386,9 +372,14 @@ int ide_config_drive_speed(ide_drive_t *drive, u8 speed)
                return error;
        }
-        id[ATA_ID_UDMA_MODES]  &= ~0xFF00;
+        if (speed >= XFER_SW_DMA_0) {
-        id[ATA_ID_MWDMA_MODES] &= ~0x0F00;
+                id[ATA_ID_UDMA_MODES]  &= ~0xFF00;
-        id[ATA_ID_SWDMA_MODES] &= ~0x0F00;
+                id[ATA_ID_MWDMA_MODES] &= ~0x0700;
+                id[ATA_ID_SWDMA_MODES] &= ~0x0700;
+                if (ata_id_is_cfa(id))
+                        id[ATA_ID_CFA_MODES] &= ~0x0E00;
+        } else  if (ata_id_is_cfa(id))
+                id[ATA_ID_CFA_MODES] &= ~0x01C0;
 skip:
 #ifdef CONFIG_BLK_DEV_IDEDMA
@@ -401,12 +392,18 @@ int ide_config_drive_speed(ide_drive_t *drive, u8 speed)
        if (speed >= XFER_UDMA_0) {
                i = 1 << (speed - XFER_UDMA_0);
                id[ATA_ID_UDMA_MODES] |= (i << 8 | i);
+        } else if (ata_id_is_cfa(id) && speed >= XFER_MW_DMA_3) {
+                i = speed - XFER_MW_DMA_2;
+                id[ATA_ID_CFA_MODES] |= i << 9;
        } else if (speed >= XFER_MW_DMA_0) {
                i = 1 << (speed - XFER_MW_DMA_0);
                id[ATA_ID_MWDMA_MODES] |= (i << 8 | i);
        } else if (speed >= XFER_SW_DMA_0) {
                i = 1 << (speed - XFER_SW_DMA_0);
                id[ATA_ID_SWDMA_MODES] |= (i << 8 | i);
+        } else if (ata_id_is_cfa(id) && speed >= XFER_PIO_5) {
+                i = speed - XFER_PIO_4;
+                id[ATA_ID_CFA_MODES] |= i << 6;
        }
        if (!drive->init_speed)