[PATCH 0/7] Add PTP support for BCM53128 switch

@@ -50,6 +50,12 @@
 /* Jumbo Frame Registers */
 #define B53_JUMBO_PAGE			0x40
 
+/* BroadSync HD Register Page */
+#define B53_BROADSYNC_PAGE		0x90
+
+/* Traffic Remarking Register Page */
+#define B53_TRAFFICREMARKING_PAGE	0x91
+
 /* EEE Control Registers Page */
 #define B53_EEE_PAGE			0x92
 

@@ -479,6 +485,38 @@
 #define   JMS_MIN_SIZE			1518
 #define   JMS_MAX_SIZE			9724
 
+/*************************************************************************
+ * BroadSync HD Page Registers
+ *************************************************************************/
+
+#define B53_BROADSYNC_EN_CTRL1		0x00
+#define B53_BROADSYNC_EN_CTRL2		0x01
+#define B53_BROADSYNC_TS_REPORT_CTRL	0x02
+#define B53_BROADSYNC_PCP_CTRL		0x03
+#define B53_BROADSYNC_MAX_SDU		0x04
+#define B53_BROADSYNC_TIMEBASE1		0x10
+#define B53_BROADSYNC_TIMEBASE2		0x11
+#define B53_BROADSYNC_TIMEBASE3		0x12
+#define B53_BROADSYNC_TIMEBASE4		0x13
+#define B53_BROADSYNC_TIMEBASE_ADJ1	0x14
+#define B53_BROADSYNC_TIMEBASE_ADJ2	0x15
+#define B53_BROADSYNC_TIMEBASE_ADJ3	0x16
+#define B53_BROADSYNC_TIMEBASE_ADJ4	0x17
+#define B53_BROADSYNC_SLOT_CNT1		0x18
+#define B53_BROADSYNC_SLOT_CNT2		0x19
+#define B53_BROADSYNC_SLOT_CNT3		0x1a
+#define B53_BROADSYNC_SLOT_CNT4		0x1b
+#define B53_BROADSYNC_SLOT_ADJ1		0x1c
+#define B53_BROADSYNC_SLOT_ADJ2		0x1d
+#define B53_BROADSYNC_SLOT_ADJ3		0x1e
+#define B53_BROADSYNC_SLOT_ADJ4		0x1f
+#define B53_BROADSYNC_CLS5_BW_CTRL	0x30
+#define B53_BROADSYNC_CLS4_BW_CTRL	0x60
+#define B53_BROADSYNC_EGRESS_TS		0x90
+#define B53_BROADSYNC_EGRESS_TS_STS	0xd0
+#define B53_BROADSYNC_LINK_STS1		0xe0
+#define B53_BROADSYNC_LINK_STS2		0xe1
+
 /*************************************************************************
  * EEE Configuration Page Registers
  *************************************************************************/

@@ -23,44 +23,13 @@
 #include <linux/mutex.h>
 #include <linux/phy.h>
 #include <linux/etherdevice.h>
-#include <net/dsa.h>
+#include <linux/dsa/b53.h>
 
 #include "b53_regs.h"
 
-struct b53_device;
 struct net_device;
 struct phylink_link_state;
 
