[PATCH 03/12] thunderbolt: Make XDomain lane bonding comply with the USB4 v2 spec

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

The USB4 v2 Inter-Domain spec "unified" the lane bonding flow so that
when the other end (with higher UUID) is not yet set the target link
width accordingly it is expected to reply with ERROR_NOT_READY.
Implement this for Linux.

Signed-off-by: Mika Westerberg <mika.westerberg@linux.intel.com>
---
 drivers/thunderbolt/xdomain.c | 65 +++++++++++++++++++++++++----------
 1 file changed, 47 insertions(+), 18 deletions(-)

diff --git a/drivers/thunderbolt/xdomain.c b/drivers/thunderbolt/xdomain.c
index 57367e18733a..680b2204875a 100644
--- a/drivers/thunderbolt/xdomain.c
+++ b/drivers/thunderbolt/xdomain.c
@@ -535,29 +535,19 @@ static int tb_xdp_link_state_status_request(struct tb_ctl *ctl, u64 route,
 }
 
 static int tb_xdp_link_state_status_response(struct tb *tb, struct tb_ctl *ctl,
-					     struct tb_xdomain *xd, u8 sequence)
+					     struct tb_xdomain *xd, u8 sequence,
+					     u8 slw, u8 sls, u8 tls, u8 tlw)
 {
 	struct tb_xdp_link_state_status_response res;
-	struct tb_port *port = tb_xdomain_downstream_port(xd);
-	u32 val[2];
-	int ret;
 
 	memset(&res, 0, sizeof(res));
 	tb_xdp_fill_header(&res.hdr, xd->route, sequence,
 			   LINK_STATE_STATUS_RESPONSE, sizeof(res));
 
-	ret = tb_port_read(port, val, TB_CFG_PORT,
-			   port->cap_phy + LANE_ADP_CS_0, ARRAY_SIZE(val));
-	if (ret)
-		return ret;
-
-	res.slw = (val[0] & LANE_ADP_CS_0_SUPPORTED_WIDTH_MASK) >>
-			LANE_ADP_CS_0_SUPPORTED_WIDTH_SHIFT;
-	res.sls = (val[0] & LANE_ADP_CS_0_SUPPORTED_SPEED_MASK) >>
-			LANE_ADP_CS_0_SUPPORTED_SPEED_SHIFT;
-	res.tls = val[1] & LANE_ADP_CS_1_TARGET_SPEED_MASK;
-	res.tlw = (val[1] & LANE_ADP_CS_1_TARGET_WIDTH_MASK) >>
-			LANE_ADP_CS_1_TARGET_WIDTH_SHIFT;
+	res.slw = slw;
+	res.sls = sls;
+	res.tls = tls;
+	res.tlw = tlw;
 
 	return __tb_xdomain_response(ctl, &res, sizeof(res),
 				     TB_CFG_PKG_XDOMAIN_RESP);
@@ -804,8 +794,47 @@ static void tb_xdp_handle_request(struct work_struct *work)
 		       route);
 
 		if (xd) {
-			ret = tb_xdp_link_state_status_response(tb, ctl, xd,
-								sequence);
+			struct tb_port *port = tb_xdomain_downstream_port(xd);
+			u8 slw, sls, tls, tlw;
+			u32 val[2];
+
+			/*
+			 * Read the adapter supported and target widths
+			 * and speeds.
+			 */
+			ret = tb_port_read(port, val, TB_CFG_PORT,
+					   port->cap_phy + LANE_ADP_CS_0,
+					   ARRAY_SIZE(val));
+			if (ret)
+				break;
+
+			slw = (val[0] & LANE_ADP_CS_0_SUPPORTED_WIDTH_MASK) >>
+				LANE_ADP_CS_0_SUPPORTED_WIDTH_SHIFT;
+			sls = (val[0] & LANE_ADP_CS_0_SUPPORTED_SPEED_MASK) >>
+				LANE_ADP_CS_0_SUPPORTED_SPEED_SHIFT;
+			tls = val[1] & LANE_ADP_CS_1_TARGET_SPEED_MASK;
+			tlw = (val[1] & LANE_ADP_CS_1_TARGET_WIDTH_MASK) >>
+				LANE_ADP_CS_1_TARGET_WIDTH_SHIFT;
+
+			/*
+			 * When we have higher UUID, we are supposed to
+			 * return ERROR_NOT_READY if the tlw is not yet
+			 * set according to the Inter-Domain spec for
+			 * USB4 v2.
+			 */
+			if (xd->state == XDOMAIN_STATE_BONDING_UUID_HIGH &&
+			    xd->target_link_width &&
+			    xd->target_link_width != tlw) {
+				tb_dbg(tb, "%llx: target link width not yet set %#x != %#x\n",
+				       route, tlw, xd->target_link_width);
+				tb_xdp_error_response(ctl, route, sequence,
+						      ERROR_NOT_READY);
+			} else {
+				tb_dbg(tb, "%llx: replying with target link width set to %#x\n",
+				       route, tlw);
+				ret = tb_xdp_link_state_status_response(tb, ctl,
+					xd, sequence, slw, sls, tls, tlw);
+			}
 		} else {
 			tb_xdp_error_response(ctl, route, sequence,
 					      ERROR_NOT_READY);
-- 
2.50.1