3 files changed, 86 insertions, 3 deletions

diff --git a/drivers/firewire/core.h b/drivers/firewire/core.h
index b2a7b651473a..7a9759bf6837 100644
--- a/drivers/firewire/core.h
+++ b/drivers/firewire/core.h
@@ -27,8 +27,12 @@ struct fw_packet;
 #define PHY_LINK_ACTIVE         0x80
 #define PHY_CONTENDER           0x40
 #define PHY_BUS_RESET           0x40
+#define PHY_EXTENDED_REGISTERS  0xe0
 #define PHY_BUS_SHORT_RESET     0x40
 #define PHY_INT_STATUS_BITS     0x3c
+#define PHY_ENABLE_ACCEL        0x02
+#define PHY_ENABLE_MULTI        0x01
+#define PHY_PAGE_SELECT         0xe0
 
 #define BANDWIDTH_AVAILABLE_INITIAL     4915
 #define BROADCAST_CHANNEL_INITIAL       (1 << 31 | 31)
diff --git a/drivers/firewire/ohci.c b/drivers/firewire/ohci.c
index 525848f71c34..e934713f3fce 100644
--- a/drivers/firewire/ohci.c
+++ b/drivers/firewire/ohci.c
@@ -236,13 +236,15 @@ static char ohci_driver_name[] = KBUILD_MODNAME;
 #define QUIRK_CYCLE_TIMER               1
 #define QUIRK_RESET_PACKET              2
 #define QUIRK_BE_HEADERS                4
+#define QUIRK_NO_1394A                  8
 
 /* In case of multiple matches in ohci_quirks[], only the first one is used. */
 static const struct {
         unsigned short vendor, device, flags;
 } ohci_quirks[] = {
         {PCI_VENDOR_ID_TI,      PCI_DEVICE_ID_TI_TSB12LV22, QUIRK_CYCLE_TIMER |
-                                                            QUIRK_RESET_PACKET},
+                                                            QUIRK_RESET_PACKET |
+                                                            QUIRK_NO_1394A},
         {PCI_VENDOR_ID_TI,      PCI_ANY_ID,     QUIRK_RESET_PACKET},
         {PCI_VENDOR_ID_AL,      PCI_ANY_ID,     QUIRK_CYCLE_TIMER},
         {PCI_VENDOR_ID_NEC,     PCI_ANY_ID,     QUIRK_CYCLE_TIMER},
@@ -257,6 +259,7 @@ MODULE_PARM_DESC(quirks, "Chip quirks (default = 0"
         ", nonatomic cycle timer = "    __stringify(QUIRK_CYCLE_TIMER)
         ", reset packet generation = "  __stringify(QUIRK_RESET_PACKET)
         ", AR/selfID endianess = "      __stringify(QUIRK_BE_HEADERS)
+        ", no 1394a enhancements = "    __stringify(QUIRK_NO_1394A)
         ")");
 
 #ifdef CONFIG_FIREWIRE_OHCI_DEBUG
@@ -504,6 +507,27 @@ static int ohci_update_phy_reg(struct fw_card *card, int addr,
         return 0;
 }
 
+static int read_paged_phy_reg(struct fw_card *card,
+                              int page, int addr, u32 *value)
+{
+        struct fw_ohci *ohci = fw_ohci(card);
+        u32 reg;
+        int err;
+
+        err = ohci_update_phy_reg(card, 7, PHY_PAGE_SELECT, page << 5);
+        if (err < 0)
+                return err;
+        flush_writes(ohci);
+        msleep(2);
+        reg = reg_read(ohci, OHCI1394_PhyControl);
+        if ((reg & OHCI1394_PhyControl_WritePending) != 0) {
+                fw_error("failed to write phy reg bits\n");
+                return -EBUSY;
+        }
+
+        return read_phy_reg(card, addr, value);
+}
+
 static int ar_context_add_page(struct ar_context *ctx)
 {
         struct device *dev = ctx->ohci->card.device;
@@ -1511,13 +1535,64 @@ static void copy_config_rom(__be32 *dest, const __be32 *src, size_t length)
                 memset(&dest[length], 0, CONFIG_ROM_SIZE - size);
 }
 
