[PATCH v2 13/28] thunderbolt: Add helper function to iterate from one port to another

[Mika Westerberg <mika.westerberg@linux.intel.com>]

We need to be able to walk from one port to another when we are creating
paths where there are multiple switches between two ports. For this
reason introduce a new function tb_port_get_next() and a new macro
tb_for_each_port().

Signed-off-by: Mika Westerberg <mika.westerberg@linux.intel.com>
---
 drivers/thunderbolt/switch.c | 60 ++++++++++++++++++++++++++++++++++++
 drivers/thunderbolt/tb.h     |  6 ++++
 2 files changed, 66 insertions(+)

diff --git a/drivers/thunderbolt/switch.c b/drivers/thunderbolt/switch.c
index 320f64ebe8b8..23b6bae8362e 100644
--- a/drivers/thunderbolt/switch.c
+++ b/drivers/thunderbolt/switch.c
@@ -683,6 +683,66 @@ void tb_port_release_out_hopid(struct tb_port *port, int hopid)
 	ida_simple_remove(&port->out_hopids, hopid);
 }
 
+/**
+ * tb_port_get_next() - Return next port for given port
+ * @start: Start port of the walk
+ * @end: End port of the walk
+ * @prev: Previous port (%NULL if this is the first)
+ *
+ * This function can be used to walk from one port to another if they
+ * are connected through zero or more switches. If the @prev is dual
+ * link port, the function follows that link and returns another end on
+ * that same link.
+ *
+ * If the walk cannot be continued, returns %NULL.
+ *
+ * Domain tb->lock must be held when this function is called.
+ */
+struct tb_port *tb_port_get_next(struct tb_port *start, struct tb_port *end,
+				 struct tb_port *prev)
+{
+	struct tb_port *port, *next;
+
+	if (!prev)
+		return start;
+
+	if (prev->sw == end->sw) {
+		if (prev != end)
+			return end;
+		return NULL;
+	}
+
+	/* Switch back to use primary links for walking */
+	if (prev->dual_link_port && prev->link_nr)
+		port = prev->dual_link_port;
+	else
+		port = prev;
+
+	if (start->sw->config.depth < end->sw->config.depth) {
+		if (port->remote &&
+		    port->remote->sw->config.depth > port->sw->config.depth)
+			next = port->remote;
+		else
+			next = tb_port_at(tb_route(end->sw), port->sw);
+	} else if (start->sw->config.depth > end->sw->config.depth) {
+		if (tb_is_upstream_port(port))
+			next = port->remote;
+		else
+			next = tb_upstream_port(port->sw);
+	} else {
+		/* Must be the same switch then */
+		if (start->sw != end->sw)
+			return NULL;
+		return end;
+	}
+
+	/* If prev was dual link return another end of that link then */
+	if (next->dual_link_port && next->link_nr != prev->link_nr)
+		return next->dual_link_port;
+
+	return next;
+}
+
 /**
  * tb_pci_port_enable() - Enable PCIe adapter port
  * @port: PCIe port to enable
diff --git a/drivers/thunderbolt/tb.h b/drivers/thunderbolt/tb.h
index bfa1cee193fd..683725915ff7 100644
--- a/drivers/thunderbolt/tb.h
+++ b/drivers/thunderbolt/tb.h
@@ -447,6 +447,12 @@ int tb_port_alloc_in_hopid(struct tb_port *port, int hopid, int max_hopid);
 void tb_port_release_in_hopid(struct tb_port *port, int hopid);
 int tb_port_alloc_out_hopid(struct tb_port *port, int hopid, int max_hopid);
 void tb_port_release_out_hopid(struct tb_port *port, int hopid);
+struct tb_port *tb_port_get_next(struct tb_port *start, struct tb_port *end,
+				 struct tb_port *prev);
+
+#define tb_for_each_port(port, start, end)			\
+	for (port = tb_port_get_next(start, end, NULL); port;	\
+	     port = tb_port_get_next(start, end, port))
 
 int tb_switch_find_vse_cap(struct tb_switch *sw, enum tb_switch_vse_cap vsec);
 int tb_port_find_cap(struct tb_port *port, enum tb_port_cap cap);
-- 
2.20.1