Merge branch 'mlxsw-move-tx-header-handling-to-pci-driver'

Petr Machata says:

====================
mlxsw: Move Tx header handling to PCI driver

Amit Cohen writes:

Tx header should be added to all packets transmitted from the CPU to
Spectrum ASICs. Historically, handling this header was added as a driver
function, as Tx header is different between Spectrum and Switch-X.

From May 2021, there is no support for SwitchX-2 ASIC, and all the relevant
code was removed.

For now, there is no justification to handle Tx header as part of
spectrum.c, we can handle this as part of PCI, in skb_transmit().

This change will also be useful when XDP support will be added to mlxsw,
as for XDP_TX and XDP_REDIRECT actions, Tx header should be added before
transmitting the packet.

Patch set overview:
Patches #1-#2 add structure to store Tx header info and initialize it
Patch #3 moves definitions of Tx header fields to txheader.h
Patch #4 moves Tx header handling to PCI driver
Patch #5 removes unnecessary attribute
====================

Link: https://patch.msgid.link/cover.1737044384.git.petrm@nvidia.com
Signed-off-by: Jakub Kicinski <kuba@kernel.org>

9 files changed, 176 insertions, 259 deletions

diff --git a/drivers/net/ethernet/mellanox/mlxsw/core.c b/drivers/net/ethernet/mellanox/mlxsw/core.c
index 4a79c0d7e7ad..2bb2b77351bd 100644
--- a/drivers/net/ethernet/mellanox/mlxsw/core.c
+++ b/drivers/net/ethernet/mellanox/mlxsw/core.c
@@ -35,6 +35,7 @@
 #include "reg.h"
 #include "resources.h"
 #include "../mlxfw/mlxfw.h"
+#include "txheader.h"
 
 static LIST_HEAD(mlxsw_core_driver_list);
 static DEFINE_SPINLOCK(mlxsw_core_driver_list_lock);
