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);