diff mbox series

[v3,net-next,10/15] net: mscc: ocelot: refactor ocelot_port_inject_frame out of ocelot_port_xmit

Message ID 20210118161731.2837700-11-olteanv@gmail.com (mailing list archive)
State Superseded
Delegated to: Netdev Maintainers
Headers show
Series tag_8021q for Ocelot switches | expand

Checks

Context Check Description
netdev/cover_letter success Link
netdev/fixes_present success Link
netdev/patch_count success Link
netdev/tree_selection success Clearly marked for net-next
netdev/subject_prefix success Link
netdev/cc_maintainers warning 1 maintainers not CCed: vladimir.oltean@nxp.com
netdev/source_inline success Was 0 now: 0
netdev/verify_signedoff success Link
netdev/module_param success Was 0 now: 0
netdev/build_32bit fail Errors and warnings before: 0 this patch: 5
netdev/kdoc success Errors and warnings before: 0 this patch: 0
netdev/verify_fixes success Link
netdev/checkpatch warning WARNING: line length of 93 exceeds 80 columns
netdev/build_allmodconfig_warn fail Errors and warnings before: 0 this patch: 5
netdev/header_inline success Link
netdev/stable success Stable not CCed

Commit Message

Vladimir Oltean Jan. 18, 2021, 4:17 p.m. UTC 
From: Vladimir Oltean <vladimir.oltean@nxp.com>

The felix DSA driver will inject some frames through register MMIO, same
as ocelot switchdev currently does. So we need to be able to reuse the
common code.

Signed-off-by: Vladimir Oltean <vladimir.oltean@nxp.com>
---
Changes in v3:
None.

Changes in v2:
Patch is new.

 drivers/net/ethernet/mscc/ocelot.c     | 78 +++++++++++++++++++++++++
 drivers/net/ethernet/mscc/ocelot.h     |  4 ++
 drivers/net/ethernet/mscc/ocelot_net.c | 81 +++-----------------------
 3 files changed, 89 insertions(+), 74 deletions(-)

Comments

Vladimir Oltean Jan. 18, 2021, 8:15 p.m. UTC | #1 
On Mon, Jan 18, 2021 at 06:17:26PM +0200, Vladimir Oltean wrote:
> From: Vladimir Oltean <vladimir.oltean@nxp.com>
> 
> The felix DSA driver will inject some frames through register MMIO, same
> as ocelot switchdev currently does. So we need to be able to reuse the
> common code.
> 
> Signed-off-by: Vladimir Oltean <vladimir.oltean@nxp.com>
> ---

Sadly there are some build errors starting with this patch when
drivers/net/ethernet/mscc/mscc_ocelot.ko is built as module.
They are due to me not using EXPORT_SYMBOL on the refactored functions
that were moved inside the common mscc_ocelot_switch_lib. I am adding
the EXPORT_SYMBOL later (in the last patch I think) but it needs to be
done now.

