1 files changed, 57 insertions, 74 deletions

diff --git a/drivers/ide/pci/cmd640.c b/drivers/ide/pci/cmd640.c
index cd1ba14984ab..1ad1e23e3105 100644
--- a/drivers/ide/pci/cmd640.c
+++ b/drivers/ide/pci/cmd640.c
@@ -521,6 +521,7 @@ static void program_drive_counts(ide_drive_t *drive, unsigned int index)
 static void cmd640_set_mode(ide_drive_t *drive, unsigned int index,
                             u8 pio_mode, unsigned int cycle_time)
 {
+        struct ide_timing *t;
         int setup_time, active_time, recovery_time, clock_time;
         u8 setup_count, active_count, recovery_count, recovery_count2, cycle_count;
         int bus_speed;
@@ -532,8 +533,11 @@ static void cmd640_set_mode(ide_drive_t *drive, unsigned int index,
 
         if (pio_mode > 5)
                 pio_mode = 5;
-        setup_time  = ide_pio_timings[pio_mode].setup_time;
-        active_time = ide_pio_timings[pio_mode].active_time;
+
+        t = ide_timing_find_mode(XFER_PIO_0 + pio_mode);
+        setup_time  = t->setup;
+        active_time = t->active;
+
         recovery_time = cycle_time - (setup_time + active_time);
         clock_time = 1000 / bus_speed;
         cycle_count = DIV_ROUND_UP(cycle_time, clock_time);
@@ -607,11 +611,40 @@ static void cmd640_set_pio_mode(ide_drive_t *drive, const u8 pio)
 
         display_clocks(index);
 }
+#endif /* CONFIG_BLK_DEV_CMD640_ENHANCED */
+
+static void cmd640_init_dev(ide_drive_t *drive)
+{
+        unsigned int i = drive->hwif->channel * 2 + drive->select.b.unit;
+
+#ifdef CONFIG_BLK_DEV_CMD640_ENHANCED
+        /*
+         * Reset timing to the slowest speed and turn off prefetch.
+         * This way, the drive identify code has a better chance.
+         */
+        setup_counts[i]    =  4;        /* max possible */
+        active_counts[i]   = 16;        /* max possible */
+        recovery_counts[i] = 16;        /* max possible */
+        program_drive_counts(drive, i);
+        set_prefetch_mode(drive, i, 0);
+        printk(KERN_INFO DRV_NAME ": drive%d timings/prefetch cleared\n", i);
+#else
+        /*
+         * Set the drive unmask flags to match the prefetch setting.
+         */
+        check_prefetch(drive, i);
+        printk(KERN_INFO DRV_NAME ": drive%d timings/prefetch(%s) preserved\n",
+                                  i, drive->no_io_32bit ? "off" : "on");
+#endif /* CONFIG_BLK_DEV_CMD640_ENHANCED */
+}
+
 
 static const struct ide_port_ops cmd640_port_ops = {
+        .init_dev               = cmd640_init_dev,
+#ifdef CONFIG_BLK_DEV_CMD640_ENHANCED
         .set_pio_mode           = cmd640_set_pio_mode,
+#endif
 };
-#endif /* CONFIG_BLK_DEV_CMD640_ENHANCED */
 
 static int pci_conf1(void)
 {
@@ -654,10 +687,8 @@ static const struct ide_port_info cmd640_port_info __initdata = {
                                   IDE_HFLAG_NO_DMA |
                                   IDE_HFLAG_ABUSE_PREFETCH |
                                   IDE_HFLAG_ABUSE_FAST_DEVSEL,
-#ifdef CONFIG_BLK_DEV_CMD640_ENHANCED
         .port_ops               = &cmd640_port_ops,
         .pio_mask               = ATA_PIO5,
-#endif
 };
 
 static int cmd640x_init_one(unsigned long base, unsigned long ctl)
@@ -683,12 +714,8 @@ static int cmd640x_init_one(unsigned long base, unsigned long ctl)
  */
 static int __init cmd640x_init(void)
 {
-#ifdef CONFIG_BLK_DEV_CMD640_ENHANCED
-        int second_port_toggled = 0;
-#endif /* CONFIG_BLK_DEV_CMD640_ENHANCED */
         int second_port_cmd640 = 0, rc;
         const char *bus_type, *port2;
-        unsigned int index;
         u8 b, cfr;
         u8 idx[4] = { 0xff, 0xff, 0xff, 0xff };
         hw_regs_t hw[2];
@@ -774,88 +801,44 @@ static int __init cmd640x_init(void)
         put_cmd640_reg(CMDTIM, 0);
         put_cmd640_reg(BRST, 0x40);
 
-        cmd_hwif1 = ide_find_port();
+        b = get_cmd640_reg(CNTRL);
 
         /*
          * Try to enable the secondary interface, if not already enabled
          */
-        if (cmd_hwif1 &&
-            cmd_hwif1->drives[0].noprobe && cmd_hwif1->drives[1].noprobe) {
-                port2 = "not probed";
+        if (secondary_port_responding()) {
+                if ((b & CNTRL_ENA_2ND)) {
+                        second_port_cmd640 = 1;
+                        port2 = "okay";
+                } else if (cmd640_vlb) {
+                        second_port_cmd640 = 1;
+                        port2 = "alive";
+                } else
+                        port2 = "not cmd640";
         } else {
-                b = get_cmd640_reg(CNTRL);
+                put_cmd640_reg(CNTRL, b ^ CNTRL_ENA_2ND); /* toggle the bit */
                 if (secondary_port_responding()) {
-                        if ((b & CNTRL_ENA_2ND)) {
-                                second_port_cmd640 = 1;
-                                port2 = "okay";
-                        } else if (cmd640_vlb) {
-                                second_port_cmd640 = 1;
-                                port2 = "alive";
-                        } else
-                                port2 = "not cmd640";
+                        second_port_cmd640 = 1;
+                        port2 = "enabled";
                 } else {
-                        put_cmd640_reg(CNTRL, b ^ CNTRL_ENA_2ND); /* toggle the bit */
-                        if (secondary_port_responding()) {
-                                second_port_cmd640 = 1;
-#ifdef CONFIG_BLK_DEV_CMD640_ENHANCED
-                                second_port_toggled = 1;
-#endif /* CONFIG_BLK_DEV_CMD640_ENHANCED */
-                                port2 = "enabled";
-                        } else {
-                                put_cmd640_reg(CNTRL, b); /* restore original setting */
-                                port2 = "not responding";
-                        }
+                        put_cmd640_reg(CNTRL, b); /* restore original setting */
+                        port2 = "not responding";
                 }
         }
 
         /*
          * Initialize data for secondary cmd640 port, if enabled
          */
-        if (second_port_cmd640 && cmd_hwif1) {
-                ide_init_port_hw(cmd_hwif1, &hw[1]);
-                idx[1] = cmd_hwif1->index;
+        if (second_port_cmd640) {
+                cmd_hwif1 = ide_find_port();
+                if (cmd_hwif1) {
+                        ide_init_port_hw(cmd_hwif1, &hw[1]);
+                        idx[1] = cmd_hwif1->index;
+                }
         }
         printk(KERN_INFO "cmd640: %sserialized, secondary interface %s\n",
                          second_port_cmd640 ? "" : "not ", port2);
 
-        /*
-         * Establish initial timings/prefetch for all drives.
-         * Do not unnecessarily disturb any prior BIOS setup of these.
-         */
-        for (index = 0; index < (2 + (second_port_cmd640 << 1)); index++) {
-                ide_drive_t *drive;
-
-                if (index > 1) {
-                        if (cmd_hwif1 == NULL)
-                                continue;
-                        drive = &cmd_hwif1->drives[index & 1];
-                } else  {
-                        if (cmd_hwif0 == NULL)
-                                continue;
-                        drive = &cmd_hwif0->drives[index & 1];
-                }
-
-#ifdef CONFIG_BLK_DEV_CMD640_ENHANCED
-                /*
-                 * Reset timing to the slowest speed and turn off prefetch.
-                 * This way, the drive identify code has a better chance.
-                 */
-                setup_counts    [index] = 4;    /* max possible */
-                active_counts   [index] = 16;   /* max possible */
-                recovery_counts [index] = 16;   /* max possible */
-                program_drive_counts(drive, index);
-                set_prefetch_mode(drive, index, 0);
-                printk("cmd640: drive%d timings/prefetch cleared\n", index);
-#else
-                /*
-                 * Set the drive unmask flags to match the prefetch setting
-                 */
-                check_prefetch(drive, index);
-                printk("cmd640: drive%d timings/prefetch(%s) preserved\n",
-                        index, drive->no_io_32bit ? "off" : "on");
-#endif /* CONFIG_BLK_DEV_CMD640_ENHANCED */
-        }
-
 #ifdef CMD640_DUMP_REGS
         cmd640_dump_regs();
 #endif