diff mbox series

[net-next] net: mscc: ocelot: always pass skb clone to ocelot_port_add_txtstamp_skb

Message ID 20200923112420.2147806-1-vladimir.oltean@nxp.com
State Accepted
Delegated to: David Miller
Headers show
Series [net-next] net: mscc: ocelot: always pass skb clone to ocelot_port_add_txtstamp_skb | expand

Commit Message

Vladimir Oltean Sept. 23, 2020, 11:24 a.m. UTC
Currently, ocelot switchdev passes the skb directly to the function that
enqueues it to the list of skb's awaiting a TX timestamp. Whereas the
felix DSA driver first clones the skb, then passes the clone to this
queue.

This matters because in the case of felix, the common IRQ handler, which
is ocelot_get_txtstamp(), currently clones the clone, and frees the
original clone. This is useless and can be simplified by using
skb_complete_tx_timestamp() instead of skb_tstamp_tx().

Signed-off-by: Vladimir Oltean <vladimir.oltean@nxp.com>
---
 drivers/net/dsa/ocelot/felix.c         |  5 ++++-
 drivers/net/ethernet/mscc/ocelot.c     | 30 ++++++++++----------------
 drivers/net/ethernet/mscc/ocelot_net.c | 22 +++++++++++++++----
 include/soc/mscc/ocelot.h              |  4 ++--
 net/dsa/tag_ocelot.c                   |  6 +++---
 5 files changed, 38 insertions(+), 29 deletions(-)

Comments

Horatiu Vultur Sept. 23, 2020, 8:08 p.m. UTC | #1
The 09/23/2020 14:24, Vladimir Oltean wrote:
> 

Hi Vladimir,

I have just one question, please see bellow.

