[PATCH v2 5/7] net: dsa: b53: Add logic for RX timestamping

@@ -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"
 

@@ -106,6 +108,32 @@ static long b53_hwtstamp_work(struct ptp_clock_info *ptp)
 	return 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 = timecounter_cyc2time(&dev->tc, BRCM_SKB_CB(skb)->meta_tstamp);
+	mutex_unlock(&dev->ptp_mutex);
+
+	shwt = skb_hwtstamps(skb);
+	memset(shwt, 0, sizeof(*shwt));
+	shwt->hwtstamp = ns_to_ktime(ns);
+
+	return false;
+}
+EXPORT_SYMBOL(b53_port_rxtstamp);
+
 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,29 @@ 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)))
 		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) {
+		if (unlikely(!pskb_may_pull(skb, *tag_len)))
+			return NULL;
+
+		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

@@ -164,7 +178,7 @@ static struct sk_buff *brcm_tag_rcv_ll(struct sk_buff *skb,
 		return NULL;
 
 	/* Remove Broadcom tag and update checksum */
-	skb_pull_rcsum(skb, BRCM_TAG_LEN);
+	skb_pull_rcsum(skb, *tag_len);
 
 	dsa_default_offload_fwd_mark(skb);
 

@@ -184,13 +198,14 @@ static struct sk_buff *brcm_tag_xmit(struct sk_buff *skb,
 static struct sk_buff *brcm_tag_rcv(struct sk_buff *skb, struct net_device *dev)
 {
 	struct sk_buff *nskb;
+	int tag_len;
 
 	/* skb->data points to the EtherType, the tag is right before it */
-	nskb = brcm_tag_rcv_ll(skb, dev, 2);
+	nskb = brcm_tag_rcv_ll(skb, dev, 2, &tag_len);
 	if (!nskb)
 		return nskb;
 
-	dsa_strip_etype_header(skb, BRCM_TAG_LEN);
+	dsa_strip_etype_header(skb, tag_len);
 
 	return nskb;
 }

@@ -295,8 +310,10 @@ static struct sk_buff *brcm_tag_xmit_prepend(struct sk_buff *skb,
 static struct sk_buff *brcm_tag_rcv_prepend(struct sk_buff *skb,
 					    struct net_device *dev)
 {
+	int tag_len;
+
 	/* tag is prepended to the packet */
-	return brcm_tag_rcv_ll(skb, dev, ETH_HLEN);
+	return brcm_tag_rcv_ll(skb, dev, ETH_HLEN, &tag_len);
 }
 
 static const struct dsa_device_ops brcm_prepend_netdev_ops = {