-struct b53_io_ops {
-	int (*read8)(struct b53_device *dev, u8 page, u8 reg, u8 *value);
-	int (*read16)(struct b53_device *dev, u8 page, u8 reg, u16 *value);
-	int (*read32)(struct b53_device *dev, u8 page, u8 reg, u32 *value);
-	int (*read48)(struct b53_device *dev, u8 page, u8 reg, u64 *value);
-	int (*read64)(struct b53_device *dev, u8 page, u8 reg, u64 *value);
-	int (*write8)(struct b53_device *dev, u8 page, u8 reg, u8 value);
-	int (*write16)(struct b53_device *dev, u8 page, u8 reg, u16 value);
-	int (*write32)(struct b53_device *dev, u8 page, u8 reg, u32 value);
-	int (*write48)(struct b53_device *dev, u8 page, u8 reg, u64 value);
-	int (*write64)(struct b53_device *dev, u8 page, u8 reg, u64 value);
-	int (*phy_read16)(struct b53_device *dev, int addr, int reg, u16 *value);
-	int (*phy_write16)(struct b53_device *dev, int addr, int reg, u16 value);
-	int (*irq_enable)(struct b53_device *dev, int port);
-	void (*irq_disable)(struct b53_device *dev, int port);
-	u8 (*serdes_map_lane)(struct b53_device *dev, int port);
-	int (*serdes_link_state)(struct b53_device *dev, int port,
-				 struct phylink_link_state *state);
-	void (*serdes_config)(struct b53_device *dev, int port,
-			      unsigned int mode,
-			      const struct phylink_link_state *state);
-	void (*serdes_an_restart)(struct b53_device *dev, int port);
-	void (*serdes_link_set)(struct b53_device *dev, int port,
-				unsigned int mode, phy_interface_t interface,
-				bool link_up);
-	void (*serdes_phylink_validate)(struct b53_device *dev, int port,
-					unsigned long *supported,
-					struct phylink_link_state *state);
-};
-
 #define B53_INVALID_LANE	0xff
 
 enum {

@@ -89,63 +58,6 @@ enum {
 #define B53_N_PORTS	9
 #define B53_N_PORTS_25	6
 
-struct b53_port {
-	u16		vlan_ctl_mask;
-	struct ethtool_eee eee;
-};
-
-struct b53_vlan {
-	u16 members;
-	u16 untag;
-	bool valid;
-};
-
-struct b53_device {
-	struct dsa_switch *ds;
-	struct b53_platform_data *pdata;
-	const char *name;
-
-	struct mutex reg_mutex;
-	struct mutex stats_mutex;
-	struct mutex arl_mutex;
-	const struct b53_io_ops *ops;
-
-	/* chip specific data */
-	u32 chip_id;
-	u8 core_rev;
-	u8 vta_regs[3];
-	u8 duplex_reg;
-	u8 jumbo_pm_reg;
-	u8 jumbo_size_reg;
-	int reset_gpio;
-	u8 num_arl_bins;
-	u16 num_arl_buckets;
-	enum dsa_tag_protocol tag_protocol;
-
-	/* used ports mask */
-	u16 enabled_ports;
-	unsigned int imp_port;
-
-	/* connect specific data */
-	u8 current_page;
-	struct device *dev;
-	u8 serdes_lane;
-
-	/* Master MDIO bus we got probed from */
-	struct mii_bus *bus;
-
-	void *priv;
-
-	/* run time configuration */
-	bool enable_jumbo;
-
-	unsigned int num_vlans;
-	struct b53_vlan *vlans;
-	bool vlan_enabled;
-	unsigned int num_ports;
-	struct b53_port *ports;
-};
-
 #define b53_for_each_port(dev, i) \
 	for (i = 0; i < B53_N_PORTS; i++) \
 		if (dev->enabled_ports & BIT(i))

@@ -0,0 +1,100 @@
+/* SPDX-License-Identifier: ISC */
+/*
+ * Copyright (C) 2011-2013 Jonas Gorski <jogo@openwrt.org>
+ *
+ * Included by drivers/net/dsa/b53/b53_priv.h and net/dsa/tag_brcm.c
+ */
+
+#include <net/dsa.h>
+
+struct b53_device;
+struct phylink_link_state;
+
+struct b53_io_ops {
+	int (*read8)(struct b53_device *dev, u8 page, u8 reg, u8 *value);
+	int (*read16)(struct b53_device *dev, u8 page, u8 reg, u16 *value);
+	int (*read32)(struct b53_device *dev, u8 page, u8 reg, u32 *value);
+	int (*read48)(struct b53_device *dev, u8 page, u8 reg, u64 *value);
+	int (*read64)(struct b53_device *dev, u8 page, u8 reg, u64 *value);
+	int (*write8)(struct b53_device *dev, u8 page, u8 reg, u8 value);
+	int (*write16)(struct b53_device *dev, u8 page, u8 reg, u16 value);
+	int (*write32)(struct b53_device *dev, u8 page, u8 reg, u32 value);
+	int (*write48)(struct b53_device *dev, u8 page, u8 reg, u64 value);
+	int (*write64)(struct b53_device *dev, u8 page, u8 reg, u64 value);
+	int (*phy_read16)(struct b53_device *dev, int addr, int reg,
+			  u16 *value);
+	int (*phy_write16)(struct b53_device *dev, int addr, int reg,
+			   u16 value);
+	int (*irq_enable)(struct b53_device *dev, int port);
+	void (*irq_disable)(struct b53_device *dev, int port);
+	u8 (*serdes_map_lane)(struct b53_device *dev, int port);
+	int (*serdes_link_state)(struct b53_device *dev, int port,
+				 struct phylink_link_state *state);
+	void (*serdes_config)(struct b53_device *dev, int port,
+			      unsigned int mode,
+			      const struct phylink_link_state *state);
+	void (*serdes_an_restart)(struct b53_device *dev, int port);
+	void (*serdes_link_set)(struct b53_device *dev, int port,
+				unsigned int mode, phy_interface_t interface,
+				bool link_up);
+	void (*serdes_phylink_validate)(struct b53_device *dev, int port,
+					unsigned long *supported,
+					struct phylink_link_state *state);
+};
+
+struct b53_port {
+	u16 vlan_ctl_mask;
+	struct ethtool_eee eee;
+};
+
+struct b53_vlan {
+	u16 members;
+	u16 untag;
+	bool valid;
+};
+
+struct b53_device {
+	struct dsa_switch *ds;
+	struct b53_platform_data *pdata;
+	const char *name;
+
+	struct mutex reg_mutex;
+	struct mutex stats_mutex;
+	struct mutex arl_mutex;
+	const struct b53_io_ops *ops;
+
+	/* chip specific data */
+	u32 chip_id;
+	u8 core_rev;
+	u8 vta_regs[3];
+	u8 duplex_reg;
+	u8 jumbo_pm_reg;
+	u8 jumbo_size_reg;
+	int reset_gpio;
+	u8 num_arl_bins;
+	u16 num_arl_buckets;
+	enum dsa_tag_protocol tag_protocol;
+
+	/* used ports mask */
+	u16 enabled_ports;
+	unsigned int imp_port;
+
+	/* connect specific data */
+	u8 current_page;
+	struct device *dev;
+	u8 serdes_lane;
+
+	/* Master MDIO bus we got probed from */
+	struct mii_bus *bus;
+
+	void *priv;
+
+	/* run time configuration */
+	bool enable_jumbo;
+
+	unsigned int num_vlans;
+	struct b53_vlan *vlans;
+	bool vlan_enabled;
+	unsigned int num_ports;
+	struct b53_port *ports;
+};

@@ -26,12 +26,15 @@
  *			see CYCLECOUNTER_MASK() helper macro
  * @mult:		cycle to nanosecond multiplier
  * @shift:		cycle to nanosecond divisor (power of two)
+ * @overflow_point:	non-power of two overflow point (optional),
+ *			smaller than mask
  */
 struct cyclecounter {
 	u64 (*read)(const struct cyclecounter *cc);
 	u64 mask;
 	u32 mult;
 	u32 shift;
+	u64 overflow_point;
 };
 
 /**

@@ -39,6 +39,9 @@ static u64 timecounter_read_delta(struct timecounter *tc)
 	/* calculate the delta since the last timecounter_read_delta(): */
 	cycle_delta = (cycle_now - tc->cycle_last) & tc->cc->mask;
 
+	if (tc->cc->overflow_point && (cycle_now - tc->cycle_last) > tc->cc->mask)
+		cycle_delta -= tc->cc->mask - tc->cc->overflow_point;
+
 	/* convert to nanoseconds: */
 	ns_offset = cyclecounter_cyc2ns(tc->cc, cycle_delta,
 					tc->mask, &tc->frac);

@@ -45,3 +45,10 @@ config B53_SERDES
 	default ARCH_BCM_NSP
 	help
 	  Select to enable support for SerDes on e.g: Northstar Plus SoCs.
+
+config B53_PTP
+	bool "B53 PTP support"
+	depends on B53
+	depends on PTP_1588_CLOCK
+	help
+	  Select to enable support for PTP

@@ -6,3 +6,4 @@ obj-$(CONFIG_B53_MDIO_DRIVER)	+= b53_mdio.o
 obj-$(CONFIG_B53_MMAP_DRIVER)	+= b53_mmap.o
 obj-$(CONFIG_B53_SRAB_DRIVER)	+= b53_srab.o
 obj-$(CONFIG_B53_SERDES)	+= b53_serdes.o
+obj-$(CONFIG_B53_PTP)		+= b53_ptp.o

@@ -31,6 +31,7 @@
 
 #include "b53_regs.h"
 #include "b53_priv.h"
+#include "b53_ptp.h"
 
 struct b53_mib_desc {
 	u8 size;

@@ -1131,12 +1132,24 @@ static int b53_setup(struct dsa_switch *ds)
 			b53_disable_port(ds, port);
 	}
 
+	if (dev->broadsync_hd) {
+		ret = b53_ptp_init(dev);
+		if (ret) {
+			dev_err(ds->dev, "failed to initialize PTP\n");
+			return ret;
+		}
+	}
+
 	return b53_setup_devlink_resources(ds);
 }
 
 static void b53_teardown(struct dsa_switch *ds)
 {
+	struct b53_device *dev = ds->priv;
+
 	dsa_devlink_resources_unregister(ds);
+	if (dev->broadsync_hd)
+		b53_ptp_exit(ds->priv);
 }
 
 static void b53_force_link(struct b53_device *dev, int port, int link)

@@ -2286,6 +2299,7 @@ static const struct dsa_switch_ops b53_switch_ops = {
 	.port_mdb_del		= b53_mdb_del,
 	.port_max_mtu		= b53_get_max_mtu,
 	.port_change_mtu	= b53_change_mtu,
+	.get_ts_info		= b53_get_ts_info,
 };
 
 struct b53_chip_data {

@@ -2301,6 +2315,7 @@ struct b53_chip_data {
 	u8 duplex_reg;
 	u8 jumbo_pm_reg;
 	u8 jumbo_size_reg;
+	bool broadsync_hd;
 };
 
 #define B53_VTA_REGS	\

@@ -2421,6 +2436,7 @@ static const struct b53_chip_data b53_switch_chips[] = {
 		.duplex_reg = B53_DUPLEX_STAT_GE,
 		.jumbo_pm_reg = B53_JUMBO_PORT_MASK,
 		.jumbo_size_reg = B53_JUMBO_MAX_SIZE,
+		.broadsync_hd = true,
 	},
 	{
 		.chip_id = BCM63XX_DEVICE_ID,

@@ -2589,6 +2605,7 @@ static int b53_switch_init(struct b53_device *dev)
 			dev->num_vlans = chip->vlans;
 			dev->num_arl_bins = chip->arl_bins;
 			dev->num_arl_buckets = chip->arl_buckets;
+			dev->broadsync_hd = chip->broadsync_hd;
 			break;
 		}
 	}

@@ -0,0 +1,191 @@
+// SPDX-License-Identifier: ISC
+/*
+ * B53 switch PTP support
+ *
+ * Author: Martin Kaistra <martin.kaistra@linutronix.de>
+ * Copyright (C) 2021 Linutronix GmbH
+ */
+
+#include "b53_priv.h"
+#include "b53_ptp.h"
+
+static int b53_ptp_gettime(struct ptp_clock_info *ptp, struct timespec64 *ts)
+{
+	struct b53_device *dev =
+		container_of(ptp, struct b53_device, ptp_clock_info);
+	u64 ns;
+
+	mutex_lock(&dev->ptp_mutex);
+	ns = timecounter_read(&dev->tc);
+	mutex_unlock(&dev->ptp_mutex);
+
+	*ts = ns_to_timespec64(ns);
+
+	return 0;
+}
+
+static int b53_ptp_settime(struct ptp_clock_info *ptp,
+			   const struct timespec64 *ts)
+{
+	struct b53_device *dev =
+		container_of(ptp, struct b53_device, ptp_clock_info);
+	u64 ns;
+
+	ns = timespec64_to_ns(ts);
+
+	mutex_lock(&dev->ptp_mutex);
+	timecounter_init(&dev->tc, &dev->cc, ns);
+	mutex_unlock(&dev->ptp_mutex);
+
+	return 0;
+}
+
+static int b53_ptp_adjfine(struct ptp_clock_info *ptp, long scaled_ppm)
+{
+	struct b53_device *dev =
+		container_of(ptp, struct b53_device, ptp_clock_info);
+	u64 adj, diff;
+	u32 mult;
+	bool neg_adj = false;
+
+	if (scaled_ppm < 0) {
+		neg_adj = true;
+		scaled_ppm = -scaled_ppm;
+	}
+
+	mult = (1 << 28);
+	adj = 64;
+	adj *= (u64)scaled_ppm;
+	diff = div_u64(adj, 15625ULL);
+
+	mutex_lock(&dev->ptp_mutex);
+	timecounter_read(&dev->tc);
+	dev->cc.mult = neg_adj ? mult - diff : mult + diff;
+	mutex_unlock(&dev->ptp_mutex);
+
+	return 0;
+}
+
+static int b53_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
+{
+	struct b53_device *dev =
+		container_of(ptp, struct b53_device, ptp_clock_info);
+
+	mutex_lock(&dev->ptp_mutex);
+	timecounter_adjtime(&dev->tc, delta);
+	mutex_unlock(&dev->ptp_mutex);
+
+	return 0;
+}
+
+static u64 b53_ptp_read(const struct cyclecounter *cc)
+{
+	struct b53_device *dev = container_of(cc, struct b53_device, cc);
+	u32 ts;
+
+	b53_read32(dev, B53_BROADSYNC_PAGE, B53_BROADSYNC_TIMEBASE1, &ts);
+
+	return ts;
+}
+
+static int b53_ptp_enable(struct ptp_clock_info *ptp,
+			  struct ptp_clock_request *rq, int on)
+{
+	return -EOPNOTSUPP;
+}
+
+static void b53_ptp_overflow_check(struct work_struct *work)
+{
+	struct delayed_work *dw = to_delayed_work(work);
+	struct b53_device *dev =
+		container_of(dw, struct b53_device, overflow_work);
+
+	mutex_lock(&dev->ptp_mutex);
+	timecounter_read(&dev->tc);
+	mutex_unlock(&dev->ptp_mutex);
+
+	schedule_delayed_work(&dev->overflow_work, B53_PTP_OVERFLOW_PERIOD);
+}
+
+int b53_ptp_init(struct b53_device *dev)
+{
+	mutex_init(&dev->ptp_mutex);
+
+	INIT_DELAYED_WORK(&dev->overflow_work, b53_ptp_overflow_check);
+
+	/* Enable BroadSync HD for all ports */
+	b53_write16(dev, B53_BROADSYNC_PAGE, B53_BROADSYNC_EN_CTRL1, 0x00ff);
+
+	/* Enable BroadSync HD Time Stamping Reporting (Egress) */
+	b53_write8(dev, B53_BROADSYNC_PAGE, B53_BROADSYNC_TS_REPORT_CTRL, 0x01);
+
+	/* Enable BroadSync HD Time Stamping for PTPv2 ingress */
+
+	/* MPORT_CTRL0 | MPORT0_TS_EN */
+	b53_write16(dev, B53_ARLCTRL_PAGE, 0x0e, (1 << 15) | 0x01);
+	/* Forward to IMP port 8 */
+	b53_write64(dev, B53_ARLCTRL_PAGE, 0x18, (1 << 8));
+	/* PTPv2 Ether Type */
+	b53_write64(dev, B53_ARLCTRL_PAGE, 0x10, (u64)0x88f7 << 48);
+
+	/* Setup PTP clock */
+	dev->ptp_clock_info.owner = THIS_MODULE;
+	snprintf(dev->ptp_clock_info.name, sizeof(dev->ptp_clock_info.name),
+		 dev_name(dev->dev));
+
+	dev->ptp_clock_info.max_adj = 1000000000ULL;
+	dev->ptp_clock_info.n_alarm = 0;
+	dev->ptp_clock_info.n_pins = 0;
+	dev->ptp_clock_info.n_ext_ts = 0;
+	dev->ptp_clock_info.n_per_out = 0;
+	dev->ptp_clock_info.pps = 0;
+	dev->ptp_clock_info.adjfine = b53_ptp_adjfine;
+	dev->ptp_clock_info.adjtime = b53_ptp_adjtime;
+	dev->ptp_clock_info.gettime64 = b53_ptp_gettime;
+	dev->ptp_clock_info.settime64 = b53_ptp_settime;
+	dev->ptp_clock_info.enable = b53_ptp_enable;
+
+	dev->ptp_clock = ptp_clock_register(&dev->ptp_clock_info, dev->dev);
+	if (IS_ERR(dev->ptp_clock))
+		return PTR_ERR(dev->ptp_clock);
+
+	/* The switch provides a 32 bit free running counter. Use the Linux
+	 * cycle counter infrastructure which is suited for such scenarios.
+	 */
+	dev->cc.read = b53_ptp_read;
+	dev->cc.mask = CYCLECOUNTER_MASK(30);
+	dev->cc.overflow_point = 999999999;
+	dev->cc.mult = (1 << 28);
+	dev->cc.shift = 28;
+
+	b53_write32(dev, B53_BROADSYNC_PAGE, B53_BROADSYNC_TIMEBASE_ADJ1, 40);
+
+	timecounter_init(&dev->tc, &dev->cc, ktime_to_ns(ktime_get_real()));
+
+	schedule_delayed_work(&dev->overflow_work, B53_PTP_OVERFLOW_PERIOD);
+
+	return 0;
+}
+
+int b53_get_ts_info(struct dsa_switch *ds, int port,
+		    struct ethtool_ts_info *info)
+{
+	struct b53_device *dev = ds->priv;
+
+	info->phc_index = dev->ptp_clock ? ptp_clock_index(dev->ptp_clock) : -1;
+	info->so_timestamping = SOF_TIMESTAMPING_TX_HARDWARE |
+				SOF_TIMESTAMPING_RX_HARDWARE |
+				SOF_TIMESTAMPING_RAW_HARDWARE;
+	info->tx_types = BIT(HWTSTAMP_TX_OFF);
+	info->rx_filters = BIT(HWTSTAMP_FILTER_NONE);
+
+	return 0;
+}
+
+void b53_ptp_exit(struct b53_device *dev)
+{
+	cancel_delayed_work_sync(&dev->overflow_work);
+	if (dev->ptp_clock)
+		ptp_clock_unregister(dev->ptp_clock);
+	dev->ptp_clock = NULL;
+}

@@ -0,0 +1,35 @@
+/* SPDX-License-Identifier: ISC */
+/*
+ * Author: Martin Kaistra <martin.kaistra@linutronix.de>
+ * Copyright (C) 2021 Linutronix GmbH
+ */
+
+#ifndef _B53_PTP_H
+#define _B53_PTP_H
+
+#include "b53_priv.h"
+
+#ifdef CONFIG_B53_PTP
+int b53_ptp_init(struct b53_device *dev);
+void b53_ptp_exit(struct b53_device *dev);
+int b53_get_ts_info(struct dsa_switch *ds, int port,
+		    struct ethtool_ts_info *info);
+#else /* !CONFIG_B53_PTP */
+
+static inline int b53_ptp_init(struct b53_device *dev)
+{
+	return 0;
+}
+
+static inline void b53_ptp_exit(struct b53_device *dev)
+{
+}
+
+static inline int b53_get_ts_info(struct dsa_switch *ds, int port,
+				  struct ethtool_ts_info *info)
+{
+	return -EOPNOTSUPP;
+}
+
+#endif
+#endif

@@ -1,10 +1,14 @@
 /* SPDX-License-Identifier: ISC */
 /*
  * Copyright (C) 2011-2013 Jonas Gorski <jogo@openwrt.org>
+ * Copyright (C) 2021 Linutronix GmbH
  *
  * Included by drivers/net/dsa/b53/b53_priv.h and net/dsa/tag_brcm.c
  */
 
+#include <linux/ptp_clock_kernel.h>
+#include <linux/timecounter.h>
+#include <linux/workqueue.h>
 #include <net/dsa.h>
 
 struct b53_device;

@@ -97,4 +101,14 @@ struct b53_device {
 	bool vlan_enabled;
 	unsigned int num_ports;
 	struct b53_port *ports;
+
+	/* PTP */
+	bool broadsync_hd;
+	struct ptp_clock *ptp_clock;
+	struct ptp_clock_info ptp_clock_info;
+	struct cyclecounter cc;
+	struct timecounter tc;
+	struct mutex ptp_mutex;
+#define B53_PTP_OVERFLOW_PERIOD (HZ / 2)
+	struct delayed_work overflow_work;
 };

@@ -2301,6 +2301,7 @@ static const struct dsa_switch_ops b53_switch_ops = {
 	.port_change_mtu	= b53_change_mtu,
 	.get_ts_info		= b53_get_ts_info,
 	.port_rxtstamp		= b53_port_rxtstamp,
+	.port_txtstamp		= b53_port_txtstamp,
 };
 
 struct b53_chip_data {

@@ -109,6 +109,64 @@ static void b53_ptp_overflow_check(struct work_struct *work)
 	schedule_delayed_work(&dev->overflow_work, B53_PTP_OVERFLOW_PERIOD);
 }
 
+static long b53_hwtstamp_work(struct ptp_clock_info *ptp)
+{
+	struct b53_device *dev =
+		container_of(ptp, struct b53_device, ptp_clock_info);
+	struct dsa_switch *ds = dev->ds;
+	int i;
+
+	for (i = 0; i < ds->num_ports; i++) {
+		struct b53_port_hwtstamp *ps;
+
+		if (!dsa_is_user_port(ds, i))
+			continue;
+
+		ps = &dev->ports[i].port_hwtstamp;
+
+		if (test_bit(B53_HWTSTAMP_TX_IN_PROGRESS, &ps->state) &&
+		    time_is_before_jiffies(ps->tx_tstamp_start +
+					   TX_TSTAMP_TIMEOUT)) {
+			dev_err(dev->dev,
+				"Timeout while waiting for Tx timestamp!\n");
+			dev_kfree_skb_any(ps->tx_skb);
+			ps->tx_skb = NULL;
+			clear_bit_unlock(B53_HWTSTAMP_TX_IN_PROGRESS,
+					 &ps->state);
+		}
+	}
+
+	return -1;
+}
+
+void b53_port_txtstamp(struct dsa_switch *ds, int port, struct sk_buff *skb)
+{
+	struct b53_device *dev = ds->priv;
+	struct b53_port_hwtstamp *ps = &dev->ports[port].port_hwtstamp;
+	struct sk_buff *clone;
+	unsigned int type;
+
+	type = ptp_classify_raw(skb);
+
+	if (type != PTP_CLASS_V2_L2)
+		return;
+
+	if (!test_bit(B53_HWTSTAMP_ENABLED, &ps->state))
+		return;
+
+	clone = skb_clone_sk(skb);
+	if (!clone)
+		return;
+
+	if (test_and_set_bit_lock(B53_HWTSTAMP_TX_IN_PROGRESS, &ps->state)) {
+		kfree_skb(clone);
+		return;
+	}
+
+	ps->tx_skb = clone;
+	ps->tx_tstamp_start = jiffies;
+}
+
 bool b53_port_rxtstamp(struct dsa_switch *ds, int port, struct sk_buff *skb,
 		       unsigned int type)
 {

@@ -172,6 +230,7 @@ int b53_ptp_init(struct b53_device *dev)
 	dev->ptp_clock_info.gettime64 = b53_ptp_gettime;
 	dev->ptp_clock_info.settime64 = b53_ptp_settime;
 	dev->ptp_clock_info.enable = b53_ptp_enable;
+	dev->ptp_clock_info.do_aux_work = b53_hwtstamp_work;
 
 	dev->ptp_clock = ptp_clock_register(&dev->ptp_clock_info, dev->dev);
 	if (IS_ERR(dev->ptp_clock))

@@ -10,6 +10,7 @@
 #include "b53_priv.h"
 
 #define SKB_PTP_TYPE(__skb) (*(unsigned int *)((__skb)->cb))
+#define TX_TSTAMP_TIMEOUT msecs_to_jiffies(40)
 
 #ifdef CONFIG_B53_PTP
 int b53_ptp_init(struct b53_device *dev);

@@ -18,6 +19,8 @@ int b53_get_ts_info(struct dsa_switch *ds, int port,
 		    struct ethtool_ts_info *info);
 bool b53_port_rxtstamp(struct dsa_switch *ds, int port, struct sk_buff *skb,
 		       unsigned int type);
+void b53_port_txtstamp(struct dsa_switch *ds, int port, struct sk_buff *skb);
+
 #else /* !CONFIG_B53_PTP */
 
 static inline int b53_ptp_init(struct b53_device *dev)

@@ -41,5 +44,11 @@ static inline bool b53_port_rxtstamp(struct dsa_switch *ds, int port,
 	return false;
 }
 
+static inline bool b53_port_txtstamp(struct dsa_switch *ds, int port,
+				     struct sk_buff *skb)
+{
+	return false;
+}
+
 #endif
 #endif

@@ -8,6 +8,7 @@
 #include <linux/dsa/brcm.h>
 #include <linux/etherdevice.h>
 #include <linux/list.h>
+#include <linux/ptp_classify.h>
 #include <linux/slab.h>
 #include <linux/dsa/b53.h>
 

@@ -85,9 +86,14 @@ static struct sk_buff *brcm_tag_xmit_ll(struct sk_buff *skb,
 					unsigned int offset)
 {
 	struct dsa_port *dp = dsa_slave_to_port(dev);
+	struct b53_device *b53_dev = dp->ds->priv;
+	unsigned int type = ptp_classify_raw(skb);
 	u16 queue = skb_get_queue_mapping(skb);
+	struct b53_port_hwtstamp *ps;
 	u8 *brcm_tag;
 
+	ps = &b53_dev->ports[dp->index].port_hwtstamp;
+
 	/* The Ethernet switch we are interfaced with needs packets to be at
 	 * least 64 bytes (including FCS) otherwise they will be discarded when
 	 * they enter the switch port logic. When Broadcom tags are enabled, we

@@ -112,7 +118,13 @@ static struct sk_buff *brcm_tag_xmit_ll(struct sk_buff *skb,
 	 */
 	brcm_tag[0] = (1 << BRCM_OPCODE_SHIFT) |
 		       ((queue & BRCM_IG_TC_MASK) << BRCM_IG_TC_SHIFT);
+
 	brcm_tag[1] = 0;
+
+	if (type == PTP_CLASS_V2_L2 &&
+	    test_bit(B53_HWTSTAMP_TX_IN_PROGRESS, &ps->state))
+		brcm_tag[1] = 1 << BRCM_IG_TS_SHIFT;
+
 	brcm_tag[2] = 0;
 	if (dp->index == 8)
 		brcm_tag[2] = BRCM_IG_DSTMAP2_MASK;

@@ -126,6 +138,32 @@ static struct sk_buff *brcm_tag_xmit_ll(struct sk_buff *skb,
 	return skb;
 }
 
+static int set_txtstamp(struct b53_device *dev,
+			struct b53_port_hwtstamp *ps,
+			int port,
+			u64 ns)
+{
+	struct skb_shared_hwtstamps shhwtstamps;
+	struct sk_buff *tmp_skb;
+
+	if (!ps->tx_skb)
+		return 0;
+
+	mutex_lock(&dev->ptp_mutex);
+	ns = timecounter_cyc2time(&dev->tc, ns);
+	mutex_unlock(&dev->ptp_mutex);
+
+	memset(&shhwtstamps, 0, sizeof(shhwtstamps));
+	shhwtstamps.hwtstamp = ns_to_ktime(ns);
+	tmp_skb = ps->tx_skb;
+	ps->tx_skb = NULL;
+
+	clear_bit_unlock(B53_HWTSTAMP_TX_IN_PROGRESS, &ps->state);
+	skb_complete_tx_timestamp(tmp_skb, &shhwtstamps);
+
+	return 0;
+}
+
 /* Frames with this tag have one of these two layouts:
  * -----------------------------------
  * | MAC DA | MAC SA | 4b tag | Type | DSA_TAG_PROTO_BRCM

@@ -143,6 +181,9 @@ static struct sk_buff *brcm_tag_rcv_ll(struct sk_buff *skb,
 				       unsigned int offset,
 				       int *tag_len)
 {
+	struct b53_port_hwtstamp *ps;
+	struct b53_device *b53_dev;
+	struct dsa_port *dp;
 	int source_port;
 	u8 *brcm_tag;
 	u32 tstamp;

@@ -174,6 +215,16 @@ static struct sk_buff *brcm_tag_rcv_ll(struct sk_buff *skb,
 	if (!skb->dev)
 		return NULL;
 
+	/* Check whether this is a status frame */
+	if (*tag_len == 8 && brcm_tag[3] & 0x20) {
+		dp = dsa_slave_to_port(skb->dev);
+		b53_dev = dp->ds->priv;
+		ps = &b53_dev->ports[source_port].port_hwtstamp;
+
+		set_txtstamp(b53_dev, ps, source_port, tstamp);
+		return NULL;
+	}
+
 	/* Remove Broadcom tag and update checksum */
 	skb_pull_rcsum(skb, *tag_len);
 

@@ -2300,6 +2300,7 @@ static const struct dsa_switch_ops b53_switch_ops = {
 	.port_max_mtu		= b53_get_max_mtu,
 	.port_change_mtu	= b53_change_mtu,
 	.get_ts_info		= b53_get_ts_info,
+	.port_rxtstamp		= b53_port_rxtstamp,
 };
 
 struct b53_chip_data {

@@ -6,6 +6,8 @@
  * Copyright (C) 2021 Linutronix GmbH
  */
 
+#include <linux/ptp_classify.h>
+
 #include "b53_priv.h"
 #include "b53_ptp.h"
 

@@ -107,6 +109,32 @@ static void b53_ptp_overflow_check(struct work_struct *work)
 	schedule_delayed_work(&dev->overflow_work, B53_PTP_OVERFLOW_PERIOD);
 }
 
+bool b53_port_rxtstamp(struct dsa_switch *ds, int port, struct sk_buff *skb,
+		       unsigned int type)
+{
+	struct b53_device *dev = ds->priv;
+	struct b53_port_hwtstamp *ps = &dev->ports[port].port_hwtstamp;
+	struct skb_shared_hwtstamps *shwt;
+	u64 ns;
+
+	if (type != PTP_CLASS_V2_L2)
+		return false;
+
+	if (!test_bit(B53_HWTSTAMP_ENABLED, &ps->state))
+		return false;
+
+	mutex_lock(&dev->ptp_mutex);
+	ns = BRCM_SKB_CB(skb)->meta_tstamp;
+	ns = timecounter_cyc2time(&dev->tc, ns);
+	mutex_unlock(&dev->ptp_mutex);
+
+	shwt = skb_hwtstamps(skb);
+	memset(shwt, 0, sizeof(*shwt));
+	shwt->hwtstamp = ns_to_ktime(ns);
+
+	return false;
+}
+
 int b53_ptp_init(struct b53_device *dev)
 {
 	mutex_init(&dev->ptp_mutex);

@@ -9,11 +9,15 @@
 
 #include "b53_priv.h"
 
+#define SKB_PTP_TYPE(__skb) (*(unsigned int *)((__skb)->cb))
+
 #ifdef CONFIG_B53_PTP
 int b53_ptp_init(struct b53_device *dev);
 void b53_ptp_exit(struct b53_device *dev);
 int b53_get_ts_info(struct dsa_switch *ds, int port,
 		    struct ethtool_ts_info *info);
+bool b53_port_rxtstamp(struct dsa_switch *ds, int port, struct sk_buff *skb,
+		       unsigned int type);
 #else /* !CONFIG_B53_PTP */
 
 static inline int b53_ptp_init(struct b53_device *dev)

@@ -31,5 +35,11 @@ static inline int b53_get_ts_info(struct dsa_switch *ds, int port,
 	return -EOPNOTSUPP;
 }
 
+static inline bool b53_port_rxtstamp(struct dsa_switch *ds, int port,
+				     struct sk_buff *skb, unsigned int type)
+{
+	return false;
+}
+
 #endif
 #endif

@@ -46,9 +46,32 @@ struct b53_io_ops {
 					struct phylink_link_state *state);
 };
 
+/* state flags for b53_port_hwtstamp::state */
+enum {
+	B53_HWTSTAMP_ENABLED,
+	B53_HWTSTAMP_TX_IN_PROGRESS,
+};
+
+struct b53_port_hwtstamp {
+	/* Port index */
+	int port_id;
+
+	/* Timestamping state */
+	unsigned long state;
+
+	/* Resources for transmit timestamping */
+	unsigned long tx_tstamp_start;
+	struct sk_buff *tx_skb;
+
+	/* Current timestamp configuration */
+	struct hwtstamp_config tstamp_config;
+};
+
 struct b53_port {
 	u16 vlan_ctl_mask;
 	struct ethtool_eee eee;
+	/* Per-port timestamping resources */
+	struct b53_port_hwtstamp port_hwtstamp;
 };
 
 struct b53_vlan {

@@ -112,3 +135,10 @@ struct b53_device {
 #define B53_PTP_OVERFLOW_PERIOD (HZ / 2)
 	struct delayed_work overflow_work;
 };
+
+struct brcm_skb_cb {
+	struct sk_buff *clone;
+	u32 meta_tstamp;
+};
+
+#define BRCM_SKB_CB(skb) ((struct brcm_skb_cb *)(skb)->cb)

@@ -9,6 +9,7 @@
 #include <linux/etherdevice.h>
 #include <linux/list.h>
 #include <linux/slab.h>
+#include <linux/dsa/b53.h>
 
 #include "dsa_priv.h"
 

@@ -31,7 +32,10 @@
 /* 6th byte in the tag */
 #define BRCM_LEG_PORT_ID	(0xf)
 
-/* Newer Broadcom tag (4 bytes) */
+/* Newer Broadcom tag (4 bytes)
+ * For egress, when opcode = 0001, additional 4 bytes are used for
+ * the time stamp.
+ */
 #define BRCM_TAG_LEN	4
 
 /* Tag is constructed and desconstructed using byte by byte access

@@ -136,19 +140,26 @@ static struct sk_buff *brcm_tag_xmit_ll(struct sk_buff *skb,
  */
 static struct sk_buff *brcm_tag_rcv_ll(struct sk_buff *skb,
 				       struct net_device *dev,
-				       unsigned int offset)
+				       unsigned int offset,
+				       int *tag_len)
 {
 	int source_port;
 	u8 *brcm_tag;
+	u32 tstamp;
+
+	*tag_len = 8;
 
-	if (unlikely(!pskb_may_pull(skb, BRCM_TAG_LEN)))
+	if (unlikely(!pskb_may_pull(skb, *tag_len)))
 		return NULL;
 
 	brcm_tag = skb->data - offset;
 
-	/* The opcode should never be different than 0b000 */
-	if (unlikely((brcm_tag[0] >> BRCM_OPCODE_SHIFT) & BRCM_OPCODE_MASK))
-		return NULL;
+	if ((brcm_tag[0] >> BRCM_OPCODE_SHIFT) & BRCM_OPCODE_MASK) {
+		tstamp = brcm_tag[4] << 24 | brcm_tag[5] << 16 | brcm_tag[6] << 8 | brcm_tag[7];
+		BRCM_SKB_CB(skb)->meta_tstamp = tstamp;
+	} else {
+		*tag_len = BRCM_TAG_LEN;
+	}
 
 	/* We should never see a reserved reason code without knowing how to
 	 * handle it