[PATCH net-next v4 0/6] net: mscc: PTP Hardware Clock (PHC) support

[PATCH net-next v4 0/6] net: mscc: PTP Hardware Clock (PHC) support

[Antoine Tenart]

Hello,

This series introduces the PTP Hardware Clock (PHC) support to the Mscc
Ocelot switch driver. In order to make use of this, a new register bank
is added and described in the device tree, as well as a new interrupt.
The use this bank and interrupt was made optional in the driver for dt
compatibility reasons.

Thanks!
Antoine

Since v3:
  - Fixed a spin_unlock_irqrestore issue.

Since v2:
  - Prevented from a possible infinite loop when reading the h/w
    timestamps.
  - s/GFP_KERNEL/GFP_ATOMIC/ in the Tx path.
  - Set rx_filter to HWTSTAMP_FILTER_PTP_V2_EVENT at probe.
  - Fixed s/w timestamping dependencies.
  - Added Paul Burton's Acked-by on patches 2 and 4.

Since v1:
  - Used list_for_each_safe() in ocelot_deinit().
  - Fixed a memory leak in ocelot_deinit() by calling
    dev_kfree_skb_any().
  - Fixed a locking issue in get_hwtimestamp().
  - Handled the NULL case of ptp_clock_register().
  - Added comments on optional dt properties.

Antoine Tenart (6):
  Documentation/bindings: net: ocelot: document the PTP bank
  Documentation/bindings: net: ocelot: document the PTP ready IRQ
  net: mscc: describe the PTP register range
  net: mscc: improve the frame header parsing readability
  net: mscc: remove the frame_info cpuq member
  net: mscc: PTP Hardware Clock (PHC) support

 .../devicetree/bindings/net/mscc-ocelot.txt   |  20 +-
 drivers/net/ethernet/mscc/ocelot.c            | 394 +++++++++++++++++-
 drivers/net/ethernet/mscc/ocelot.h            |  49 ++-
 drivers/net/ethernet/mscc/ocelot_board.c      | 144 ++++++-
 drivers/net/ethernet/mscc/ocelot_ptp.h        |  41 ++
 drivers/net/ethernet/mscc/ocelot_regs.c       |  11 +
 6 files changed, 630 insertions(+), 29 deletions(-)
 create mode 100644 drivers/net/ethernet/mscc/ocelot_ptp.h

-- 
2.21.0

[PATCH net-next v4 1/6] Documentation/bindings: net: ocelot: document the PTP bank

[Antoine Tenart]

One additional register range needs to be described within the Ocelot
device tree node: the PTP. This patch documents the binding needed to do
so.

Signed-off-by: Antoine Tenart <antoine.tenart@bootlin.com>
---
 Documentation/devicetree/bindings/net/mscc-ocelot.txt | 9 ++++++---
 1 file changed, 6 insertions(+), 3 deletions(-)

diff --git a/Documentation/devicetree/bindings/net/mscc-ocelot.txt b/Documentation/devicetree/bindings/net/mscc-ocelot.txt
index 9e5c17d426ce..4d05a3b0f786 100644
--- a/Documentation/devicetree/bindings/net/mscc-ocelot.txt
+++ b/Documentation/devicetree/bindings/net/mscc-ocelot.txt
@@ -12,6 +12,7 @@ Required properties:
   - "sys"
   - "rew"
   - "qs"
+  - "ptp" (optional due to backward compatibility)
   - "qsys"
   - "ana"
   - "portX" with X from 0 to the number of last port index available on that
@@ -44,6 +45,7 @@ Example:
 		reg = <0x1010000 0x10000>,
 		      <0x1030000 0x10000>,
 		      <0x1080000 0x100>,
+		      <0x10e0000 0x10000>,
 		      <0x11e0000 0x100>,
 		      <0x11f0000 0x100>,
 		      <0x1200000 0x100>,
@@ -57,9 +59,10 @@ Example:
 		      <0x1280000 0x100>,
 		      <0x1800000 0x80000>,
 		      <0x1880000 0x10000>;
-		reg-names = "sys", "rew", "qs", "port0", "port1", "port2",
-			    "port3", "port4", "port5", "port6", "port7",
-			    "port8", "port9", "port10", "qsys", "ana";
+		reg-names = "sys", "rew", "qs", "ptp", "port0", "port1",
+			    "port2", "port3", "port4", "port5", "port6",
+			    "port7", "port8", "port9", "port10", "qsys",
+			    "ana";
 		interrupts = <21 22>;
 		interrupt-names = "xtr", "inj";
 
-- 
2.21.0

[PATCH net-next v4 2/6] Documentation/bindings: net: ocelot: document the PTP ready IRQ

[Antoine Tenart]

One additional interrupt needs to be described within the Ocelot device
tree node: the PTP ready one. This patch documents the binding needed to
do so.

Signed-off-by: Antoine Tenart <antoine.tenart@bootlin.com>
---
 Documentation/devicetree/bindings/net/mscc-ocelot.txt | 11 ++++++-----
 1 file changed, 6 insertions(+), 5 deletions(-)

diff --git a/Documentation/devicetree/bindings/net/mscc-ocelot.txt b/Documentation/devicetree/bindings/net/mscc-ocelot.txt
index 4d05a3b0f786..3b6290b45ce5 100644
--- a/Documentation/devicetree/bindings/net/mscc-ocelot.txt
+++ b/Documentation/devicetree/bindings/net/mscc-ocelot.txt
@@ -17,9 +17,10 @@ Required properties:
   - "ana"
   - "portX" with X from 0 to the number of last port index available on that
     switch
-- interrupts: Should contain the switch interrupts for frame extraction and
-  frame injection
-- interrupt-names: should contain the interrupt names: "xtr", "inj"
+- interrupts: Should contain the switch interrupts for frame extraction,
+  frame injection and PTP ready.
+- interrupt-names: should contain the interrupt names: "xtr", "inj". Can contain
+  "ptp_rdy" which is optional due to backward compatibility.
 - ethernet-ports: A container for child nodes representing switch ports.
 
 The ethernet-ports container has the following properties
@@ -63,8 +64,8 @@ Example:
 			    "port2", "port3", "port4", "port5", "port6",
 			    "port7", "port8", "port9", "port10", "qsys",
 			    "ana";
