1 files changed, 11 insertions, 33 deletions

diff --git a/drivers/ide/pci/it8213.c b/drivers/ide/pci/it8213.c
index ecf4ce078dce..6dab0daf4943 100644
--- a/drivers/ide/pci/it8213.c
+++ b/drivers/ide/pci/it8213.c
@@ -18,37 +18,6 @@
 #include <asm/io.h>
 
 /**
- *      it8213_dma_2_pio                -       return the PIO mode matching DMA
- *      @xfer_rate: transfer speed
- *
- *      Returns the nearest equivalent PIO timing for the DMA
- *      mode requested by the controller.
- */
-
-static u8 it8213_dma_2_pio (u8 xfer_rate) {
-        switch(xfer_rate) {
-                case XFER_UDMA_6:
-                case XFER_UDMA_5:
-                case XFER_UDMA_4:
-                case XFER_UDMA_3:
-                case XFER_UDMA_2:
-                case XFER_UDMA_1:
-                case XFER_UDMA_0:
-                case XFER_MW_DMA_2:
-                        return 4;
-                case XFER_MW_DMA_1:
-                        return 3;
-                case XFER_SW_DMA_2:
-                        return 2;
-                case XFER_MW_DMA_0:
-                case XFER_SW_DMA_1:
-                case XFER_SW_DMA_0:
-                default:
-                        return 0;
-        }
-}
-
-/**
  *      it8213_set_pio_mode     -       set host controller for PIO mode
  *      @drive: drive
  *      @pio: PIO mode number
@@ -124,7 +93,7 @@ static void it8213_set_dma_mode(ide_drive_t *drive, const u8 speed)
         int w_flag              = 0x10 << drive->dn;
         int u_speed             = 0;
         u16                     reg4042, reg4a;
-        u8                      reg48, reg54, reg55;
+        u8                      reg48, reg54, reg55, pio;
 
         pci_read_config_word(dev, maslave, &reg4042);
         pci_read_config_byte(dev, 0x48, &reg48);
@@ -165,7 +134,11 @@ static void it8213_set_dma_mode(ide_drive_t *drive, const u8 speed)
                                 pci_write_config_byte(dev, 0x54, reg54 | v_flag);
                 } else
                         pci_write_config_byte(dev, 0x54, reg54 & ~v_flag);
+
+                pio = 4;
         } else {
+                const u8 mwdma_to_pio[] = { 0, 3, 4 };
+
                 if (reg48 & u_flag)
                         pci_write_config_byte(dev, 0x48, reg48 & ~u_flag);
                 if (reg4a & a_speed)
@@ -174,9 +147,14 @@ static void it8213_set_dma_mode(ide_drive_t *drive, const u8 speed)
                         pci_write_config_byte(dev, 0x54, reg54 & ~v_flag);
                 if (reg55 & w_flag)
                         pci_write_config_byte(dev, 0x55, (u8) reg55 & ~w_flag);
+
+                if (speed >= XFER_MW_DMA_0)
+                        pio = mwdma_to_pio[speed - XFER_MW_DMA_0];
+                else
+                        pio = 2; /* only SWDMA2 is allowed */
         }
 
-        it8213_set_pio_mode(drive, it8213_dma_2_pio(speed));
+        it8213_set_pio_mode(drive, pio);
 }
 
 /**