+static int configure_1394a_enhancements(struct fw_ohci *ohci)
+{
+        bool enable_1394a;
+        u32 reg, phy_compliance;
+        int clear, set, offset;
+
+        /* Check if the driver should configure link and PHY. */
+        if (!(reg_read(ohci, OHCI1394_HCControlSet) &
+              OHCI1394_HCControl_programPhyEnable))
+                return 0;
+
+        /* Paranoia: check whether the PHY supports 1394a, too. */
+        enable_1394a = false;
+        if (read_phy_reg(&ohci->card, 2, &reg) < 0)
+                return -EIO;
+        if ((reg & PHY_EXTENDED_REGISTERS) == PHY_EXTENDED_REGISTERS) {
+                if (read_paged_phy_reg(&ohci->card, 1, 8, &phy_compliance) < 0)
+                        return -EIO;
+                if (phy_compliance >= 1)
+                        enable_1394a = true;
+        }
+
+        if (ohci->quirks & QUIRK_NO_1394A)
+                enable_1394a = false;
+
+        /* Configure PHY and link consistently. */
+        if (enable_1394a) {
+                clear = 0;
+                set = PHY_ENABLE_ACCEL | PHY_ENABLE_MULTI;
+        } else {
+                clear = PHY_ENABLE_ACCEL | PHY_ENABLE_MULTI;
+                set = 0;
+        }
+        if (ohci_update_phy_reg(&ohci->card, 5, clear, set) < 0)
+                return -EIO;
+        flush_writes(ohci);
+        msleep(2);
+
+        if (enable_1394a)
+                offset = OHCI1394_HCControlSet;
+        else
+                offset = OHCI1394_HCControlClear;
+        reg_write(ohci, offset, OHCI1394_HCControl_aPhyEnhanceEnable);
+
+        /* Clean up: configuration has been taken care of. */
+        reg_write(ohci, OHCI1394_HCControlClear,
+                  OHCI1394_HCControl_programPhyEnable);
+
+        return 0;
+}
+
 static int ohci_enable(struct fw_card *card,
                        const __be32 *config_rom, size_t length)
 {
         struct fw_ohci *ohci = fw_ohci(card);
         struct pci_dev *dev = to_pci_dev(card->device);
         u32 lps;
-        int i;
+        int i, err;
 
         if (software_reset(ohci)) {
                 fw_error("Failed to reset ohci card.\n");
@@ -1581,6 +1656,10 @@ static int ohci_enable(struct fw_card *card,
         if (param_debug & OHCI_PARAM_DEBUG_BUSRESETS)
                 reg_write(ohci, OHCI1394_IntMaskSet, OHCI1394_busReset);
 
+        err = configure_1394a_enhancements(ohci);
+        if (err < 0)
+                return err;
+
         /* Activate link_on bit and contender bit in our self ID packets.*/
         if (ohci_update_phy_reg(card, 4, 0,
                                 PHY_LINK_ACTIVE | PHY_CONTENDER) < 0)
diff --git a/drivers/firewire/ohci.h b/drivers/firewire/ohci.h
index ba492d85c516..d49e1469a986 100644
--- a/drivers/firewire/ohci.h
+++ b/drivers/firewire/ohci.h
@@ -67,7 +67,7 @@
 #define   OHCI1394_PhyControl_ReadDone          0x80000000
 #define   OHCI1394_PhyControl_ReadData(r)       (((r) & 0x00ff0000) >> 16)
 #define   OHCI1394_PhyControl_Write(addr, data) (((addr) << 8) | (data) | 0x00004000)
-#define   OHCI1394_PhyControl_WriteDone         0x00004000
+#define   OHCI1394_PhyControl_WritePending      0x00004000
 #define OHCI1394_IsochronousCycleTimer        0x0F0
 #define OHCI1394_AsReqFilterHiSet             0x100
 #define OHCI1394_AsReqFilterHiClear           0x104