> Changes in v3:
> None.
> 
> Changes in v2:
> Patch is new.
> 
>  drivers/net/ethernet/mscc/ocelot.c     | 78 +++++++++++++++++++++++++
>  drivers/net/ethernet/mscc/ocelot.h     |  4 ++
>  drivers/net/ethernet/mscc/ocelot_net.c | 81 +++-----------------------
>  3 files changed, 89 insertions(+), 74 deletions(-)
> 
> diff --git a/drivers/net/ethernet/mscc/ocelot.c b/drivers/net/ethernet/mscc/ocelot.c
> index 895df050abba..7aba384fe6bf 100644
> --- a/drivers/net/ethernet/mscc/ocelot.c
> +++ b/drivers/net/ethernet/mscc/ocelot.c
> @@ -561,6 +561,84 @@ void ocelot_get_txtstamp(struct ocelot *ocelot)
>  }
>  EXPORT_SYMBOL(ocelot_get_txtstamp);
>  
> +/* Generate the IFH for frame injection
> + *
> + * The IFH is a 128bit-value
> + * bit 127: bypass the analyzer processing
> + * bit 56-67: destination mask
> + * bit 28-29: pop_cnt: 3 disables all rewriting of the frame
> + * bit 20-27: cpu extraction queue mask
> + * bit 16: tag type 0: C-tag, 1: S-tag
> + * bit 0-11: VID
> + */
> +static int ocelot_gen_ifh(u32 *ifh, struct frame_info *info)
> +{
> +	ifh[0] = IFH_INJ_BYPASS | ((0x1ff & info->rew_op) << 21);
> +	ifh[1] = (0xf00 & info->port) >> 8;
> +	ifh[2] = (0xff & info->port) << 24;
> +	ifh[3] = (info->tag_type << 16) | info->vid;
> +
> +	return 0;
> +}
> +
> +bool ocelot_can_inject(struct ocelot *ocelot, int grp)
> +{
> +	u32 val = ocelot_read(ocelot, QS_INJ_STATUS);
> +
> +	if (!(val & QS_INJ_STATUS_FIFO_RDY(BIT(grp))))
> +		return false;
> +	if (val & QS_INJ_STATUS_WMARK_REACHED(BIT(grp)))
> +		return false;
> +
> +	return true;
> +}
> +
> +void ocelot_port_inject_frame(struct ocelot *ocelot, int port, int grp,
> +			      u32 rew_op, struct sk_buff *skb)
> +{
> +	struct frame_info info = {};
> +	u32 ifh[OCELOT_TAG_LEN / 4];
> +	unsigned int i, count, last;
> +
> +	ocelot_write_rix(ocelot, QS_INJ_CTRL_GAP_SIZE(1) |
> +			 QS_INJ_CTRL_SOF, QS_INJ_CTRL, grp);
> +
> +	info.port = BIT(port);
> +	info.tag_type = IFH_TAG_TYPE_C;
> +	info.vid = skb_vlan_tag_get(skb);
> +	info.rew_op = rew_op;
> +
> +	ocelot_gen_ifh(ifh, &info);
> +
> +	for (i = 0; i < OCELOT_TAG_LEN / 4; i++)
> +		ocelot_write_rix(ocelot, (__force u32)cpu_to_be32(ifh[i]),
> +				 QS_INJ_WR, grp);
> +
> +	count = DIV_ROUND_UP(skb->len, 4);
> +	last = skb->len % 4;
> +	for (i = 0; i < count; i++)
> +		ocelot_write_rix(ocelot, ((u32 *)skb->data)[i], QS_INJ_WR, grp);
> +
> +	/* Add padding */
> +	while (i < (OCELOT_BUFFER_CELL_SZ / 4)) {
> +		ocelot_write_rix(ocelot, 0, QS_INJ_WR, grp);
> +		i++;
> +	}
> +
> +	/* Indicate EOF and valid bytes in last word */
> +	ocelot_write_rix(ocelot, QS_INJ_CTRL_GAP_SIZE(1) |
> +			 QS_INJ_CTRL_VLD_BYTES(skb->len < OCELOT_BUFFER_CELL_SZ ? 0 : last) |
> +			 QS_INJ_CTRL_EOF,
> +			 QS_INJ_CTRL, grp);
> +
> +	/* Add dummy CRC */
> +	ocelot_write_rix(ocelot, 0, QS_INJ_WR, grp);
> +	skb_tx_timestamp(skb);
> +
> +	skb->dev->stats.tx_packets++;
> +	skb->dev->stats.tx_bytes += skb->len;
> +}
> +
>  int ocelot_fdb_add(struct ocelot *ocelot, int port,
>  		   const unsigned char *addr, u16 vid)
>  {
> diff --git a/drivers/net/ethernet/mscc/ocelot.h b/drivers/net/ethernet/mscc/ocelot.h
> index e8621dbc14f7..cf6493e55eab 100644
> --- a/drivers/net/ethernet/mscc/ocelot.h
> +++ b/drivers/net/ethernet/mscc/ocelot.h
> @@ -127,6 +127,10 @@ int ocelot_port_devlink_init(struct ocelot *ocelot, int port,
>  			     enum devlink_port_flavour flavour);
>  void ocelot_port_devlink_teardown(struct ocelot *ocelot, int port);
>  
> +bool ocelot_can_inject(struct ocelot *ocelot, int grp);
> +void ocelot_port_inject_frame(struct ocelot *ocelot, int port, int grp,
> +			      u32 rew_op, struct sk_buff *skb);
> +
>  extern struct notifier_block ocelot_netdevice_nb;
>  extern struct notifier_block ocelot_switchdev_nb;
>  extern struct notifier_block ocelot_switchdev_blocking_nb;
> diff --git a/drivers/net/ethernet/mscc/ocelot_net.c b/drivers/net/ethernet/mscc/ocelot_net.c
> index 55847d2a83e1..9a29d7d3b0e2 100644
> --- a/drivers/net/ethernet/mscc/ocelot_net.c
> +++ b/drivers/net/ethernet/mscc/ocelot_net.c
> @@ -488,53 +488,20 @@ static int ocelot_port_stop(struct net_device *dev)
>  	return 0;
>  }
>  
> -/* Generate the IFH for frame injection
> - *
> - * The IFH is a 128bit-value
> - * bit 127: bypass the analyzer processing
> - * bit 56-67: destination mask
> - * bit 28-29: pop_cnt: 3 disables all rewriting of the frame
> - * bit 20-27: cpu extraction queue mask
> - * bit 16: tag type 0: C-tag, 1: S-tag
> - * bit 0-11: VID
> - */
> -static int ocelot_gen_ifh(u32 *ifh, struct frame_info *info)
> -{
> -	ifh[0] = IFH_INJ_BYPASS | ((0x1ff & info->rew_op) << 21);
> -	ifh[1] = (0xf00 & info->port) >> 8;
> -	ifh[2] = (0xff & info->port) << 24;
> -	ifh[3] = (info->tag_type << 16) | info->vid;
> -
> -	return 0;
> -}
> -
> -static int ocelot_port_xmit(struct sk_buff *skb, struct net_device *dev)
> +static netdev_tx_t ocelot_port_xmit(struct sk_buff *skb, struct net_device *dev)
>  {
>  	struct ocelot_port_private *priv = netdev_priv(dev);
> -	struct skb_shared_info *shinfo = skb_shinfo(skb);
>  	struct ocelot_port *ocelot_port = &priv->port;
>  	struct ocelot *ocelot = ocelot_port->ocelot;
> -	u32 val, ifh[OCELOT_TAG_LEN / 4];
> -	struct frame_info info = {};
> -	u8 grp = 0; /* Send everything on CPU group 0 */
> -	unsigned int i, count, last;
>  	int port = priv->chip_port;
> +	u32 rew_op = 0;
>  
> -	val = ocelot_read(ocelot, QS_INJ_STATUS);
> -	if (!(val & QS_INJ_STATUS_FIFO_RDY(BIT(grp))) ||
> -	    (val & QS_INJ_STATUS_WMARK_REACHED(BIT(grp))))
> +	if (!ocelot_can_inject(ocelot, 0))
>  		return NETDEV_TX_BUSY;
>  
> -	ocelot_write_rix(ocelot, QS_INJ_CTRL_GAP_SIZE(1) |
> -			 QS_INJ_CTRL_SOF, QS_INJ_CTRL, grp);
> -
> -	info.port = BIT(port);
> -	info.tag_type = IFH_TAG_TYPE_C;
> -	info.vid = skb_vlan_tag_get(skb);
> -
>  	/* Check if timestamping is needed */
> -	if (ocelot->ptp && (shinfo->tx_flags & SKBTX_HW_TSTAMP)) {
> -		info.rew_op = ocelot_port->ptp_cmd;
> +	if (ocelot->ptp && (skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP)) {
> +		rew_op = ocelot_port->ptp_cmd;
>  
>  		if (ocelot_port->ptp_cmd == IFH_REW_OP_TWO_STEP_PTP) {
>  			struct sk_buff *clone;
> @@ -547,45 +514,11 @@ static int ocelot_port_xmit(struct sk_buff *skb, struct net_device *dev)
>  
>  			ocelot_port_add_txtstamp_skb(ocelot, port, clone);
>  
> -			info.rew_op |= clone->cb[0] << 3;
> +			rew_op |= clone->cb[0] << 3;
>  		}
>  	}
>  
> -	if (ocelot->ptp && shinfo->tx_flags & SKBTX_HW_TSTAMP) {
> -		info.rew_op = ocelot_port->ptp_cmd;
> -		if (ocelot_port->ptp_cmd == IFH_REW_OP_TWO_STEP_PTP)
> -			info.rew_op |= skb->cb[0] << 3;
> -	}
> -
> -	ocelot_gen_ifh(ifh, &info);
> -
> -	for (i = 0; i < OCELOT_TAG_LEN / 4; i++)
> -		ocelot_write_rix(ocelot, (__force u32)cpu_to_be32(ifh[i]),
> -				 QS_INJ_WR, grp);
> -
> -	count = DIV_ROUND_UP(skb->len, 4);
> -	last = skb->len % 4;
> -	for (i = 0; i < count; i++)
> -		ocelot_write_rix(ocelot, ((u32 *)skb->data)[i], QS_INJ_WR, grp);
> -
> -	/* Add padding */
> -	while (i < (OCELOT_BUFFER_CELL_SZ / 4)) {
> -		ocelot_write_rix(ocelot, 0, QS_INJ_WR, grp);
> -		i++;
> -	}
> -
> -	/* Indicate EOF and valid bytes in last word */
> -	ocelot_write_rix(ocelot, QS_INJ_CTRL_GAP_SIZE(1) |
> -			 QS_INJ_CTRL_VLD_BYTES(skb->len < OCELOT_BUFFER_CELL_SZ ? 0 : last) |
> -			 QS_INJ_CTRL_EOF,
> -			 QS_INJ_CTRL, grp);
> -
> -	/* Add dummy CRC */
> -	ocelot_write_rix(ocelot, 0, QS_INJ_WR, grp);
> -	skb_tx_timestamp(skb);
> -
> -	dev->stats.tx_packets++;
> -	dev->stats.tx_bytes += skb->len;
> +	ocelot_port_inject_frame(ocelot, port, 0, rew_op, skb);
>  
>  	kfree_skb(skb);
>  
> -- 
> 2.25.1
>
kernel test robot Jan. 19, 2021, 12:51 a.m. UTC | #2 
Hi Vladimir,

I love your patch! Yet something to improve:

[auto build test ERROR on net-next/master]

url:    https://github.com/0day-ci/linux/commits/Vladimir-Oltean/tag_8021q-for-Ocelot-switches/20210119-004520
base:   https://git.kernel.org/pub/scm/linux/kernel/git/davem/net-next.git 213b97b125808aa220ba460a7af09d8f3772f669
config: sh-allmodconfig (attached as .config)
compiler: sh4-linux-gcc (GCC) 9.3.0
reproduce (this is a W=1 build):
        wget https://raw.githubusercontent.com/intel/lkp-tests/master/sbin/make.cross -O ~/bin/make.cross
        chmod +x ~/bin/make.cross
        # https://github.com/0day-ci/linux/commit/b0c94dae99772294285cc3219500b73026853e83
        git remote add linux-review https://github.com/0day-ci/linux
        git fetch --no-tags linux-review Vladimir-Oltean/tag_8021q-for-Ocelot-switches/20210119-004520
        git checkout b0c94dae99772294285cc3219500b73026853e83
        # save the attached .config to linux build tree
        COMPILER_INSTALL_PATH=$HOME/0day COMPILER=gcc-9.3.0 make.cross ARCH=sh 

If you fix the issue, kindly add following tag as appropriate
Reported-by: kernel test robot <lkp@intel.com>

All errors (new ones prefixed by >>, old ones prefixed by <<):

>> ERROR: modpost: "ocelot_can_inject" [drivers/net/ethernet/mscc/mscc_ocelot.ko] undefined!
ERROR: modpost: "ocelot_port_inject_frame" [drivers/net/ethernet/mscc/mscc_ocelot.ko] undefined!
ERROR: modpost: "__delay" [drivers/net/mdio/mdio-cavium.ko] undefined!
ERROR: modpost: "__udivdi3" [fs/btrfs/btrfs.ko] undefined!
ERROR: modpost: "__umoddi3" [fs/btrfs/btrfs.ko] undefined!

Kconfig warnings: (for reference only)
   WARNING: unmet direct dependencies detected for SND_ATMEL_SOC_PDC
   Depends on SOUND && !UML && SND && SND_SOC && SND_ATMEL_SOC && HAS_DMA
   Selected by
   - SND_ATMEL_SOC_SSC && SOUND && !UML && SND && SND_SOC && SND_ATMEL_SOC
   - SND_ATMEL_SOC_SSC_PDC && SOUND && !UML && SND && SND_SOC && SND_ATMEL_SOC && ATMEL_SSC

---
0-DAY CI Kernel Test Service, Intel Corporation
https://lists.01.org/hyperkitty/list/kbuild-all@lists.01.org
diff mbox series

Patch

diff --git a/drivers/net/ethernet/mscc/ocelot.c b/drivers/net/ethernet/mscc/ocelot.c
index 895df050abba..7aba384fe6bf 100644
--- a/drivers/net/ethernet/mscc/ocelot.c
+++ b/drivers/net/ethernet/mscc/ocelot.c
@@ -561,6 +561,84 @@  void ocelot_get_txtstamp(struct ocelot *ocelot)
 }
 EXPORT_SYMBOL(ocelot_get_txtstamp);
 
+/* Generate the IFH for frame injection
+ *
+ * The IFH is a 128bit-value
+ * bit 127: bypass the analyzer processing
+ * bit 56-67: destination mask
+ * bit 28-29: pop_cnt: 3 disables all rewriting of the frame
+ * bit 20-27: cpu extraction queue mask
+ * bit 16: tag type 0: C-tag, 1: S-tag
+ * bit 0-11: VID
+ */
+static int ocelot_gen_ifh(u32 *ifh, struct frame_info *info)
+{
+	ifh[0] = IFH_INJ_BYPASS | ((0x1ff & info->rew_op) << 21);
+	ifh[1] = (0xf00 & info->port) >> 8;
+	ifh[2] = (0xff & info->port) << 24;
+	ifh[3] = (info->tag_type << 16) | info->vid;
+
+	return 0;
+}
+
+bool ocelot_can_inject(struct ocelot *ocelot, int grp)
+{
+	u32 val = ocelot_read(ocelot, QS_INJ_STATUS);
+
+	if (!(val & QS_INJ_STATUS_FIFO_RDY(BIT(grp))))
+		return false;
+	if (val & QS_INJ_STATUS_WMARK_REACHED(BIT(grp)))
+		return false;
+
+	return true;
+}
+
+void ocelot_port_inject_frame(struct ocelot *ocelot, int port, int grp,
+			      u32 rew_op, struct sk_buff *skb)
+{
+	struct frame_info info = {};
+	u32 ifh[OCELOT_TAG_LEN / 4];
+	unsigned int i, count, last;
+
+	ocelot_write_rix(ocelot, QS_INJ_CTRL_GAP_SIZE(1) |
+			 QS_INJ_CTRL_SOF, QS_INJ_CTRL, grp);
+
+	info.port = BIT(port);
+	info.tag_type = IFH_TAG_TYPE_C;
+	info.vid = skb_vlan_tag_get(skb);
+	info.rew_op = rew_op;
+
+	ocelot_gen_ifh(ifh, &info);
+
+	for (i = 0; i < OCELOT_TAG_LEN / 4; i++)
+		ocelot_write_rix(ocelot, (__force u32)cpu_to_be32(ifh[i]),
+				 QS_INJ_WR, grp);
+
+	count = DIV_ROUND_UP(skb->len, 4);
+	last = skb->len % 4;
+	for (i = 0; i < count; i++)
+		ocelot_write_rix(ocelot, ((u32 *)skb->data)[i], QS_INJ_WR, grp);
+
+	/* Add padding */
+	while (i < (OCELOT_BUFFER_CELL_SZ / 4)) {
+		ocelot_write_rix(ocelot, 0, QS_INJ_WR, grp);
+		i++;
+	}
+
+	/* Indicate EOF and valid bytes in last word */
+	ocelot_write_rix(ocelot, QS_INJ_CTRL_GAP_SIZE(1) |
+			 QS_INJ_CTRL_VLD_BYTES(skb->len < OCELOT_BUFFER_CELL_SZ ? 0 : last) |
+			 QS_INJ_CTRL_EOF,
+			 QS_INJ_CTRL, grp);
+
+	/* Add dummy CRC */
+	ocelot_write_rix(ocelot, 0, QS_INJ_WR, grp);
+	skb_tx_timestamp(skb);
+
+	skb->dev->stats.tx_packets++;
+	skb->dev->stats.tx_bytes += skb->len;
+}
+
 int ocelot_fdb_add(struct ocelot *ocelot, int port,
 		   const unsigned char *addr, u16 vid)
 {
diff --git a/drivers/net/ethernet/mscc/ocelot.h b/drivers/net/ethernet/mscc/ocelot.h
index e8621dbc14f7..cf6493e55eab 100644
--- a/drivers/net/ethernet/mscc/ocelot.h
+++ b/drivers/net/ethernet/mscc/ocelot.h
@@ -127,6 +127,10 @@  int ocelot_port_devlink_init(struct ocelot *ocelot, int port,
 			     enum devlink_port_flavour flavour);
 void ocelot_port_devlink_teardown(struct ocelot *ocelot, int port);
 
+bool ocelot_can_inject(struct ocelot *ocelot, int grp);
+void ocelot_port_inject_frame(struct ocelot *ocelot, int port, int grp,
+			      u32 rew_op, struct sk_buff *skb);
+
 extern struct notifier_block ocelot_netdevice_nb;
 extern struct notifier_block ocelot_switchdev_nb;
 extern struct notifier_block ocelot_switchdev_blocking_nb;
diff --git a/drivers/net/ethernet/mscc/ocelot_net.c b/drivers/net/ethernet/mscc/ocelot_net.c
index 55847d2a83e1..9a29d7d3b0e2 100644
--- a/drivers/net/ethernet/mscc/ocelot_net.c
+++ b/drivers/net/ethernet/mscc/ocelot_net.c
@@ -488,53 +488,20 @@  static int ocelot_port_stop(struct net_device *dev)
 	return 0;
 }
 
-/* Generate the IFH for frame injection
- *
- * The IFH is a 128bit-value
- * bit 127: bypass the analyzer processing
- * bit 56-67: destination mask
- * bit 28-29: pop_cnt: 3 disables all rewriting of the frame
- * bit 20-27: cpu extraction queue mask
- * bit 16: tag type 0: C-tag, 1: S-tag
- * bit 0-11: VID
- */
-static int ocelot_gen_ifh(u32 *ifh, struct frame_info *info)
-{
-	ifh[0] = IFH_INJ_BYPASS | ((0x1ff & info->rew_op) << 21);
-	ifh[1] = (0xf00 & info->port) >> 8;
-	ifh[2] = (0xff & info->port) << 24;
-	ifh[3] = (info->tag_type << 16) | info->vid;
-
-	return 0;
-}
-
-static int ocelot_port_xmit(struct sk_buff *skb, struct net_device *dev)
+static netdev_tx_t ocelot_port_xmit(struct sk_buff *skb, struct net_device *dev)
 {
 	struct ocelot_port_private *priv = netdev_priv(dev);
-	struct skb_shared_info *shinfo = skb_shinfo(skb);
 	struct ocelot_port *ocelot_port = &priv->port;
 	struct ocelot *ocelot = ocelot_port->ocelot;
-	u32 val, ifh[OCELOT_TAG_LEN / 4];
-	struct frame_info info = {};
-	u8 grp = 0; /* Send everything on CPU group 0 */
-	unsigned int i, count, last;
 	int port = priv->chip_port;
+	u32 rew_op = 0;
 
-	val = ocelot_read(ocelot, QS_INJ_STATUS);
-	if (!(val & QS_INJ_STATUS_FIFO_RDY(BIT(grp))) ||
-	    (val & QS_INJ_STATUS_WMARK_REACHED(BIT(grp))))
+	if (!ocelot_can_inject(ocelot, 0))
 		return NETDEV_TX_BUSY;
 
-	ocelot_write_rix(ocelot, QS_INJ_CTRL_GAP_SIZE(1) |
-			 QS_INJ_CTRL_SOF, QS_INJ_CTRL, grp);
-
-	info.port = BIT(port);
-	info.tag_type = IFH_TAG_TYPE_C;
-	info.vid = skb_vlan_tag_get(skb);
-
 	/* Check if timestamping is needed */
-	if (ocelot->ptp && (shinfo->tx_flags & SKBTX_HW_TSTAMP)) {
-		info.rew_op = ocelot_port->ptp_cmd;
+	if (ocelot->ptp && (skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP)) {
+		rew_op = ocelot_port->ptp_cmd;
 
 		if (ocelot_port->ptp_cmd == IFH_REW_OP_TWO_STEP_PTP) {
 			struct sk_buff *clone;
@@ -547,45 +514,11 @@  static int ocelot_port_xmit(struct sk_buff *skb, struct net_device *dev)
 
 			ocelot_port_add_txtstamp_skb(ocelot, port, clone);
 
-			info.rew_op |= clone->cb[0] << 3;
+			rew_op |= clone->cb[0] << 3;
 		}
 	}
 
-	if (ocelot->ptp && shinfo->tx_flags & SKBTX_HW_TSTAMP) {
-		info.rew_op = ocelot_port->ptp_cmd;
-		if (ocelot_port->ptp_cmd == IFH_REW_OP_TWO_STEP_PTP)
-			info.rew_op |= skb->cb[0] << 3;
-	}
-
-	ocelot_gen_ifh(ifh, &info);
-
-	for (i = 0; i < OCELOT_TAG_LEN / 4; i++)
-		ocelot_write_rix(ocelot, (__force u32)cpu_to_be32(ifh[i]),
-				 QS_INJ_WR, grp);
-
-	count = DIV_ROUND_UP(skb->len, 4);
-	last = skb->len % 4;
-	for (i = 0; i < count; i++)
-		ocelot_write_rix(ocelot, ((u32 *)skb->data)[i], QS_INJ_WR, grp);
-
-	/* Add padding */
-	while (i < (OCELOT_BUFFER_CELL_SZ / 4)) {
-		ocelot_write_rix(ocelot, 0, QS_INJ_WR, grp);
-		i++;
-	}
-
-	/* Indicate EOF and valid bytes in last word */
-	ocelot_write_rix(ocelot, QS_INJ_CTRL_GAP_SIZE(1) |
-			 QS_INJ_CTRL_VLD_BYTES(skb->len < OCELOT_BUFFER_CELL_SZ ? 0 : last) |
-			 QS_INJ_CTRL_EOF,
-			 QS_INJ_CTRL, grp);
-
-	/* Add dummy CRC */
-	ocelot_write_rix(ocelot, 0, QS_INJ_WR, grp);
-	skb_tx_timestamp(skb);
-
-	dev->stats.tx_packets++;
-	dev->stats.tx_bytes += skb->len;
+	ocelot_port_inject_frame(ocelot, port, 0, rew_op, skb);
 
 	kfree_skb(skb);