thunderbolt: Set port configured for both ends of the link

Both ends of the link needs to have this set. Otherwise the link is not
re-established properly after sleep. Now since it is possible to have
mixed USB4 and Thunderbolt 1, 2 and 3 devices we need to split the link
configuration functionality to happen per port so we can pick the
correct implementation.

Signed-off-by: Mika Westerberg <mika.westerberg@linux.intel.com>

4 files changed, 87 insertions, 94 deletions

diff --git a/drivers/thunderbolt/lc.c b/drivers/thunderbolt/lc.c
index b2f62ba0421d..5c209a570360 100644
--- a/drivers/thunderbolt/lc.c
+++ b/drivers/thunderbolt/lc.c
@@ -45,7 +45,7 @@ static int find_port_lc_cap(struct tb_port *port)
 	return sw->cap_lc + start + phys * size;
 }
 
-static int tb_lc_configure_lane(struct tb_port *port, bool configure)
+static int tb_lc_set_port_configured(struct tb_port *port, bool configured)
 {
 	bool upstream = tb_is_upstream_port(port);
 	struct tb_switch *sw = port->sw;
@@ -69,7 +69,7 @@ static int tb_lc_configure_lane(struct tb_port *port, bool configure)
 	else
 		lane = TB_LC_SX_CTRL_L2C;
 
-	if (configure) {
+	if (configured) {
 		ctrl |= lane;
 		if (upstream)
 			ctrl |= TB_LC_SX_CTRL_UPSTREAM;
@@ -83,49 +83,25 @@ static int tb_lc_configure_lane(struct tb_port *port, bool configure)
 }
 
 /**
- * tb_lc_configure_link() - Let LC know about configured link
- * @sw: Switch that is being added
+ * tb_lc_configure_port() - Let LC know about configured port
+ * @port: Port that is set as configured
  *
- * Informs LC of both parent switch and @sw that there is established
- * link between the two.
+ * Sets the port configured for power management purposes.
  */
-int tb_lc_configure_link(struct tb_switch *sw)
+int tb_lc_configure_port(struct tb_port *port)
 {
-	struct tb_port *up, *down;
-	int ret;
-
-	up = tb_upstream_port(sw);
-	down = tb_port_at(tb_route(sw), tb_to_switch(sw->dev.parent));
-
-	/* Configure parent link toward this switch */
-	ret = tb_lc_configure_lane(down, true);
-	if (ret)
-		return ret;
-
-	/* Configure upstream link from this switch to the parent */
-	ret = tb_lc_configure_lane(up, true);
-	if (ret)
-		tb_lc_configure_lane(down, false);
-
-	return ret;
+	return tb_lc_set_port_configured(port, true);
 }
 
 /**
- * tb_lc_unconfigure_link() - Let LC know about unconfigured link
- * @sw: Switch to unconfigure
+ * tb_lc_unconfigure_port() - Let LC know about unconfigured port
+ * @port: Port that is set as configured
  *
- * Informs LC of both parent switch and @sw that the link between the
- * two does not exist anymore.
+ * Sets the port unconfigured for power management purposes.
  */
-void tb_lc_unconfigure_link(struct tb_switch *sw)
+void tb_lc_unconfigure_port(struct tb_port *port)
 {
-	struct tb_port *up, *down;
-
-	up = tb_upstream_port(sw);
-	down = tb_port_at(tb_route(sw), tb_to_switch(sw->dev.parent));
-
-	tb_lc_configure_lane(up, false);
-	tb_lc_configure_lane(down, false);
+	tb_lc_set_port_configured(port, false);
 }
 
 /**
diff --git a/drivers/thunderbolt/switch.c b/drivers/thunderbolt/switch.c
index 5d99d69107eb..50d2af5ba491 100644
--- a/drivers/thunderbolt/switch.c
+++ b/drivers/thunderbolt/switch.c
@@ -2320,12 +2320,24 @@ void tb_switch_lane_bonding_disable(struct tb_switch *sw)
  */
 int tb_switch_configure_link(struct tb_switch *sw)
 {
+	struct tb_port *up, *down;
+	int ret;
+
 	if (!tb_route(sw) || tb_switch_is_icm(sw))
 		return 0;
 
-	if (tb_switch_is_usb4(sw))
-		return usb4_switch_configure_link(sw);
-	return tb_lc_configure_link(sw);
+	up = tb_upstream_port(sw);
+	if (tb_switch_is_usb4(up->sw))
+		ret = usb4_port_configure(up);
+	else
+		ret = tb_lc_configure_port(up);
+	if (ret)
+		return ret;
+
+	down = up->remote;
+	if (tb_switch_is_usb4(down->sw))
+		return usb4_port_configure(down);
+	return tb_lc_configure_port(down);
 }
 
 /**
@@ -2337,15 +2349,24 @@ int tb_switch_configure_link(struct tb_switch *sw)
  */
 void tb_switch_unconfigure_link(struct tb_switch *sw)
 {
+	struct tb_port *up, *down;
+
 	if (sw->is_unplugged)
 		return;
 	if (!tb_route(sw) || tb_switch_is_icm(sw))
 		return;
 
-	if (tb_switch_is_usb4(sw))
-		usb4_switch_unconfigure_link(sw);
+	up = tb_upstream_port(sw);
+	if (tb_switch_is_usb4(up->sw))
+		usb4_port_unconfigure(up);
+	else
+		tb_lc_unconfigure_port(up);
+
+	down = up->remote;
+	if (tb_switch_is_usb4(down->sw))
+		usb4_port_unconfigure(down);
 	else
-		tb_lc_unconfigure_link(sw);
+		tb_lc_unconfigure_port(down);
 }
 
 /**
diff --git a/drivers/thunderbolt/tb.h b/drivers/thunderbolt/tb.h
index 4f7f3b44af00..0a0317e56043 100644
--- a/drivers/thunderbolt/tb.h
+++ b/drivers/thunderbolt/tb.h
@@ -846,8 +846,8 @@ int tb_drom_read(struct tb_switch *sw);
 int tb_drom_read_uid_only(struct tb_switch *sw, u64 *uid);
 
 int tb_lc_read_uuid(struct tb_switch *sw, u32 *uuid);
-int tb_lc_configure_link(struct tb_switch *sw);
-void tb_lc_unconfigure_link(struct tb_switch *sw);
+int tb_lc_configure_port(struct tb_port *port);
+void tb_lc_unconfigure_port(struct tb_port *port);
 int tb_lc_set_sleep(struct tb_switch *sw);
 bool tb_lc_lane_bonding_possible(struct tb_switch *sw);
 bool tb_lc_dp_sink_query(struct tb_switch *sw, struct tb_port *in);
@@ -902,8 +902,6 @@ int usb4_switch_setup(struct tb_switch *sw);
 int usb4_switch_read_uid(struct tb_switch *sw, u64 *uid);
 int usb4_switch_drom_read(struct tb_switch *sw, unsigned int address, void *buf,
 			  size_t size);
-int usb4_switch_configure_link(struct tb_switch *sw);
-void usb4_switch_unconfigure_link(struct tb_switch *sw);
 bool usb4_switch_lane_bonding_possible(struct tb_switch *sw);
 int usb4_switch_set_sleep(struct tb_switch *sw);
 int usb4_switch_nvm_sector_size(struct tb_switch *sw);
@@ -921,6 +919,8 @@ struct tb_port *usb4_switch_map_usb3_down(struct tb_switch *sw,
 					  const struct tb_port *port);
 
 int usb4_port_unlock(struct tb_port *port);
+int usb4_port_configure(struct tb_port *port);
+void usb4_port_unconfigure(struct tb_port *port);
 int usb4_port_enumerate_retimers(struct tb_port *port);
 
 int usb4_port_retimer_read(struct tb_port *port, u8 index, u8 reg, void *buf,
diff --git a/drivers/thunderbolt/usb4.c b/drivers/thunderbolt/usb4.c
index dd601a6db23c..b2677427789f 100644
--- a/drivers/thunderbolt/usb4.c
+++ b/drivers/thunderbolt/usb4.c
@@ -338,54 +338,6 @@ int usb4_switch_drom_read(struct tb_switch *sw, unsigned int address, void *buf,
 				 usb4_switch_drom_read_block, sw);
 }
 
-static int usb4_set_port_configured(struct tb_port *port, bool configured)
-{
-	int ret;
-	u32 val;
-
-	ret = tb_port_read(port, &val, TB_CFG_PORT,
-			   port->cap_usb4 + PORT_CS_19, 1);
-	if (ret)
-		return ret;
-
-	if (configured)
-		val |= PORT_CS_19_PC;
-	else
-		val &= ~PORT_CS_19_PC;
-
-	return tb_port_write(port, &val, TB_CFG_PORT,
-			     port->cap_usb4 + PORT_CS_19, 1);
-}
-
-/**
- * usb4_switch_configure_link() - Set upstream USB4 link configured
- * @sw: USB4 router
- *
- * Sets the upstream USB4 link to be configured for power management
- * purposes.
- */
-int usb4_switch_configure_link(struct tb_switch *sw)
-{
-	struct tb_port *up;
-
-	up = tb_upstream_port(sw);
-	return usb4_set_port_configured(up, true);
-}
-
-/**
- * usb4_switch_unconfigure_link() - Un-set upstream USB4 link configuration
- * @sw: USB4 router
- *
- * Reverse of usb4_switch_configure_link().
- */
-void usb4_switch_unconfigure_link(struct tb_switch *sw)
-{
-	struct tb_port *up;
-
-	up = tb_upstream_port(sw);
-	usb4_set_port_configured(up, false);
-}
-
 /**
  * usb4_switch_lane_bonding_possible() - Are conditions met for lane bonding
  * @sw: USB4 router
@@ -789,6 +741,50 @@ int usb4_port_unlock(struct tb_port *port)
 	return tb_port_write(port, &val, TB_CFG_PORT, ADP_CS_4, 1);
 }
 
+static int usb4_port_set_configured(struct tb_port *port, bool configured)
+{
+	int ret;
+	u32 val;
+
+	if (!port->cap_usb4)
+		return -EINVAL;
+
+	ret = tb_port_read(port, &val, TB_CFG_PORT,
+			   port->cap_usb4 + PORT_CS_19, 1);
+	if (ret)
+		return ret;
+
+	if (configured)
+		val |= PORT_CS_19_PC;
+	else
+		val &= ~PORT_CS_19_PC;
+
+	return tb_port_write(port, &val, TB_CFG_PORT,
+			     port->cap_usb4 + PORT_CS_19, 1);
+}
+
+/**
+ * usb4_port_configure() - Set USB4 port configured
+ * @port: USB4 router
+ *
+ * Sets the USB4 link to be configured for power management purposes.
+ */
+int usb4_port_configure(struct tb_port *port)
+{
+	return usb4_port_set_configured(port, true);
+}
+
+/**
+ * usb4_port_unconfigure() - Set USB4 port unconfigured
+ * @port: USB4 router
+ *
+ * Sets the USB4 link to be unconfigured for power management purposes.
+ */
+void usb4_port_unconfigure(struct tb_port *port)
+{
+	usb4_port_set_configured(port, false);
+}
+
 static int usb4_port_wait_for_bit(struct tb_port *port, u32 offset, u32 bit,
 				  u32 value, int timeout_msec)
 {