thunderbolt: Introduce tb_switch_next_cap()

This is similar to tb_port_next_cap() but instead allows walking
capability list of a switch (router). Convert tb_switch_find_cap() and
tb_switch_find_vse_cap() to use this as well.

Signed-off-by: Mika Westerberg <mika.westerberg@linux.intel.com>
Reviewed-by: Greg Kroah-Hartman <gregkh@linuxfoundation.org>

diff --git a/drivers/thunderbolt/cap.c b/drivers/thunderbolt/cap.c
index c45b3a4..6f571e9 100644 (file)
--- a/drivers/thunderbolt/cap.c
+++ b/drivers/thunderbolt/cap.c
@@ -133,6 +133,50 @@ int tb_port_find_cap(struct tb_port *port, enum tb_port_cap cap)
 }
 
 /**
+ * tb_switch_next_cap() - Return next capability in the linked list
+ * @sw: Switch to find the capability for
+ * @offset: Previous capability offset (%0 for start)
+ *
+ * Finds dword offset of the next capability in router config space
+ * capability list and returns it. Passing %0 returns the first entry in
+ * the capability list. If no next capability is found returns %0. In case
+ * of failure returns negative errno.
+ */
+int tb_switch_next_cap(struct tb_switch *sw, unsigned int offset)
+{
+       struct tb_cap_any header;
+       int ret;
+
+       if (!offset)
+               return sw->config.first_cap_offset;
+
+       ret = tb_sw_read(sw, &header, TB_CFG_SWITCH, offset, 2);
+       if (ret)
+               return ret;
+
+       switch (header.basic.cap) {
+       case TB_SWITCH_CAP_TMU:
+               ret = header.basic.next;
+               break;
+
+       case TB_SWITCH_CAP_VSE:
+               if (!header.extended_short.length)
+                       ret = header.extended_long.next;
+               else
+                       ret = header.extended_short.next;
+               break;
+
+       default:
+               tb_sw_dbg(sw, "unknown capability %#x at %#x\n",
+                         header.basic.cap, offset);
+               ret = -EINVAL;
+               break;
+       }
+
+       return ret >= VSE_CAP_OFFSET_MAX ? 0 : ret;
+}
+
+/**
  * tb_switch_find_cap() - Find switch capability
  * @sw Switch to find the capability for
  * @cap: Capability to look
@@ -143,21 +187,23 @@ int tb_port_find_cap(struct tb_port *port, enum tb_port_cap cap)
  */
 int tb_switch_find_cap(struct tb_switch *sw, enum tb_switch_cap cap)
 {
-       int offset = sw->config.first_cap_offset;
+       int offset = 0;
 
-       while (offset > 0 && offset < CAP_OFFSET_MAX) {
+       do {
                struct tb_cap_any header;
                int ret;
 
+               offset = tb_switch_next_cap(sw, offset);
+               if (offset < 0)
+                       return offset;
+
                ret = tb_sw_read(sw, &header, TB_CFG_SWITCH, offset, 1);
                if (ret)
                        return ret;
 
                if (header.basic.cap == cap)
                        return offset;
-
-               offset = header.basic.next;
-       }
+       } while (offset);
 
        return -ENOENT;
 }
@@ -174,37 +220,24 @@ int tb_switch_find_cap(struct tb_switch *sw, enum tb_switch_cap cap)
  */
 int tb_switch_find_vse_cap(struct tb_switch *sw, enum tb_switch_vse_cap vsec)
 {
-       struct tb_cap_any header;
-       int offset;
-
-       offset = tb_switch_find_cap(sw, TB_SWITCH_CAP_VSE);
-       if (offset < 0)
-               return offset;
+       int offset = 0;
 
-       while (offset > 0 && offset < VSE_CAP_OFFSET_MAX) {
+       do {
+               struct tb_cap_any header;
                int ret;
 
-               ret = tb_sw_read(sw, &header, TB_CFG_SWITCH, offset, 2);
+               offset = tb_switch_next_cap(sw, offset);
+               if (offset < 0)
+                       return offset;
+
+               ret = tb_sw_read(sw, &header, TB_CFG_SWITCH, offset, 1);
                if (ret)
                        return ret;
 
-               /*
-                * Extended vendor specific capabilities come in two
-                * flavors: short and long. The latter is used when
-                * offset is over 0xff.
-                */
-               if (offset >= CAP_OFFSET_MAX) {
-                       if (header.extended_long.vsec_id == vsec)
-                               return offset;
-                       offset = header.extended_long.next;
-               } else {
-                       if (header.extended_short.vsec_id == vsec)
-                               return offset;
-                       if (!header.extended_short.length)
-                               return -ENOENT;
-                       offset = header.extended_short.next;
-               }
-       }
+               if (header.extended_short.cap == TB_SWITCH_CAP_VSE &&
+                   header.extended_short.vsec_id == vsec)
+                       return offset;
+       } while (offset);
 
        return -ENOENT;
 }

diff --git a/drivers/thunderbolt/tb.h b/drivers/thunderbolt/tb.h
index 54e8fad..a1d5de5 100644 (file)
--- a/drivers/thunderbolt/tb.h
+++ b/drivers/thunderbolt/tb.h
@@ -822,6 +822,7 @@ int tb_port_get_link_speed(struct tb_port *port);
 
 int tb_switch_find_vse_cap(struct tb_switch *sw, enum tb_switch_vse_cap vsec);
 int tb_switch_find_cap(struct tb_switch *sw, enum tb_switch_cap cap);
+int tb_switch_next_cap(struct tb_switch *sw, unsigned int offset);
 int tb_port_find_cap(struct tb_port *port, enum tb_port_cap cap);
 int tb_port_next_cap(struct tb_port *port, unsigned int offset);
 bool tb_port_is_enabled(struct tb_port *port);