-		interrupts = <21 22>;
-		interrupt-names = "xtr", "inj";
+		interrupts = <18 21 22>;
+		interrupt-names = "ptp_rdy", "xtr", "inj";
 
 		ethernet-ports {
 			#address-cells = <1>;
-- 
2.21.0

[PATCH net-next v4 3/6] net: mscc: describe the PTP register range

[Antoine Tenart]

This patch adds support for using the PTP register range, and adds a
description of its registers. This bank is used when configuring PTP.

Signed-off-by: Antoine Tenart <antoine.tenart@bootlin.com>
---
 drivers/net/ethernet/mscc/ocelot.h       |  9 ++++++
 drivers/net/ethernet/mscc/ocelot_board.c | 10 +++++-
 drivers/net/ethernet/mscc/ocelot_ptp.h   | 41 ++++++++++++++++++++++++
 drivers/net/ethernet/mscc/ocelot_regs.c  | 11 +++++++
 4 files changed, 70 insertions(+), 1 deletion(-)
 create mode 100644 drivers/net/ethernet/mscc/ocelot_ptp.h

diff --git a/drivers/net/ethernet/mscc/ocelot.h b/drivers/net/ethernet/mscc/ocelot.h
index f7eeb4806897..e0da8b4eddf2 100644
--- a/drivers/net/ethernet/mscc/ocelot.h
+++ b/drivers/net/ethernet/mscc/ocelot.h
@@ -23,6 +23,7 @@
 #include "ocelot_sys.h"
 #include "ocelot_qs.h"
 #include "ocelot_tc.h"
+#include "ocelot_ptp.h"
 
 #define PGID_AGGR    64
 #define PGID_SRC     80
@@ -71,6 +72,7 @@ enum ocelot_target {
 	SYS,
 	S2,
 	HSIO,
+	PTP,
 	TARGET_MAX,
 };
 
@@ -343,6 +345,13 @@ enum ocelot_reg {
 	S2_CACHE_ACTION_DAT,
 	S2_CACHE_CNT_DAT,
 	S2_CACHE_TG_DAT,
+	PTP_PIN_CFG = PTP << TARGET_OFFSET,
+	PTP_PIN_TOD_SEC_MSB,
+	PTP_PIN_TOD_SEC_LSB,
+	PTP_PIN_TOD_NSEC,
+	PTP_CFG_MISC,
+	PTP_CLK_CFG_ADJ_CFG,
+	PTP_CLK_CFG_ADJ_FREQ,
 };
 
 enum ocelot_regfield {
diff --git a/drivers/net/ethernet/mscc/ocelot_board.c b/drivers/net/ethernet/mscc/ocelot_board.c
index 2451d4a96490..990027f04d1b 100644
--- a/drivers/net/ethernet/mscc/ocelot_board.c
+++ b/drivers/net/ethernet/mscc/ocelot_board.c
@@ -182,6 +182,7 @@ static int mscc_ocelot_probe(struct platform_device *pdev)
 	struct {
 		enum ocelot_target id;
 		char *name;
+		u8 optional:1;
 	} res[] = {
 		{ SYS, "sys" },
 		{ REW, "rew" },
@@ -189,6 +190,7 @@ static int mscc_ocelot_probe(struct platform_device *pdev)
 		{ ANA, "ana" },
 		{ QS, "qs" },
 		{ S2, "s2" },
+		{ PTP, "ptp", 1 },
 	};
 
 	if (!np && !pdev->dev.platform_data)
@@ -205,8 +207,14 @@ static int mscc_ocelot_probe(struct platform_device *pdev)
 		struct regmap *target;
 
 		target = ocelot_io_platform_init(ocelot, pdev, res[i].name);
-		if (IS_ERR(target))
+		if (IS_ERR(target)) {
+			if (res[i].optional) {
+				ocelot->targets[res[i].id] = NULL;
+				continue;
+			}
+
 			return PTR_ERR(target);
+		}
 
 		ocelot->targets[res[i].id] = target;
 	}
diff --git a/drivers/net/ethernet/mscc/ocelot_ptp.h b/drivers/net/ethernet/mscc/ocelot_ptp.h
new file mode 100644
index 000000000000..9ede14a12573
--- /dev/null
+++ b/drivers/net/ethernet/mscc/ocelot_ptp.h
@@ -0,0 +1,41 @@
+/* SPDX-License-Identifier: (GPL-2.0 OR MIT) */
+/*
+ * Microsemi Ocelot Switch driver
+ *
+ * License: Dual MIT/GPL
+ * Copyright (c) 2017 Microsemi Corporation
+ */
+
+#ifndef _MSCC_OCELOT_PTP_H_
+#define _MSCC_OCELOT_PTP_H_
+
+#define PTP_PIN_CFG_RSZ			0x20
+#define PTP_PIN_TOD_SEC_MSB_RSZ		PTP_PIN_CFG_RSZ
+#define PTP_PIN_TOD_SEC_LSB_RSZ		PTP_PIN_CFG_RSZ
+#define PTP_PIN_TOD_NSEC_RSZ		PTP_PIN_CFG_RSZ
+
+#define PTP_PIN_CFG_DOM			BIT(0)
+#define PTP_PIN_CFG_SYNC		BIT(2)
+#define PTP_PIN_CFG_ACTION(x)		((x) << 3)
+#define PTP_PIN_CFG_ACTION_MASK		PTP_PIN_CFG_ACTION(0x7)
+
+enum {
+	PTP_PIN_ACTION_IDLE = 0,
+	PTP_PIN_ACTION_LOAD,
+	PTP_PIN_ACTION_SAVE,
+	PTP_PIN_ACTION_CLOCK,
+	PTP_PIN_ACTION_DELTA,
+	PTP_PIN_ACTION_NOSYNC,
+	PTP_PIN_ACTION_SYNC,
+};
+
+#define PTP_CFG_MISC_PTP_EN		BIT(2)
+
+#define PSEC_PER_SEC			1000000000000LL
+
+#define PTP_CFG_CLK_ADJ_CFG_ENA		BIT(0)
+#define PTP_CFG_CLK_ADJ_CFG_DIR		BIT(1)
+
+#define PTP_CFG_CLK_ADJ_FREQ_NS		BIT(30)
+
+#endif
diff --git a/drivers/net/ethernet/mscc/ocelot_regs.c b/drivers/net/ethernet/mscc/ocelot_regs.c
index 6c387f994ec5..e59977d20400 100644
--- a/drivers/net/ethernet/mscc/ocelot_regs.c
+++ b/drivers/net/ethernet/mscc/ocelot_regs.c
@@ -234,6 +234,16 @@ static const u32 ocelot_s2_regmap[] = {
 	REG(S2_CACHE_TG_DAT,               0x000388),
 };
 
+static const u32 ocelot_ptp_regmap[] = {
+	REG(PTP_PIN_CFG,                   0x000000),
+	REG(PTP_PIN_TOD_SEC_MSB,           0x000004),
+	REG(PTP_PIN_TOD_SEC_LSB,           0x000008),
+	REG(PTP_PIN_TOD_NSEC,              0x00000c),
+	REG(PTP_CFG_MISC,                  0x0000a0),
+	REG(PTP_CLK_CFG_ADJ_CFG,           0x0000a4),
+	REG(PTP_CLK_CFG_ADJ_FREQ,          0x0000a8),
+};
+
 static const u32 *ocelot_regmap[] = {
 	[ANA] = ocelot_ana_regmap,
 	[QS] = ocelot_qs_regmap,
@@ -241,6 +251,7 @@ static const u32 *ocelot_regmap[] = {
 	[REW] = ocelot_rew_regmap,
 	[SYS] = ocelot_sys_regmap,
 	[S2] = ocelot_s2_regmap,
+	[PTP] = ocelot_ptp_regmap,
 };
 
 static const struct reg_field ocelot_regfields[] = {
-- 
2.21.0

[PATCH net-next v4 4/6] net: mscc: improve the frame header parsing readability

[Antoine Tenart]

This cosmetic patch improves the frame header parsing readability by
introducing a new macro to access and mask its fields.

Signed-off-by: Antoine Tenart <antoine.tenart@bootlin.com>
---
 drivers/net/ethernet/mscc/ocelot_board.c | 24 +++++++++++++-----------
 1 file changed, 13 insertions(+), 11 deletions(-)

diff --git a/drivers/net/ethernet/mscc/ocelot_board.c b/drivers/net/ethernet/mscc/ocelot_board.c
index 990027f04d1b..5e4f1718dd99 100644
--- a/drivers/net/ethernet/mscc/ocelot_board.c
+++ b/drivers/net/ethernet/mscc/ocelot_board.c
@@ -16,24 +16,26 @@
 
 #include "ocelot.h"
 
-static int ocelot_parse_ifh(u32 *ifh, struct frame_info *info)
+#define IFH_EXTRACT_BITFIELD64(x, o, w) (((x) >> (o)) & GENMASK_ULL((w) - 1, 0))
+
+static int ocelot_parse_ifh(u32 *_ifh, struct frame_info *info)
 {
-	int i;
 	u8 llen, wlen;
+	u64 ifh[2];
+
+	ifh[0] = be64_to_cpu(((__force __be64 *)_ifh)[0]);
+	ifh[1] = be64_to_cpu(((__force __be64 *)_ifh)[1]);
 
-	/* The IFH is in network order, switch to CPU order */
-	for (i = 0; i < IFH_LEN; i++)
-		ifh[i] = ntohl((__force __be32)ifh[i]);
+	wlen = IFH_EXTRACT_BITFIELD64(ifh[0], 7,  8);
+	llen = IFH_EXTRACT_BITFIELD64(ifh[0], 15,  6);
 
-	wlen = (ifh[1] >> 7) & 0xff;
-	llen = (ifh[1] >> 15) & 0x3f;
 	info->len = OCELOT_BUFFER_CELL_SZ * wlen + llen - 80;
 
-	info->port = (ifh[2] & GENMASK(14, 11)) >> 11;
+	info->port = IFH_EXTRACT_BITFIELD64(ifh[1], 43, 4);
 
-	info->cpuq = (ifh[3] & GENMASK(27, 20)) >> 20;
-	info->tag_type = (ifh[3] & BIT(16)) >> 16;
-	info->vid = ifh[3] & GENMASK(11, 0);
+	info->cpuq = IFH_EXTRACT_BITFIELD64(ifh[1], 20, 8);
+	info->tag_type = IFH_EXTRACT_BITFIELD64(ifh[1], 16,  1);
+	info->vid = IFH_EXTRACT_BITFIELD64(ifh[1], 0,  12);
 
 	return 0;
 }
-- 
2.21.0

[PATCH net-next v4 5/6] net: mscc: remove the frame_info cpuq member

[Antoine Tenart]

In struct frame_info, the cpuq member is never used. This cosmetic patch
removes it from the structure, and from the parsing of the frame header
as it's only set but never used.

Signed-off-by: Antoine Tenart <antoine.tenart@bootlin.com>
---
 drivers/net/ethernet/mscc/ocelot.h       | 1 -
 drivers/net/ethernet/mscc/ocelot_board.c | 1 -
 2 files changed, 2 deletions(-)

diff --git a/drivers/net/ethernet/mscc/ocelot.h b/drivers/net/ethernet/mscc/ocelot.h
index e0da8b4eddf2..515dee6fa8a6 100644
--- a/drivers/net/ethernet/mscc/ocelot.h
+++ b/drivers/net/ethernet/mscc/ocelot.h
@@ -45,7 +45,6 @@ struct frame_info {
 	u32 len;
 	u16 port;
 	u16 vid;
-	u8 cpuq;
 	u8 tag_type;
 };
 
diff --git a/drivers/net/ethernet/mscc/ocelot_board.c b/drivers/net/ethernet/mscc/ocelot_board.c
index 5e4f1718dd99..df8d15994a89 100644
--- a/drivers/net/ethernet/mscc/ocelot_board.c
+++ b/drivers/net/ethernet/mscc/ocelot_board.c
@@ -33,7 +33,6 @@ static int ocelot_parse_ifh(u32 *_ifh, struct frame_info *info)
 
 	info->port = IFH_EXTRACT_BITFIELD64(ifh[1], 43, 4);
 
-	info->cpuq = IFH_EXTRACT_BITFIELD64(ifh[1], 20, 8);
 	info->tag_type = IFH_EXTRACT_BITFIELD64(ifh[1], 16,  1);
 	info->vid = IFH_EXTRACT_BITFIELD64(ifh[1], 0,  12);
 
-- 
2.21.0

[PATCH net-next v4 6/6] net: mscc: PTP Hardware Clock (PHC) support

[Antoine Tenart]

This patch adds support for PTP Hardware Clock (PHC) to the Ocelot
switch for both PTP 1-step and 2-step modes.

Signed-off-by: Antoine Tenart <antoine.tenart@bootlin.com>
---
 drivers/net/ethernet/mscc/ocelot.c       | 394 ++++++++++++++++++++++-
 drivers/net/ethernet/mscc/ocelot.h       |  39 +++
 drivers/net/ethernet/mscc/ocelot_board.c | 111 ++++++-
 3 files changed, 536 insertions(+), 8 deletions(-)

diff --git a/drivers/net/ethernet/mscc/ocelot.c b/drivers/net/ethernet/mscc/ocelot.c
index b71e4ecbe469..b08fcec73a30 100644
--- a/drivers/net/ethernet/mscc/ocelot.c
+++ b/drivers/net/ethernet/mscc/ocelot.c
@@ -14,6 +14,7 @@
 #include <linux/module.h>
 #include <linux/netdevice.h>
 #include <linux/phy.h>
+#include <linux/ptp_clock_kernel.h>
 #include <linux/skbuff.h>
 #include <linux/iopoll.h>
 #include <net/arp.h>
@@ -538,7 +539,7 @@ static int ocelot_port_stop(struct net_device *dev)
  */
 static int ocelot_gen_ifh(u32 *ifh, struct frame_info *info)
 {
-	ifh[0] = IFH_INJ_BYPASS;
+	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;
@@ -548,6 +549,7 @@ static int ocelot_gen_ifh(u32 *ifh, struct frame_info *info)
 
 static int ocelot_port_xmit(struct sk_buff *skb, struct net_device *dev)
 {
+	struct skb_shared_info *shinfo = skb_shinfo(skb);
 	struct ocelot_port *port = netdev_priv(dev);
 	struct ocelot *ocelot = port->ocelot;
 	u32 val, ifh[IFH_LEN];
@@ -566,6 +568,14 @@ static int ocelot_port_xmit(struct sk_buff *skb, struct net_device *dev)
 	info.port = BIT(port->chip_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 = port->ptp_cmd;
+		if (port->ptp_cmd == IFH_REW_OP_TWO_STEP_PTP)
+			info.rew_op |= (port->ts_id  % 4) << 3;
+	}
+
 	ocelot_gen_ifh(ifh, &info);
 
 	for (i = 0; i < IFH_LEN; i++)
@@ -596,11 +606,51 @@ static int ocelot_port_xmit(struct sk_buff *skb, struct net_device *dev)
 
 	dev->stats.tx_packets++;
 	dev->stats.tx_bytes += skb->len;
-	dev_kfree_skb_any(skb);
+
+	if (ocelot->ptp && shinfo->tx_flags & SKBTX_HW_TSTAMP &&
+	    port->ptp_cmd == IFH_REW_OP_TWO_STEP_PTP) {
+		struct ocelot_skb *oskb =
+			kzalloc(sizeof(struct ocelot_skb), GFP_ATOMIC);
+
+		oskb->skb = skb;
+		oskb->id = port->ts_id % 4;
+		port->ts_id++;
+
+		list_add_tail(&oskb->head, &port->skbs);
+	} else {
+		dev_kfree_skb_any(skb);
+	}
 
 	return NETDEV_TX_OK;
 }
 
+void ocelot_get_hwtimestamp(struct ocelot *ocelot, struct timespec64 *ts)
+{
+	unsigned long flags;
+	u32 val;
+
+	spin_lock_irqsave(&ocelot->ptp_clock_lock, flags);
+
+	/* Read current PTP time to get seconds */
+	val = ocelot_read_rix(ocelot, PTP_PIN_CFG, TOD_ACC_PIN);
+
+	val &= ~(PTP_PIN_CFG_SYNC | PTP_PIN_CFG_ACTION_MASK | PTP_PIN_CFG_DOM);
+	val |= PTP_PIN_CFG_ACTION(PTP_PIN_ACTION_SAVE);
+	ocelot_write_rix(ocelot, val, PTP_PIN_CFG, TOD_ACC_PIN);
+	ts->tv_sec = ocelot_read_rix(ocelot, PTP_PIN_TOD_SEC_LSB, TOD_ACC_PIN);
+
+	/* Read packet HW timestamp from FIFO */
+	val = ocelot_read(ocelot, SYS_PTP_TXSTAMP);
+	ts->tv_nsec = SYS_PTP_TXSTAMP_PTP_TXSTAMP(val);
+
+	/* Sec has incremented since the ts was registered */
+	if ((ts->tv_sec & 0x1) != !!(val & SYS_PTP_TXSTAMP_PTP_TXSTAMP_SEC))
+		ts->tv_sec--;
+
+	spin_unlock_irqrestore(&ocelot->ptp_clock_lock, flags);
+}
+EXPORT_SYMBOL(ocelot_get_hwtimestamp);
+
 static int ocelot_mc_unsync(struct net_device *dev, const unsigned char *addr)
 {
 	struct ocelot_port *port = netdev_priv(dev);
@@ -917,6 +967,97 @@ static int ocelot_get_port_parent_id(struct net_device *dev,
 	return 0;
 }
 
+static int ocelot_hwstamp_get(struct ocelot_port *port, struct ifreq *ifr)
+{
+	struct ocelot *ocelot = port->ocelot;
+
+	return copy_to_user(ifr->ifr_data, &ocelot->hwtstamp_config,
+			    sizeof(ocelot->hwtstamp_config)) ? -EFAULT : 0;
+}
+
+static int ocelot_hwstamp_set(struct ocelot_port *port, struct ifreq *ifr)
+{
+	struct ocelot *ocelot = port->ocelot;
+	struct hwtstamp_config cfg;
+
+	if (copy_from_user(&cfg, ifr->ifr_data, sizeof(cfg)))
+		return -EFAULT;
+
+	/* reserved for future extensions */
+	if (cfg.flags)
+		return -EINVAL;
+
+	/* Tx type sanity check */
+	switch (cfg.tx_type) {
+	case HWTSTAMP_TX_ON:
+		port->ptp_cmd = IFH_REW_OP_TWO_STEP_PTP;
+		break;
+	case HWTSTAMP_TX_ONESTEP_SYNC:
+		/* IFH_REW_OP_ONE_STEP_PTP updates the correctional field, we
+		 * need to update the origin time.
+		 */
+		port->ptp_cmd = IFH_REW_OP_ORIGIN_PTP;
+		break;
+	case HWTSTAMP_TX_OFF:
+		port->ptp_cmd = 0;
+		break;
+	default:
+		return -ERANGE;
+	}
+
+	mutex_lock(&ocelot->ptp_lock);
+
+	switch (cfg.rx_filter) {
+	case HWTSTAMP_FILTER_NONE:
+		break;
+	case HWTSTAMP_FILTER_ALL:
+	case HWTSTAMP_FILTER_SOME:
+	case HWTSTAMP_FILTER_PTP_V1_L4_EVENT:
+	case HWTSTAMP_FILTER_PTP_V1_L4_SYNC:
+	case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ:
+	case HWTSTAMP_FILTER_NTP_ALL:
+	case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
+	case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
+	case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
+	case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
+	case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
+	case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
+	case HWTSTAMP_FILTER_PTP_V2_EVENT:
+	case HWTSTAMP_FILTER_PTP_V2_SYNC:
+	case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
+		cfg.rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
+		break;
+	default:
+		mutex_unlock(&ocelot->ptp_lock);
+		return -ERANGE;
+	}
+
+	/* Commit back the result & save it */
+	memcpy(&ocelot->hwtstamp_config, &cfg, sizeof(cfg));
+	mutex_unlock(&ocelot->ptp_lock);
+
+	return copy_to_user(ifr->ifr_data, &cfg, sizeof(cfg)) ? -EFAULT : 0;
+}
+
+static int ocelot_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd)
+{
+	struct ocelot_port *port = netdev_priv(dev);
+	struct ocelot *ocelot = port->ocelot;
+
+	/* The function is only used for PTP operations for now */
+	if (!ocelot->ptp)
+		return -EOPNOTSUPP;
+
+	switch (cmd) {
+	case SIOCSHWTSTAMP:
+		return ocelot_hwstamp_set(port, ifr);
+	case SIOCGHWTSTAMP:
+		return ocelot_hwstamp_get(port, ifr);
+	default:
+		return -EOPNOTSUPP;
+	}
+}
+
 static const struct net_device_ops ocelot_port_netdev_ops = {
 	.ndo_open			= ocelot_port_open,
 	.ndo_stop			= ocelot_port_stop,
@@ -933,6 +1074,7 @@ static const struct net_device_ops ocelot_port_netdev_ops = {
 	.ndo_set_features		= ocelot_set_features,
 	.ndo_get_port_parent_id		= ocelot_get_port_parent_id,
 	.ndo_setup_tc			= ocelot_setup_tc,
+	.ndo_do_ioctl			= ocelot_ioctl,
 };
 
 static void ocelot_get_strings(struct net_device *netdev, u32 sset, u8 *data)
@@ -1014,12 +1156,37 @@ static int ocelot_get_sset_count(struct net_device *dev, int sset)
 	return ocelot->num_stats;
 }
 
+static int ocelot_get_ts_info(struct net_device *dev,
+			      struct ethtool_ts_info *info)
+{
+	struct ocelot_port *ocelot_port = netdev_priv(dev);
+	struct ocelot *ocelot = ocelot_port->ocelot;
+
+	if (!ocelot->ptp)
+		return ethtool_op_get_ts_info(dev, info);
+
+	info->phc_index = ocelot->ptp_clock ?
+			  ptp_clock_index(ocelot->ptp_clock) : -1;
+	info->so_timestamping |= SOF_TIMESTAMPING_TX_SOFTWARE |
+				 SOF_TIMESTAMPING_RX_SOFTWARE |
+				 SOF_TIMESTAMPING_SOFTWARE |
+				 SOF_TIMESTAMPING_TX_HARDWARE |
+				 SOF_TIMESTAMPING_RX_HARDWARE |
+				 SOF_TIMESTAMPING_RAW_HARDWARE;
+	info->tx_types = BIT(HWTSTAMP_TX_OFF) | BIT(HWTSTAMP_TX_ON) |
+			 BIT(HWTSTAMP_TX_ONESTEP_SYNC);
+	info->rx_filters = BIT(HWTSTAMP_FILTER_NONE) | BIT(HWTSTAMP_FILTER_ALL);
+
+	return 0;
+}
+
 static const struct ethtool_ops ocelot_ethtool_ops = {
 	.get_strings		= ocelot_get_strings,
 	.get_ethtool_stats	= ocelot_get_ethtool_stats,
 	.get_sset_count		= ocelot_get_sset_count,
 	.get_link_ksettings	= phy_ethtool_get_link_ksettings,
 	.set_link_ksettings	= phy_ethtool_set_link_ksettings,
+	.get_ts_info		= ocelot_get_ts_info,
 };
 
 static int ocelot_port_attr_stp_state_set(struct ocelot_port *ocelot_port,
@@ -1629,6 +1796,196 @@ struct notifier_block ocelot_switchdev_blocking_nb __read_mostly = {
 };
 EXPORT_SYMBOL(ocelot_switchdev_blocking_nb);
 
+int ocelot_ptp_gettime64(struct ptp_clock_info *ptp, struct timespec64 *ts)
+{
+	struct ocelot *ocelot = container_of(ptp, struct ocelot, ptp_info);
+	unsigned long flags;
+	time64_t s;
+	u32 val;
+	s64 ns;
+
+	spin_lock_irqsave(&ocelot->ptp_clock_lock, flags);
+
+	val = ocelot_read_rix(ocelot, PTP_PIN_CFG, TOD_ACC_PIN);
+	val &= ~(PTP_PIN_CFG_SYNC | PTP_PIN_CFG_ACTION_MASK | PTP_PIN_CFG_DOM);
+	val |= PTP_PIN_CFG_ACTION(PTP_PIN_ACTION_SAVE);
+	ocelot_write_rix(ocelot, val, PTP_PIN_CFG, TOD_ACC_PIN);
+
+	s = ocelot_read_rix(ocelot, PTP_PIN_TOD_SEC_MSB, TOD_ACC_PIN) & 0xffff;
+	s <<= 32;
+	s += ocelot_read_rix(ocelot, PTP_PIN_TOD_SEC_LSB, TOD_ACC_PIN);
+	ns = ocelot_read_rix(ocelot, PTP_PIN_TOD_NSEC, TOD_ACC_PIN);
+
+	spin_unlock_irqrestore(&ocelot->ptp_clock_lock, flags);
+
+	/* Deal with negative values */
+	if (ns >= 0x3ffffff0 && ns <= 0x3fffffff) {
+		s--;
+		ns &= 0xf;
+		ns += 999999984;
+	}
+
+	set_normalized_timespec64(ts, s, ns);
+	return 0;
+}
+EXPORT_SYMBOL(ocelot_ptp_gettime64);
+
+static int ocelot_ptp_settime64(struct ptp_clock_info *ptp,
+				const struct timespec64 *ts)
+{
+	struct ocelot *ocelot = container_of(ptp, struct ocelot, ptp_info);
+	unsigned long flags;
+	u32 val;
+
+	spin_lock_irqsave(&ocelot->ptp_clock_lock, flags);
+
+	val = ocelot_read_rix(ocelot, PTP_PIN_CFG, TOD_ACC_PIN);
+	val &= ~(PTP_PIN_CFG_SYNC | PTP_PIN_CFG_ACTION_MASK | PTP_PIN_CFG_DOM);
+	val |= PTP_PIN_CFG_ACTION(PTP_PIN_ACTION_IDLE);
+
+	ocelot_write_rix(ocelot, val, PTP_PIN_CFG, TOD_ACC_PIN);
+
+	ocelot_write_rix(ocelot, lower_32_bits(ts->tv_sec), PTP_PIN_TOD_SEC_LSB,
+			 TOD_ACC_PIN);
+	ocelot_write_rix(ocelot, upper_32_bits(ts->tv_sec), PTP_PIN_TOD_SEC_MSB,
+			 TOD_ACC_PIN);
+	ocelot_write_rix(ocelot, ts->tv_nsec, PTP_PIN_TOD_NSEC, TOD_ACC_PIN);
+
+	val = ocelot_read_rix(ocelot, PTP_PIN_CFG, TOD_ACC_PIN);
+	val &= ~(PTP_PIN_CFG_SYNC | PTP_PIN_CFG_ACTION_MASK | PTP_PIN_CFG_DOM);
+	val |= PTP_PIN_CFG_ACTION(PTP_PIN_ACTION_LOAD);
+
+	ocelot_write_rix(ocelot, val, PTP_PIN_CFG, TOD_ACC_PIN);
+
+	spin_unlock_irqrestore(&ocelot->ptp_clock_lock, flags);
+	return 0;
+}
+
+static int ocelot_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
+{
+	if (delta > -(NSEC_PER_SEC / 2) && delta < (NSEC_PER_SEC / 2)) {
+		struct ocelot *ocelot = container_of(ptp, struct ocelot, ptp_info);
+		unsigned long flags;
+		u32 val;
+
+		spin_lock_irqsave(&ocelot->ptp_clock_lock, flags);
+
+		val = ocelot_read_rix(ocelot, PTP_PIN_CFG, TOD_ACC_PIN);
+		val &= ~(PTP_PIN_CFG_SYNC | PTP_PIN_CFG_ACTION_MASK | PTP_PIN_CFG_DOM);
+		val |= PTP_PIN_CFG_ACTION(PTP_PIN_ACTION_IDLE);
+
+		ocelot_write_rix(ocelot, val, PTP_PIN_CFG, TOD_ACC_PIN);
+
+		ocelot_write_rix(ocelot, 0, PTP_PIN_TOD_SEC_LSB, TOD_ACC_PIN);
+		ocelot_write_rix(ocelot, 0, PTP_PIN_TOD_SEC_MSB, TOD_ACC_PIN);
+		ocelot_write_rix(ocelot, delta, PTP_PIN_TOD_NSEC, TOD_ACC_PIN);
+
+		val = ocelot_read_rix(ocelot, PTP_PIN_CFG, TOD_ACC_PIN);
+		val &= ~(PTP_PIN_CFG_SYNC | PTP_PIN_CFG_ACTION_MASK | PTP_PIN_CFG_DOM);
+		val |= PTP_PIN_CFG_ACTION(PTP_PIN_ACTION_DELTA);
+
+		ocelot_write_rix(ocelot, val, PTP_PIN_CFG, TOD_ACC_PIN);
+
+		spin_unlock_irqrestore(&ocelot->ptp_clock_lock, flags);
+	} else {
+		/* Fall back using ocelot_ptp_settime64 which is not exact. */
+		struct timespec64 ts;
+		u64 now;
+
+		ocelot_ptp_gettime64(ptp, &ts);
+
+		now = ktime_to_ns(timespec64_to_ktime(ts));
+		ts = ns_to_timespec64(now + delta);
+
+		ocelot_ptp_settime64(ptp, &ts);
+	}
+	return 0;
+}
+
+static int ocelot_ptp_adjfine(struct ptp_clock_info *ptp, long scaled_ppm)
+{
+	struct ocelot *ocelot = container_of(ptp, struct ocelot, ptp_info);
+	u32 unit = 0, direction = 0;
+	unsigned long flags;
+	u64 adj = 0;
+
+	spin_lock_irqsave(&ocelot->ptp_clock_lock, flags);
+
+	if (!scaled_ppm)
+		goto disable_adj;
+
+	if (scaled_ppm < 0) {
+		direction = PTP_CFG_CLK_ADJ_CFG_DIR;
+		scaled_ppm = -scaled_ppm;
+	}
+
+	adj = PSEC_PER_SEC << 16;
+	do_div(adj, scaled_ppm);
+	do_div(adj, 1000);
+
+	/* If the adjustment value is too large, use ns instead */
+	if (adj >= (1L << 30)) {
+		unit = PTP_CFG_CLK_ADJ_FREQ_NS;
+		do_div(adj, 1000);
+	}
+
+	/* Still too big */
+	if (adj >= (1L << 30))
+		goto disable_adj;
+
+	ocelot_write(ocelot, unit | adj, PTP_CLK_CFG_ADJ_FREQ);
+	ocelot_write(ocelot, PTP_CFG_CLK_ADJ_CFG_ENA | direction,
+		     PTP_CLK_CFG_ADJ_CFG);
+
+	spin_unlock_irqrestore(&ocelot->ptp_clock_lock, flags);
+	return 0;
+
+disable_adj:
+	ocelot_write(ocelot, 0, PTP_CLK_CFG_ADJ_CFG);
+
+	spin_unlock_irqrestore(&ocelot->ptp_clock_lock, flags);
+	return 0;
+}
+
+static struct ptp_clock_info ocelot_ptp_clock_info = {
+	.owner		= THIS_MODULE,
+	.name		= "ocelot ptp",
+	.max_adj	= 0x7fffffff,
+	.n_alarm	= 0,
+	.n_ext_ts	= 0,
+	.n_per_out	= 0,
+	.n_pins		= 0,
+	.pps		= 0,
+	.gettime64	= ocelot_ptp_gettime64,
+	.settime64	= ocelot_ptp_settime64,
+	.adjtime	= ocelot_ptp_adjtime,
+	.adjfine	= ocelot_ptp_adjfine,
+};
+
+static int ocelot_init_timestamp(struct ocelot *ocelot)
+{
+	ocelot->ptp_info = ocelot_ptp_clock_info;
+	ocelot->ptp_clock = ptp_clock_register(&ocelot->ptp_info, ocelot->dev);
+	if (IS_ERR(ocelot->ptp_clock))
+		return PTR_ERR(ocelot->ptp_clock);
+	/* Check if PHC support is missing at the configuration level */
+	if (!ocelot->ptp_clock)
+		return 0;
+
+	ocelot_write(ocelot, SYS_PTP_CFG_PTP_STAMP_WID(30), SYS_PTP_CFG);
+	ocelot_write(ocelot, 0xffffffff, ANA_TABLES_PTP_ID_LOW);
+	ocelot_write(ocelot, 0xffffffff, ANA_TABLES_PTP_ID_HIGH);
+
+	ocelot_write(ocelot, PTP_CFG_MISC_PTP_EN, PTP_CFG_MISC);
+
+	/* There is no device reconfiguration, PTP Rx stamping is always
+	 * enabled.
+	 */
+	ocelot->hwtstamp_config.rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
+
+	return 0;
+}
+
 int ocelot_probe_port(struct ocelot *ocelot, u8 port,
 		      void __iomem *regs,
 		      struct phy_device *phy)
@@ -1661,6 +2018,8 @@ int ocelot_probe_port(struct ocelot *ocelot, u8 port,
 	ocelot_mact_learn(ocelot, PGID_CPU, dev->dev_addr, ocelot_port->pvid,
 			  ENTRYTYPE_LOCKED);
 
+	INIT_LIST_HEAD(&ocelot_port->skbs);
+
 	err = register_netdev(dev);
 	if (err) {
 		dev_err(ocelot->dev, "register_netdev failed\n");
@@ -1684,7 +2043,7 @@ EXPORT_SYMBOL(ocelot_probe_port);
 int ocelot_init(struct ocelot *ocelot)
 {
 	u32 port;
-	int i, cpu = ocelot->num_phys_ports;
+	int i, ret, cpu = ocelot->num_phys_ports;
 	char queue_name[32];
 
 	ocelot->lags = devm_kcalloc(ocelot->dev, ocelot->num_phys_ports,
@@ -1699,6 +2058,8 @@ int ocelot_init(struct ocelot *ocelot)
 		return -ENOMEM;
 
 	mutex_init(&ocelot->stats_lock);
+	mutex_init(&ocelot->ptp_lock);
+	spin_lock_init(&ocelot->ptp_clock_lock);
 	snprintf(queue_name, sizeof(queue_name), "%s-stats",
 		 dev_name(ocelot->dev));
 	ocelot->stats_queue = create_singlethread_workqueue(queue_name);
@@ -1812,15 +2173,42 @@ int ocelot_init(struct ocelot *ocelot)
 	INIT_DELAYED_WORK(&ocelot->stats_work, ocelot_check_stats_work);
 	queue_delayed_work(ocelot->stats_queue, &ocelot->stats_work,
 			   OCELOT_STATS_CHECK_DELAY);
+
+	if (ocelot->ptp) {
+		ret = ocelot_init_timestamp(ocelot);
+		if (ret) {
+			dev_err(ocelot->dev,
+				"Timestamp initialization failed\n");
+			return ret;
+		}
+	}
+
 	return 0;
 }
 EXPORT_SYMBOL(ocelot_init);
 
 void ocelot_deinit(struct ocelot *ocelot)
 {
+	struct list_head *pos, *tmp;
+	struct ocelot_port *port;
+	struct ocelot_skb *entry;
+	int i;
+
 	destroy_workqueue(ocelot->stats_queue);
 	mutex_destroy(&ocelot->stats_lock);
 	ocelot_ace_deinit();
+
+	for (i = 0; i < ocelot->num_phys_ports; i++) {
+		port = ocelot->ports[i];
+
+		list_for_each_safe(pos, tmp, &port->skbs) {
+			entry = list_entry(pos, struct ocelot_skb, head);
+
+			list_del(pos);
+			dev_kfree_skb_any(entry->skb);
+			kfree(entry);
+		}
+	}
 }
 EXPORT_SYMBOL(ocelot_deinit);
 
diff --git a/drivers/net/ethernet/mscc/ocelot.h b/drivers/net/ethernet/mscc/ocelot.h
index 515dee6fa8a6..e40773c01a44 100644
--- a/drivers/net/ethernet/mscc/ocelot.h
+++ b/drivers/net/ethernet/mscc/ocelot.h
@@ -11,9 +11,11 @@
 #include <linux/bitops.h>
 #include <linux/etherdevice.h>
 #include <linux/if_vlan.h>
+#include <linux/net_tstamp.h>
 #include <linux/phy.h>
 #include <linux/phy/phy.h>
 #include <linux/platform_device.h>
+#include <linux/ptp_clock_kernel.h>
 #include <linux/regmap.h>
 
 #include "ocelot_ana.h"
@@ -39,6 +41,8 @@
 
 #define OCELOT_STATS_CHECK_DELAY (2 * HZ)
 
+#define OCELOT_PTP_QUEUE_SZ	128
+
 #define IFH_LEN 4
 
 struct frame_info {
@@ -46,6 +50,8 @@ struct frame_info {
 	u16 port;
 	u16 vid;
 	u8 tag_type;
+	u16 rew_op;
+	u32 timestamp;	/* rew_val */
 };
 
 #define IFH_INJ_BYPASS	BIT(31)
@@ -54,6 +60,12 @@ struct frame_info {
 #define IFH_TAG_TYPE_C 0
 #define IFH_TAG_TYPE_S 1
 
+#define IFH_REW_OP_NOOP			0x0
+#define IFH_REW_OP_DSCP			0x1
+#define IFH_REW_OP_ONE_STEP_PTP		0x2
+#define IFH_REW_OP_TWO_STEP_PTP		0x3
+#define IFH_REW_OP_ORIGIN_PTP		0x5
+
 #define OCELOT_SPEED_2500 0
 #define OCELOT_SPEED_1000 1
 #define OCELOT_SPEED_100  2
@@ -401,6 +413,13 @@ enum ocelot_regfield {
 	REGFIELD_MAX
 };
 
+enum ocelot_clk_pins {
+	ALT_PPS_PIN	= 1,
+	EXT_CLK_PIN,
+	ALT_LDST_PIN,
+	TOD_ACC_PIN
+};
+
 struct ocelot_multicast {
 	struct list_head list;
 	unsigned char addr[ETH_ALEN];
@@ -450,6 +469,13 @@ struct ocelot {
 	u64 *stats;
 	struct delayed_work stats_work;
 	struct workqueue_struct *stats_queue;
+
+	u8 ptp:1;
+	struct ptp_clock *ptp_clock;
+	struct ptp_clock_info ptp_info;
+	struct hwtstamp_config hwtstamp_config;
+	struct mutex ptp_lock; /* Protects the PTP interface state */
+	spinlock_t ptp_clock_lock; /* Protects the PTP clock */
 };
 
 struct ocelot_port {
@@ -473,6 +499,16 @@ struct ocelot_port {
 	struct phy *serdes;
 
 	struct ocelot_port_tc tc;
+
+	u8 ptp_cmd;
+	struct list_head skbs;
+	u8 ts_id;
+};
+
+struct ocelot_skb {
+	struct list_head head;
+	struct sk_buff *skb;
+	u8 id;
 };
 
 u32 __ocelot_read_ix(struct ocelot *ocelot, u32 reg, u32 offset);
@@ -517,4 +553,7 @@ extern struct notifier_block ocelot_netdevice_nb;
 extern struct notifier_block ocelot_switchdev_nb;
 extern struct notifier_block ocelot_switchdev_blocking_nb;
 
+int ocelot_ptp_gettime64(struct ptp_clock_info *ptp, struct timespec64 *ts);
+void ocelot_get_hwtimestamp(struct ocelot *ocelot, struct timespec64 *ts);
+
 #endif
diff --git a/drivers/net/ethernet/mscc/ocelot_board.c b/drivers/net/ethernet/mscc/ocelot_board.c
index df8d15994a89..0b14e7110e7f 100644
--- a/drivers/net/ethernet/mscc/ocelot_board.c
+++ b/drivers/net/ethernet/mscc/ocelot_board.c
@@ -31,6 +31,8 @@ static int ocelot_parse_ifh(u32 *_ifh, struct frame_info *info)
 
 	info->len = OCELOT_BUFFER_CELL_SZ * wlen + llen - 80;
 
+	info->timestamp = IFH_EXTRACT_BITFIELD64(ifh[0], 21, 32);
+
 	info->port = IFH_EXTRACT_BITFIELD64(ifh[1], 43, 4);
 
 	info->tag_type = IFH_EXTRACT_BITFIELD64(ifh[1], 16,  1);
@@ -98,7 +100,11 @@ static irqreturn_t ocelot_xtr_irq_handler(int irq, void *arg)
 		int sz, len, buf_len;
 		u32 ifh[4];
 		u32 val;
-		struct frame_info info;
+		struct frame_info info = {};
+		struct timespec64 ts;
+		struct skb_shared_hwtstamps *shhwtstamps;
+		u64 tod_in_ns;
+		u64 full_ts_in_ns;
 
 		for (i = 0; i < IFH_LEN; i++) {
 			err = ocelot_rx_frame_word(ocelot, grp, true, &ifh[i]);
@@ -145,6 +151,22 @@ static irqreturn_t ocelot_xtr_irq_handler(int irq, void *arg)
 			break;
 		}
 
+		if (ocelot->ptp) {
+			ocelot_ptp_gettime64(&ocelot->ptp_info, &ts);
+
+			tod_in_ns = ktime_set(ts.tv_sec, ts.tv_nsec);
+			if ((tod_in_ns & 0xffffffff) < info.timestamp)
+				full_ts_in_ns = (((tod_in_ns >> 32) - 1) << 32) |
+						info.timestamp;
+			else
+				full_ts_in_ns = (tod_in_ns & GENMASK_ULL(63, 32)) |
+						info.timestamp;
+
+			shhwtstamps = skb_hwtstamps(skb);
+			memset(shhwtstamps, 0, sizeof(struct skb_shared_hwtstamps));
+			shhwtstamps->hwtstamp = full_ts_in_ns;
+		}
+
 		/* Everything we see on an interface that is in the HW bridge
 		 * has already been forwarded.
 		 */
@@ -164,6 +186,70 @@ static irqreturn_t ocelot_xtr_irq_handler(int irq, void *arg)
 	return IRQ_HANDLED;
 }
 
+static irqreturn_t ocelot_ptp_rdy_irq_handler(int irq, void *arg)
+{
+	int budget = OCELOT_PTP_QUEUE_SZ;
+	struct ocelot *ocelot = arg;
+
+	do {
+		struct skb_shared_hwtstamps shhwtstamps;
+		struct list_head *pos, *tmp;
+		struct sk_buff *skb = NULL;
+		struct ocelot_skb *entry;
+		struct ocelot_port *port;
+		struct timespec64 ts;
+		u32 val, id, txport;
+
+		/* Prevent from infinite loop */
+		if (unlikely(!--budget))
+			break;
+
+		val = ocelot_read(ocelot, SYS_PTP_STATUS);
+
+		/* Check if a timestamp can be retrieved */
+		if (!(val & SYS_PTP_STATUS_PTP_MESS_VLD))
+			break;
+
+		WARN_ON(val & SYS_PTP_STATUS_PTP_OVFL);
+
+		/* Retrieve the ts ID and Tx port */
+		id = SYS_PTP_STATUS_PTP_MESS_ID_X(val);
+		txport = SYS_PTP_STATUS_PTP_MESS_TXPORT_X(val);
+
+		/* Retrieve its associated skb */
+		port = ocelot->ports[txport];
+
+		list_for_each_safe(pos, tmp, &port->skbs) {
+			entry = list_entry(pos, struct ocelot_skb, head);
+			if (entry->id != id)
+				continue;
+
+			skb = entry->skb;
+
+			list_del(pos);
+			kfree(entry);
+		}
+
+		/* Next ts */
+		ocelot_write(ocelot, SYS_PTP_NXT_PTP_NXT, SYS_PTP_NXT);
+
+		if (unlikely(!skb))
+			continue;
+
+		/* Get the h/w timestamp */
+		ocelot_get_hwtimestamp(ocelot, &ts);
+
+		/* 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, &shhwtstamps);
+
+		dev_kfree_skb_any(skb);
+	} while (true);
+
+	return IRQ_HANDLED;
+}
+
 static const struct of_device_id mscc_ocelot_match[] = {
 	{ .compatible = "mscc,vsc7514-switch" },
 	{ }
@@ -172,8 +258,8 @@ MODULE_DEVICE_TABLE(of, mscc_ocelot_match);
 
 static int mscc_ocelot_probe(struct platform_device *pdev)
 {
-	int err, irq;
 	unsigned int i;
+	int err, irq_xtr, irq_ptp_rdy;
 	struct device_node *np = pdev->dev.of_node;
 	struct device_node *ports, *portnp;
 	struct ocelot *ocelot;
@@ -232,16 +318,31 @@ static int mscc_ocelot_probe(struct platform_device *pdev)
 	if (err)
 		return err;
 
-	irq = platform_get_irq_byname(pdev, "xtr");
-	if (irq < 0)
+	irq_xtr = platform_get_irq_byname(pdev, "xtr");
+	if (irq_xtr < 0)
 		return -ENODEV;
 
-	err = devm_request_threaded_irq(&pdev->dev, irq, NULL,
+	err = devm_request_threaded_irq(&pdev->dev, irq_xtr, NULL,
 					ocelot_xtr_irq_handler, IRQF_ONESHOT,
 					"frame extraction", ocelot);
 	if (err)
 		return err;
 
+
+	irq_ptp_rdy = platform_get_irq_byname(pdev, "ptp_rdy");
+	if (irq_ptp_rdy > 0) {
+		err = devm_request_threaded_irq(&pdev->dev, irq_ptp_rdy, NULL,
+						ocelot_ptp_rdy_irq_handler,
+						IRQF_ONESHOT, "ptp ready",
+						ocelot);
+		if (err)
+			return err;
+
+		/* Check if we can support PTP */
+		if (ocelot->targets[PTP])
+			ocelot->ptp = 1;
+	}
+
 	regmap_field_write(ocelot->regfields[SYS_RESET_CFG_MEM_INIT], 1);
 	regmap_field_write(ocelot->regfields[SYS_RESET_CFG_MEM_ENA], 1);
 
-- 
2.21.0

Re: [PATCH net-next v4 6/6] net: mscc: PTP Hardware Clock (PHC) support

[Saeed Mahameed]

On Thu, 2019-07-25 at 16:27 +0200, Antoine Tenart wrote:
> This patch adds support for PTP Hardware Clock (PHC) to the Ocelot
> switch for both PTP 1-step and 2-step modes.
> 
> Signed-off-by: Antoine Tenart <antoine.tenart@bootlin.com>
> ---
>  drivers/net/ethernet/mscc/ocelot.c       | 394
> ++++++++++++++++++++++-
>  drivers/net/ethernet/mscc/ocelot.h       |  39 +++
>  drivers/net/ethernet/mscc/ocelot_board.c | 111 ++++++-
>  3 files changed, 536 insertions(+), 8 deletions(-)
> 
> diff --git a/drivers/net/ethernet/mscc/ocelot.c
> b/drivers/net/ethernet/mscc/ocelot.c
> index b71e4ecbe469..b08fcec73a30 100644
> --- a/drivers/net/ethernet/mscc/ocelot.c
> +++ b/drivers/net/ethernet/mscc/ocelot.c
> @@ -14,6 +14,7 @@
>  #include <linux/module.h>
>  #include <linux/netdevice.h>
>  #include <linux/phy.h>
> +#include <linux/ptp_clock_kernel.h>
>  #include <linux/skbuff.h>
>  #include <linux/iopoll.h>
>  #include <net/arp.h>
> @@ -538,7 +539,7 @@ static int ocelot_port_stop(struct net_device
> *dev)
>   */
>  static int ocelot_gen_ifh(u32 *ifh, struct frame_info *info)
>  {
> -	ifh[0] = IFH_INJ_BYPASS;
> +	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;
> @@ -548,6 +549,7 @@ static int ocelot_gen_ifh(u32 *ifh, struct
> frame_info *info)
>  
>  static int ocelot_port_xmit(struct sk_buff *skb, struct net_device
> *dev)
>  {
> +	struct skb_shared_info *shinfo = skb_shinfo(skb);
>  	struct ocelot_port *port = netdev_priv(dev);
>  	struct ocelot *ocelot = port->ocelot;
>  	u32 val, ifh[IFH_LEN];
> @@ -566,6 +568,14 @@ static int ocelot_port_xmit(struct sk_buff *skb,
> struct net_device *dev)
>  	info.port = BIT(port->chip_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 = port->ptp_cmd;
> +		if (port->ptp_cmd == IFH_REW_OP_TWO_STEP_PTP)
> +			info.rew_op |= (port->ts_id  % 4) << 3;
> +	}
> +
>  	ocelot_gen_ifh(ifh, &info);
>  
>  	for (i = 0; i < IFH_LEN; i++)
> @@ -596,11 +606,51 @@ static int ocelot_port_xmit(struct sk_buff
> *skb, struct net_device *dev)
>  
>  	dev->stats.tx_packets++;
>  	dev->stats.tx_bytes += skb->len;
> -	dev_kfree_skb_any(skb);
> +
> +	if (ocelot->ptp && shinfo->tx_flags & SKBTX_HW_TSTAMP &&
> +	    port->ptp_cmd == IFH_REW_OP_TWO_STEP_PTP) {
> +		struct ocelot_skb *oskb =
> +			kzalloc(sizeof(struct ocelot_skb), GFP_ATOMIC);
> +

Device drivers normally pre allocate descriptor info ring array to
avoid dynamic atomic allocations of private data on data path.

> +		oskb->skb = skb;
> +		oskb->id = port->ts_id % 4;
> +		port->ts_id++;

missing skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS; ?
see 3.1 Hardware Timestamping Implementation: Device Drivers
https://www.kernel.org/doc/Documentation/networking/timestamping.txt

> +
> +		list_add_tail(&oskb->head, &port->skbs);
> +	} else {
> +		dev_kfree_skb_any(skb);
> +	}
>  
>  	return NETDEV_TX_OK;
>  }
>  
> +void ocelot_get_hwtimestamp(struct ocelot *ocelot, struct timespec64
> *ts)
> +{
> +	unsigned long flags;
> +	u32 val;
> +
> +	spin_lock_irqsave(&ocelot->ptp_clock_lock, flags);
> +
> +	/* Read current PTP time to get seconds */
> +	val = ocelot_read_rix(ocelot, PTP_PIN_CFG, TOD_ACC_PIN);
> +
> +	val &= ~(PTP_PIN_CFG_SYNC | PTP_PIN_CFG_ACTION_MASK |
> PTP_PIN_CFG_DOM);
> +	val |= PTP_PIN_CFG_ACTION(PTP_PIN_ACTION_SAVE);
> +	ocelot_write_rix(ocelot, val, PTP_PIN_CFG, TOD_ACC_PIN);
> +	ts->tv_sec = ocelot_read_rix(ocelot, PTP_PIN_TOD_SEC_LSB,
> TOD_ACC_PIN);
> +
> +	/* Read packet HW timestamp from FIFO */
> +	val = ocelot_read(ocelot, SYS_PTP_TXSTAMP);
> +	ts->tv_nsec = SYS_PTP_TXSTAMP_PTP_TXSTAMP(val);
> +
> +	/* Sec has incremented since the ts was registered */
> +	if ((ts->tv_sec & 0x1) != !!(val &
> SYS_PTP_TXSTAMP_PTP_TXSTAMP_SEC))
> +		ts->tv_sec--;
> +
> +	spin_unlock_irqrestore(&ocelot->ptp_clock_lock, flags);
> +}
> +EXPORT_SYMBOL(ocelot_get_hwtimestamp);
> +

Why EXPORT_SYMBOL? this is the last patch and it is touching one
driver.

>  static int ocelot_mc_unsync(struct net_device *dev, const unsigned
> char *addr)
>  {
>  	struct ocelot_port *port = netdev_priv(dev);
> @@ -917,6 +967,97 @@ static int ocelot_get_port_parent_id(struct
> net_device *dev,
>  	return 0;
>  }
>  
> +static int ocelot_hwstamp_get(struct ocelot_port *port, struct ifreq
> *ifr)
> +{
> +	struct ocelot *ocelot = port->ocelot;
> +
> +	return copy_to_user(ifr->ifr_data, &ocelot->hwtstamp_config,
> +			    sizeof(ocelot->hwtstamp_config)) ? -EFAULT
> : 0;
> +}
> +
> +static int ocelot_hwstamp_set(struct ocelot_port *port, struct ifreq
> *ifr)
> +{
> +	struct ocelot *ocelot = port->ocelot;
> +	struct hwtstamp_config cfg;
> +
> +	if (copy_from_user(&cfg, ifr->ifr_data, sizeof(cfg)))
> +		return -EFAULT;
> +
> +	/* reserved for future extensions */
> +	if (cfg.flags)
> +		return -EINVAL;
> +
> +	/* Tx type sanity check */
> +	switch (cfg.tx_type) {
> +	case HWTSTAMP_TX_ON:
> +		port->ptp_cmd = IFH_REW_OP_TWO_STEP_PTP;
> +		break;
> +	case HWTSTAMP_TX_ONESTEP_SYNC:
> +		/* IFH_REW_OP_ONE_STEP_PTP updates the correctional
> field, we
> +		 * need to update the origin time.
> +		 */
> +		port->ptp_cmd = IFH_REW_OP_ORIGIN_PTP;
> +		break;
> +	case HWTSTAMP_TX_OFF:
> +		port->ptp_cmd = 0;
> +		break;
> +	default:
> +		return -ERANGE;
> +	}
> +
> +	mutex_lock(&ocelot->ptp_lock);
> +
> +	switch (cfg.rx_filter) {
> +	case HWTSTAMP_FILTER_NONE:
> +		break;
> +	case HWTSTAMP_FILTER_ALL:
> +	case HWTSTAMP_FILTER_SOME:
> +	case HWTSTAMP_FILTER_PTP_V1_L4_EVENT:
> +	case HWTSTAMP_FILTER_PTP_V1_L4_SYNC:
> +	case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ:
> +	case HWTSTAMP_FILTER_NTP_ALL:
> +	case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
> +	case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
> +	case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
> +	case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
> +	case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
> +	case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
> +	case HWTSTAMP_FILTER_PTP_V2_EVENT:
> +	case HWTSTAMP_FILTER_PTP_V2_SYNC:
> +	case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
> +		cfg.rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
> +		break;
> +	default:
> +		mutex_unlock(&ocelot->ptp_lock);
> +		return -ERANGE;
> +	}
> +
> +	/* Commit back the result & save it */
> +	memcpy(&ocelot->hwtstamp_config, &cfg, sizeof(cfg));
> +	mutex_unlock(&ocelot->ptp_lock);
> +
> +	return copy_to_user(ifr->ifr_data, &cfg, sizeof(cfg)) ? -EFAULT
> : 0;
> +}
> +
> +static int ocelot_ioctl(struct net_device *dev, struct ifreq *ifr,
> int cmd)
> +{
> +	struct ocelot_port *port = netdev_priv(dev);
> +	struct ocelot *ocelot = port->ocelot;
> +
> +	/* The function is only used for PTP operations for now */
> +	if (!ocelot->ptp)
> +		return -EOPNOTSUPP;
> +
> +	switch (cmd) {
> +	case SIOCSHWTSTAMP:
> +		return ocelot_hwstamp_set(port, ifr);
> +	case SIOCGHWTSTAMP:
> +		return ocelot_hwstamp_get(port, ifr);
> +	default:
> +		return -EOPNOTSUPP;
> +	}
> +}
> +
>  static const struct net_device_ops ocelot_port_netdev_ops = {
>  	.ndo_open			= ocelot_port_open,
>  	.ndo_stop			= ocelot_port_stop,
> @@ -933,6 +1074,7 @@ static const struct net_device_ops
> ocelot_port_netdev_ops = {
>  	.ndo_set_features		= ocelot_set_features,
>  	.ndo_get_port_parent_id		= ocelot_get_port_parent_id,
>  	.ndo_setup_tc			= ocelot_setup_tc,
> +	.ndo_do_ioctl			= ocelot_ioctl,
>  };
>  
>  static void ocelot_get_strings(struct net_device *netdev, u32 sset,
> u8 *data)
> @@ -1014,12 +1156,37 @@ static int ocelot_get_sset_count(struct
> net_device *dev, int sset)
>  	return ocelot->num_stats;
>  }
>  
> +static int ocelot_get_ts_info(struct net_device *dev,
> +			      struct ethtool_ts_info *info)
> +{
> +	struct ocelot_port *ocelot_port = netdev_priv(dev);
> +	struct ocelot *ocelot = ocelot_port->ocelot;
> +
> +	if (!ocelot->ptp)
> +		return ethtool_op_get_ts_info(dev, info);
> +
> +	info->phc_index = ocelot->ptp_clock ?
> +			  ptp_clock_index(ocelot->ptp_clock) : -1;
> +	info->so_timestamping |= SOF_TIMESTAMPING_TX_SOFTWARE |
> +				 SOF_TIMESTAMPING_RX_SOFTWARE |
> +				 SOF_TIMESTAMPING_SOFTWARE |
> +				 SOF_TIMESTAMPING_TX_HARDWARE |
> +				 SOF_TIMESTAMPING_RX_HARDWARE |
> +				 SOF_TIMESTAMPING_RAW_HARDWARE;
> +	info->tx_types = BIT(HWTSTAMP_TX_OFF) | BIT(HWTSTAMP_TX_ON) |
> +			 BIT(HWTSTAMP_TX_ONESTEP_SYNC);
> +	info->rx_filters = BIT(HWTSTAMP_FILTER_NONE) |
> BIT(HWTSTAMP_FILTER_ALL);
> +
> +	return 0;
> +}
> +
>  static const struct ethtool_ops ocelot_ethtool_ops = {
>  	.get_strings		= ocelot_get_strings,
>  	.get_ethtool_stats	= ocelot_get_ethtool_stats,
>  	.get_sset_count		= ocelot_get_sset_count,
>  	.get_link_ksettings	= phy_ethtool_get_link_ksettings,
>  	.set_link_ksettings	= phy_ethtool_set_link_ksettings,
> +	.get_ts_info		= ocelot_get_ts_info,
>  };
>  
>  static int ocelot_port_attr_stp_state_set(struct ocelot_port
> *ocelot_port,
> @@ -1629,6 +1796,196 @@ struct notifier_block
> ocelot_switchdev_blocking_nb __read_mostly = {
>  };
>  EXPORT_SYMBOL(ocelot_switchdev_blocking_nb);
>  
> +int ocelot_ptp_gettime64(struct ptp_clock_info *ptp, struct
> timespec64 *ts)
> +{
> +	struct ocelot *ocelot = container_of(ptp, struct ocelot,
> ptp_info);
> +	unsigned long flags;
> +	time64_t s;
> +	u32 val;
> +	s64 ns;
> +
> +	spin_lock_irqsave(&ocelot->ptp_clock_lock, flags);
> +
> +	val = ocelot_read_rix(ocelot, PTP_PIN_CFG, TOD_ACC_PIN);
> +	val &= ~(PTP_PIN_CFG_SYNC | PTP_PIN_CFG_ACTION_MASK |
> PTP_PIN_CFG_DOM);
> +	val |= PTP_PIN_CFG_ACTION(PTP_PIN_ACTION_SAVE);
> +	ocelot_write_rix(ocelot, val, PTP_PIN_CFG, TOD_ACC_PIN);
> +
> +	s = ocelot_read_rix(ocelot, PTP_PIN_TOD_SEC_MSB, TOD_ACC_PIN) &
> 0xffff;
> +	s <<= 32;
> +	s += ocelot_read_rix(ocelot, PTP_PIN_TOD_SEC_LSB, TOD_ACC_PIN);
> +	ns = ocelot_read_rix(ocelot, PTP_PIN_TOD_NSEC, TOD_ACC_PIN);
> +
> +	spin_unlock_irqrestore(&ocelot->ptp_clock_lock, flags);
> +
> +	/* Deal with negative values */
> +	if (ns >= 0x3ffffff0 && ns <= 0x3fffffff) {
> +		s--;
> +		ns &= 0xf;
> +		ns += 999999984;
> +	}
> +
> +	set_normalized_timespec64(ts, s, ns);
> +	return 0;
> +}
> +EXPORT_SYMBOL(ocelot_ptp_gettime64);
> +
> +static int ocelot_ptp_settime64(struct ptp_clock_info *ptp,
> +				const struct timespec64 *ts)
> +{
> +	struct ocelot *ocelot = container_of(ptp, struct ocelot,
> ptp_info);
> +	unsigned long flags;
> +	u32 val;
> +
> +	spin_lock_irqsave(&ocelot->ptp_clock_lock, flags);
> +
> +	val = ocelot_read_rix(ocelot, PTP_PIN_CFG, TOD_ACC_PIN);
> +	val &= ~(PTP_PIN_CFG_SYNC | PTP_PIN_CFG_ACTION_MASK |
> PTP_PIN_CFG_DOM);
> +	val |= PTP_PIN_CFG_ACTION(PTP_PIN_ACTION_IDLE);
> +
> +	ocelot_write_rix(ocelot, val, PTP_PIN_CFG, TOD_ACC_PIN);
> +
> +	ocelot_write_rix(ocelot, lower_32_bits(ts->tv_sec),
> PTP_PIN_TOD_SEC_LSB,
> +			 TOD_ACC_PIN);
> +	ocelot_write_rix(ocelot, upper_32_bits(ts->tv_sec),
> PTP_PIN_TOD_SEC_MSB,
> +			 TOD_ACC_PIN);
> +	ocelot_write_rix(ocelot, ts->tv_nsec, PTP_PIN_TOD_NSEC,
> TOD_ACC_PIN);
> +
> +	val = ocelot_read_rix(ocelot, PTP_PIN_CFG, TOD_ACC_PIN);
> +	val &= ~(PTP_PIN_CFG_SYNC | PTP_PIN_CFG_ACTION_MASK |
> PTP_PIN_CFG_DOM);
> +	val |= PTP_PIN_CFG_ACTION(PTP_PIN_ACTION_LOAD);
> +
> +	ocelot_write_rix(ocelot, val, PTP_PIN_CFG, TOD_ACC_PIN);
> +
> +	spin_unlock_irqrestore(&ocelot->ptp_clock_lock, flags);
> +	return 0;
> +}
> +
> +static int ocelot_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
> +{
> +	if (delta > -(NSEC_PER_SEC / 2) && delta < (NSEC_PER_SEC / 2))
> {
> +		struct ocelot *ocelot = container_of(ptp, struct
> ocelot, ptp_info);
> +		unsigned long flags;
> +		u32 val;
> +
> +		spin_lock_irqsave(&ocelot->ptp_clock_lock, flags);
> +
> +		val = ocelot_read_rix(ocelot, PTP_PIN_CFG,
> TOD_ACC_PIN);
> +		val &= ~(PTP_PIN_CFG_SYNC | PTP_PIN_CFG_ACTION_MASK |
> PTP_PIN_CFG_DOM);
> +		val |= PTP_PIN_CFG_ACTION(PTP_PIN_ACTION_IDLE);
> +
> +		ocelot_write_rix(ocelot, val, PTP_PIN_CFG,
> TOD_ACC_PIN);
> +
> +		ocelot_write_rix(ocelot, 0, PTP_PIN_TOD_SEC_LSB,
> TOD_ACC_PIN);
> +		ocelot_write_rix(ocelot, 0, PTP_PIN_TOD_SEC_MSB,
> TOD_ACC_PIN);
> +		ocelot_write_rix(ocelot, delta, PTP_PIN_TOD_NSEC,
> TOD_ACC_PIN);
> +
> +		val = ocelot_read_rix(ocelot, PTP_PIN_CFG,
> TOD_ACC_PIN);
> +		val &= ~(PTP_PIN_CFG_SYNC | PTP_PIN_CFG_ACTION_MASK |
> PTP_PIN_CFG_DOM);
> +		val |= PTP_PIN_CFG_ACTION(PTP_PIN_ACTION_DELTA);
> +
> +		ocelot_write_rix(ocelot, val, PTP_PIN_CFG,
> TOD_ACC_PIN);
> +
> +		spin_unlock_irqrestore(&ocelot->ptp_clock_lock, flags);
> +	} else {
> +		/* Fall back using ocelot_ptp_settime64 which is not
> exact. */
> +		struct timespec64 ts;
> +		u64 now;
> +
> +		ocelot_ptp_gettime64(ptp, &ts);
> +
> +		now = ktime_to_ns(timespec64_to_ktime(ts));
> +		ts = ns_to_timespec64(now + delta);
> +
> +		ocelot_ptp_settime64(ptp, &ts);
> +	}
> +	return 0;
> +}
> +
> +static int ocelot_ptp_adjfine(struct ptp_clock_info *ptp, long
> scaled_ppm)
> +{
> +	struct ocelot *ocelot = container_of(ptp, struct ocelot,
> ptp_info);
> +	u32 unit = 0, direction = 0;
> +	unsigned long flags;
> +	u64 adj = 0;
> +
> +	spin_lock_irqsave(&ocelot->ptp_clock_lock, flags);
> +
> +	if (!scaled_ppm)
> +		goto disable_adj;
> +
> +	if (scaled_ppm < 0) {
> +		direction = PTP_CFG_CLK_ADJ_CFG_DIR;
> +		scaled_ppm = -scaled_ppm;
> +	}
> +
> +	adj = PSEC_PER_SEC << 16;
> +	do_div(adj, scaled_ppm);
> +	do_div(adj, 1000);
> +
> +	/* If the adjustment value is too large, use ns instead */
> +	if (adj >= (1L << 30)) {
> +		unit = PTP_CFG_CLK_ADJ_FREQ_NS;
> +		do_div(adj, 1000);
> +	}
> +
> +	/* Still too big */
> +	if (adj >= (1L << 30))
> +		goto disable_adj;
> +
> +	ocelot_write(ocelot, unit | adj, PTP_CLK_CFG_ADJ_FREQ);
> +	ocelot_write(ocelot, PTP_CFG_CLK_ADJ_CFG_ENA | direction,
> +		     PTP_CLK_CFG_ADJ_CFG);
> +
> +	spin_unlock_irqrestore(&ocelot->ptp_clock_lock, flags);
> +	return 0;
> +
> +disable_adj:
> +	ocelot_write(ocelot, 0, PTP_CLK_CFG_ADJ_CFG);
> +
> +	spin_unlock_irqrestore(&ocelot->ptp_clock_lock, flags);
> +	return 0;
> +}
> +
> +static struct ptp_clock_info ocelot_ptp_clock_info = {
> +	.owner		= THIS_MODULE,
> +	.name		= "ocelot ptp",
> +	.max_adj	= 0x7fffffff,
> +	.n_alarm	= 0,
> +	.n_ext_ts	= 0,
> +	.n_per_out	= 0,
> +	.n_pins		= 0,
> +	.pps		= 0,
> +	.gettime64	= ocelot_ptp_gettime64,
> +	.settime64	= ocelot_ptp_settime64,
> +	.adjtime	= ocelot_ptp_adjtime,
> +	.adjfine	= ocelot_ptp_adjfine,
> +};
> +
> +static int ocelot_init_timestamp(struct ocelot *ocelot)
> +{
> +	ocelot->ptp_info = ocelot_ptp_clock_info;
> +	ocelot->ptp_clock = ptp_clock_register(&ocelot->ptp_info,
> ocelot->dev);
> +	if (IS_ERR(ocelot->ptp_clock))
> +		return PTR_ERR(ocelot->ptp_clock);
> +	/* Check if PHC support is missing at the configuration level
> */
> +	if (!ocelot->ptp_clock)
> +		return 0;
> +
> +	ocelot_write(ocelot, SYS_PTP_CFG_PTP_STAMP_WID(30),
> SYS_PTP_CFG);
> +	ocelot_write(ocelot, 0xffffffff, ANA_TABLES_PTP_ID_LOW);
> +	ocelot_write(ocelot, 0xffffffff, ANA_TABLES_PTP_ID_HIGH);
> +
> +	ocelot_write(ocelot, PTP_CFG_MISC_PTP_EN, PTP_CFG_MISC);
> +
> +	/* There is no device reconfiguration, PTP Rx stamping is
> always
> +	 * enabled.
> +	 */
> +	ocelot->hwtstamp_config.rx_filter =
> HWTSTAMP_FILTER_PTP_V2_EVENT;
> +
> +	return 0;
> +}
> +
>  int ocelot_probe_port(struct ocelot *ocelot, u8 port,
>  		      void __iomem *regs,
>  		      struct phy_device *phy)
> @@ -1661,6 +2018,8 @@ int ocelot_probe_port(struct ocelot *ocelot, u8
> port,
>  	ocelot_mact_learn(ocelot, PGID_CPU, dev->dev_addr, ocelot_port-
> >pvid,
>  			  ENTRYTYPE_LOCKED);
>  
> +	INIT_LIST_HEAD(&ocelot_port->skbs);
> +
>  	err = register_netdev(dev);
>  	if (err) {
>  		dev_err(ocelot->dev, "register_netdev failed\n");
> @@ -1684,7 +2043,7 @@ EXPORT_SYMBOL(ocelot_probe_port);
>  int ocelot_init(struct ocelot *ocelot)
>  {
>  	u32 port;
> -	int i, cpu = ocelot->num_phys_ports;
> +	int i, ret, cpu = ocelot->num_phys_ports;
>  	char queue_name[32];
>  
>  	ocelot->lags = devm_kcalloc(ocelot->dev, ocelot-
> >num_phys_ports,
> @@ -1699,6 +2058,8 @@ int ocelot_init(struct ocelot *ocelot)
>  		return -ENOMEM;
>  
>  	mutex_init(&ocelot->stats_lock);
> +	mutex_init(&ocelot->ptp_lock);
> +	spin_lock_init(&ocelot->ptp_clock_lock);
>  	snprintf(queue_name, sizeof(queue_name), "%s-stats",
>  		 dev_name(ocelot->dev));
>  	ocelot->stats_queue =
> create_singlethread_workqueue(queue_name);
> @@ -1812,15 +2173,42 @@ int ocelot_init(struct ocelot *ocelot)
>  	INIT_DELAYED_WORK(&ocelot->stats_work,
> ocelot_check_stats_work);
>  	queue_delayed_work(ocelot->stats_queue, &ocelot->stats_work,
>  			   OCELOT_STATS_CHECK_DELAY);
> +
> +	if (ocelot->ptp) {
> +		ret = ocelot_init_timestamp(ocelot);
> +		if (ret) {
> +			dev_err(ocelot->dev,
> +				"Timestamp initialization failed\n");
> +			return ret;
> +		}
> +	}
> +
>  	return 0;
>  }
>  EXPORT_SYMBOL(ocelot_init);
>  
>  void ocelot_deinit(struct ocelot *ocelot)
>  {
> +	struct list_head *pos, *tmp;
> +	struct ocelot_port *port;
> +	struct ocelot_skb *entry;
> +	int i;
> +
>  	destroy_workqueue(ocelot->stats_queue);
>  	mutex_destroy(&ocelot->stats_lock);
>  	ocelot_ace_deinit();
> +
> +	for (i = 0; i < ocelot->num_phys_ports; i++) {
> +		port = ocelot->ports[i];
> +
> +		list_for_each_safe(pos, tmp, &port->skbs) {
> +			entry = list_entry(pos, struct ocelot_skb,
> head);
> +
> +			list_del(pos);
> +			dev_kfree_skb_any(entry->skb);
> +			kfree(entry);
> +		}
> +	}
>  }
>  EXPORT_SYMBOL(ocelot_deinit);
>  
> diff --git a/drivers/net/ethernet/mscc/ocelot.h
> b/drivers/net/ethernet/mscc/ocelot.h
> index 515dee6fa8a6..e40773c01a44 100644
> --- a/drivers/net/ethernet/mscc/ocelot.h
> +++ b/drivers/net/ethernet/mscc/ocelot.h
> @@ -11,9 +11,11 @@
>  #include <linux/bitops.h>
>  #include <linux/etherdevice.h>
>  #include <linux/if_vlan.h>
> +#include <linux/net_tstamp.h>
>  #include <linux/phy.h>
>  #include <linux/phy/phy.h>
>  #include <linux/platform_device.h>
> +#include <linux/ptp_clock_kernel.h>
>  #include <linux/regmap.h>
>  
>  #include "ocelot_ana.h"
> @@ -39,6 +41,8 @@
>  
>  #define OCELOT_STATS_CHECK_DELAY (2 * HZ)
>  
> +#define OCELOT_PTP_QUEUE_SZ	128
> +
>  #define IFH_LEN 4
>  
>  struct frame_info {
> @@ -46,6 +50,8 @@ struct frame_info {
>  	u16 port;
>  	u16 vid;
>  	u8 tag_type;
> +	u16 rew_op;
> +	u32 timestamp;	/* rew_val */
>  };
>  
>  #define IFH_INJ_BYPASS	BIT(31)
> @@ -54,6 +60,12 @@ struct frame_info {
>  #define IFH_TAG_TYPE_C 0
>  #define IFH_TAG_TYPE_S 1
>  
> +#define IFH_REW_OP_NOOP			0x0
> +#define IFH_REW_OP_DSCP			0x1
> +#define IFH_REW_OP_ONE_STEP_PTP		0x2
> +#define IFH_REW_OP_TWO_STEP_PTP		0x3
> +#define IFH_REW_OP_ORIGIN_PTP		0x5
> +
>  #define OCELOT_SPEED_2500 0
>  #define OCELOT_SPEED_1000 1
>  #define OCELOT_SPEED_100  2
> @@ -401,6 +413,13 @@ enum ocelot_regfield {
>  	REGFIELD_MAX
>  };
>  
> +enum ocelot_clk_pins {
> +	ALT_PPS_PIN	= 1,
> +	EXT_CLK_PIN,
> +	ALT_LDST_PIN,
> +	TOD_ACC_PIN
> +};
> +
>  struct ocelot_multicast {
>  	struct list_head list;
>  	unsigned char addr[ETH_ALEN];
> @@ -450,6 +469,13 @@ struct ocelot {
>  	u64 *stats;
>  	struct delayed_work stats_work;
>  	struct workqueue_struct *stats_queue;
> +
> +	u8 ptp:1;
> +	struct ptp_clock *ptp_clock;
> +	struct ptp_clock_info ptp_info;
> +	struct hwtstamp_config hwtstamp_config;
> +	struct mutex ptp_lock; /* Protects the PTP interface state */
> +	spinlock_t ptp_clock_lock; /* Protects the PTP clock */
>  };
>  
>  struct ocelot_port {
> @@ -473,6 +499,16 @@ struct ocelot_port {
>  	struct phy *serdes;
>  
>  	struct ocelot_port_tc tc;
> +
> +	u8 ptp_cmd;
> +	struct list_head skbs;
> +	u8 ts_id;
> +};
> +
> +struct ocelot_skb {
> +	struct list_head head;
> +	struct sk_buff *skb;
> +	u8 id;
>  };
>  
>  u32 __ocelot_read_ix(struct ocelot *ocelot, u32 reg, u32 offset);
> @@ -517,4 +553,7 @@ extern struct notifier_block ocelot_netdevice_nb;
>  extern struct notifier_block ocelot_switchdev_nb;
>  extern struct notifier_block ocelot_switchdev_blocking_nb;
>  
> +int ocelot_ptp_gettime64(struct ptp_clock_info *ptp, struct
> timespec64 *ts);
> +void ocelot_get_hwtimestamp(struct ocelot *ocelot, struct timespec64
> *ts);
> +
>  #endif
> diff --git a/drivers/net/ethernet/mscc/ocelot_board.c
> b/drivers/net/ethernet/mscc/ocelot_board.c
> index df8d15994a89..0b14e7110e7f 100644
> --- a/drivers/net/ethernet/mscc/ocelot_board.c
> +++ b/drivers/net/ethernet/mscc/ocelot_board.c
> @@ -31,6 +31,8 @@ static int ocelot_parse_ifh(u32 *_ifh, struct
> frame_info *info)
>  
>  	info->len = OCELOT_BUFFER_CELL_SZ * wlen + llen - 80;
>  
> +	info->timestamp = IFH_EXTRACT_BITFIELD64(ifh[0], 21, 32);
> +
>  	info->port = IFH_EXTRACT_BITFIELD64(ifh[1], 43, 4);
>  
>  	info->tag_type = IFH_EXTRACT_BITFIELD64(ifh[1], 16,  1);
> @@ -98,7 +100,11 @@ static irqreturn_t ocelot_xtr_irq_handler(int
> irq, void *arg)
>  		int sz, len, buf_len;
>  		u32 ifh[4];
>  		u32 val;
> -		struct frame_info info;
> +		struct frame_info info = {};
> +		struct timespec64 ts;
> +		struct skb_shared_hwtstamps *shhwtstamps;
> +		u64 tod_in_ns;
> +		u64 full_ts_in_ns;

reverse xmas tree.

>  
>  		for (i = 0; i < IFH_LEN; i++) {
>  			err = ocelot_rx_frame_word(ocelot, grp, true,
> &ifh[i]);
> @@ -145,6 +151,22 @@ static irqreturn_t ocelot_xtr_irq_handler(int
> irq, void *arg)
>  			break;
>  		}
>  
> +		if (ocelot->ptp) {
> +			ocelot_ptp_gettime64(&ocelot->ptp_info, &ts);
> +
> +			tod_in_ns = ktime_set(ts.tv_sec, ts.tv_nsec);
> +			if ((tod_in_ns & 0xffffffff) < info.timestamp)
> +				full_ts_in_ns = (((tod_in_ns >> 32) -
> 1) << 32) |
> +						info.timestamp;
> +			else
> +				full_ts_in_ns = (tod_in_ns &
> GENMASK_ULL(63, 32)) |
> +						info.timestamp;
> +
> +			shhwtstamps = skb_hwtstamps(skb);
> +			memset(shhwtstamps, 0, sizeof(struct
> skb_shared_hwtstamps));
> +			shhwtstamps->hwtstamp = full_ts_in_ns;

the right way to set the timestamp is by calling: 
skb_tstamp_tx(skb, &tstamp);

> +		}
> +
>  		/* Everything we see on an interface that is in the HW
> bridge
>  		 * has already been forwarded.
>  		 */
> @@ -164,6 +186,70 @@ static irqreturn_t ocelot_xtr_irq_handler(int
> irq, void *arg)
>  	return IRQ_HANDLED;
>  }
>  
> +static irqreturn_t ocelot_ptp_rdy_irq_handler(int irq, void *arg)
> +{
> +	int budget = OCELOT_PTP_QUEUE_SZ;
> +	struct ocelot *ocelot = arg;
> +
> +	do {
> +		struct skb_shared_hwtstamps shhwtstamps;
> +		struct list_head *pos, *tmp;
> +		struct sk_buff *skb = NULL;
> +		struct ocelot_skb *entry;
> +		struct ocelot_port *port;
> +		struct timespec64 ts;
> +		u32 val, id, txport;
> +
> +		/* Prevent from infinite loop */
> +		if (unlikely(!--budget))
> +			break;

when budget gets to 1 you break, while you still have 1 to go :)

I assume OCELOT_PTP_QUEUE_SZ > 0, just make this the loop condition and
avoid infinite loops by design.

> +
> +		val = ocelot_read(ocelot, SYS_PTP_STATUS);
> +
> +		/* Check if a timestamp can be retrieved */
> +		if (!(val & SYS_PTP_STATUS_PTP_MESS_VLD))
> +			break;
> +
> +		WARN_ON(val & SYS_PTP_STATUS_PTP_OVFL);
> +
> +		/* Retrieve the ts ID and Tx port */
> +		id = SYS_PTP_STATUS_PTP_MESS_ID_X(val);
> +		txport = SYS_PTP_STATUS_PTP_MESS_TXPORT_X(val);
> +
> +		/* Retrieve its associated skb */
> +		port = ocelot->ports[txport];
> +
> +		list_for_each_safe(pos, tmp, &port->skbs) {
> +			entry = list_entry(pos, struct ocelot_skb,
> head);
> +			if (entry->id != id)
> +				continue;
> +
> +			skb = entry->skb;
> +
> +			list_del(pos);
> +			kfree(entry);
> +		}
> +
> +		/* Next ts */
> +		ocelot_write(ocelot, SYS_PTP_NXT_PTP_NXT, SYS_PTP_NXT);
> +
> +		if (unlikely(!skb))
> +			continue;
> +
> +		/* Get the h/w timestamp */
> +		ocelot_get_hwtimestamp(ocelot, &ts);
> +
> +		/* 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, &shhwtstamps);
> +
> +		dev_kfree_skb_any(skb);
> +	} while (true);
> +
> +	return IRQ_HANDLED;
> +}
> +
>  static const struct of_device_id mscc_ocelot_match[] = {
>  	{ .compatible = "mscc,vsc7514-switch" },
>  	{ }
> @@ -172,8 +258,8 @@ MODULE_DEVICE_TABLE(of, mscc_ocelot_match);
>  
>  static int mscc_ocelot_probe(struct platform_device *pdev)
>  {
> -	int err, irq;
>  	unsigned int i;
> +	int err, irq_xtr, irq_ptp_rdy;
>  	struct device_node *np = pdev->dev.of_node;
>  	struct device_node *ports, *portnp;
>  	struct ocelot *ocelot;

reverse xmas tree

> @@ -232,16 +318,31 @@ static int mscc_ocelot_probe(struct
> platform_device *pdev)
>  	if (err)
>  		return err;
>  
> -	irq = platform_get_irq_byname(pdev, "xtr");
> -	if (irq < 0)
> +	irq_xtr = platform_get_irq_byname(pdev, "xtr");
> +	if (irq_xtr < 0)
>  		return -ENODEV;
>  
> -	err = devm_request_threaded_irq(&pdev->dev, irq, NULL,
> +	err = devm_request_threaded_irq(&pdev->dev, irq_xtr, NULL,
>  					ocelot_xtr_irq_handler,
> IRQF_ONESHOT,
>  					"frame extraction", ocelot);
>  	if (err)
>  		return err;
>  
> +
> +	irq_ptp_rdy = platform_get_irq_byname(pdev, "ptp_rdy");
> +	if (irq_ptp_rdy > 0) {
> +		err = devm_request_threaded_irq(&pdev->dev,
> irq_ptp_rdy, NULL,
> +						ocelot_ptp_rdy_irq_hand
> ler,
> +						IRQF_ONESHOT, "ptp
> ready",
> +						ocelot);
> +		if (err)
> +			return err;
> +
> +		/* Check if we can support PTP */
> +		if (ocelot->targets[PTP])
> +			ocelot->ptp = 1;
> +	}
> +
>  	regmap_field_write(ocelot->regfields[SYS_RESET_CFG_MEM_INIT],
> 1);
>  	regmap_field_write(ocelot->regfields[SYS_RESET_CFG_MEM_ENA],
> 1);
>  

Re: [PATCH net-next v4 6/6] net: mscc: PTP Hardware Clock (PHC) support

[antoine.tenart]

Hello Saeed,

On Fri, Jul 26, 2019 at 08:52:10PM +0000, Saeed Mahameed wrote:
> On Thu, 2019-07-25 at 16:27 +0200, Antoine Tenart wrote:
> >  
> >  	dev->stats.tx_packets++;
> >  	dev->stats.tx_bytes += skb->len;
> > -	dev_kfree_skb_any(skb);
> > +
> > +	if (ocelot->ptp && shinfo->tx_flags & SKBTX_HW_TSTAMP &&
> > +	    port->ptp_cmd == IFH_REW_OP_TWO_STEP_PTP) {
> > +		struct ocelot_skb *oskb =
> > +			kzalloc(sizeof(struct ocelot_skb), GFP_ATOMIC);
> > +
> 
> Device drivers normally pre allocate descriptor info ring array to
> avoid dynamic atomic allocations of private data on data path.

This depends on drivers. It's a good thing to do but I don't think it's
required here for a first version, we can improve this later.

> > +		oskb->skb = skb;
> > +		oskb->id = port->ts_id % 4;
> > +		port->ts_id++;
> 
> missing skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS; ?
> see 3.1 Hardware Timestamping Implementation: Device Drivers
> https://www.kernel.org/doc/Documentation/networking/timestamping.txt

Right, I'll add this.

> > +void ocelot_get_hwtimestamp(struct ocelot *ocelot, struct timespec64
> > *ts)
> > +{
> > +	unsigned long flags;
> > +	u32 val;
> > +
> > +	spin_lock_irqsave(&ocelot->ptp_clock_lock, flags);
> > +
> > +	/* Read current PTP time to get seconds */
> > +	val = ocelot_read_rix(ocelot, PTP_PIN_CFG, TOD_ACC_PIN);
> > +
> > +	val &= ~(PTP_PIN_CFG_SYNC | PTP_PIN_CFG_ACTION_MASK |
> > PTP_PIN_CFG_DOM);
> > +	val |= PTP_PIN_CFG_ACTION(PTP_PIN_ACTION_SAVE);
> > +	ocelot_write_rix(ocelot, val, PTP_PIN_CFG, TOD_ACC_PIN);
> > +	ts->tv_sec = ocelot_read_rix(ocelot, PTP_PIN_TOD_SEC_LSB,
> > TOD_ACC_PIN);
> > +
> > +	/* Read packet HW timestamp from FIFO */
> > +	val = ocelot_read(ocelot, SYS_PTP_TXSTAMP);
> > +	ts->tv_nsec = SYS_PTP_TXSTAMP_PTP_TXSTAMP(val);
> > +
> > +	/* Sec has incremented since the ts was registered */
> > +	if ((ts->tv_sec & 0x1) != !!(val &
> > SYS_PTP_TXSTAMP_PTP_TXSTAMP_SEC))
> > +		ts->tv_sec--;
> > +
> > +	spin_unlock_irqrestore(&ocelot->ptp_clock_lock, flags);
> > +}
> > +EXPORT_SYMBOL(ocelot_get_hwtimestamp);
> > +
> 
> Why EXPORT_SYMBOL? this is the last patch and it is touching one
> driver.

Because the function is used in ocelot_board.c, which can be part of
another module (the mscc/ driver provides 2 modules). You can find
other examples of this in this driver.

> > @@ -98,7 +100,11 @@ static irqreturn_t ocelot_xtr_irq_handler(int
> > irq, void *arg)
> >  		int sz, len, buf_len;
> >  		u32 ifh[4];
> >  		u32 val;
> > -		struct frame_info info;
> > +		struct frame_info info = {};
> > +		struct timespec64 ts;
> > +		struct skb_shared_hwtstamps *shhwtstamps;
> > +		u64 tod_in_ns;
> > +		u64 full_ts_in_ns;
> 
> reverse xmas tree.

OK.

> > @@ -145,6 +151,22 @@ static irqreturn_t ocelot_xtr_irq_handler(int
> > irq, void *arg)
> >  			break;
> >  		}
> >  
> > +		if (ocelot->ptp) {
> > +			ocelot_ptp_gettime64(&ocelot->ptp_info, &ts);
> > +
> > +			tod_in_ns = ktime_set(ts.tv_sec, ts.tv_nsec);
> > +			if ((tod_in_ns & 0xffffffff) < info.timestamp)
> > +				full_ts_in_ns = (((tod_in_ns >> 32) -
> > 1) << 32) |
> > +						info.timestamp;
> > +			else
> > +				full_ts_in_ns = (tod_in_ns &
> > GENMASK_ULL(63, 32)) |
> > +						info.timestamp;
> > +
> > +			shhwtstamps = skb_hwtstamps(skb);
> > +			memset(shhwtstamps, 0, sizeof(struct
> > skb_shared_hwtstamps));
> > +			shhwtstamps->hwtstamp = full_ts_in_ns;
> 
> the right way to set the timestamp is by calling: 
> skb_tstamp_tx(skb, &tstamp);

I'll fix this.

> > +static irqreturn_t ocelot_ptp_rdy_irq_handler(int irq, void *arg)
> > +{
> > +	int budget = OCELOT_PTP_QUEUE_SZ;
> > +	struct ocelot *ocelot = arg;
> > +
> > +	do {
> > +		struct skb_shared_hwtstamps shhwtstamps;
> > +		struct list_head *pos, *tmp;
> > +		struct sk_buff *skb = NULL;
> > +		struct ocelot_skb *entry;
> > +		struct ocelot_port *port;
> > +		struct timespec64 ts;
> > +		u32 val, id, txport;
> > +
> > +		/* Prevent from infinite loop */
> > +		if (unlikely(!--budget))
> > +			break;
> 
> when budget gets to 1 you break, while you still have 1 to go :)
> 
> I assume OCELOT_PTP_QUEUE_SZ > 0, just make this the loop condition and
> avoid infinite loops by design.

That's right, I'll fix it :)

> >  static int mscc_ocelot_probe(struct platform_device *pdev)
> >  {
> > -	int err, irq;
> >  	unsigned int i;
> > +	int err, irq_xtr, irq_ptp_rdy;
> >  	struct device_node *np = pdev->dev.of_node;
> >  	struct device_node *ports, *portnp;
> >  	struct ocelot *ocelot;
> 
> reverse xmas tree

Well, the xmas tree isn't there before my addition. Do you want me to
rework the whole variable definitions in this function (which is
completely unrelated to this patch)?

Thanks,
Antoine

-- 
Antoine Ténart, Bootlin
Embedded Linux and Kernel engineering
https://bootlin.com

Re: [PATCH net-next v4 6/6] net: mscc: PTP Hardware Clock (PHC) support

[antoine.tenart]

On Wed, Jul 31, 2019 at 09:46:09AM +0200, antoine.tenart@bootlin.com wrote:
> On Fri, Jul 26, 2019 at 08:52:10PM +0000, Saeed Mahameed wrote:
> > On Thu, 2019-07-25 at 16:27 +0200, Antoine Tenart wrote:
> > > @@ -145,6 +151,22 @@ static irqreturn_t ocelot_xtr_irq_handler(int
> > > irq, void *arg)
> > >  			break;
> > >  		}
> > >  
> > > +		if (ocelot->ptp) {
> > > +			ocelot_ptp_gettime64(&ocelot->ptp_info, &ts);
> > > +
> > > +			tod_in_ns = ktime_set(ts.tv_sec, ts.tv_nsec);
> > > +			if ((tod_in_ns & 0xffffffff) < info.timestamp)
> > > +				full_ts_in_ns = (((tod_in_ns >> 32) -
> > > 1) << 32) |
> > > +						info.timestamp;
> > > +			else
> > > +				full_ts_in_ns = (tod_in_ns &
> > > GENMASK_ULL(63, 32)) |
> > > +						info.timestamp;
> > > +
> > > +			shhwtstamps = skb_hwtstamps(skb);
> > > +			memset(shhwtstamps, 0, sizeof(struct
> > > skb_shared_hwtstamps));
> > > +			shhwtstamps->hwtstamp = full_ts_in_ns;
> > 
> > the right way to set the timestamp is by calling: 
> > skb_tstamp_tx(skb, &tstamp);
> 
> I'll fix this.

This is in the Rx path, so we do not have to call this.

Thanks,
Antoine

-- 
Antoine Ténart, Bootlin
Embedded Linux and Kernel engineering
https://bootlin.com