[PATCH V2 2/8] phy: tegra: xusb: t210: add usb3 port fake support

[Nagarjuna Kristam <nkristam@xxxxxxxxxx>]

On Tegra210, usb2 only otg/peripheral ports dont work in device mode.
They need an assosciated usb3 port to work in device mode. Identify
an unused usb3 port and assign it as a fake USB3 port to USB2 only
port whose mode is otg/peripheral

Based on work by BH Hsieh <bhsieh@xxxxxxxxxx>.

Signed-off-by: Nagarjuna Kristam <nkristam@xxxxxxxxxx>
---
 drivers/phy/tegra/xusb-tegra210.c | 40 +++++++++++++++++++++++++++++++++++++++
 drivers/phy/tegra/xusb.c          | 29 ++++++++++++++++++++++++++++
 drivers/phy/tegra/xusb.h          |  6 ++++++
 3 files changed, 75 insertions(+)

diff --git a/drivers/phy/tegra/xusb-tegra210.c b/drivers/phy/tegra/xusb-tegra210.c
index 4beebcc..48478bc4 100644
--- a/drivers/phy/tegra/xusb-tegra210.c
+++ b/drivers/phy/tegra/xusb-tegra210.c
@@ -58,6 +58,7 @@
 #define XUSB_PADCTL_SS_PORT_MAP_PORTX_MAP_SHIFT(x) ((x) * 5)
 #define XUSB_PADCTL_SS_PORT_MAP_PORTX_MAP_MASK(x) (0x7 << ((x) * 5))
 #define XUSB_PADCTL_SS_PORT_MAP_PORTX_MAP(x, v) (((v) & 0x7) << ((x) * 5))
+#define XUSB_PADCTL_SS_PORT_MAP_PORT_DISABLED 0x7
 
 #define XUSB_PADCTL_ELPG_PROGRAM1 0x024
 #define XUSB_PADCTL_ELPG_PROGRAM1_AUX_MUX_LP0_VCORE_DOWN (1 << 31)
@@ -952,6 +953,15 @@ static int tegra210_usb2_phy_power_on(struct phy *phy)
 
 	priv = to_tegra210_xusb_padctl(padctl);
 
+	if (port->usb3_port_fake != -1) {
+		value = padctl_readl(padctl, XUSB_PADCTL_SS_PORT_MAP);
+		value &= ~XUSB_PADCTL_SS_PORT_MAP_PORTX_MAP_MASK(
+					port->usb3_port_fake);
+		value |= XUSB_PADCTL_SS_PORT_MAP_PORTX_MAP(
+					port->usb3_port_fake, index);
+		padctl_writel(padctl, value, XUSB_PADCTL_SS_PORT_MAP);
+	}
+
 	value = padctl_readl(padctl, XUSB_PADCTL_USB2_BIAS_PAD_CTL0);
 	value &= ~((XUSB_PADCTL_USB2_BIAS_PAD_CTL0_HS_SQUELCH_LEVEL_MASK <<
 		    XUSB_PADCTL_USB2_BIAS_PAD_CTL0_HS_SQUELCH_LEVEL_SHIFT) |
@@ -1086,6 +1096,15 @@ static int tegra210_usb2_phy_power_off(struct phy *phy)
 
 	mutex_lock(&padctl->lock);
 
+	if (port->usb3_port_fake != -1) {
+		value = padctl_readl(padctl, XUSB_PADCTL_SS_PORT_MAP);
+		value &= ~XUSB_PADCTL_SS_PORT_MAP_PORTX_MAP_MASK(
+					port->usb3_port_fake);
+		value |= XUSB_PADCTL_SS_PORT_MAP_PORTX_MAP(port->usb3_port_fake,
+					XUSB_PADCTL_SS_PORT_MAP_PORT_DISABLED);
+		padctl_writel(padctl, value, XUSB_PADCTL_SS_PORT_MAP);
+	}
+
 	if (WARN_ON(pad->enable == 0))
 		goto out;
 
@@ -1784,6 +1803,27 @@ static const struct tegra_xusb_pad_soc * const tegra210_pads[] = {
 
 static int tegra210_usb2_port_enable(struct tegra_xusb_port *port)
 {
+	struct tegra_xusb_usb2_port *usb2 = to_usb2_port(port);
+	struct tegra_xusb_padctl *padctl = port->padctl;
+
+	/* Disable usb3_port_fake usage by default and assign if needed */
+	usb2->usb3_port_fake = -1;
+	if (usb2 && (usb2->mode == USB_DR_MODE_OTG ||
+		    usb2->mode == USB_DR_MODE_PERIPHERAL)) {
+		if (!tegra_xusb_usb3_port_has_companion(padctl, port->index)) {
+			int fake = tegra_xusb_find_unused_usb3_port(padctl);
+
+			if (fake >= 0) {
+				dev_dbg(&port->dev, "Found unused usb3 port: %d\n",
+					 fake);
+				usb2->usb3_port_fake = fake;
+			} else {
+				dev_err(&port->dev, "usb2-%u has neither a companion nor an unused usb3 port to fake it\n",
+					port->index);
+				return -ENODEV;
+			}
+		}
+	}
 	return 0;
 }
 
diff --git a/drivers/phy/tegra/xusb.c b/drivers/phy/tegra/xusb.c
index c6178a0..e89746d 100644
--- a/drivers/phy/tegra/xusb.c
+++ b/drivers/phy/tegra/xusb.c
@@ -799,6 +799,35 @@ static void __tegra_xusb_remove_ports(struct tegra_xusb_padctl *padctl)
 	}
 }
 
