1 files changed, 602 insertions, 150 deletions

diff --git a/drivers/thunderbolt/tmu.c b/drivers/thunderbolt/tmu.c
index e4a07a26f693..cf779874c675 100644
--- a/drivers/thunderbolt/tmu.c
+++ b/drivers/thunderbolt/tmu.c
@@ -11,33 +11,115 @@
 
 #include "tb.h"
 
-static const char *tb_switch_tmu_mode_name(const struct tb_switch *sw)
+static const unsigned int tmu_rates[] = {
+	[TB_SWITCH_TMU_MODE_OFF] = 0,
+	[TB_SWITCH_TMU_MODE_LOWRES] = 1000,
+	[TB_SWITCH_TMU_MODE_HIFI_UNI] = 16,
+	[TB_SWITCH_TMU_MODE_HIFI_BI] = 16,
+	[TB_SWITCH_TMU_MODE_MEDRES_ENHANCED_UNI] = 16,
+};
+
+static const struct {
+	unsigned int freq_meas_window;
+	unsigned int avg_const;
+	unsigned int delta_avg_const;
+	unsigned int repl_timeout;
+	unsigned int repl_threshold;
+	unsigned int repl_n;
+	unsigned int dirswitch_n;
+} tmu_params[] = {
+	[TB_SWITCH_TMU_MODE_OFF] = { },
+	[TB_SWITCH_TMU_MODE_LOWRES] = { 30, 4, },
+	[TB_SWITCH_TMU_MODE_HIFI_UNI] = { 800, 8, },
+	[TB_SWITCH_TMU_MODE_HIFI_BI] = { 800, 8, },
+	[TB_SWITCH_TMU_MODE_MEDRES_ENHANCED_UNI] = {
+		800, 4, 0, 3125, 25, 128, 255,
+	},
+};
+
+static const char *tmu_mode_name(enum tb_switch_tmu_mode mode)
 {
-	bool root_switch = !tb_route(sw);
-
-	switch (sw->tmu.rate) {
-	case TB_SWITCH_TMU_RATE_OFF:
+	switch (mode) {
+	case TB_SWITCH_TMU_MODE_OFF:
 		return "off";
-
-	case TB_SWITCH_TMU_RATE_HIFI:
-		/* Root switch does not have upstream directionality */
-		if (root_switch)
-			return "HiFi";
-		if (sw->tmu.unidirectional)
-			return "uni-directional, HiFi";
+	case TB_SWITCH_TMU_MODE_LOWRES:
+		return "uni-directional, LowRes";
+	case TB_SWITCH_TMU_MODE_HIFI_UNI:
+		return "uni-directional, HiFi";
+	case TB_SWITCH_TMU_MODE_HIFI_BI:
 		return "bi-directional, HiFi";
-
-	case TB_SWITCH_TMU_RATE_NORMAL:
-		if (root_switch)
-			return "normal";
-		return "uni-directional, normal";
-
+	case TB_SWITCH_TMU_MODE_MEDRES_ENHANCED_UNI:
+		return "enhanced uni-directional, MedRes";
 	default:
 		return "unknown";
 	}
 }
 
-static bool tb_switch_tmu_ucap_supported(struct tb_switch *sw)
+static bool tb_switch_tmu_enhanced_is_supported(const struct tb_switch *sw)
+{
+	return usb4_switch_version(sw) > 1;
+}
+
+static int tb_switch_set_tmu_mode_params(struct tb_switch *sw,
+					 enum tb_switch_tmu_mode mode)
+{
+	u32 freq, avg, val;
+	int ret;
+
+	freq = tmu_params[mode].freq_meas_window;
+	avg = tmu_params[mode].avg_const;
+
+	ret = tb_sw_read(sw, &val, TB_CFG_SWITCH,
+			 sw->tmu.cap + TMU_RTR_CS_0, 1);
+	if (ret)
+		return ret;
+
+	val &= ~TMU_RTR_CS_0_FREQ_WIND_MASK;
+	val |= FIELD_PREP(TMU_RTR_CS_0_FREQ_WIND_MASK, freq);
+
+	ret = tb_sw_write(sw, &val, TB_CFG_SWITCH,
+			  sw->tmu.cap + TMU_RTR_CS_0, 1);
+	if (ret)
+		return ret;
+
+	ret = tb_sw_read(sw, &val, TB_CFG_SWITCH,
+			 sw->tmu.cap + TMU_RTR_CS_15, 1);
+	if (ret)
+		return ret;
+
+	val &= ~TMU_RTR_CS_15_FREQ_AVG_MASK &
+		~TMU_RTR_CS_15_DELAY_AVG_MASK &
+		~TMU_RTR_CS_15_OFFSET_AVG_MASK &
+		~TMU_RTR_CS_15_ERROR_AVG_MASK;
+	val |=  FIELD_PREP(TMU_RTR_CS_15_FREQ_AVG_MASK, avg) |
+		FIELD_PREP(TMU_RTR_CS_15_DELAY_AVG_MASK, avg) |
+		FIELD_PREP(TMU_RTR_CS_15_OFFSET_AVG_MASK, avg) |
+		FIELD_PREP(TMU_RTR_CS_15_ERROR_AVG_MASK, avg);
+
+	ret = tb_sw_write(sw, &val, TB_CFG_SWITCH,
+			 sw->tmu.cap + TMU_RTR_CS_15, 1);
+	if (ret)
+		return ret;
+
+	if (tb_switch_tmu_enhanced_is_supported(sw)) {
+		u32 delta_avg = tmu_params[mode].delta_avg_const;
+
+		ret = tb_sw_read(sw, &val, TB_CFG_SWITCH,
+				 sw->tmu.cap + TMU_RTR_CS_18, 1);
+		if (ret)
+			return ret;
+
+		val &= ~TMU_RTR_CS_18_DELTA_AVG_CONST_MASK;
+		val |= FIELD_PREP(TMU_RTR_CS_18_DELTA_AVG_CONST_MASK, delta_avg);
+
+		ret = tb_sw_write(sw, &val, TB_CFG_SWITCH,
+				  sw->tmu.cap + TMU_RTR_CS_18, 1);
+	}
+
+	return ret;
+}
+
+static bool tb_switch_tmu_ucap_is_supported(struct tb_switch *sw)
 {
 	int ret;
 	u32 val;
@@ -133,6 +215,103 @@ static bool tb_port_tmu_is_unidirectional(struct tb_port *port)
 	return val & TMU_ADP_CS_3_UDM;
 }
 
+static bool tb_port_tmu_is_enhanced(struct tb_port *port)
+{
+	int ret;
+	u32 val;
+
+	ret = tb_port_read(port, &val, TB_CFG_PORT,
+			   port->cap_tmu + TMU_ADP_CS_8, 1);
+	if (ret)
+		return false;
+
+	return val & TMU_ADP_CS_8_EUDM;
+}
+
+/* Can be called to non-v2 lane adapters too */
+static int tb_port_tmu_enhanced_enable(struct tb_port *port, bool enable)
+{
+	int ret;
+	u32 val;
+
+	if (!tb_switch_tmu_enhanced_is_supported(port->sw))
+		return 0;
+
+	ret = tb_port_read(port, &val, TB_CFG_PORT,
+			   port->cap_tmu + TMU_ADP_CS_8, 1);
+	if (ret)
+		return ret;
+
+	if (enable)
+		val |= TMU_ADP_CS_8_EUDM;
+	else
+		val &= ~TMU_ADP_CS_8_EUDM;
+
+	return tb_port_write(port, &val, TB_CFG_PORT,
+			     port->cap_tmu + TMU_ADP_CS_8, 1);
+}
+
+static int tb_port_set_tmu_mode_params(struct tb_port *port,
+				       enum tb_switch_tmu_mode mode)
+{
+	u32 repl_timeout, repl_threshold, repl_n, dirswitch_n, val;
+	int ret;
+
+	repl_timeout = tmu_params[mode].repl_timeout;
+	repl_threshold = tmu_params[mode].repl_threshold;
+	repl_n = tmu_params[mode].repl_n;
+	dirswitch_n = tmu_params[mode].dirswitch_n;
+
+	ret = tb_port_read(port, &val, TB_CFG_PORT,
+			   port->cap_tmu + TMU_ADP_CS_8, 1);
+	if (ret)
+		return ret;
+
+	val &= ~TMU_ADP_CS_8_REPL_TIMEOUT_MASK;
+	val &= ~TMU_ADP_CS_8_REPL_THRESHOLD_MASK;
+	val |= FIELD_PREP(TMU_ADP_CS_8_REPL_TIMEOUT_MASK, repl_timeout);
+	val |= FIELD_PREP(TMU_ADP_CS_8_REPL_THRESHOLD_MASK, repl_threshold);
+
+	ret = tb_port_write(port, &val, TB_CFG_PORT,
+			    port->cap_tmu + TMU_ADP_CS_8, 1);
+	if (ret)
+		return ret;
+
+	ret = tb_port_read(port, &val, TB_CFG_PORT,
+			   port->cap_tmu + TMU_ADP_CS_9, 1);
+	if (ret)
+		return ret;
+
+	val &= ~TMU_ADP_CS_9_REPL_N_MASK;
+	val &= ~TMU_ADP_CS_9_DIRSWITCH_N_MASK;
+	val |= FIELD_PREP(TMU_ADP_CS_9_REPL_N_MASK, repl_n);
+	val |= FIELD_PREP(TMU_ADP_CS_9_DIRSWITCH_N_MASK, dirswitch_n);
+
+	return tb_port_write(port, &val, TB_CFG_PORT,
+			     port->cap_tmu + TMU_ADP_CS_9, 1);
+}
+
+/* Can be called to non-v2 lane adapters too */
+static int tb_port_tmu_rate_write(struct tb_port *port, int rate)
+{
+	int ret;
+	u32 val;
+
+	if (!tb_switch_tmu_enhanced_is_supported(port->sw))
+		return 0;
+
+	ret = tb_port_read(port, &val, TB_CFG_PORT,
+			   port->cap_tmu + TMU_ADP_CS_9, 1);
+	if (ret)
+		return ret;
+
+	val &= ~TMU_ADP_CS_9_ADP_TS_INTERVAL_MASK;
+	val |= FIELD_PREP(TMU_ADP_CS_9_ADP_TS_INTERVAL_MASK, rate);
+
+	return tb_port_write(port, &val, TB_CFG_PORT,
+			     port->cap_tmu + TMU_ADP_CS_9, 1);
+}
+
 static int tb_port_tmu_time_sync(struct tb_port *port, bool time_sync)
 {
 	u32 val = time_sync ? TMU_ADP_CS_6_DTS : 0;
@@ -175,13 +354,59 @@ static int tb_switch_tmu_set_time_disruption(struct tb_switch *sw, bool set)
 	return tb_sw_write(sw, &val, TB_CFG_SWITCH, offset, 1);
 }
 
+static int tmu_mode_init(struct tb_switch *sw)
+{
+	bool enhanced, ucap;
+	int ret, rate;
+
+	ucap = tb_switch_tmu_ucap_is_supported(sw);
+	if (ucap)
+		tb_sw_dbg(sw, "TMU: supports uni-directional mode\n");
+	enhanced = tb_switch_tmu_enhanced_is_supported(sw);
+	if (enhanced)
+		tb_sw_dbg(sw, "TMU: supports enhanced uni-directional mode\n");
+
+	ret = tb_switch_tmu_rate_read(sw);
+	if (ret < 0)
+		return ret;
+	rate = ret;
+
+	/* Off by default */
+	sw->tmu.mode = TB_SWITCH_TMU_MODE_OFF;
+
+	if (tb_route(sw)) {
+		struct tb_port *up = tb_upstream_port(sw);
+
+		if (enhanced && tb_port_tmu_is_enhanced(up)) {
+			sw->tmu.mode = TB_SWITCH_TMU_MODE_MEDRES_ENHANCED_UNI;
+		} else if (ucap && tb_port_tmu_is_unidirectional(up)) {
+			if (tmu_rates[TB_SWITCH_TMU_MODE_LOWRES] == rate)
+				sw->tmu.mode = TB_SWITCH_TMU_MODE_LOWRES;
+			else if (tmu_rates[TB_SWITCH_TMU_MODE_HIFI_UNI] == rate)
+				sw->tmu.mode = TB_SWITCH_TMU_MODE_HIFI_UNI;
+		} else if (rate) {
+			sw->tmu.mode = TB_SWITCH_TMU_MODE_HIFI_BI;
+		}
+	} else if (rate) {
+		sw->tmu.mode = TB_SWITCH_TMU_MODE_HIFI_BI;
+	}
+
+	/* Update the initial request to match the current mode */
+	sw->tmu.mode_request = sw->tmu.mode;
+	sw->tmu.has_ucap = ucap;
+
+	return 0;
+}
+
 /**
  * tb_switch_tmu_init() - Initialize switch TMU structures
- * @sw: Switch to initialized
+ * @sw: Switch to be initialized
  *
  * This function must be called before other TMU related functions to
- * makes the internal structures are filled in correctly. Does not
+ * make sure the internal structures are filled in correctly. Does not
  * change any hardware configuration.
+ *
+ * Return: %0 on success, negative errno otherwise.
  */
 int tb_switch_tmu_init(struct tb_switch *sw)
 {
@@ -203,27 +428,11 @@ int tb_switch_tmu_init(struct tb_switch *sw)
 			port->cap_tmu = cap;
 	}
 
-	ret = tb_switch_tmu_rate_read(sw);
-	if (ret < 0)
+	ret = tmu_mode_init(sw);
+	if (ret)
 		return ret;
 
-	sw->tmu.rate = ret;
-
-	sw->tmu.has_ucap = tb_switch_tmu_ucap_supported(sw);
-	if (sw->tmu.has_ucap) {
-		tb_sw_dbg(sw, "TMU: supports uni-directional mode\n");
-
-		if (tb_route(sw)) {
-			struct tb_port *up = tb_upstream_port(sw);
-
-			sw->tmu.unidirectional =
-				tb_port_tmu_is_unidirectional(up);
-		}
-	} else {
-		sw->tmu.unidirectional = false;
-	}
-
-	tb_sw_dbg(sw, "TMU: current mode: %s\n", tb_switch_tmu_mode_name(sw));
+	tb_sw_dbg(sw, "TMU: current mode: %s\n", tmu_mode_name(sw->tmu.mode));
 	return 0;
 }
 