@@ -677,7 +678,7 @@ struct mlxsw_reg_trans {
 	struct list_head bulk_list;
 	struct mlxsw_core *core;
 	struct sk_buff *tx_skb;
-	struct mlxsw_tx_info tx_info;
+	struct mlxsw_txhdr_info txhdr_info;
 	struct delayed_work timeout_dw;
 	unsigned int retries;
 	u64 tid;
@@ -737,12 +738,11 @@ static int mlxsw_emad_transmit(struct mlxsw_core *mlxsw_core,
 	if (!skb)
 		return -ENOMEM;
 
-	trace_devlink_hwmsg(priv_to_devlink(mlxsw_core), false, 0,
-			    skb->data + mlxsw_core->driver->txhdr_len,
-			    skb->len - mlxsw_core->driver->txhdr_len);
+	trace_devlink_hwmsg(priv_to_devlink(mlxsw_core), false, 0, skb->data,
+			    skb->len);
 
 	atomic_set(&trans->active, 1);
-	err = mlxsw_core_skb_transmit(mlxsw_core, skb, &trans->tx_info);
+	err = mlxsw_core_skb_transmit(mlxsw_core, skb, &trans->txhdr_info);
 	if (err) {
 		dev_kfree_skb(skb);
 		return err;
@@ -944,7 +944,7 @@ static struct sk_buff *mlxsw_emad_alloc(const struct mlxsw_core *mlxsw_core,
 
 	emad_len = (reg_len + sizeof(u32) + MLXSW_EMAD_ETH_HDR_LEN +
 		    (MLXSW_EMAD_OP_TLV_LEN + MLXSW_EMAD_END_TLV_LEN) *
-		    sizeof(u32) + mlxsw_core->driver->txhdr_len);
+		    sizeof(u32) + MLXSW_TXHDR_LEN);
 	if (mlxsw_core->emad.enable_string_tlv)
 		emad_len += MLXSW_EMAD_STRING_TLV_LEN * sizeof(u32);
 	if (mlxsw_core->emad.enable_latency_tlv)
@@ -984,8 +984,8 @@ static int mlxsw_emad_reg_access(struct mlxsw_core *mlxsw_core,
 	list_add_tail(&trans->bulk_list, bulk_list);
 	trans->core = mlxsw_core;
 	trans->tx_skb = skb;
-	trans->tx_info.local_port = MLXSW_PORT_CPU_PORT;
-	trans->tx_info.is_emad = true;
+	trans->txhdr_info.tx_info.local_port = MLXSW_PORT_CPU_PORT;
+	trans->txhdr_info.tx_info.is_emad = true;
 	INIT_DELAYED_WORK(&trans->timeout_dw, mlxsw_emad_trans_timeout_work);
 	trans->tid = tid;
 	init_completion(&trans->completion);
@@ -995,7 +995,6 @@ static int mlxsw_emad_reg_access(struct mlxsw_core *mlxsw_core,
 	trans->type = type;
 
 	mlxsw_emad_construct(mlxsw_core, skb, reg, payload, type, trans->tid);
-	mlxsw_core->driver->txhdr_construct(skb, &trans->tx_info);
 
 	spin_lock_bh(&mlxsw_core->emad.trans_list_lock);
 	list_add_tail_rcu(&trans->list, &mlxsw_core->emad.trans_list);
@@ -2330,10 +2329,10 @@ bool mlxsw_core_skb_transmit_busy(struct mlxsw_core *mlxsw_core,
 EXPORT_SYMBOL(mlxsw_core_skb_transmit_busy);
 
 int mlxsw_core_skb_transmit(struct mlxsw_core *mlxsw_core, struct sk_buff *skb,
-			    const struct mlxsw_tx_info *tx_info)
+			    const struct mlxsw_txhdr_info *txhdr_info)
 {
 	return mlxsw_core->bus->skb_transmit(mlxsw_core->bus_priv, skb,
-					     tx_info);
+					     txhdr_info);
 }
 EXPORT_SYMBOL(mlxsw_core_skb_transmit);
 
diff --git a/drivers/net/ethernet/mellanox/mlxsw/core.h b/drivers/net/ethernet/mellanox/mlxsw/core.h
index 24c3ff6fcf71..1a871397a6df 100644
--- a/drivers/net/ethernet/mellanox/mlxsw/core.h
+++ b/drivers/net/ethernet/mellanox/mlxsw/core.h
@@ -72,6 +72,12 @@ struct mlxsw_tx_info {
 	bool is_emad;
 };
 
+struct mlxsw_txhdr_info {
+	struct mlxsw_tx_info tx_info;
+	bool data;
+	u16 max_fid; /* Used for PTP packets which are sent as data. */
+};
+
 struct mlxsw_rx_md_info {
 	struct napi_struct *napi;
 	u32 cookie_index;
@@ -95,7 +101,7 @@ struct mlxsw_rx_md_info {
 bool mlxsw_core_skb_transmit_busy(struct mlxsw_core *mlxsw_core,
 				  const struct mlxsw_tx_info *tx_info);
 int mlxsw_core_skb_transmit(struct mlxsw_core *mlxsw_core, struct sk_buff *skb,
-			    const struct mlxsw_tx_info *tx_info);
+			    const struct mlxsw_txhdr_info *txhdr_info);
 void mlxsw_core_ptp_transmitted(struct mlxsw_core *mlxsw_core,
 				struct sk_buff *skb, u16 local_port);
 
@@ -426,8 +432,6 @@ struct mlxsw_driver {
 	int (*trap_policer_counter_get)(struct mlxsw_core *mlxsw_core,
 					const struct devlink_trap_policer *policer,
 					u64 *p_drops);
-	void (*txhdr_construct)(struct sk_buff *skb,
-				const struct mlxsw_tx_info *tx_info);
 	int (*resources_register)(struct mlxsw_core *mlxsw_core);
 	int (*kvd_sizes_get)(struct mlxsw_core *mlxsw_core,
 			     const struct mlxsw_config_profile *profile,
@@ -440,7 +444,6 @@ struct mlxsw_driver {
 	void (*ptp_transmitted)(struct mlxsw_core *mlxsw_core,
 				struct sk_buff *skb, u16 local_port);
 
-	u8 txhdr_len;
 	const struct mlxsw_config_profile *profile;
 	bool sdq_supports_cqe_v2;
 };
@@ -487,7 +490,7 @@ struct mlxsw_bus {
 	bool (*skb_transmit_busy)(void *bus_priv,
 				  const struct mlxsw_tx_info *tx_info);
 	int (*skb_transmit)(void *bus_priv, struct sk_buff *skb,
-			    const struct mlxsw_tx_info *tx_info);
+			    const struct mlxsw_txhdr_info *txhdr_info);
 	int (*cmd_exec)(void *bus_priv, u16 opcode, u8 opcode_mod,
 			u32 in_mod, bool out_mbox_direct,
 			char *in_mbox, size_t in_mbox_size,
diff --git a/drivers/net/ethernet/mellanox/mlxsw/i2c.c b/drivers/net/ethernet/mellanox/mlxsw/i2c.c
index 1e150ce1c73a..f9f565c1036d 100644
--- a/drivers/net/ethernet/mellanox/mlxsw/i2c.c
+++ b/drivers/net/ethernet/mellanox/mlxsw/i2c.c
@@ -516,7 +516,7 @@ static bool mlxsw_i2c_skb_transmit_busy(void *bus_priv,
 }
 
 static int mlxsw_i2c_skb_transmit(void *bus_priv, struct sk_buff *skb,
-				  const struct mlxsw_tx_info *tx_info)
+				  const struct mlxsw_txhdr_info *txhdr_info)
 {
 	return 0;
 }
diff --git a/drivers/net/ethernet/mellanox/mlxsw/pci.c b/drivers/net/ethernet/mellanox/mlxsw/pci.c
index 0863dca2fc0b..5b44c931b660 100644
--- a/drivers/net/ethernet/mellanox/mlxsw/pci.c
+++ b/drivers/net/ethernet/mellanox/mlxsw/pci.c
@@ -21,6 +21,7 @@
 #include "cmd.h"
 #include "port.h"
 #include "resources.h"
+#include "txheader.h"
 
 #define mlxsw_pci_write32(mlxsw_pci, reg, val) \
 	iowrite32be(val, (mlxsw_pci)->hw_addr + (MLXSW_PCI_ ## reg))
@@ -2095,6 +2096,39 @@ static void mlxsw_pci_fini(void *bus_priv)
 	mlxsw_pci_free_irq_vectors(mlxsw_pci);
 }
 
+static int mlxsw_pci_txhdr_construct(struct sk_buff *skb,
+				     const struct mlxsw_txhdr_info *txhdr_info)
+{
+	const struct mlxsw_tx_info tx_info = txhdr_info->tx_info;
+	char *txhdr;
+
+	if (skb_cow_head(skb, MLXSW_TXHDR_LEN))
+		return -ENOMEM;
+
+	txhdr = skb_push(skb, MLXSW_TXHDR_LEN);
+	memset(txhdr, 0, MLXSW_TXHDR_LEN);
+
+	mlxsw_tx_hdr_version_set(txhdr, MLXSW_TXHDR_VERSION_1);
+	mlxsw_tx_hdr_proto_set(txhdr, MLXSW_TXHDR_PROTO_ETH);
+	mlxsw_tx_hdr_swid_set(txhdr, 0);
+
+	if (unlikely(txhdr_info->data)) {
+		u16 fid = txhdr_info->max_fid + tx_info.local_port - 1;
+
+		mlxsw_tx_hdr_rx_is_router_set(txhdr, true);
+		mlxsw_tx_hdr_fid_valid_set(txhdr, true);
+		mlxsw_tx_hdr_fid_set(txhdr, fid);
+		mlxsw_tx_hdr_type_set(txhdr, MLXSW_TXHDR_TYPE_DATA);
+	} else {
+		mlxsw_tx_hdr_ctl_set(txhdr, MLXSW_TXHDR_ETH_CTL);
+		mlxsw_tx_hdr_control_tclass_set(txhdr, 1);
+		mlxsw_tx_hdr_port_mid_set(txhdr, tx_info.local_port);
+		mlxsw_tx_hdr_type_set(txhdr, MLXSW_TXHDR_TYPE_CONTROL);
+	}
+
+	return 0;
+}
+
 static struct mlxsw_pci_queue *
 mlxsw_pci_sdq_pick(struct mlxsw_pci *mlxsw_pci,
 		   const struct mlxsw_tx_info *tx_info)
@@ -2122,7 +2156,7 @@ static bool mlxsw_pci_skb_transmit_busy(void *bus_priv,
 }
 
 static int mlxsw_pci_skb_transmit(void *bus_priv, struct sk_buff *skb,
-				  const struct mlxsw_tx_info *tx_info)
+				  const struct mlxsw_txhdr_info *txhdr_info)
 {
 	struct mlxsw_pci *mlxsw_pci = bus_priv;
 	struct mlxsw_pci_queue *q;
@@ -2131,13 +2165,17 @@ static int mlxsw_pci_skb_transmit(void *bus_priv, struct sk_buff *skb,
 	int i;
 	int err;
 
+	err = mlxsw_pci_txhdr_construct(skb, txhdr_info);
+	if (err)
+		return err;
+
 	if (skb_shinfo(skb)->nr_frags > MLXSW_PCI_WQE_SG_ENTRIES - 1) {
 		err = skb_linearize(skb);
 		if (err)
 			return err;
 	}
 
-	q = mlxsw_pci_sdq_pick(mlxsw_pci, tx_info);
+	q = mlxsw_pci_sdq_pick(mlxsw_pci, &txhdr_info->tx_info);
 	spin_lock_bh(&q->lock);
 	elem_info = mlxsw_pci_queue_elem_info_producer_get(q);
 	if (!elem_info) {
@@ -2145,7 +2183,7 @@ static int mlxsw_pci_skb_transmit(void *bus_priv, struct sk_buff *skb,
 		err = -EAGAIN;
 		goto unlock;
 	}
-	mlxsw_skb_cb(skb)->tx_info = *tx_info;
+	mlxsw_skb_cb(skb)->tx_info = txhdr_info->tx_info;
 	elem_info->sdq.skb = skb;
 
 	wqe = elem_info->elem;
diff --git a/drivers/net/ethernet/mellanox/mlxsw/spectrum.c b/drivers/net/ethernet/mellanox/mlxsw/spectrum.c
index aa71993daf28..d714311fd884 100644
--- a/drivers/net/ethernet/mellanox/mlxsw/spectrum.c
+++ b/drivers/net/ethernet/mellanox/mlxsw/spectrum.c
@@ -107,74 +107,6 @@ static const unsigned char mlxsw_sp2_mac_mask[ETH_ALEN] = {
 	0xff, 0xff, 0xff, 0xff, 0xf0, 0x00
 };
 
-/* tx_hdr_version
- * Tx header version.
- * Must be set to 1.
- */
-MLXSW_ITEM32(tx, hdr, version, 0x00, 28, 4);
-
-/* tx_hdr_ctl
- * Packet control type.
- * 0 - Ethernet control (e.g. EMADs, LACP)
- * 1 - Ethernet data
- */
-MLXSW_ITEM32(tx, hdr, ctl, 0x00, 26, 2);
-
-/* tx_hdr_proto
- * Packet protocol type. Must be set to 1 (Ethernet).
- */
-MLXSW_ITEM32(tx, hdr, proto, 0x00, 21, 3);
-
-/* tx_hdr_rx_is_router
- * Packet is sent from the router. Valid for data packets only.
- */
-MLXSW_ITEM32(tx, hdr, rx_is_router, 0x00, 19, 1);
-
-/* tx_hdr_fid_valid
- * Indicates if the 'fid' field is valid and should be used for
- * forwarding lookup. Valid for data packets only.
- */
-MLXSW_ITEM32(tx, hdr, fid_valid, 0x00, 16, 1);
-
-/* tx_hdr_swid
- * Switch partition ID. Must be set to 0.
- */
-MLXSW_ITEM32(tx, hdr, swid, 0x00, 12, 3);
-
-/* tx_hdr_control_tclass
- * Indicates if the packet should use the control TClass and not one
- * of the data TClasses.
- */
-MLXSW_ITEM32(tx, hdr, control_tclass, 0x00, 6, 1);
-
-/* tx_hdr_etclass
- * Egress TClass to be used on the egress device on the egress port.
- */
-MLXSW_ITEM32(tx, hdr, etclass, 0x00, 0, 4);
-
-/* tx_hdr_port_mid
- * Destination local port for unicast packets.
- * Destination multicast ID for multicast packets.
- *
- * Control packets are directed to a specific egress port, while data
- * packets are transmitted through the CPU port (0) into the switch partition,
- * where forwarding rules are applied.
- */
-MLXSW_ITEM32(tx, hdr, port_mid, 0x04, 16, 16);
-
-/* tx_hdr_fid
- * Forwarding ID used for L2 forwarding lookup. Valid only if 'fid_valid' is
- * set, otherwise calculated based on the packet's VID using VID to FID mapping.
- * Valid for data packets only.
- */
-MLXSW_ITEM32(tx, hdr, fid, 0x08, 16, 16);
-
-/* tx_hdr_type
- * 0 - Data packets
- * 6 - Control packets
- */
-MLXSW_ITEM32(tx, hdr, type, 0x0C, 0, 4);
-
 int mlxsw_sp_flow_counter_get(struct mlxsw_sp *mlxsw_sp,
 			      unsigned int counter_index, bool clear,
 			      u64 *packets, u64 *bytes)
@@ -233,61 +165,6 @@ void mlxsw_sp_flow_counter_free(struct mlxsw_sp *mlxsw_sp,
 			       counter_index);
 }
 
-void mlxsw_sp_txhdr_construct(struct sk_buff *skb,
-			      const struct mlxsw_tx_info *tx_info)
-{
-	char *txhdr = skb_push(skb, MLXSW_TXHDR_LEN);
-
-	memset(txhdr, 0, MLXSW_TXHDR_LEN);
-
-	mlxsw_tx_hdr_version_set(txhdr, MLXSW_TXHDR_VERSION_1);
-	mlxsw_tx_hdr_ctl_set(txhdr, MLXSW_TXHDR_ETH_CTL);
-	mlxsw_tx_hdr_proto_set(txhdr, MLXSW_TXHDR_PROTO_ETH);
-	mlxsw_tx_hdr_swid_set(txhdr, 0);
-	mlxsw_tx_hdr_control_tclass_set(txhdr, 1);
-	mlxsw_tx_hdr_port_mid_set(txhdr, tx_info->local_port);
-	mlxsw_tx_hdr_type_set(txhdr, MLXSW_TXHDR_TYPE_CONTROL);
-}
-
-int
-mlxsw_sp_txhdr_ptp_data_construct(struct mlxsw_core *mlxsw_core,
-				  struct mlxsw_sp_port *mlxsw_sp_port,
-				  struct sk_buff *skb,
-				  const struct mlxsw_tx_info *tx_info)
-{
-	char *txhdr;
-	u16 max_fid;
-	int err;
-
-	if (skb_cow_head(skb, MLXSW_TXHDR_LEN)) {
-		err = -ENOMEM;
-		goto err_skb_cow_head;
-	}
-
-	if (!MLXSW_CORE_RES_VALID(mlxsw_core, FID)) {
-		err = -EIO;
-		goto err_res_valid;
-	}
-	max_fid = MLXSW_CORE_RES_GET(mlxsw_core, FID);
-
-	txhdr = skb_push(skb, MLXSW_TXHDR_LEN);
-	memset(txhdr, 0, MLXSW_TXHDR_LEN);
-
-	mlxsw_tx_hdr_version_set(txhdr, MLXSW_TXHDR_VERSION_1);
-	mlxsw_tx_hdr_proto_set(txhdr, MLXSW_TXHDR_PROTO_ETH);
-	mlxsw_tx_hdr_rx_is_router_set(txhdr, true);
-	mlxsw_tx_hdr_fid_valid_set(txhdr, true);
-	mlxsw_tx_hdr_fid_set(txhdr, max_fid + tx_info->local_port - 1);
-	mlxsw_tx_hdr_type_set(txhdr, MLXSW_TXHDR_TYPE_DATA);
-	return 0;
-
-err_res_valid:
-err_skb_cow_head:
-	this_cpu_inc(mlxsw_sp_port->pcpu_stats->tx_dropped);
-	dev_kfree_skb_any(skb);
-	return err;
-}
-
 static bool mlxsw_sp_skb_requires_ts(struct sk_buff *skb)
 {
 	unsigned int type;
@@ -299,30 +176,49 @@ static bool mlxsw_sp_skb_requires_ts(struct sk_buff *skb)
 	return !!ptp_parse_header(skb, type);
 }
 
-static int mlxsw_sp_txhdr_handle(struct mlxsw_core *mlxsw_core,
-				 struct mlxsw_sp_port *mlxsw_sp_port,
-				 struct sk_buff *skb,
-				 const struct mlxsw_tx_info *tx_info)
+static void mlxsw_sp_txhdr_info_data_init(struct mlxsw_core *mlxsw_core,
+					  struct sk_buff *skb,
+					  struct mlxsw_txhdr_info *txhdr_info)
 {
-	struct mlxsw_sp *mlxsw_sp = mlxsw_core_driver_priv(mlxsw_core);
+	/* Resource validation was done as part of PTP init. */
+	u16 max_fid = MLXSW_CORE_RES_GET(mlxsw_core, FID);
+
+	txhdr_info->data = true;
+	txhdr_info->max_fid = max_fid;
+}
 
-	/* In Spectrum-2 and Spectrum-3, PTP events that require a time stamp
-	 * need special handling and cannot be transmitted as regular control
-	 * packets.
+static struct sk_buff *
+mlxsw_sp_vlan_tag_push(struct mlxsw_sp *mlxsw_sp, struct sk_buff *skb)
+{
+	/* In some Spectrum ASICs, in order for PTP event packets to have their
+	 * correction field correctly set on the egress port they must be
+	 * transmitted as data packets. Such packets ingress the ASIC via the
+	 * CPU port and must have a VLAN tag, as the CPU port is not configured
+	 * with a PVID. Push the default VLAN (4095), which is configured as
+	 * egress untagged on all the ports.
 	 */
-	if (unlikely(mlxsw_sp_skb_requires_ts(skb)))
-		return mlxsw_sp->ptp_ops->txhdr_construct(mlxsw_core,
-							  mlxsw_sp_port, skb,
-							  tx_info);
+	if (skb_vlan_tagged(skb))
+		return skb;
 
-	if (skb_cow_head(skb, MLXSW_TXHDR_LEN)) {
-		this_cpu_inc(mlxsw_sp_port->pcpu_stats->tx_dropped);
-		dev_kfree_skb_any(skb);
-		return -ENOMEM;
-	}
+	return vlan_insert_tag_set_proto(skb, htons(ETH_P_8021Q),
+					 MLXSW_SP_DEFAULT_VID);
+}
 
-	mlxsw_sp_txhdr_construct(skb, tx_info);
-	return 0;
+static struct sk_buff *
+mlxsw_sp_txhdr_preparations(struct mlxsw_sp *mlxsw_sp, struct sk_buff *skb,
+			    struct mlxsw_txhdr_info *txhdr_info)
+{
+	if (likely(!mlxsw_sp_skb_requires_ts(skb)))
+		return skb;
+
+	if (!mlxsw_sp->ptp_ops->tx_as_data)
+		return skb;
+
+	/* Special handling for PTP events that require a time stamp and cannot
+	 * be transmitted as regular control packets.
+	 */
+	mlxsw_sp_txhdr_info_data_init(mlxsw_sp->core, skb, txhdr_info);
+	return mlxsw_sp_vlan_tag_push(mlxsw_sp, skb);
 }
 
 enum mlxsw_reg_spms_state mlxsw_sp_stp_spms_state(u8 state)
@@ -721,16 +617,16 @@ static netdev_tx_t mlxsw_sp_port_xmit(struct sk_buff *skb,
 	struct mlxsw_sp_port *mlxsw_sp_port = netdev_priv(dev);
 	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
 	struct mlxsw_sp_port_pcpu_stats *pcpu_stats;
-	const struct mlxsw_tx_info tx_info = {
-		.local_port = mlxsw_sp_port->local_port,
-		.is_emad = false,
+	struct mlxsw_txhdr_info txhdr_info = {
+		.tx_info.local_port = mlxsw_sp_port->local_port,
+		.tx_info.is_emad = false,
 	};
 	u64 len;
 	int err;
 
 	memset(skb->cb, 0, sizeof(struct mlxsw_skb_cb));
 
-	if (mlxsw_core_skb_transmit_busy(mlxsw_sp->core, &tx_info))
+	if (mlxsw_core_skb_transmit_busy(mlxsw_sp->core, &txhdr_info.tx_info))
 		return NETDEV_TX_BUSY;
 
 	if (eth_skb_pad(skb)) {
@@ -738,10 +634,11 @@ static netdev_tx_t mlxsw_sp_port_xmit(struct sk_buff *skb,
 		return NETDEV_TX_OK;
 	}
 
-	err = mlxsw_sp_txhdr_handle(mlxsw_sp->core, mlxsw_sp_port, skb,
-				    &tx_info);
-	if (err)
+	skb = mlxsw_sp_txhdr_preparations(mlxsw_sp, skb, &txhdr_info);
+	if (!skb) {
+		this_cpu_inc(mlxsw_sp_port->pcpu_stats->tx_dropped);
 		return NETDEV_TX_OK;
+	}
 
 	/* TX header is consumed by HW on the way so we shouldn't count its
 	 * bytes as being sent.
@@ -751,7 +648,7 @@ static netdev_tx_t mlxsw_sp_port_xmit(struct sk_buff *skb,
 	/* Due to a race we might fail here because of a full queue. In that
 	 * unlikely case we simply drop the packet.
 	 */
-	err = mlxsw_core_skb_transmit(mlxsw_sp->core, skb, &tx_info);
+	err = mlxsw_core_skb_transmit(mlxsw_sp->core, skb, &txhdr_info);
 
 	if (!err) {
 		pcpu_stats = this_cpu_ptr(mlxsw_sp_port->pcpu_stats);
@@ -2792,7 +2689,6 @@ static const struct mlxsw_sp_ptp_ops mlxsw_sp1_ptp_ops = {
 	.get_stats_count = mlxsw_sp1_get_stats_count,
 	.get_stats_strings = mlxsw_sp1_get_stats_strings,
 	.get_stats	= mlxsw_sp1_get_stats,
-	.txhdr_construct = mlxsw_sp_ptp_txhdr_construct,
 };
 
 static const struct mlxsw_sp_ptp_ops mlxsw_sp2_ptp_ops = {
@@ -2811,7 +2707,7 @@ static const struct mlxsw_sp_ptp_ops mlxsw_sp2_ptp_ops = {
 	.get_stats_count = mlxsw_sp2_get_stats_count,
 	.get_stats_strings = mlxsw_sp2_get_stats_strings,
 	.get_stats	= mlxsw_sp2_get_stats,
-	.txhdr_construct = mlxsw_sp2_ptp_txhdr_construct,
+	.tx_as_data     = true,
 };
 
 static const struct mlxsw_sp_ptp_ops mlxsw_sp4_ptp_ops = {
@@ -2830,7 +2726,6 @@ static const struct mlxsw_sp_ptp_ops mlxsw_sp4_ptp_ops = {
 	.get_stats_count = mlxsw_sp2_get_stats_count,
 	.get_stats_strings = mlxsw_sp2_get_stats_strings,
 	.get_stats	= mlxsw_sp2_get_stats,
-	.txhdr_construct = mlxsw_sp_ptp_txhdr_construct,
 };
 
 struct mlxsw_sp_sample_trigger_node {
@@ -3992,11 +3887,9 @@ static struct mlxsw_driver mlxsw_sp1_driver = {
 	.trap_policer_fini		= mlxsw_sp_trap_policer_fini,
 	.trap_policer_set		= mlxsw_sp_trap_policer_set,
 	.trap_policer_counter_get	= mlxsw_sp_trap_policer_counter_get,
-	.txhdr_construct		= mlxsw_sp_txhdr_construct,
 	.resources_register		= mlxsw_sp1_resources_register,
 	.kvd_sizes_get			= mlxsw_sp_kvd_sizes_get,
 	.ptp_transmitted		= mlxsw_sp_ptp_transmitted,
-	.txhdr_len			= MLXSW_TXHDR_LEN,
 	.profile			= &mlxsw_sp1_config_profile,
 	.sdq_supports_cqe_v2		= false,
 };
@@ -4030,10 +3923,8 @@ static struct mlxsw_driver mlxsw_sp2_driver = {
 	.trap_policer_fini		= mlxsw_sp_trap_policer_fini,
 	.trap_policer_set		= mlxsw_sp_trap_policer_set,
 	.trap_policer_counter_get	= mlxsw_sp_trap_policer_counter_get,
-	.txhdr_construct		= mlxsw_sp_txhdr_construct,
 	.resources_register		= mlxsw_sp2_resources_register,
 	.ptp_transmitted		= mlxsw_sp_ptp_transmitted,
-	.txhdr_len			= MLXSW_TXHDR_LEN,
 	.profile			= &mlxsw_sp2_config_profile,
 	.sdq_supports_cqe_v2		= true,
 };
@@ -4067,10 +3958,8 @@ static struct mlxsw_driver mlxsw_sp3_driver = {
 	.trap_policer_fini		= mlxsw_sp_trap_policer_fini,
 	.trap_policer_set		= mlxsw_sp_trap_policer_set,
 	.trap_policer_counter_get	= mlxsw_sp_trap_policer_counter_get,
-	.txhdr_construct		= mlxsw_sp_txhdr_construct,
 	.resources_register		= mlxsw_sp2_resources_register,
 	.ptp_transmitted		= mlxsw_sp_ptp_transmitted,
-	.txhdr_len			= MLXSW_TXHDR_LEN,
 	.profile			= &mlxsw_sp2_config_profile,
 	.sdq_supports_cqe_v2		= true,
 };
@@ -4102,10 +3991,8 @@ static struct mlxsw_driver mlxsw_sp4_driver = {
 	.trap_policer_fini		= mlxsw_sp_trap_policer_fini,
 	.trap_policer_set		= mlxsw_sp_trap_policer_set,
 	.trap_policer_counter_get	= mlxsw_sp_trap_policer_counter_get,
-	.txhdr_construct		= mlxsw_sp_txhdr_construct,
 	.resources_register		= mlxsw_sp2_resources_register,
 	.ptp_transmitted		= mlxsw_sp_ptp_transmitted,
-	.txhdr_len			= MLXSW_TXHDR_LEN,
 	.profile			= &mlxsw_sp4_config_profile,
 	.sdq_supports_cqe_v2		= true,
 };
diff --git a/drivers/net/ethernet/mellanox/mlxsw/spectrum.h b/drivers/net/ethernet/mellanox/mlxsw/spectrum.h
index 8d3c61287696..b10f80fc651b 100644
--- a/drivers/net/ethernet/mellanox/mlxsw/spectrum.h
+++ b/drivers/net/ethernet/mellanox/mlxsw/spectrum.h
@@ -243,10 +243,7 @@ struct mlxsw_sp_ptp_ops {
 	void (*get_stats_strings)(u8 **p);
 	void (*get_stats)(struct mlxsw_sp_port *mlxsw_sp_port,
 			  u64 *data, int data_index);
-	int (*txhdr_construct)(struct mlxsw_core *mlxsw_core,
-			       struct mlxsw_sp_port *mlxsw_sp_port,
-			       struct sk_buff *skb,
-			       const struct mlxsw_tx_info *tx_info);
+	bool tx_as_data;
 };
 
 struct mlxsw_sp_fid_core_ops {
@@ -711,12 +708,6 @@ int mlxsw_sp_flow_counter_alloc(struct mlxsw_sp *mlxsw_sp,
 				unsigned int *p_counter_index);
 void mlxsw_sp_flow_counter_free(struct mlxsw_sp *mlxsw_sp,
 				unsigned int counter_index);
-void mlxsw_sp_txhdr_construct(struct sk_buff *skb,
-			      const struct mlxsw_tx_info *tx_info);
-int mlxsw_sp_txhdr_ptp_data_construct(struct mlxsw_core *mlxsw_core,
-				      struct mlxsw_sp_port *mlxsw_sp_port,
-				      struct sk_buff *skb,
-				      const struct mlxsw_tx_info *tx_info);
 bool mlxsw_sp_port_dev_check(const struct net_device *dev);
 struct mlxsw_sp *mlxsw_sp_lower_get(struct net_device *dev);
 struct mlxsw_sp_port *mlxsw_sp_port_dev_lower_find(struct net_device *dev);
diff --git a/drivers/net/ethernet/mellanox/mlxsw/spectrum_ptp.c b/drivers/net/ethernet/mellanox/mlxsw/spectrum_ptp.c
index d94081c7658e..ca8b9d18fbb9 100644
--- a/drivers/net/ethernet/mellanox/mlxsw/spectrum_ptp.c
+++ b/drivers/net/ethernet/mellanox/mlxsw/spectrum_ptp.c
@@ -1353,6 +1353,10 @@ struct mlxsw_sp_ptp_state *mlxsw_sp2_ptp_init(struct mlxsw_sp *mlxsw_sp)
 	struct mlxsw_sp2_ptp_state *ptp_state;
 	int err;
 
+	/* Max FID will be used in data path, check validity as part of init. */
+	if (!MLXSW_CORE_RES_VALID(mlxsw_sp->core, FID))
+		return ERR_PTR(-EIO);
+
 	ptp_state = kzalloc(sizeof(*ptp_state), GFP_KERNEL);
 	if (!ptp_state)
 		return ERR_PTR(-ENOMEM);
@@ -1679,43 +1683,3 @@ int mlxsw_sp2_ptp_get_ts_info(struct mlxsw_sp *mlxsw_sp,
 
 	return 0;
 }
-
-int mlxsw_sp_ptp_txhdr_construct(struct mlxsw_core *mlxsw_core,
-				 struct mlxsw_sp_port *mlxsw_sp_port,
-				 struct sk_buff *skb,
-				 const struct mlxsw_tx_info *tx_info)
-{
-	if (skb_cow_head(skb, MLXSW_TXHDR_LEN)) {
-		this_cpu_inc(mlxsw_sp_port->pcpu_stats->tx_dropped);
-		dev_kfree_skb_any(skb);
-		return -ENOMEM;
-	}
-
-	mlxsw_sp_txhdr_construct(skb, tx_info);
-	return 0;
-}
-
-int mlxsw_sp2_ptp_txhdr_construct(struct mlxsw_core *mlxsw_core,
-				  struct mlxsw_sp_port *mlxsw_sp_port,
-				  struct sk_buff *skb,
-				  const struct mlxsw_tx_info *tx_info)
-{
-	/* In Spectrum-2 and Spectrum-3, in order for PTP event packets to have
-	 * their correction field correctly set on the egress port they must be
-	 * transmitted as data packets. Such packets ingress the ASIC via the
-	 * CPU port and must have a VLAN tag, as the CPU port is not configured
-	 * with a PVID. Push the default VLAN (4095), which is configured as
-	 * egress untagged on all the ports.
-	 */
-	if (!skb_vlan_tagged(skb)) {
-		skb = vlan_insert_tag_set_proto(skb, htons(ETH_P_8021Q),
-						MLXSW_SP_DEFAULT_VID);
-		if (!skb) {
-			this_cpu_inc(mlxsw_sp_port->pcpu_stats->tx_dropped);
-			return -ENOMEM;
-		}
-	}
-
-	return mlxsw_sp_txhdr_ptp_data_construct(mlxsw_core, mlxsw_sp_port, skb,
-						 tx_info);
-}
diff --git a/drivers/net/ethernet/mellanox/mlxsw/spectrum_ptp.h b/drivers/net/ethernet/mellanox/mlxsw/spectrum_ptp.h
index c8aa1452fbb9..102db9060135 100644
--- a/drivers/net/ethernet/mellanox/mlxsw/spectrum_ptp.h
+++ b/drivers/net/ethernet/mellanox/mlxsw/spectrum_ptp.h
@@ -49,11 +49,6 @@ void mlxsw_sp1_get_stats_strings(u8 **p);
 void mlxsw_sp1_get_stats(struct mlxsw_sp_port *mlxsw_sp_port,
 			 u64 *data, int data_index);
 
-int mlxsw_sp_ptp_txhdr_construct(struct mlxsw_core *mlxsw_core,
-				 struct mlxsw_sp_port *mlxsw_sp_port,
-				 struct sk_buff *skb,
-				 const struct mlxsw_tx_info *tx_info);
-
 struct mlxsw_sp_ptp_clock *
 mlxsw_sp2_ptp_clock_init(struct mlxsw_sp *mlxsw_sp, struct device *dev);
 
@@ -78,11 +73,6 @@ int mlxsw_sp2_ptp_hwtstamp_set(struct mlxsw_sp_port *mlxsw_sp_port,
 int mlxsw_sp2_ptp_get_ts_info(struct mlxsw_sp *mlxsw_sp,
 			      struct kernel_ethtool_ts_info *info);
 
-int mlxsw_sp2_ptp_txhdr_construct(struct mlxsw_core *mlxsw_core,
-				  struct mlxsw_sp_port *mlxsw_sp_port,
-				  struct sk_buff *skb,
-				  const struct mlxsw_tx_info *tx_info);
-
 #else
 
 static inline struct mlxsw_sp_ptp_clock *
@@ -157,15 +147,6 @@ static inline void mlxsw_sp1_get_stats(struct mlxsw_sp_port *mlxsw_sp_port,
 {
 }
 
-static inline int
-mlxsw_sp_ptp_txhdr_construct(struct mlxsw_core *mlxsw_core,
-			     struct mlxsw_sp_port *mlxsw_sp_port,
-			     struct sk_buff *skb,
-			     const struct mlxsw_tx_info *tx_info)
-{
-	return -EOPNOTSUPP;
-}
-
 static inline struct mlxsw_sp_ptp_clock *
 mlxsw_sp2_ptp_clock_init(struct mlxsw_sp *mlxsw_sp, struct device *dev)
 {
@@ -211,15 +192,6 @@ mlxsw_sp2_ptp_hwtstamp_set(struct mlxsw_sp_port *mlxsw_sp_port,
 {
 	return -EOPNOTSUPP;
 }
-
-static inline int
-mlxsw_sp2_ptp_txhdr_construct(struct mlxsw_core *mlxsw_core,
-			      struct mlxsw_sp_port *mlxsw_sp_port,
-			      struct sk_buff *skb,
-			      const struct mlxsw_tx_info *tx_info)
-{
-	return -EOPNOTSUPP;
-}
 #endif
 
 static inline void mlxsw_sp2_ptp_shaper_work(struct work_struct *work)
diff --git a/drivers/net/ethernet/mellanox/mlxsw/txheader.h b/drivers/net/ethernet/mellanox/mlxsw/txheader.h
index da51dd9d5e44..e78cba5821b6 100644
--- a/drivers/net/ethernet/mellanox/mlxsw/txheader.h
+++ b/drivers/net/ethernet/mellanox/mlxsw/txheader.h
@@ -4,6 +4,69 @@
 #ifndef _MLXSW_TXHEADER_H
 #define _MLXSW_TXHEADER_H
 
+/* tx_hdr_version
+ * Tx header version.
+ * Must be set to 1.
+ */
+MLXSW_ITEM32(tx, hdr, version, 0x00, 28, 4);
+
+/* tx_hdr_ctl
+ * Packet control type.
+ * 0 - Ethernet control (e.g. EMADs, LACP)
+ * 1 - Ethernet data
+ */
+MLXSW_ITEM32(tx, hdr, ctl, 0x00, 26, 2);
+
+/* tx_hdr_proto
+ * Packet protocol type. Must be set to 1 (Ethernet).
+ */
+MLXSW_ITEM32(tx, hdr, proto, 0x00, 21, 3);
+
+/* tx_hdr_rx_is_router
+ * Packet is sent from the router. Valid for data packets only.
+ */
+MLXSW_ITEM32(tx, hdr, rx_is_router, 0x00, 19, 1);
+
+/* tx_hdr_fid_valid
+ * Indicates if the 'fid' field is valid and should be used for
+ * forwarding lookup. Valid for data packets only.
+ */
+MLXSW_ITEM32(tx, hdr, fid_valid, 0x00, 16, 1);
+
+/* tx_hdr_swid
+ * Switch partition ID. Must be set to 0.
+ */
+MLXSW_ITEM32(tx, hdr, swid, 0x00, 12, 3);
+
+/* tx_hdr_control_tclass
+ * Indicates if the packet should use the control TClass and not one
+ * of the data TClasses.
+ */
+MLXSW_ITEM32(tx, hdr, control_tclass, 0x00, 6, 1);
+
+/* tx_hdr_port_mid
+ * Destination local port for unicast packets.
+ * Destination multicast ID for multicast packets.
+ *
+ * Control packets are directed to a specific egress port, while data
+ * packets are transmitted through the CPU port (0) into the switch partition,
+ * where forwarding rules are applied.
+ */
+MLXSW_ITEM32(tx, hdr, port_mid, 0x04, 16, 16);
+
+/* tx_hdr_fid
+ * Forwarding ID used for L2 forwarding lookup. Valid only if 'fid_valid' is
+ * set, otherwise calculated based on the packet's VID using VID to FID mapping.
+ * Valid for data packets only.
+ */
+MLXSW_ITEM32(tx, hdr, fid, 0x08, 16, 16);
+
+/* tx_hdr_type
+ * 0 - Data packets
+ * 6 - Control packets
+ */
+MLXSW_ITEM32(tx, hdr, type, 0x0C, 0, 4);
+
 #define MLXSW_TXHDR_LEN 0x10
 #define MLXSW_TXHDR_VERSION_0 0
 #define MLXSW_TXHDR_VERSION_1 1