1 files changed, 73 insertions, 52 deletions

diff --git a/drivers/ide/ide-lib.c b/drivers/ide/ide-lib.c
index 850ef63cc986..8237d89eec6e 100644
--- a/drivers/ide/ide-lib.c
+++ b/drivers/ide/ide-lib.c
@@ -71,75 +71,96 @@ EXPORT_SYMBOL(ide_xfer_verbose);
 /**
  *      ide_dma_speed   -       compute DMA speed
  *      @drive: drive
- *      @mode; intended mode
+ *      @mode:  modes available
  *
  *      Checks the drive capabilities and returns the speed to use
- *      for the transfer. Returns -1 if the requested mode is unknown
- *      (eg PIO)
+ *      for the DMA transfer.  Returns 0 if the drive is incapable
+ *      of DMA transfers.
  */
  
 u8 ide_dma_speed(ide_drive_t *drive, u8 mode)
 {
         struct hd_driveid *id   = drive->id;
         ide_hwif_t *hwif        = HWIF(drive);
+        u8 ultra_mask, mwdma_mask, swdma_mask;
         u8 speed = 0;
 
         if (drive->media != ide_disk && hwif->atapi_dma == 0)
                 return 0;
 
-        switch(mode) {
-                case 0x04:
-                        if ((id->dma_ultra & 0x0040) &&
-                            (id->dma_ultra & hwif->ultra_mask))
-                                { speed = XFER_UDMA_6; break; }
-                case 0x03:
-                        if ((id->dma_ultra & 0x0020) &&
-                            (id->dma_ultra & hwif->ultra_mask))
-                                { speed = XFER_UDMA_5; break; }
-                case 0x02:
-                        if ((id->dma_ultra & 0x0010) &&
-                            (id->dma_ultra & hwif->ultra_mask))
-                                { speed = XFER_UDMA_4; break; }
-                        if ((id->dma_ultra & 0x0008) &&
-                            (id->dma_ultra & hwif->ultra_mask))
-                                { speed = XFER_UDMA_3; break; }
-                case 0x01:
-                        if ((id->dma_ultra & 0x0004) &&
-                            (id->dma_ultra & hwif->ultra_mask))
-                                { speed = XFER_UDMA_2; break; }
-                        if ((id->dma_ultra & 0x0002) &&
-                            (id->dma_ultra & hwif->ultra_mask))
-                                { speed = XFER_UDMA_1; break; }
-                        if ((id->dma_ultra & 0x0001) &&
-                            (id->dma_ultra & hwif->ultra_mask))
-                                { speed = XFER_UDMA_0; break; }
-                case 0x00:
-                        if ((id->dma_mword & 0x0004) &&
-                            (id->dma_mword & hwif->mwdma_mask))
-                                { speed = XFER_MW_DMA_2; break; }
-                        if ((id->dma_mword & 0x0002) &&
-                            (id->dma_mword & hwif->mwdma_mask))
-                                { speed = XFER_MW_DMA_1; break; }
-                        if ((id->dma_mword & 0x0001) &&
-                            (id->dma_mword & hwif->mwdma_mask))
-                                { speed = XFER_MW_DMA_0; break; }
-                        if ((id->dma_1word & 0x0004) &&
-                            (id->dma_1word & hwif->swdma_mask))
-                                { speed = XFER_SW_DMA_2; break; }
-                        if ((id->dma_1word & 0x0002) &&
-                            (id->dma_1word & hwif->swdma_mask))
-                                { speed = XFER_SW_DMA_1; break; }
-                        if ((id->dma_1word & 0x0001) &&
-                            (id->dma_1word & hwif->swdma_mask))
-                                { speed = XFER_SW_DMA_0; break; }
-        }
+        /* Capable of UltraDMA modes? */
+        ultra_mask = id->dma_ultra & hwif->ultra_mask;
+
+        if (!(id->field_valid & 4))
+                mode = 0;       /* fallback to MW/SW DMA if no UltraDMA */
+
+        switch (mode) {
+        case 4:
+                if (ultra_mask & 0x40) {
+                        speed = XFER_UDMA_6;
+                        break;
+                }
+        case 3:
+                if (ultra_mask & 0x20) {
+                        speed = XFER_UDMA_5;
+                        break;
+                }
+        case 2:
+                if (ultra_mask & 0x10) {
+                        speed = XFER_UDMA_4;
+                        break;
+                }
+                if (ultra_mask & 0x08) {
+                        speed = XFER_UDMA_3;
+                        break;
+                }
+        case 1:
+                if (ultra_mask & 0x04) {
+                        speed = XFER_UDMA_2;
+                        break;
+                }
+                if (ultra_mask & 0x02) {
+                        speed = XFER_UDMA_1;
+                        break;
+                }
+                if (ultra_mask & 0x01) {
+                        speed = XFER_UDMA_0;
+                        break;
+                }
+        case 0:
+                mwdma_mask = id->dma_mword & hwif->mwdma_mask;
 
-//      printk("%s: %s: mode 0x%02x, speed 0x%02x\n",
-//              __FUNCTION__, drive->name, mode, speed);
+                if (mwdma_mask & 0x04) {
+                        speed = XFER_MW_DMA_2;
+                        break;
+                }
+                if (mwdma_mask & 0x02) {
+                        speed = XFER_MW_DMA_1;
+                        break;
+                }
+                if (mwdma_mask & 0x01) {
+                        speed = XFER_MW_DMA_0;
+                        break;
+                }
+
+                swdma_mask = id->dma_1word & hwif->swdma_mask;
+
+                if (swdma_mask & 0x04) {
+                        speed = XFER_SW_DMA_2;
+                        break;
+                }
+                if (swdma_mask & 0x02) {
+                        speed = XFER_SW_DMA_1;
+                        break;
+                }
+                if (swdma_mask & 0x01) {
+                        speed = XFER_SW_DMA_0;
+                        break;
+                }
+        }
 
         return speed;
 }
-
 EXPORT_SYMBOL(ide_dma_speed);