1 files changed, 33 insertions, 11 deletions

diff --git a/drivers/ide/legacy/qd65xx.c b/drivers/ide/legacy/qd65xx.c
index 60a6ed1088d..bba29df5f21 100644
--- a/drivers/ide/legacy/qd65xx.c
+++ b/drivers/ide/legacy/qd65xx.c
@@ -305,13 +305,33 @@ static int __init qd_testreg(int port)
  * called to setup an ata channel : adjusts attributes & links for tuning
  */
 
-static void __init qd_setup(ide_hwif_t *hwif, int base, int config,
-                            unsigned int data0, unsigned int data1)
+static void __init qd_setup(ide_hwif_t *hwif, int base, int config)
 {
         hwif->select_data = base;
         hwif->config_data = config;
-        hwif->drives[0].drive_data = data0;
-        hwif->drives[1].drive_data = data1;
+}
+
+static void __init qd6500_port_init_devs(ide_hwif_t *hwif)
+{
+        u8 base = hwif->select_data, config = QD_CONFIG(hwif);
+
+        hwif->drives[0].drive_data = QD6500_DEF_DATA;
+        hwif->drives[1].drive_data = QD6500_DEF_DATA;
+}
+
+static void __init qd6580_port_init_devs(ide_hwif_t *hwif)
+{
+        u16 t1, t2;
+        u8 base = hwif->select_data, config = QD_CONFIG(hwif);
+
+        if (QD_CONTROL(hwif) & QD_CONTR_SEC_DISABLED) {
+                t1 = QD6580_DEF_DATA;
+                t2 = QD6580_DEF_DATA2;
+        } else
+                t2 = t1 = hwif->channel ? QD6580_DEF_DATA2 : QD6580_DEF_DATA;
+
+        hwif->drives[0].drive_data = t1;
+        hwif->drives[1].drive_data = t2;
 }
 
 /*
@@ -396,8 +416,9 @@ static int __init qd_probe(int base)
                         return 1;
                 }
 
-                qd_setup(hwif, base, config, QD6500_DEF_DATA, QD6500_DEF_DATA);
+                qd_setup(hwif, base, config);
 
+                hwif->port_init_devs = qd6500_port_init_devs;
                 hwif->set_pio_mode = &qd6500_set_pio_mode;
 
                 idx[unit] = unit;
@@ -429,9 +450,10 @@ static int __init qd_probe(int base)
                         hwif = &ide_hwifs[unit];
                         printk(KERN_INFO "%s: qd6580: single IDE board\n",
                                          hwif->name);
-                        qd_setup(hwif, base, config | (control << 8),
-                                 QD6580_DEF_DATA, QD6580_DEF_DATA2);
 
+                        qd_setup(hwif, base, config | (control << 8));
+
+                        hwif->port_init_devs = qd6580_port_init_devs;
                         hwif->set_pio_mode = &qd6580_set_pio_mode;
 
                         idx[unit] = unit;
@@ -450,14 +472,14 @@ static int __init qd_probe(int base)
                         printk(KERN_INFO "%s&%s: qd6580: dual IDE board\n",
                                         hwif->name, mate->name);
 
-                        qd_setup(hwif, base, config | (control << 8),
-                                 QD6580_DEF_DATA, QD6580_DEF_DATA);
+                        qd_setup(hwif, base, config | (control << 8));
 
+                        hwif->port_init_devs = qd6580_port_init_devs;
                         hwif->set_pio_mode = &qd6580_set_pio_mode;
 
-                        qd_setup(mate, base, config | (control << 8),
-                                 QD6580_DEF_DATA2, QD6580_DEF_DATA2);
+                        qd_setup(mate, base, config | (control << 8));
 
+                        mate->port_init_devs = qd6580_port_init_devs;
                         mate->set_pio_mode = &qd6580_set_pio_mode;
 
                         idx[0] = 0;