[PATCH] libata: add xfer_mask handling functions

Add ata_pack_xfermask(), ata_xfer_mask2mode(), ata_xfer_mode2mask(),
ata_xfer_mode2shift() and ata_id_xfermask().  These functions will be
used by following patches to simplify xfer_mask handling.

Signed-off-by: Tejun Heo <htejun@gmail.com>
Signed-off-by: Jeff Garzik <jeff@garzik.org>

diff --git a/drivers/scsi/libata-core.c b/drivers/scsi/libata-core.c
index 18418c8..9946618 100644 (file)
--- a/drivers/scsi/libata-core.c
+++ b/drivers/scsi/libata-core.c
@@ -231,6 +231,108 @@ int ata_rwcmd_protocol(struct ata_queued_cmd *qc)
        return -1;
 }
 
+/**
+ *     ata_pack_xfermask - Pack pio, mwdma and udma masks into xfer_mask
+ *     @pio_mask: pio_mask
+ *     @mwdma_mask: mwdma_mask
+ *     @udma_mask: udma_mask
+ *
+ *     Pack @pio_mask, @mwdma_mask and @udma_mask into a single
+ *     unsigned int xfer_mask.
+ *
+ *     LOCKING:
+ *     None.
+ *
+ *     RETURNS:
+ *     Packed xfer_mask.
+ */
+static unsigned int ata_pack_xfermask(unsigned int pio_mask,
+                                     unsigned int mwdma_mask,
+                                     unsigned int udma_mask)
+{
+       return ((pio_mask << ATA_SHIFT_PIO) & ATA_MASK_PIO) |
+               ((mwdma_mask << ATA_SHIFT_MWDMA) & ATA_MASK_MWDMA) |
+               ((udma_mask << ATA_SHIFT_UDMA) & ATA_MASK_UDMA);
+}
+
+static const struct ata_xfer_ent {
+       unsigned int shift, bits;
+       u8 base;
+} ata_xfer_tbl[] = {
+       { ATA_SHIFT_PIO, ATA_BITS_PIO, XFER_PIO_0 },
+       { ATA_SHIFT_MWDMA, ATA_BITS_MWDMA, XFER_MW_DMA_0 },
+       { ATA_SHIFT_UDMA, ATA_BITS_UDMA, XFER_UDMA_0 },
+       { -1, },
+};
+
+/**
+ *     ata_xfer_mask2mode - Find matching XFER_* for the given xfer_mask
+ *     @xfer_mask: xfer_mask of interest
+ *
+ *     Return matching XFER_* value for @xfer_mask.  Only the highest
+ *     bit of @xfer_mask is considered.
+ *
+ *     LOCKING:
+ *     None.
+ *
+ *     RETURNS:
+ *     Matching XFER_* value, 0 if no match found.
+ */
+static u8 ata_xfer_mask2mode(unsigned int xfer_mask)
+{
+       int highbit = fls(xfer_mask) - 1;
+       const struct ata_xfer_ent *ent;
+
+       for (ent = ata_xfer_tbl; ent->shift >= 0; ent++)
+               if (highbit >= ent->shift && highbit < ent->shift + ent->bits)
+                       return ent->base + highbit - ent->shift;
+       return 0;
+}
+
+/**
+ *     ata_xfer_mode2mask - Find matching xfer_mask for XFER_*
+ *     @xfer_mode: XFER_* of interest
+ *
+ *     Return matching xfer_mask for @xfer_mode.
+ *
+ *     LOCKING:
+ *     None.
+ *
+ *     RETURNS:
+ *     Matching xfer_mask, 0 if no match found.
+ */
+static unsigned int ata_xfer_mode2mask(u8 xfer_mode)
+{
+       const struct ata_xfer_ent *ent;
+
+       for (ent = ata_xfer_tbl; ent->shift >= 0; ent++)
+               if (xfer_mode >= ent->base && xfer_mode < ent->base + ent->bits)
+                       return 1 << (ent->shift + xfer_mode - ent->base);
+       return 0;
+}
+
+/**
+ *     ata_xfer_mode2shift - Find matching xfer_shift for XFER_*
+ *     @xfer_mode: XFER_* of interest
+ *
+ *     Return matching xfer_shift for @xfer_mode.
+ *
+ *     LOCKING:
+ *     None.
+ *
+ *     RETURNS:
+ *     Matching xfer_shift, -1 if no match found.
+ */
+static int ata_xfer_mode2shift(unsigned int xfer_mode)
+{
+       const struct ata_xfer_ent *ent;
+
+       for (ent = ata_xfer_tbl; ent->shift >= 0; ent++)
+               if (xfer_mode >= ent->base && xfer_mode < ent->base + ent->bits)
+                       return ent->shift;
+       return -1;
+}
+
 static const char * const xfer_mode_str[] = {
        "PIO0",
        "PIO1",
@@ -682,6 +784,51 @@ static inline void ata_dump_id(const u16 *id)
                id[93]);
 }
 
+/**
+ *     ata_id_xfermask - Compute xfermask from the given IDENTIFY data
+ *     @id: IDENTIFY data to compute xfer mask from
+ *
+ *     Compute the xfermask for this device. This is not as trivial
+ *     as it seems if we must consider early devices correctly.
+ *
+ *     FIXME: pre IDE drive timing (do we care ?).
+ *
+ *     LOCKING:
+ *     None.
+ *
+ *     RETURNS:
+ *     Computed xfermask
+ */
+static unsigned int ata_id_xfermask(const u16 *id)
+{
+       unsigned int pio_mask, mwdma_mask, udma_mask;
+
+       /* Usual case. Word 53 indicates word 64 is valid */
+       if (id[ATA_ID_FIELD_VALID] & (1 << 1)) {
+               pio_mask = id[ATA_ID_PIO_MODES] & 0x03;
+               pio_mask <<= 3;
+               pio_mask |= 0x7;
+       } else {
+               /* If word 64 isn't valid then Word 51 high byte holds
+                * the PIO timing number for the maximum. Turn it into
+                * a mask.
+                */
+               pio_mask = (2 << (id[ATA_ID_OLD_PIO_MODES] & 0xFF)) - 1 ;
+
+               /* But wait.. there's more. Design your standards by
+                * committee and you too can get a free iordy field to
+                * process. However its the speeds not the modes that
+                * are supported... Note drivers using the timing API
+                * will get this right anyway
+                */
+       }
+
+       mwdma_mask = id[ATA_ID_MWDMA_MODES] & 0x07;
+       udma_mask = id[ATA_ID_UDMA_MODES] & 0xff;
+
+       return ata_pack_xfermask(pio_mask, mwdma_mask, udma_mask);
+}
+
 /*
  *     Compute the PIO modes available for this device. This is not as
  *     trivial as it seems if we must consider early devices correctly.