From: Mika Westerberg <mika.westerberg@linux.intel.com>
Date: Wed, 29 Apr 2020 17:00:30 +0300
Subject: thunderbolt: Make tb_next_port_on_path() work with tree topologies
Git-commit: 69eb79f7d294f92696de8010432758dbd3d1ecb3
Patch-mainline: v5.9-rc1
References: jsc#SLE-14130

USB4 makes it possible to have tree topology of devices connected in the
same way than USB3. This was actually possible in Thunderbolt 1, 2 and 3
as well but all the available devices only had two ports which allows
building only daisy-chains of devices.

With USB4 it is possible for example that there is DP IN adapter as part
of eGPU device router and that should be tunneled over the tree topology
to a DP OUT adapter. This updates the tb_next_port_on_path() to support
such topologies.

Signed-off-by: Mika Westerberg <mika.westerberg@linux.intel.com>
Acked-by: Nicolas Saenz Julienne <nsaenzjulienne@suse.de>
---
 drivers/thunderbolt/switch.c | 17 ++++++++++++-----
 1 file changed, 12 insertions(+), 5 deletions(-)

diff --git a/drivers/thunderbolt/switch.c b/drivers/thunderbolt/switch.c
index 95b75a712ade..29db484d2c74 100644
--- a/drivers/thunderbolt/switch.c
+++ b/drivers/thunderbolt/switch.c
@@ -850,6 +850,13 @@ void tb_port_release_out_hopid(struct tb_port *port, int hopid)
 	ida_simple_remove(&port->out_hopids, hopid);
 }
 
+static inline bool tb_switch_is_reachable(const struct tb_switch *parent,
+					  const struct tb_switch *sw)
+{
+	u64 mask = (1ULL << parent->config.depth * 8) - 1;
+	return (tb_route(parent) & mask) == (tb_route(sw) & mask);
+}
+
 /**
  * tb_next_port_on_path() - Return next port for given port on a path
  * @start: Start port of the walk
@@ -879,12 +886,12 @@ struct tb_port *tb_next_port_on_path(struct tb_port *start, struct tb_port *end,
 		return end;
 	}
 
-	if (start->sw->config.depth < end->sw->config.depth) {
+	if (tb_switch_is_reachable(prev->sw, end->sw)) {
+		next = tb_port_at(tb_route(end->sw), prev->sw);
+		/* Walk down the topology if next == prev */
 		if (prev->remote &&
-		    prev->remote->sw->config.depth > prev->sw->config.depth)
+		    (next == prev || next->dual_link_port == prev))
 			next = prev->remote;
-		else
-			next = tb_port_at(tb_route(end->sw), prev->sw);
 	} else {
 		if (tb_is_upstream_port(prev)) {
 			next = prev->remote;
@@ -901,7 +908,7 @@ struct tb_port *tb_next_port_on_path(struct tb_port *start, struct tb_port *end,
 		}
 	}
 
-	return next;
+	return next != prev ? next : NULL;
 }
 
 static int tb_port_get_link_speed(struct tb_port *port)