[PATCH 08/20] thunderbolt: Enable USB4 v2 PCIe TLP/DLLP extended encapsulation

[Mika Westerberg <mika.westerberg@xxxxxxxxxxxxxxx>]

From: Gil Fine <gil.fine@xxxxxxxxx>

USB4 v2 spec introduces modified encapsulation of PCIe TLP and DLLP
packets. This improves the PCIe tunneled traffic usage by reducing
overhead. Enable this if both sides of the link support it.

Signed-off-by: Gil Fine <gil.fine@xxxxxxxxx>
Signed-off-by: Mika Westerberg <mika.westerberg@xxxxxxxxxxxxxxx>
---
 drivers/thunderbolt/tb.h      |  2 ++
 drivers/thunderbolt/tb_regs.h |  2 ++
 drivers/thunderbolt/tunnel.c  | 37 ++++++++++++++++++++++++++++++++---
 drivers/thunderbolt/usb4.c    | 34 ++++++++++++++++++++++++++++++++
 4 files changed, 72 insertions(+), 3 deletions(-)

diff --git a/drivers/thunderbolt/tb.h b/drivers/thunderbolt/tb.h
index ca2888a8b703..e1f1ed7dd60c 100644
--- a/drivers/thunderbolt/tb.h
+++ b/drivers/thunderbolt/tb.h
@@ -1302,6 +1302,8 @@ int usb4_dp_port_allocated_bw(struct tb_port *port);
 int usb4_dp_port_allocate_bw(struct tb_port *port, int bw);
 int usb4_dp_port_requested_bw(struct tb_port *port);
 
+int usb4_pci_port_set_ext_encapsulation(struct tb_port *port, bool enable);
+
 static inline bool tb_is_usb4_port_device(const struct device *dev)
 {
 	return dev->type == &usb4_port_device_type;
diff --git a/drivers/thunderbolt/tb_regs.h b/drivers/thunderbolt/tb_regs.h
index c8e40ef09903..549cc79c7313 100644
--- a/drivers/thunderbolt/tb_regs.h
+++ b/drivers/thunderbolt/tb_regs.h
@@ -451,6 +451,8 @@ struct tb_regs_port_header {
 /* PCIe adapter registers */
 #define ADP_PCIE_CS_0				0x00
 #define ADP_PCIE_CS_0_PE			BIT(31)
+#define ADP_PCIE_CS_1				0x01
+#define ADP_PCIE_CS_1_EE			BIT(0)
 
 /* USB adapter registers */
 #define ADP_USB3_CS_0				0x00
diff --git a/drivers/thunderbolt/tunnel.c b/drivers/thunderbolt/tunnel.c
index 7df5f90e21d4..f1d0ab2b39a2 100644
--- a/drivers/thunderbolt/tunnel.c
+++ b/drivers/thunderbolt/tunnel.c
@@ -153,18 +153,49 @@ static struct tb_tunnel *tb_tunnel_alloc(struct tb *tb, size_t npaths,
 	return tunnel;
 }
 
+static int tb_pci_set_ext_encapsulation(struct tb_tunnel *tunnel, bool enable)
+{
+	int ret;
+
+	/* Only supported of both routers are at least USB4 v2 */
+	if (usb4_switch_version(tunnel->src_port->sw) < 2 ||
+	    usb4_switch_version(tunnel->dst_port->sw) < 2)
+		return 0;
+
+	ret = usb4_pci_port_set_ext_encapsulation(tunnel->src_port, enable);
+	if (ret)
+		return ret;
+
+	ret = usb4_pci_port_set_ext_encapsulation(tunnel->dst_port, enable);
+	if (ret)
+		return ret;
+
+	tb_tunnel_dbg(tunnel, "extended encapsulation %sabled\n",
+		      enable ? "en" : "dis");
+	return 0;
+}
+
 static int tb_pci_activate(struct tb_tunnel *tunnel, bool activate)
 {
 	int res;
 
+	if (activate) {
+		res = tb_pci_set_ext_encapsulation(tunnel, activate);
+		if (res)
+			return res;
+	}
+
 	res = tb_pci_port_enable(tunnel->src_port, activate);
 	if (res)
 		return res;
 
-	if (tb_port_is_pcie_up(tunnel->dst_port))
-		return tb_pci_port_enable(tunnel->dst_port, activate);
+	if (tb_port_is_pcie_up(tunnel->dst_port)) {
+		res = tb_pci_port_enable(tunnel->dst_port, activate);
+		if (res)
+			return res;
+	}
 
-	return 0;
+	return activate ? 0 : tb_pci_set_ext_encapsulation(tunnel, activate);
 }
 
 static int tb_pci_init_credits(struct tb_path_hop *hop)
diff --git a/drivers/thunderbolt/usb4.c b/drivers/thunderbolt/usb4.c
index 9f5a98347bee..b80972cb5b9d 100644
--- a/drivers/thunderbolt/usb4.c
+++ b/drivers/thunderbolt/usb4.c
@@ -2796,3 +2796,37 @@ int usb4_dp_port_requested_bw(struct tb_port *port)
 
 	return (val & ADP_DP_CS_8_REQUESTED_BW_MASK) * granularity;
 }
+
+/**
+ * usb4_pci_port_set_ext_encapsulation() - Enable/disable extended encapsulation
+ * @port: PCIe adapter
+ * @enable: Enable/disable extended encapsulation
+ *
+ * Can be called to any adapter. Enables or disables extended
+ * encapsulation used in PCIe tunneling. Returns %0 on success and
+ * negative errno otherwise.
+ */
+int usb4_pci_port_set_ext_encapsulation(struct tb_port *port, bool enable)
+{
+	u32 val;
+	int ret;
+
+	if (!tb_port_is_pcie_up(port) && !tb_port_is_pcie_down(port))
+		return 0;
+
+	if (usb4_switch_version(port->sw) < 2)
+		return 0;
+
+	ret = tb_port_read(port, &val, TB_CFG_PORT,
+			   port->cap_adap + ADP_PCIE_CS_1, 1);
+	if (ret)
+		return ret;
+
+	if (enable)
+		val |= ADP_PCIE_CS_1_EE;
+	else
+		val &= ~ADP_PCIE_CS_1_EE;
+
+	return tb_port_write(port, &val, TB_CFG_PORT,
+			     port->cap_adap + ADP_PCIE_CS_1, 1);
+}
-- 
2.39.2