4 files changed, 63 insertions, 1 deletions

diff --git a/drivers/thunderbolt/lc.c b/drivers/thunderbolt/lc.c
index 41e6c738f6c8..bc671730a11f 100644
--- a/drivers/thunderbolt/lc.c
+++ b/drivers/thunderbolt/lc.c
@@ -158,6 +158,41 @@ void tb_lc_unconfigure_xdomain(struct tb_port *port)
 	tb_lc_set_xdomain_configured(port, false);
 }
 
+/**
+ * tb_lc_start_lane_initialization() - Start lane initialization
+ * @port: Device router lane 0 adapter
+ *
+ * Starts lane initialization for @port after the router resumed from
+ * sleep. Should be called for those downstream lane adapters that were
+ * not connected (tb_lc_configure_port() was not called) before sleep.
+ *
+ * Returns %0 in success and negative errno in case of failure.
+ */
+int tb_lc_start_lane_initialization(struct tb_port *port)
+{
+	struct tb_switch *sw = port->sw;
+	int ret, cap;
+	u32 ctrl;
+
+	if (!tb_route(sw))
+		return 0;
+
+	if (sw->generation < 2)
+		return 0;
+
+	cap = find_port_lc_cap(port);
+	if (cap < 0)
+		return cap;
+
+	ret = tb_sw_read(sw, &ctrl, TB_CFG_SWITCH, cap + TB_LC_SX_CTRL, 1);
+	if (ret)
+		return ret;
+
+	ctrl |= TB_LC_SX_CTRL_SLI;
+
+	return tb_sw_write(sw, &ctrl, TB_CFG_SWITCH, cap + TB_LC_SX_CTRL, 1);
+}
+
 static int tb_lc_set_wake_one(struct tb_switch *sw, unsigned int offset,
 			      unsigned int flags)
 {
diff --git a/drivers/thunderbolt/switch.c b/drivers/thunderbolt/switch.c
index 602e1835bf61..ad992e6204d9 100644
--- a/drivers/thunderbolt/switch.c
+++ b/drivers/thunderbolt/switch.c
@@ -1065,6 +1065,17 @@ void tb_port_lane_bonding_disable(struct tb_port *port)
 	tb_port_set_link_width(port, 1);
 }
 
+static int tb_port_start_lane_initialization(struct tb_port *port)
+{
+	int ret;
+
+	if (tb_switch_is_usb4(port->sw))
+		return 0;
+
+	ret = tb_lc_start_lane_initialization(port);
+	return ret == -EINVAL ? 0 : ret;
+}
+
 /**
  * tb_port_is_enabled() - Is the adapter port enabled
  * @port: Port to check
@@ -2694,8 +2705,22 @@ int tb_switch_resume(struct tb_switch *sw)
 
 	/* check for surviving downstream switches */
 	tb_switch_for_each_port(sw, port) {
-		if (!tb_port_has_remote(port) && !port->xdomain)
+		if (!tb_port_has_remote(port) && !port->xdomain) {
+			/*
+			 * For disconnected downstream lane adapters
+			 * start lane initialization now so we detect
+			 * future connects.
+			 */
+			if (!tb_is_upstream_port(port) && tb_port_is_null(port))
+				tb_port_start_lane_initialization(port);
 			continue;
+		} else if (port->xdomain) {
+			/*
+			 * Start lane initialization for XDomain so the
+			 * link gets re-established.
+			 */
+			tb_port_start_lane_initialization(port);
+		}
 
 		if (tb_wait_for_port(port, true) <= 0) {
 			tb_port_warn(port,
diff --git a/drivers/thunderbolt/tb.h b/drivers/thunderbolt/tb.h
index 554feda1e359..34ae83b9e52a 100644
--- a/drivers/thunderbolt/tb.h
+++ b/drivers/thunderbolt/tb.h
@@ -923,6 +923,7 @@ int tb_lc_configure_port(struct tb_port *port);
 void tb_lc_unconfigure_port(struct tb_port *port);
 int tb_lc_configure_xdomain(struct tb_port *port);
 void tb_lc_unconfigure_xdomain(struct tb_port *port);
+int tb_lc_start_lane_initialization(struct tb_port *port);
 int tb_lc_set_wake(struct tb_switch *sw, unsigned int flags);
 int tb_lc_set_sleep(struct tb_switch *sw);
 bool tb_lc_lane_bonding_possible(struct tb_switch *sw);
diff --git a/drivers/thunderbolt/tb_regs.h b/drivers/thunderbolt/tb_regs.h
index ae427a953489..626751e06292 100644
--- a/drivers/thunderbolt/tb_regs.h
+++ b/drivers/thunderbolt/tb_regs.h
@@ -464,6 +464,7 @@ struct tb_regs_hop {
 #define TB_LC_SX_CTRL_L1D		BIT(17)
 #define TB_LC_SX_CTRL_L2C		BIT(20)
 #define TB_LC_SX_CTRL_L2D		BIT(21)
+#define TB_LC_SX_CTRL_SLI		BIT(29)
 #define TB_LC_SX_CTRL_UPSTREAM		BIT(30)
 #define TB_LC_SX_CTRL_SLP		BIT(31)