> Currently, ocelot switchdev passes the skb directly to the function that
> enqueues it to the list of skb's awaiting a TX timestamp. Whereas the
> felix DSA driver first clones the skb, then passes the clone to this
> queue.
> 
> This matters because in the case of felix, the common IRQ handler, which
> is ocelot_get_txtstamp(), currently clones the clone, and frees the
> original clone. This is useless and can be simplified by using
> skb_complete_tx_timestamp() instead of skb_tstamp_tx().
> 
> Signed-off-by: Vladimir Oltean <vladimir.oltean@nxp.com>
> ---
>  drivers/net/dsa/ocelot/felix.c         |  5 ++++-
>  drivers/net/ethernet/mscc/ocelot.c     | 30 ++++++++++----------------
>  drivers/net/ethernet/mscc/ocelot_net.c | 22 +++++++++++++++----
>  include/soc/mscc/ocelot.h              |  4 ++--
>  net/dsa/tag_ocelot.c                   |  6 +++---
>  5 files changed, 38 insertions(+), 29 deletions(-)
> 
> diff --git a/drivers/net/dsa/ocelot/felix.c b/drivers/net/dsa/ocelot/felix.c
> index 0b2bb8d7325c..1ec4fea5a546 100644
> --- a/drivers/net/dsa/ocelot/felix.c
> +++ b/drivers/net/dsa/ocelot/felix.c
> @@ -668,8 +668,11 @@ static bool felix_txtstamp(struct dsa_switch *ds, int port,
>         struct ocelot *ocelot = ds->priv;
>         struct ocelot_port *ocelot_port = ocelot->ports[port];
> 
> -       if (!ocelot_port_add_txtstamp_skb(ocelot_port, clone))
> +       if (ocelot->ptp && (skb_shinfo(clone)->tx_flags & SKBTX_HW_TSTAMP) &&
> +           ocelot_port->ptp_cmd == IFH_REW_OP_TWO_STEP_PTP) {
> +               ocelot_port_add_txtstamp_skb(ocelot, port, clone);
>                 return true;
> +       }
> 
>         return false;
>  }
> diff --git a/drivers/net/ethernet/mscc/ocelot.c b/drivers/net/ethernet/mscc/ocelot.c
> index 977d2263cbe1..58b969b85643 100644
> --- a/drivers/net/ethernet/mscc/ocelot.c
> +++ b/drivers/net/ethernet/mscc/ocelot.c
> @@ -413,26 +413,20 @@ void ocelot_port_disable(struct ocelot *ocelot, int port)
>  }
>  EXPORT_SYMBOL(ocelot_port_disable);
> 
> -int ocelot_port_add_txtstamp_skb(struct ocelot_port *ocelot_port,
> -                                struct sk_buff *skb)
> +void ocelot_port_add_txtstamp_skb(struct ocelot *ocelot, int port,
> +                                 struct sk_buff *clone)
>  {
> -       struct skb_shared_info *shinfo = skb_shinfo(skb);
> -       struct ocelot *ocelot = ocelot_port->ocelot;
> +       struct ocelot_port *ocelot_port = ocelot->ports[port];
> 
> -       if (ocelot->ptp && shinfo->tx_flags & SKBTX_HW_TSTAMP &&
> -           ocelot_port->ptp_cmd == IFH_REW_OP_TWO_STEP_PTP) {
> -               spin_lock(&ocelot_port->ts_id_lock);
> +       spin_lock(&ocelot_port->ts_id_lock);
> 
> -               shinfo->tx_flags |= SKBTX_IN_PROGRESS;
> -               /* Store timestamp ID in cb[0] of sk_buff */
> -               skb->cb[0] = ocelot_port->ts_id;
> -               ocelot_port->ts_id = (ocelot_port->ts_id + 1) % 4;
> -               skb_queue_tail(&ocelot_port->tx_skbs, skb);
> +       skb_shinfo(clone)->tx_flags |= SKBTX_IN_PROGRESS;
> +       /* Store timestamp ID in cb[0] of sk_buff */
> +       clone->cb[0] = ocelot_port->ts_id;
> +       ocelot_port->ts_id = (ocelot_port->ts_id + 1) % 4;
> +       skb_queue_tail(&ocelot_port->tx_skbs, clone);
> 
> -               spin_unlock(&ocelot_port->ts_id_lock);
> -               return 0;
> -       }
> -       return -ENODATA;
> +       spin_unlock(&ocelot_port->ts_id_lock);
>  }
>  EXPORT_SYMBOL(ocelot_port_add_txtstamp_skb);
> 
> @@ -511,9 +505,7 @@ void ocelot_get_txtstamp(struct ocelot *ocelot)
>                 /* Set the timestamp into the skb */
>                 memset(&shhwtstamps, 0, sizeof(shhwtstamps));
>                 shhwtstamps.hwtstamp = ktime_set(ts.tv_sec, ts.tv_nsec);
> -               skb_tstamp_tx(skb_match, &shhwtstamps);
> -
> -               dev_kfree_skb_any(skb_match);
> +               skb_complete_tx_timestamp(skb_match, &shhwtstamps);
> 
>                 /* Next ts */
>                 ocelot_write(ocelot, SYS_PTP_NXT_PTP_NXT, SYS_PTP_NXT);
> diff --git a/drivers/net/ethernet/mscc/ocelot_net.c b/drivers/net/ethernet/mscc/ocelot_net.c
> index 8490e42e9e2d..028a0150f97d 100644
> --- a/drivers/net/ethernet/mscc/ocelot_net.c
> +++ b/drivers/net/ethernet/mscc/ocelot_net.c
> @@ -330,7 +330,6 @@ static int ocelot_port_xmit(struct sk_buff *skb, struct net_device *dev)
>         u8 grp = 0; /* Send everything on CPU group 0 */
>         unsigned int i, count, last;
>         int port = priv->chip_port;
> -       bool do_tstamp;
> 
>         val = ocelot_read(ocelot, QS_INJ_STATUS);
>         if (!(val & QS_INJ_STATUS_FIFO_RDY(BIT(grp))) ||
> @@ -345,7 +344,23 @@ static int ocelot_port_xmit(struct sk_buff *skb, struct net_device *dev)
>         info.vid = skb_vlan_tag_get(skb);
> 
>         /* Check if timestamping is needed */
> -       do_tstamp = (ocelot_port_add_txtstamp_skb(ocelot_port, skb) == 0);
> +       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) {
> +                       struct sk_buff *clone;
> +
> +                       clone = skb_clone_sk(skb);
> +                       if (!clone) {
> +                               kfree_skb(skb);
> +                               return NETDEV_TX_OK;

Why do you return NETDEV_TX_OK?
Because the frame is not sent yet.

> +                       }
> +
> +                       ocelot_port_add_txtstamp_skb(ocelot, port, clone);
> +
> +                       info.rew_op |= clone->cb[0] << 3;
> +               }
> +       }
> 
>         if (ocelot->ptp && shinfo->tx_flags & SKBTX_HW_TSTAMP) {
>                 info.rew_op = ocelot_port->ptp_cmd;
> @@ -383,8 +398,7 @@ static int ocelot_port_xmit(struct sk_buff *skb, struct net_device *dev)
>         dev->stats.tx_packets++;
>         dev->stats.tx_bytes += skb->len;
> 
> -       if (!do_tstamp)
> -               dev_kfree_skb_any(skb);
> +       kfree_skb(skb);
> 
>         return NETDEV_TX_OK;
>  }
> diff --git a/include/soc/mscc/ocelot.h b/include/soc/mscc/ocelot.h
> index df252c22f985..2ca7ba829c76 100644
> --- a/include/soc/mscc/ocelot.h
> +++ b/include/soc/mscc/ocelot.h
> @@ -711,8 +711,8 @@ int ocelot_vlan_add(struct ocelot *ocelot, int port, u16 vid, bool pvid,
>  int ocelot_vlan_del(struct ocelot *ocelot, int port, u16 vid);
>  int ocelot_hwstamp_get(struct ocelot *ocelot, int port, struct ifreq *ifr);
>  int ocelot_hwstamp_set(struct ocelot *ocelot, int port, struct ifreq *ifr);
> -int ocelot_port_add_txtstamp_skb(struct ocelot_port *ocelot_port,
> -                                struct sk_buff *skb);
> +void ocelot_port_add_txtstamp_skb(struct ocelot *ocelot, int port,
> +                                 struct sk_buff *clone);
>  void ocelot_get_txtstamp(struct ocelot *ocelot);
>  void ocelot_port_set_maxlen(struct ocelot *ocelot, int port, size_t sdu);
>  int ocelot_get_max_mtu(struct ocelot *ocelot, int port);
> diff --git a/net/dsa/tag_ocelot.c b/net/dsa/tag_ocelot.c
> index b4fc05cafaa6..d1a7e224adff 100644
> --- a/net/dsa/tag_ocelot.c
> +++ b/net/dsa/tag_ocelot.c
> @@ -137,6 +137,7 @@ static struct sk_buff *ocelot_xmit(struct sk_buff *skb,
>                                    struct net_device *netdev)
>  {
>         struct dsa_port *dp = dsa_slave_to_port(netdev);
> +       struct sk_buff *clone = DSA_SKB_CB(skb)->clone;
>         struct dsa_switch *ds = dp->ds;
>         struct ocelot *ocelot = ds->priv;
>         struct ocelot_port *ocelot_port;
> @@ -159,9 +160,8 @@ static struct sk_buff *ocelot_xmit(struct sk_buff *skb,
>         qos_class = skb->priority;
>         packing(injection, &qos_class, 19,  17, OCELOT_TAG_LEN, PACK, 0);
> 
> -       if (ocelot->ptp && (skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP)) {
> -               struct sk_buff *clone = DSA_SKB_CB(skb)->clone;
> -
> +       /* TX timestamping was requested */
> +       if (clone) {
>                 rew_op = ocelot_port->ptp_cmd;
>                 /* Retrieve timestamp ID populated inside skb->cb[0] of the
>                  * clone by ocelot_port_add_txtstamp_skb
> --
> 2.25.1
>
David Miller Sept. 23, 2020, 8:17 p.m. UTC | #2
From: Horatiu Vultur <horatiu.vultur@microchip.com>
Date: Wed, 23 Sep 2020 22:08:00 +0200

> The 09/23/2020 14:24, Vladimir Oltean wrote:
>> @@ -345,7 +344,23 @@ static int ocelot_port_xmit(struct sk_buff *skb, struct net_device *dev)
>>         info.vid = skb_vlan_tag_get(skb);
>> 
>>         /* Check if timestamping is needed */
>> -       do_tstamp = (ocelot_port_add_txtstamp_skb(ocelot_port, skb) == 0);
>> +       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) {
>> +                       struct sk_buff *clone;
>> +
>> +                       clone = skb_clone_sk(skb);
>> +                       if (!clone) {
>> +                               kfree_skb(skb);
>> +                               return NETDEV_TX_OK;
> 
> Why do you return NETDEV_TX_OK?
> Because the frame is not sent yet.

Returning anything other than NETDEV_TX_OK will cause the networking stack
to try and requeue 'skb'.
Vladimir Oltean Sept. 23, 2020, 8:22 p.m. UTC | #3
On Wed, Sep 23, 2020 at 10:08:00PM +0200, Horatiu Vultur wrote:
> The 09/23/2020 14:24, Vladimir Oltean wrote:
> > +               if (ocelot_port->ptp_cmd == IFH_REW_OP_TWO_STEP_PTP) {
> > +                       struct sk_buff *clone;
> > +
> > +                       clone = skb_clone_sk(skb);
> > +                       if (!clone) {
> > +                               kfree_skb(skb);
> > +                               return NETDEV_TX_OK;
>
> Why do you return NETDEV_TX_OK?
> Because the frame is not sent yet.

I suppose I _could_ increment the tx_dropped counters, if that's what
you mean.

I was also going to request David to let me send a v2, because I want to
replace the kfree_skb at the end of this function with a proper
consume_skb, so that this won't appear as a false positive in the drop
monitor.

Thanks,
-Vladimir
Horatiu Vultur Sept. 23, 2020, 8:35 p.m. UTC | #4
The 09/23/2020 20:22, Vladimir Oltean wrote:
> EXTERNAL EMAIL: Do not click links or open attachments unless you know the content is safe
> 
> On Wed, Sep 23, 2020 at 10:08:00PM +0200, Horatiu Vultur wrote:
> > The 09/23/2020 14:24, Vladimir Oltean wrote:
> > > +               if (ocelot_port->ptp_cmd == IFH_REW_OP_TWO_STEP_PTP) {
> > > +                       struct sk_buff *clone;
> > > +
> > > +                       clone = skb_clone_sk(skb);
> > > +                       if (!clone) {
> > > +                               kfree_skb(skb);
> > > +                               return NETDEV_TX_OK;
> >
> > Why do you return NETDEV_TX_OK?
> > Because the frame is not sent yet.
> 
> I suppose I _could_ increment the tx_dropped counters, if that's what
> you mean.

Yeah, something like that I was thinking.

Also I am just thinking, not sure if it is correct but, can you return
NETDEV_TX_BUSY and not free the skb?

> 
> I was also going to request David to let me send a v2, because I want to
> replace the kfree_skb at the end of this function with a proper
> consume_skb, so that this won't appear as a false positive in the drop
> monitor.
> 
> Thanks,
> -Vladimir
Vladimir Oltean Sept. 23, 2020, 8:45 p.m. UTC | #5
On Wed, Sep 23, 2020 at 10:35:30PM +0200, Horatiu Vultur wrote:
> The 09/23/2020 20:22, Vladimir Oltean wrote:
> > On Wed, Sep 23, 2020 at 10:08:00PM +0200, Horatiu Vultur wrote:
> > > The 09/23/2020 14:24, Vladimir Oltean wrote:
> > > > +               if (ocelot_port->ptp_cmd == IFH_REW_OP_TWO_STEP_PTP) {
> > > > +                       struct sk_buff *clone;
> > > > +
> > > > +                       clone = skb_clone_sk(skb);
> > > > +                       if (!clone) {
> > > > +                               kfree_skb(skb);
> > > > +                               return NETDEV_TX_OK;
> > >
> > > Why do you return NETDEV_TX_OK?
> > > Because the frame is not sent yet.
> >
> > I suppose I _could_ increment the tx_dropped counters, if that's what
> > you mean.
>
> Yeah, something like that I was thinking.
>
> Also I am just thinking, not sure if it is correct but, can you return
> NETDEV_TX_BUSY and not free the skb?
>

Do you have a use case for NETDEV_TX_BUSY instead of plain dropping the
skb, some situation where it would be better?

I admit I haven't tested this particular code path, but my intuition
tells me that under OOM, the last thing you need is some networking
driver just trying and trying again to send a packet.

Documentation/networking/driver.rst:

1) The ndo_start_xmit method must not return NETDEV_TX_BUSY under
   any normal circumstances.  It is considered a hard error unless
   there is no way your device can tell ahead of time when it's
   transmit function will become busy.

Looking up the uses of NETDEV_TX_BUSY, I see pretty much only congestion
type of events.

Thanks,
-Vladimir
Horatiu Vultur Sept. 23, 2020, 9:46 p.m. UTC | #6
The 09/23/2020 20:45, Vladimir Oltean wrote:
> 
> On Wed, Sep 23, 2020 at 10:35:30PM +0200, Horatiu Vultur wrote:
> > The 09/23/2020 20:22, Vladimir Oltean wrote:
> > > On Wed, Sep 23, 2020 at 10:08:00PM +0200, Horatiu Vultur wrote:
> > > > The 09/23/2020 14:24, Vladimir Oltean wrote:
> > > > > +               if (ocelot_port->ptp_cmd == IFH_REW_OP_TWO_STEP_PTP) {
> > > > > +                       struct sk_buff *clone;
> > > > > +
> > > > > +                       clone = skb_clone_sk(skb);
> > > > > +                       if (!clone) {
> > > > > +                               kfree_skb(skb);
> > > > > +                               return NETDEV_TX_OK;
> > > >
> > > > Why do you return NETDEV_TX_OK?
> > > > Because the frame is not sent yet.
> > >
> > > I suppose I _could_ increment the tx_dropped counters, if that's what
> > > you mean.
> >
> > Yeah, something like that I was thinking.
> >
> > Also I am just thinking, not sure if it is correct but, can you return
> > NETDEV_TX_BUSY and not free the skb?
> >
> 
> Do you have a use case for NETDEV_TX_BUSY instead of plain dropping the
> skb, some situation where it would be better?

Not really.

> 
> I admit I haven't tested this particular code path, but my intuition
> tells me that under OOM, the last thing you need is some networking
> driver just trying and trying again to send a packet.

Yes, I totally understand your point and I aggree with you.

> 
> Documentation/networking/driver.rst:

I looked also initially in this document, that is the reason why I was
not sure if it is correct to return NETDEV_TX_BUSY.

> 
> 1) The ndo_start_xmit method must not return NETDEV_TX_BUSY under
>    any normal circumstances.  It is considered a hard error unless
>    there is no way your device can tell ahead of time when it's
>    transmit function will become busy.
> 
> Looking up the uses of NETDEV_TX_BUSY, I see pretty much only congestion
> type of events.
> 
> Thanks,
> -Vladimir
Richard Cochran Sept. 24, 2020, 2:18 a.m. UTC | #7
On Wed, Sep 23, 2020 at 02:24:20PM +0300, Vladimir Oltean wrote:
> Currently, ocelot switchdev passes the skb directly to the function that
> enqueues it to the list of skb's awaiting a TX timestamp. Whereas the
> felix DSA driver first clones the skb, then passes the clone to this
> queue.
> 
> This matters because in the case of felix, the common IRQ handler, which
> is ocelot_get_txtstamp(), currently clones the clone, and frees the
> original clone. This is useless and can be simplified by using
> skb_complete_tx_timestamp() instead of skb_tstamp_tx().
> 
> Signed-off-by: Vladimir Oltean <vladimir.oltean@nxp.com>

Acked-by: Richard Cochran <richardcochran@gmail.com>
David Miller Sept. 25, 2020, 2:48 a.m. UTC | #8
From: Vladimir Oltean <vladimir.oltean@nxp.com>
Date: Wed, 23 Sep 2020 14:24:20 +0300

> Currently, ocelot switchdev passes the skb directly to the function that
> enqueues it to the list of skb's awaiting a TX timestamp. Whereas the
> felix DSA driver first clones the skb, then passes the clone to this
> queue.
> 
> This matters because in the case of felix, the common IRQ handler, which
> is ocelot_get_txtstamp(), currently clones the clone, and frees the
> original clone. This is useless and can be simplified by using
> skb_complete_tx_timestamp() instead of skb_tstamp_tx().
> 
> Signed-off-by: Vladimir Oltean <vladimir.oltean@nxp.com>

Applied, thank you.
diff mbox series

Patch

diff --git a/drivers/net/dsa/ocelot/felix.c b/drivers/net/dsa/ocelot/felix.c
index 0b2bb8d7325c..1ec4fea5a546 100644
--- a/drivers/net/dsa/ocelot/felix.c
+++ b/drivers/net/dsa/ocelot/felix.c
@@ -668,8 +668,11 @@  static bool felix_txtstamp(struct dsa_switch *ds, int port,
 	struct ocelot *ocelot = ds->priv;
 	struct ocelot_port *ocelot_port = ocelot->ports[port];
 
-	if (!ocelot_port_add_txtstamp_skb(ocelot_port, clone))
+	if (ocelot->ptp && (skb_shinfo(clone)->tx_flags & SKBTX_HW_TSTAMP) &&
+	    ocelot_port->ptp_cmd == IFH_REW_OP_TWO_STEP_PTP) {
+		ocelot_port_add_txtstamp_skb(ocelot, port, clone);
 		return true;
+	}
 
 	return false;
 }
diff --git a/drivers/net/ethernet/mscc/ocelot.c b/drivers/net/ethernet/mscc/ocelot.c
index 977d2263cbe1..58b969b85643 100644
--- a/drivers/net/ethernet/mscc/ocelot.c
+++ b/drivers/net/ethernet/mscc/ocelot.c
@@ -413,26 +413,20 @@  void ocelot_port_disable(struct ocelot *ocelot, int port)
 }
 EXPORT_SYMBOL(ocelot_port_disable);
 
-int ocelot_port_add_txtstamp_skb(struct ocelot_port *ocelot_port,
-				 struct sk_buff *skb)
+void ocelot_port_add_txtstamp_skb(struct ocelot *ocelot, int port,
+				  struct sk_buff *clone)
 {
-	struct skb_shared_info *shinfo = skb_shinfo(skb);
-	struct ocelot *ocelot = ocelot_port->ocelot;
+	struct ocelot_port *ocelot_port = ocelot->ports[port];
 
-	if (ocelot->ptp && shinfo->tx_flags & SKBTX_HW_TSTAMP &&
-	    ocelot_port->ptp_cmd == IFH_REW_OP_TWO_STEP_PTP) {
-		spin_lock(&ocelot_port->ts_id_lock);
+	spin_lock(&ocelot_port->ts_id_lock);
 
-		shinfo->tx_flags |= SKBTX_IN_PROGRESS;
-		/* Store timestamp ID in cb[0] of sk_buff */
-		skb->cb[0] = ocelot_port->ts_id;
-		ocelot_port->ts_id = (ocelot_port->ts_id + 1) % 4;
-		skb_queue_tail(&ocelot_port->tx_skbs, skb);
+	skb_shinfo(clone)->tx_flags |= SKBTX_IN_PROGRESS;
+	/* Store timestamp ID in cb[0] of sk_buff */
+	clone->cb[0] = ocelot_port->ts_id;
+	ocelot_port->ts_id = (ocelot_port->ts_id + 1) % 4;
+	skb_queue_tail(&ocelot_port->tx_skbs, clone);
 
-		spin_unlock(&ocelot_port->ts_id_lock);
-		return 0;
-	}
-	return -ENODATA;
+	spin_unlock(&ocelot_port->ts_id_lock);
 }
 EXPORT_SYMBOL(ocelot_port_add_txtstamp_skb);
 
@@ -511,9 +505,7 @@  void ocelot_get_txtstamp(struct ocelot *ocelot)
 		/* Set the timestamp into the skb */
 		memset(&shhwtstamps, 0, sizeof(shhwtstamps));
 		shhwtstamps.hwtstamp = ktime_set(ts.tv_sec, ts.tv_nsec);
-		skb_tstamp_tx(skb_match, &shhwtstamps);
-
-		dev_kfree_skb_any(skb_match);
+		skb_complete_tx_timestamp(skb_match, &shhwtstamps);
 
 		/* Next ts */
 		ocelot_write(ocelot, SYS_PTP_NXT_PTP_NXT, SYS_PTP_NXT);
diff --git a/drivers/net/ethernet/mscc/ocelot_net.c b/drivers/net/ethernet/mscc/ocelot_net.c
index 8490e42e9e2d..028a0150f97d 100644
--- a/drivers/net/ethernet/mscc/ocelot_net.c
+++ b/drivers/net/ethernet/mscc/ocelot_net.c
@@ -330,7 +330,6 @@  static int ocelot_port_xmit(struct sk_buff *skb, struct net_device *dev)
 	u8 grp = 0; /* Send everything on CPU group 0 */
 	unsigned int i, count, last;
 	int port = priv->chip_port;
-	bool do_tstamp;
 
 	val = ocelot_read(ocelot, QS_INJ_STATUS);
 	if (!(val & QS_INJ_STATUS_FIFO_RDY(BIT(grp))) ||
@@ -345,7 +344,23 @@  static int ocelot_port_xmit(struct sk_buff *skb, struct net_device *dev)
 	info.vid = skb_vlan_tag_get(skb);
 
 	/* Check if timestamping is needed */
-	do_tstamp = (ocelot_port_add_txtstamp_skb(ocelot_port, skb) == 0);
+	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) {
+			struct sk_buff *clone;
+
+			clone = skb_clone_sk(skb);
+			if (!clone) {
+				kfree_skb(skb);
+				return NETDEV_TX_OK;
+			}
+
+			ocelot_port_add_txtstamp_skb(ocelot, port, clone);
+
+			info.rew_op |= clone->cb[0] << 3;
+		}
+	}
 
 	if (ocelot->ptp && shinfo->tx_flags & SKBTX_HW_TSTAMP) {
 		info.rew_op = ocelot_port->ptp_cmd;
@@ -383,8 +398,7 @@  static int ocelot_port_xmit(struct sk_buff *skb, struct net_device *dev)
 	dev->stats.tx_packets++;
 	dev->stats.tx_bytes += skb->len;
 
-	if (!do_tstamp)
-		dev_kfree_skb_any(skb);
+	kfree_skb(skb);
 
 	return NETDEV_TX_OK;
 }
diff --git a/include/soc/mscc/ocelot.h b/include/soc/mscc/ocelot.h
index df252c22f985..2ca7ba829c76 100644
--- a/include/soc/mscc/ocelot.h
+++ b/include/soc/mscc/ocelot.h
@@ -711,8 +711,8 @@  int ocelot_vlan_add(struct ocelot *ocelot, int port, u16 vid, bool pvid,
 int ocelot_vlan_del(struct ocelot *ocelot, int port, u16 vid);
 int ocelot_hwstamp_get(struct ocelot *ocelot, int port, struct ifreq *ifr);
 int ocelot_hwstamp_set(struct ocelot *ocelot, int port, struct ifreq *ifr);
-int ocelot_port_add_txtstamp_skb(struct ocelot_port *ocelot_port,
-				 struct sk_buff *skb);
+void ocelot_port_add_txtstamp_skb(struct ocelot *ocelot, int port,
+				  struct sk_buff *clone);
 void ocelot_get_txtstamp(struct ocelot *ocelot);
 void ocelot_port_set_maxlen(struct ocelot *ocelot, int port, size_t sdu);
 int ocelot_get_max_mtu(struct ocelot *ocelot, int port);
diff --git a/net/dsa/tag_ocelot.c b/net/dsa/tag_ocelot.c
index b4fc05cafaa6..d1a7e224adff 100644
--- a/net/dsa/tag_ocelot.c
+++ b/net/dsa/tag_ocelot.c
@@ -137,6 +137,7 @@  static struct sk_buff *ocelot_xmit(struct sk_buff *skb,
 				   struct net_device *netdev)
 {
 	struct dsa_port *dp = dsa_slave_to_port(netdev);
+	struct sk_buff *clone = DSA_SKB_CB(skb)->clone;
 	struct dsa_switch *ds = dp->ds;
 	struct ocelot *ocelot = ds->priv;
 	struct ocelot_port *ocelot_port;
@@ -159,9 +160,8 @@  static struct sk_buff *ocelot_xmit(struct sk_buff *skb,
 	qos_class = skb->priority;
 	packing(injection, &qos_class, 19,  17, OCELOT_TAG_LEN, PACK, 0);
 
-	if (ocelot->ptp && (skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP)) {
-		struct sk_buff *clone = DSA_SKB_CB(skb)->clone;
-
+	/* TX timestamping was requested */
+	if (clone) {
 		rew_op = ocelot_port->ptp_cmd;
 		/* Retrieve timestamp ID populated inside skb->cb[0] of the
 		 * clone by ocelot_port_add_txtstamp_skb