@@ -232,6 +441,8 @@ int tb_switch_tmu_init(struct tb_switch *sw)
  * @sw: Switch whose time to update
  *
  * Updates switch local time using time posting procedure.
+ *
+ * Return: %0 on success, negative errno otherwise.
  */
 int tb_switch_tmu_post_time(struct tb_switch *sw)
 {
@@ -259,7 +470,7 @@ int tb_switch_tmu_post_time(struct tb_switch *sw)
 		return ret;
 
 	for (i = 0; i < ARRAY_SIZE(gm_local_time); i++)
-		tb_sw_dbg(root_switch, "local_time[%d]=0x%08x\n", i,
+		tb_sw_dbg(root_switch, "TMU: local_time[%d]=0x%08x\n", i,
 			  gm_local_time[i]);
 
 	/* Convert to nanoseconds (drop fractional part) */
@@ -326,76 +537,99 @@ out:
 	return ret;
 }
 
+static int disable_enhanced(struct tb_port *up, struct tb_port *down)
+{
+	int ret;
+
+	/*
+	 * Router may already been disconnected so ignore errors on the
+	 * upstream port.
+	 */
+	tb_port_tmu_rate_write(up, 0);
+	tb_port_tmu_enhanced_enable(up, false);
+
+	ret = tb_port_tmu_rate_write(down, 0);
+	if (ret)
+		return ret;
+	return tb_port_tmu_enhanced_enable(down, false);
+}
+
 /**
  * tb_switch_tmu_disable() - Disable TMU of a switch
  * @sw: Switch whose TMU to disable
  *
  * Turns off TMU of @sw if it is enabled. If not enabled does nothing.
+ *
+ * Return: %0 on success, negative errno otherwise.
  */
 int tb_switch_tmu_disable(struct tb_switch *sw)
 {
-	/*
-	 * No need to disable TMU on devices that don't support CLx since
-	 * on these devices e.g. Alpine Ridge and earlier, the TMU mode
-	 * HiFi bi-directional is enabled by default and we don't change it.
-	 */
-	if (!tb_switch_is_clx_supported(sw))
-		return 0;
-
 	/* Already disabled? */
-	if (sw->tmu.rate == TB_SWITCH_TMU_RATE_OFF)
+	if (sw->tmu.mode == TB_SWITCH_TMU_MODE_OFF)
 		return 0;
 
-
 	if (tb_route(sw)) {
-		bool unidirectional = tb_switch_tmu_hifi_is_enabled(sw, true);
-		struct tb_switch *parent = tb_switch_parent(sw);
 		struct tb_port *down, *up;
 		int ret;
 
-		down = tb_port_at(tb_route(sw), parent);
+		down = tb_switch_downstream_port(sw);
 		up = tb_upstream_port(sw);
 		/*
 		 * In case of uni-directional time sync, TMU handshake is
 		 * initiated by upstream router. In case of bi-directional
 		 * time sync, TMU handshake is initiated by downstream router.
-		 * Therefore, we change the rate to off in the respective
-		 * router.
+		 * We change downstream router's rate to off for both uni/bidir
+		 * cases although it is needed only for the bi-directional mode.
+		 * We avoid changing upstream router's mode since it might
+		 * have another downstream router plugged, that is set to
+		 * uni-directional mode and we don't want to change it's TMU
+		 * mode.
 		 */
-		if (unidirectional)
-			tb_switch_tmu_rate_write(parent, TB_SWITCH_TMU_RATE_OFF);
-		else
-			tb_switch_tmu_rate_write(sw, TB_SWITCH_TMU_RATE_OFF);
+		ret = tb_switch_tmu_rate_write(sw, tmu_rates[TB_SWITCH_TMU_MODE_OFF]);
+		if (ret)
+			return ret;
 
 		tb_port_tmu_time_sync_disable(up);
 		ret = tb_port_tmu_time_sync_disable(down);
 		if (ret)
 			return ret;
 
-		if (unidirectional) {
+		switch (sw->tmu.mode) {
+		case TB_SWITCH_TMU_MODE_LOWRES:
+		case TB_SWITCH_TMU_MODE_HIFI_UNI:
 			/* The switch may be unplugged so ignore any errors */
 			tb_port_tmu_unidirectional_disable(up);
 			ret = tb_port_tmu_unidirectional_disable(down);
 			if (ret)
 				return ret;
+			break;
+
+		case TB_SWITCH_TMU_MODE_MEDRES_ENHANCED_UNI:
+			ret = disable_enhanced(up, down);
+			if (ret)
+				return ret;
+			break;
+
+		default:
+			break;
 		}
 	} else {
-		tb_switch_tmu_rate_write(sw, TB_SWITCH_TMU_RATE_OFF);
+		tb_switch_tmu_rate_write(sw, tmu_rates[TB_SWITCH_TMU_MODE_OFF]);
 	}
 
-	sw->tmu.unidirectional = false;
-	sw->tmu.rate = TB_SWITCH_TMU_RATE_OFF;
+	sw->tmu.mode = TB_SWITCH_TMU_MODE_OFF;
 
 	tb_sw_dbg(sw, "TMU: disabled\n");
 	return 0;
 }
 
-static void __tb_switch_tmu_off(struct tb_switch *sw, bool unidirectional)
+/* Called only when there is failure enabling requested mode */
+static void tb_switch_tmu_off(struct tb_switch *sw)
 {
-	struct tb_switch *parent = tb_switch_parent(sw);
+	unsigned int rate = tmu_rates[TB_SWITCH_TMU_MODE_OFF];
 	struct tb_port *down, *up;
 
-	down = tb_port_at(tb_route(sw), parent);
+	down = tb_switch_downstream_port(sw);
 	up = tb_upstream_port(sw);
 	/*
 	 * In case of any failure in one of the steps when setting
@@ -406,27 +640,38 @@ static void __tb_switch_tmu_off(struct tb_switch *sw, bool unidirectional)
 	 */
 	tb_port_tmu_time_sync_disable(down);
 	tb_port_tmu_time_sync_disable(up);
-	if (unidirectional)
-		tb_switch_tmu_rate_write(parent, TB_SWITCH_TMU_RATE_OFF);
-	else
-		tb_switch_tmu_rate_write(sw, TB_SWITCH_TMU_RATE_OFF);
 
+	switch (sw->tmu.mode_request) {
+	case TB_SWITCH_TMU_MODE_LOWRES:
+	case TB_SWITCH_TMU_MODE_HIFI_UNI:
+		tb_switch_tmu_rate_write(tb_switch_parent(sw), rate);
+		break;
+	case TB_SWITCH_TMU_MODE_MEDRES_ENHANCED_UNI:
+		disable_enhanced(up, down);
+		break;
+	default:
+		break;
+	}
+
+	/* Always set the rate to 0 */
+	tb_switch_tmu_rate_write(sw, rate);
+
+	tb_switch_set_tmu_mode_params(sw, sw->tmu.mode);
 	tb_port_tmu_unidirectional_disable(down);
 	tb_port_tmu_unidirectional_disable(up);
 }
 
 /*
  * This function is called when the previous TMU mode was
- * TB_SWITCH_TMU_RATE_OFF.
+ * TB_SWITCH_TMU_MODE_OFF.
  */
-static int __tb_switch_tmu_enable_bidirectional(struct tb_switch *sw)
+static int tb_switch_tmu_enable_bidirectional(struct tb_switch *sw)
 {
-	struct tb_switch *parent = tb_switch_parent(sw);
 	struct tb_port *up, *down;
 	int ret;
 
 	up = tb_upstream_port(sw);
-	down = tb_port_at(tb_route(sw), parent);
+	down = tb_switch_downstream_port(sw);
 
 	ret = tb_port_tmu_unidirectional_disable(up);
 	if (ret)
@@ -436,7 +681,7 @@ static int __tb_switch_tmu_enable_bidirectional(struct tb_switch *sw)
 	if (ret)
 		goto out;
 
-	ret = tb_switch_tmu_rate_write(sw, TB_SWITCH_TMU_RATE_HIFI);
+	ret = tb_switch_tmu_rate_write(sw, tmu_rates[TB_SWITCH_TMU_MODE_HIFI_BI]);
 	if (ret)
 		goto out;
 
@@ -451,12 +696,14 @@ static int __tb_switch_tmu_enable_bidirectional(struct tb_switch *sw)
 	return 0;
 
 out:
-	__tb_switch_tmu_off(sw, false);
+	tb_switch_tmu_off(sw);
 	return ret;
 }
 
-static int tb_switch_tmu_objection_mask(struct tb_switch *sw)
+/* Only needed for Titan Ridge */
+static int tb_switch_tmu_disable_objections(struct tb_switch *sw)
 {
+	struct tb_port *up = tb_upstream_port(sw);
 	u32 val;
 	int ret;
 
@@ -467,32 +714,34 @@ static int tb_switch_tmu_objection_mask(struct tb_switch *sw)
 
 	val &= ~TB_TIME_VSEC_3_CS_9_TMU_OBJ_MASK;
 
-	return tb_sw_write(sw, &val, TB_CFG_SWITCH,
-			   sw->cap_vsec_tmu + TB_TIME_VSEC_3_CS_9, 1);
-}
-
-static int tb_switch_tmu_unidirectional_enable(struct tb_switch *sw)
-{
-	struct tb_port *up = tb_upstream_port(sw);
+	ret = tb_sw_write(sw, &val, TB_CFG_SWITCH,
+			  sw->cap_vsec_tmu + TB_TIME_VSEC_3_CS_9, 1);
+	if (ret)
+		return ret;
 
 	return tb_port_tmu_write(up, TMU_ADP_CS_6,
 				 TMU_ADP_CS_6_DISABLE_TMU_OBJ_MASK,
-				 TMU_ADP_CS_6_DISABLE_TMU_OBJ_MASK);
+				 TMU_ADP_CS_6_DISABLE_TMU_OBJ_CL1 |
+				 TMU_ADP_CS_6_DISABLE_TMU_OBJ_CL2);
 }
 
 /*
  * This function is called when the previous TMU mode was
- * TB_SWITCH_TMU_RATE_OFF.
+ * TB_SWITCH_TMU_MODE_OFF.
  */
-static int __tb_switch_tmu_enable_unidirectional(struct tb_switch *sw)
+static int tb_switch_tmu_enable_unidirectional(struct tb_switch *sw)
 {
-	struct tb_switch *parent = tb_switch_parent(sw);
 	struct tb_port *up, *down;
 	int ret;
 
 	up = tb_upstream_port(sw);
-	down = tb_port_at(tb_route(sw), parent);
-	ret = tb_switch_tmu_rate_write(parent, TB_SWITCH_TMU_RATE_HIFI);
+	down = tb_switch_downstream_port(sw);
+	ret = tb_switch_tmu_rate_write(tb_switch_parent(sw),
+				       tmu_rates[sw->tmu.mode_request]);
+	if (ret)
+		return ret;
+
+	ret = tb_switch_set_tmu_mode_params(sw, sw->tmu.mode_request);
 	if (ret)
 		return ret;
 
@@ -515,39 +764,200 @@ static int __tb_switch_tmu_enable_unidirectional(struct tb_switch *sw)
 	return 0;
 
 out:
-	__tb_switch_tmu_off(sw, true);
+	tb_switch_tmu_off(sw);
 	return ret;
 }
 
-static int tb_switch_tmu_hifi_enable(struct tb_switch *sw)
+/*
+ * This function is called when the previous TMU mode was
+ * TB_SWITCH_TMU_RATE_OFF.
+ */
+static int tb_switch_tmu_enable_enhanced(struct tb_switch *sw)
 {
-	bool unidirectional = sw->tmu.unidirectional_request;
+	unsigned int rate = tmu_rates[sw->tmu.mode_request];
+	struct tb_port *up, *down;
 	int ret;
 
-	if (unidirectional && !sw->tmu.has_ucap)
-		return -EOPNOTSUPP;
+	/* Router specific parameters first */
+	ret = tb_switch_set_tmu_mode_params(sw, sw->tmu.mode_request);
+	if (ret)
+		return ret;
+
+	up = tb_upstream_port(sw);
+	down = tb_switch_downstream_port(sw);
+
+	ret = tb_port_set_tmu_mode_params(up, sw->tmu.mode_request);
+	if (ret)
+		goto out;
+
+	ret = tb_port_tmu_rate_write(up, rate);
+	if (ret)
+		goto out;
 
+	ret = tb_port_tmu_enhanced_enable(up, true);
+	if (ret)
+		goto out;
+
+	ret = tb_port_set_tmu_mode_params(down, sw->tmu.mode_request);
+	if (ret)
+		goto out;
+
+	ret = tb_port_tmu_rate_write(down, rate);
+	if (ret)
+		goto out;
+
+	ret = tb_port_tmu_enhanced_enable(down, true);
+	if (ret)
+		goto out;
+
+	return 0;
+
+out:
+	tb_switch_tmu_off(sw);
+	return ret;
+}
+
+static void tb_switch_tmu_change_mode_prev(struct tb_switch *sw)
+{
+	unsigned int rate = tmu_rates[sw->tmu.mode];
+	struct tb_port *down, *up;
+
+	down = tb_switch_downstream_port(sw);
+	up = tb_upstream_port(sw);
 	/*
-	 * No need to enable TMU on devices that don't support CLx since on
-	 * these devices e.g. Alpine Ridge and earlier, the TMU mode HiFi
-	 * bi-directional is enabled by default.
+	 * In case of any failure in one of the steps when change mode,
+	 * get back to the TMU configurations in previous mode.
+	 * In case of additional failures in the functions below,
+	 * ignore them since the caller shall already report a failure.
 	 */
-	if (!tb_switch_is_clx_supported(sw))
-		return 0;
+	switch (sw->tmu.mode) {
+	case TB_SWITCH_TMU_MODE_LOWRES:
+	case TB_SWITCH_TMU_MODE_HIFI_UNI:
+		tb_port_tmu_set_unidirectional(down, true);
+		tb_switch_tmu_rate_write(tb_switch_parent(sw), rate);
+		break;
+
+	case TB_SWITCH_TMU_MODE_HIFI_BI:
+		tb_port_tmu_set_unidirectional(down, false);
+		tb_switch_tmu_rate_write(sw, rate);
+		break;
 
-	if (tb_switch_tmu_hifi_is_enabled(sw, sw->tmu.unidirectional_request))
-		return 0;
+	default:
+		break;
+	}
 
-	if (tb_switch_is_titan_ridge(sw) && unidirectional) {
-		/* Titan Ridge supports only CL0s */
-		if (!tb_switch_is_cl0s_enabled(sw))
-			return -EOPNOTSUPP;
+	tb_switch_set_tmu_mode_params(sw, sw->tmu.mode);
+
+	switch (sw->tmu.mode) {
+	case TB_SWITCH_TMU_MODE_LOWRES:
+	case TB_SWITCH_TMU_MODE_HIFI_UNI:
+		tb_port_tmu_set_unidirectional(up, true);
+		break;
+
+	case TB_SWITCH_TMU_MODE_HIFI_BI:
+		tb_port_tmu_set_unidirectional(up, false);
+		break;
 
-		ret = tb_switch_tmu_objection_mask(sw);
+	default:
+		break;
+	}
+}
+
+static int tb_switch_tmu_change_mode(struct tb_switch *sw)
+{
+	unsigned int rate = tmu_rates[sw->tmu.mode_request];
+	struct tb_port *up, *down;
+	int ret;
+
+	up = tb_upstream_port(sw);
+	down = tb_switch_downstream_port(sw);
+
+	/* Program the upstream router downstream facing lane adapter */
+	switch (sw->tmu.mode_request) {
+	case TB_SWITCH_TMU_MODE_LOWRES:
+	case TB_SWITCH_TMU_MODE_HIFI_UNI:
+		ret = tb_port_tmu_set_unidirectional(down, true);
 		if (ret)
-			return ret;
+			goto out;
+		ret = tb_switch_tmu_rate_write(tb_switch_parent(sw), rate);
+		if (ret)
+			goto out;
+		break;
+
+	case TB_SWITCH_TMU_MODE_HIFI_BI:
+		ret = tb_port_tmu_set_unidirectional(down, false);
+		if (ret)
+			goto out;
+		ret = tb_switch_tmu_rate_write(sw, rate);
+		if (ret)
+			goto out;
+		break;
+
+	default:
+		/* Not allowed to change modes from other than above */
+		return -EINVAL;
+	}
+
+	ret = tb_switch_set_tmu_mode_params(sw, sw->tmu.mode_request);
+	if (ret)
+		goto out;
+
+	/* Program the new mode and the downstream router lane adapter */
+	switch (sw->tmu.mode_request) {
+	case TB_SWITCH_TMU_MODE_LOWRES:
+	case TB_SWITCH_TMU_MODE_HIFI_UNI:
+		ret = tb_port_tmu_set_unidirectional(up, true);
+		if (ret)
+			goto out;
+		break;
 
-		ret = tb_switch_tmu_unidirectional_enable(sw);
+	case TB_SWITCH_TMU_MODE_HIFI_BI:
+		ret = tb_port_tmu_set_unidirectional(up, false);
+		if (ret)
+			goto out;
+		break;
+
+	default:
+		/* Not allowed to change modes from other than above */
+		return -EINVAL;
+	}
+
+	ret = tb_port_tmu_time_sync_enable(down);
+	if (ret)
+		goto out;
+
+	ret = tb_port_tmu_time_sync_enable(up);
+	if (ret)
+		goto out;
+
+	return 0;
+
+out:
+	tb_switch_tmu_change_mode_prev(sw);
+	return ret;
+}
+
+/**
+ * tb_switch_tmu_enable() - Enable TMU on a router
+ * @sw: Router whose TMU to enable
+ *
+ * Enables TMU of a router to be in uni-directional Normal/HiFi or
+ * bi-directional HiFi mode. Calling tb_switch_tmu_configure() is
+ * required before calling this function.
+ *
+ * Return: %0 on success, negative errno otherwise.
+ */
+int tb_switch_tmu_enable(struct tb_switch *sw)
+{
+	int ret;
+
+	if (tb_switch_tmu_is_enabled(sw))
+		return 0;
+
+	if (tb_switch_is_titan_ridge(sw) &&
+	    (sw->tmu.mode_request == TB_SWITCH_TMU_MODE_LOWRES ||
+	     sw->tmu.mode_request == TB_SWITCH_TMU_MODE_HIFI_UNI)) {
+		ret = tb_switch_tmu_disable_objections(sw);
 		if (ret)
 			return ret;
 	}
@@ -557,16 +967,35 @@ static int tb_switch_tmu_hifi_enable(struct tb_switch *sw)
 		return ret;
 
 	if (tb_route(sw)) {
-		/* The used mode changes are from OFF to HiFi-Uni/HiFi-BiDir */
-		if (sw->tmu.rate == TB_SWITCH_TMU_RATE_OFF) {
-			if (unidirectional)
-				ret = __tb_switch_tmu_enable_unidirectional(sw);
-			else
-				ret = __tb_switch_tmu_enable_bidirectional(sw);
-			if (ret)
-				return ret;
+		/*
+		 * The used mode changes are from OFF to
+		 * HiFi-Uni/HiFi-BiDir/Normal-Uni or from Normal-Uni to
+		 * HiFi-Uni.
+		 */
+		if (sw->tmu.mode == TB_SWITCH_TMU_MODE_OFF) {
+			switch (sw->tmu.mode_request) {
+			case TB_SWITCH_TMU_MODE_LOWRES:
+			case TB_SWITCH_TMU_MODE_HIFI_UNI:
+				ret = tb_switch_tmu_enable_unidirectional(sw);
+				break;
+
+			case TB_SWITCH_TMU_MODE_HIFI_BI:
+				ret = tb_switch_tmu_enable_bidirectional(sw);
+				break;
+			case TB_SWITCH_TMU_MODE_MEDRES_ENHANCED_UNI:
+				ret = tb_switch_tmu_enable_enhanced(sw);
+				break;
+			default:
+				ret = -EINVAL;
+				break;
+			}
+		} else if (sw->tmu.mode == TB_SWITCH_TMU_MODE_LOWRES ||
+			   sw->tmu.mode == TB_SWITCH_TMU_MODE_HIFI_UNI ||
+			   sw->tmu.mode == TB_SWITCH_TMU_MODE_HIFI_BI) {
+			ret = tb_switch_tmu_change_mode(sw);
+		} else {
+			ret = -EINVAL;
 		}
-		sw->tmu.unidirectional = unidirectional;
 	} else {
 		/*
 		 * Host router port configurations are written as
@@ -574,47 +1003,70 @@ static int tb_switch_tmu_hifi_enable(struct tb_switch *sw)
 		 * of the child node - see above.
 		 * Here only the host router' rate configuration is written.
 		 */
-		ret = tb_switch_tmu_rate_write(sw, TB_SWITCH_TMU_RATE_HIFI);
-		if (ret)
-			return ret;
+		ret = tb_switch_tmu_rate_write(sw, tmu_rates[sw->tmu.mode_request]);
 	}
 
-	sw->tmu.rate = TB_SWITCH_TMU_RATE_HIFI;
+	if (ret) {
+		tb_sw_warn(sw, "TMU: failed to enable mode %s: %d\n",
+			   tmu_mode_name(sw->tmu.mode_request), ret);
+	} else {
+		sw->tmu.mode = sw->tmu.mode_request;
+		tb_sw_dbg(sw, "TMU: mode set to: %s\n", tmu_mode_name(sw->tmu.mode));
+	}
 
-	tb_sw_dbg(sw, "TMU: mode set to: %s\n", tb_switch_tmu_mode_name(sw));
 	return tb_switch_tmu_set_time_disruption(sw, false);
 }
 
 /**
- * tb_switch_tmu_enable() - Enable TMU on a router
- * @sw: Router whose TMU to enable
+ * tb_switch_tmu_configure() - Configure the TMU mode
+ * @sw: Router whose mode to change
+ * @mode: Mode to configure
  *
- * Enables TMU of a router to be in uni-directional or bi-directional HiFi mode.
- * Calling tb_switch_tmu_configure() is required before calling this function,
- * to select the mode HiFi and directionality (uni-directional/bi-directional).
- * In both modes all tunneling should work. Uni-directional mode is required for
- * CLx (Link Low-Power) to work.
+ * Selects the TMU mode that is enabled when tb_switch_tmu_enable() is
+ * next called.
+ *
+ * Return:
+ * * %0 - On success.
+ * * %-EOPNOTSUPP - If the requested mode is not possible (not supported by
+ *   the router and/or topology).
+ * * Negative errno - Another error occurred.
  */
-int tb_switch_tmu_enable(struct tb_switch *sw)
+int tb_switch_tmu_configure(struct tb_switch *sw, enum tb_switch_tmu_mode mode)
 {
-	if (sw->tmu.rate_request == TB_SWITCH_TMU_RATE_NORMAL)
-		return -EOPNOTSUPP;
+	switch (mode) {
+	case TB_SWITCH_TMU_MODE_OFF:
+		break;
 
-	return tb_switch_tmu_hifi_enable(sw);
-}
+	case TB_SWITCH_TMU_MODE_LOWRES:
+	case TB_SWITCH_TMU_MODE_HIFI_UNI:
+		if (!sw->tmu.has_ucap)
+			return -EOPNOTSUPP;
+		break;
 
-/**
- * tb_switch_tmu_configure() - Configure the TMU rate and directionality
- * @sw: Router whose mode to change
- * @rate: Rate to configure Off/LowRes/HiFi
- * @unidirectional: If uni-directional (bi-directional otherwise)
- *
- * Selects the rate of the TMU and directionality (uni-directional or
- * bi-directional). Must be called before tb_switch_tmu_enable().
- */
-void tb_switch_tmu_configure(struct tb_switch *sw,
-			     enum tb_switch_tmu_rate rate, bool unidirectional)
-{
-	sw->tmu.unidirectional_request = unidirectional;
-	sw->tmu.rate_request = rate;
+	case TB_SWITCH_TMU_MODE_HIFI_BI:
+		break;
+
+	case TB_SWITCH_TMU_MODE_MEDRES_ENHANCED_UNI: {
+		const struct tb_switch *parent_sw = tb_switch_parent(sw);
+
+		if (!parent_sw || !tb_switch_tmu_enhanced_is_supported(parent_sw))
+			return -EOPNOTSUPP;
+		if (!tb_switch_tmu_enhanced_is_supported(sw))
+			return -EOPNOTSUPP;
+
+		break;
+	}
+
+	default:
+		tb_sw_warn(sw, "TMU: unsupported mode %u\n", mode);
+		return -EINVAL;
+	}
+
+	if (sw->tmu.mode_request != mode) {
+		tb_sw_dbg(sw, "TMU: mode change %s -> %s requested\n",
+			  tmu_mode_name(sw->tmu.mode), tmu_mode_name(mode));
+		sw->tmu.mode_request = mode;
+	}
+
+	return 0;
 }