firewire: ohci: Add support for TSB41BA3D phy

This patch implements a work around for the Texas Instruments PHY
TSB41BA3D.  This phy has a bug at least in combination with the TI LLCs
TSB82AA2B and TSB12LV26.  The selfid coming from the locally connected
phy is not propagated into the selfid buffer of the OHCI (see
http://www.ti.com/litv/pdf/sllz059 for details).  The main idea is to
construct the selfid ourselves.

Signed-off-by: Stephan Gatzka <stephan@gatzka.org>
Signed-off-by: Stefan Richter <stefanr@s5r6.in-berlin.de>

drivers/firewire/ohci.c

@@ -264,6 +264,8 @@ static char ohci_driver_name[] = KBUILD_MODNAME;
 #define PCI_DEVICE_ID_AGERE_FW643	0x5901
 #define PCI_DEVICE_ID_JMICRON_JMB38X_FW	0x2380
 #define PCI_DEVICE_ID_TI_TSB12LV22	0x8009
+#define PCI_DEVICE_ID_TI_TSB12LV26	0x8020
+#define PCI_DEVICE_ID_TI_TSB82AA2	0x8025
 #define PCI_VENDOR_ID_PINNACLE_SYSTEMS	0x11bd
 
 #define QUIRK_CYCLE_TIMER		1
@@ -271,6 +273,7 @@ static char ohci_driver_name[] = KBUILD_MODNAME;
 #define QUIRK_BE_HEADERS		4
 #define QUIRK_NO_1394A			8
 #define QUIRK_NO_MSI			16
+#define QUIRK_TI_SLLZ059		32
 
 /* In case of multiple matches in ohci_quirks[], only the first one is used. */
 static const struct {
@@ -300,6 +303,12 @@ static const struct {
 	{PCI_VENDOR_ID_TI, PCI_DEVICE_ID_TI_TSB12LV22, PCI_ANY_ID,
 		QUIRK_CYCLE_TIMER | QUIRK_RESET_PACKET | QUIRK_NO_1394A},
 
+	{PCI_VENDOR_ID_TI, PCI_DEVICE_ID_TI_TSB12LV26, PCI_ANY_ID,
+		QUIRK_RESET_PACKET | QUIRK_TI_SLLZ059},
+
+	{PCI_VENDOR_ID_TI, PCI_DEVICE_ID_TI_TSB82AA2, PCI_ANY_ID,
+		QUIRK_RESET_PACKET | QUIRK_TI_SLLZ059},
+
 	{PCI_VENDOR_ID_TI, PCI_ANY_ID, PCI_ANY_ID,
 		QUIRK_RESET_PACKET},
 
@@ -316,6 +325,7 @@ MODULE_PARM_DESC(quirks, "Chip quirks (default = 0"
 	", AR/selfID endianess = "	__stringify(QUIRK_BE_HEADERS)
 	", no 1394a enhancements = "	__stringify(QUIRK_NO_1394A)
 	", disable MSI = "		__stringify(QUIRK_NO_MSI)
+	", workaround for TI SLLZ059 errata = "	__stringify(QUIRK_TI_SLLZ059)
 	")");
 
 #define OHCI_PARAM_DEBUG_AT_AR		1
@@ -1714,6 +1724,114 @@ static u32 update_bus_time(struct fw_ohci *ohci)
 	return ohci->bus_time | cycle_time_seconds;
 }
 
+static int get_status_for_port(struct fw_ohci *ohci, int port_index)
+{
+	int reg;
+
+	mutex_lock(&ohci->phy_reg_mutex);
+	reg = write_phy_reg(ohci, 7, port_index);
+	mutex_unlock(&ohci->phy_reg_mutex);
+	if (reg < 0)
+		return reg;
+
+	mutex_lock(&ohci->phy_reg_mutex);
+	reg = read_phy_reg(ohci, 8);
+	mutex_unlock(&ohci->phy_reg_mutex);
+	if (reg < 0)
+		return reg;
+
+	switch (reg & 0x0f) {
+	case 0x06:
+		return 2;	/* is child node (connected to parent node) */
+	case 0x0e:
+		return 3;	/* is parent node (connected to child node) */
+	}
+	return 1;		/* not connected */
+}
+
+static int get_self_id_pos(struct fw_ohci *ohci, u32 self_id,
+	int self_id_count)
+{
+	int i;
+	u32 entry;
+	for (i = 0; i < self_id_count; i++) {
+		entry = ohci->self_id_buffer[i];
+		if ((self_id & 0xff000000) == (entry & 0xff000000))
+			return -1;
+		if ((self_id & 0xff000000) < (entry & 0xff000000))
+			return i;
+	}
+	return i;
+}
+
+/*
+ * This function implements a work around for the Texas Instruments PHY
+ * TSB41BA3D. This phy has a bug at least in combination with the TI
+ * LLCs TSB82AA2B and TSB12LV26. The selfid coming from the locally
+ * connected phy is not propagated into the selfid buffer of the OHCI
+ * (see http://www.ti.com/litv/pdf/sllz059 for details).
+ * The main idea is to construct the selfid ourselves.
+ */
+
+static int find_and_insert_self_id(struct fw_ohci *ohci, int self_id_count)
+{
+	int reg;
+	int i;
+	int pos;
+	int status;
+	u32 self_id;
+
+/*
+ * preset bits in self_id
+ *
+ * link active: 0b1
+ * speed: 0b11
+ * bridge: 0b00
+ * contender: 0b1
+ * initiated reset: 0b0
+ * more packets: 0b0
+ */
+	self_id = 0x8040C800;
+
+	reg = reg_read(ohci, OHCI1394_NodeID);
+	if (!(reg & OHCI1394_NodeID_idValid)) {
+		fw_notify("node ID not valid, new bus reset in progress\n");
+		return -EBUSY;
+	}
+	self_id |= ((reg & 0x3f) << 24); /* phy ID */
+
+	mutex_lock(&ohci->phy_reg_mutex);
+	reg = read_phy_reg(ohci, 4);
+	mutex_unlock(&ohci->phy_reg_mutex);
+	if (reg < 0)
+		return reg;
+	self_id |= ((reg & 0x07) << 8); /* power class */
+
+	mutex_lock(&ohci->phy_reg_mutex);
+	reg = read_phy_reg(ohci, 1);
+	mutex_unlock(&ohci->phy_reg_mutex);
+	if (reg < 0)
+		return reg;
+	self_id |= ((reg & 0x3f) << 16); /* gap count */
+
+	for (i = 0; i < 3; i++) {
+		status = get_status_for_port(ohci, i);
+		if (status < 0)
+			return status;
+		self_id |= ((status & 0x3) << (6 - (i * 2)));
+	}
+
+	pos = get_self_id_pos(ohci, self_id, self_id_count);
+	if (pos >= 0) {
+		memmove(&(ohci->self_id_buffer[pos+1]),
+			&(ohci->self_id_buffer[pos]),
+			(self_id_count - pos) * sizeof(*ohci->self_id_buffer));
+		ohci->self_id_buffer[pos] = self_id;
+		self_id_count++;
+	}
+	return self_id_count;
+}
+
 static void bus_reset_work(struct work_struct *work)
 {
 	struct fw_ohci *ohci =
@@ -1755,10 +1873,12 @@ static void bus_reset_work(struct work_struct *work)
 	 * bit extra to get the actual number of self IDs.
 	 */
 	self_id_count = (reg >> 3) & 0xff;
-	if (self_id_count == 0 || self_id_count > 252) {
+
+	if (self_id_count > 252) {
 		fw_notify("inconsistent self IDs\n");
 		return;
 	}
+
 	generation = (cond_le32_to_cpu(ohci->self_id_cpu[0]) >> 16) & 0xff;
 	rmb();
 
@@ -1770,6 +1890,19 @@ static void bus_reset_work(struct work_struct *work)
 		ohci->self_id_buffer[j] =
 				cond_le32_to_cpu(ohci->self_id_cpu[i]);
 	}
+
+	if (ohci->quirks & QUIRK_TI_SLLZ059) {
+		self_id_count = find_and_insert_self_id(ohci, self_id_count);
+		if (self_id_count < 0) {
+			fw_notify("could not construct local self IDs\n");
+			return;
+		}
+	}
+
+	if (self_id_count == 0) {
+		fw_notify("inconsistent self IDs\n");
+		return;
+	}
 	rmb();
 
 	/*
@@ -2050,13 +2183,50 @@ static int configure_1394a_enhancements(struct fw_ohci *ohci)
 	return 0;
 }
 
+#define TSB41BA3D_VID 0x00080028
+#define TSB41BA3D_PID 0x00833005
+
+static int probe_tsb41ba3d(struct fw_ohci *ohci)
+{
+	int reg;
+	int i;
+	int vendor_id;
+	int product_id;
+
+	reg = read_phy_reg(ohci, 2);
+	if (reg < 0)
+		return reg;
+
+	if ((reg & PHY_EXTENDED_REGISTERS) == PHY_EXTENDED_REGISTERS) {
+		vendor_id = 0;
+		for (i = 10; i < 13; i++) {
+			reg = read_paged_phy_reg(ohci, 1, i);
+			if (reg < 0)
+				return reg;
+			vendor_id = (vendor_id << 8) | reg;
+		}
+		product_id = 0;
+		for (i = 13; i < 16; i++) {
+			reg = read_paged_phy_reg(ohci, 1, i);
+			if (reg < 0)
+				return reg;
+			product_id = (product_id << 8) | reg;
+		}
+
+		if ((vendor_id == TSB41BA3D_VID) &&
+			(product_id == TSB41BA3D_PID))
+			return 1;
+	}
+	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, seconds, version, irqs;
-	int i, ret;
+	int i, ret, tsb41ba3d_found;
 
 	if (software_reset(ohci)) {
 		fw_error("Failed to reset ohci card.\n");
@@ -2087,6 +2257,17 @@ static int ohci_enable(struct fw_card *card,
 		return -EIO;
 	}
 
+	if (ohci->quirks & QUIRK_TI_SLLZ059) {
+		tsb41ba3d_found = probe_tsb41ba3d(ohci);
+		if (tsb41ba3d_found < 0)
+			return tsb41ba3d_found;
+		if (!tsb41ba3d_found) {
+			fw_notify("No TSB41BA3D found, "
+				  "resetting QUIRK_TI_SLLZ059\n");
+			ohci->quirks &= ~QUIRK_TI_SLLZ059;
+		}
+	}
+
 	reg_write(ohci, OHCI1394_HCControlClear,
 		  OHCI1394_HCControl_noByteSwapData);