+bool tegra_xusb_usb3_port_has_companion(struct tegra_xusb_padctl *padctl,
+						       unsigned int index)
+{
+	unsigned int i;
+	struct tegra_xusb_usb3_port *usb3;
+
+	for (i = 0; i < padctl->soc->ports.usb3.count; i++) {
+		usb3 = tegra_xusb_find_usb3_port(padctl, i);
+		if (usb3 && usb3->port == index)
+			return true;
+	}
+
+	return false;
+}
+
+int tegra_xusb_find_unused_usb3_port(struct tegra_xusb_padctl *padctl)
+{
+	struct device_node *np;
+	unsigned int i;
+
+	for (i = 0; i < padctl->soc->ports.usb3.count; i++) {
+		np = tegra_xusb_find_port_node(padctl, "usb3", i);
+		if (!np || !of_device_is_available(np))
+			return i;
+	}
+
+	return -ENODEV;
+}
+
 static int tegra_xusb_setup_ports(struct tegra_xusb_padctl *padctl)
 {
 	struct tegra_xusb_port *port;
diff --git a/drivers/phy/tegra/xusb.h b/drivers/phy/tegra/xusb.h
index 17cc8dc..b263165 100644
--- a/drivers/phy/tegra/xusb.h
+++ b/drivers/phy/tegra/xusb.h
@@ -274,6 +274,7 @@ struct tegra_xusb_usb2_port {
 	struct regulator *supply;
 	bool internal;
 	enum usb_dr_mode mode;
+	int usb3_port_fake;
 };
 
 static inline struct tegra_xusb_usb2_port *
@@ -413,6 +414,11 @@ struct tegra_xusb_lane *tegra_xusb_find_lane(struct tegra_xusb_padctl *padctl,
 					     const char *name,
 					     unsigned int index);
 
+bool tegra_xusb_usb3_port_has_companion(struct tegra_xusb_padctl *padctl,
+					unsigned int index);
+
+int tegra_xusb_find_unused_usb3_port(struct tegra_xusb_padctl *padctl);
+
 #if defined(CONFIG_ARCH_TEGRA_124_SOC) || defined(CONFIG_ARCH_TEGRA_132_SOC)
 extern const struct tegra_xusb_padctl_soc tegra124_xusb_padctl_soc;
 #endif
-- 
2.7.4