[net-next,v1,07/12] net: dsa: microchip: ptp: add packet reception timestamping

Message ID 20221128103227.23171-8-arun.ramadoss@microchip.com
State New
Headers
Series net: dsa: microchip: add PTP support for KSZ9563/KSZ8563 and LAN937x |

Commit Message

Arun Ramadoss Nov. 28, 2022, 10:32 a.m. UTC
  From: Christian Eggers <ceggers@arri.de>

This patch adds the routines for timestamping received ptp packets.
Whenever the ptp packet is received, the 4 byte hardware time stamped
value is append to its packet. This 4 byte value is extracted from the
tail tag and reconstructed to absolute time and assigned to skb
hwtstamp.

Signed-off-by: Christian Eggers <ceggers@arri.de>
Co-developed-by: Arun Ramadoss <arun.ramadoss@microchip.com>
Signed-off-by: Arun Ramadoss <arun.ramadoss@microchip.com>

---
RFC v2 -> Patch v1
- Fixed compilation issue
---
 drivers/net/dsa/microchip/ksz_common.c | 13 +++++
 drivers/net/dsa/microchip/ksz_ptp.c    | 28 +++++++++++
 drivers/net/dsa/microchip/ksz_ptp.h    |  6 +++
 include/linux/dsa/ksz_common.h         | 15 ++++++
 net/dsa/tag_ksz.c                      | 68 +++++++++++++++++++++++---
 5 files changed, 124 insertions(+), 6 deletions(-)
  

Comments

kernel test robot Nov. 29, 2022, 12:43 a.m. UTC | #1
Hi Arun,

I love your patch! Yet something to improve:

[auto build test ERROR on a6e3d86ece0b42a571a11055ace5c3148cb7ce76]

url:    https://github.com/intel-lab-lkp/linux/commits/Arun-Ramadoss/net-dsa-microchip-add-PTP-support-for-KSZ9563-KSZ8563-and-LAN937x/20221128-183731
base:   a6e3d86ece0b42a571a11055ace5c3148cb7ce76
patch link:    https://lore.kernel.org/r/20221128103227.23171-8-arun.ramadoss%40microchip.com
patch subject: [Patch net-next v1 07/12] net: dsa: microchip: ptp: add packet reception timestamping
config: arm-randconfig-r046-20221128
compiler: arm-linux-gnueabi-gcc (GCC) 12.1.0
reproduce (this is a W=1 build):
        wget https://raw.githubusercontent.com/intel/lkp-tests/master/sbin/make.cross -O ~/bin/make.cross
        chmod +x ~/bin/make.cross
        # https://github.com/intel-lab-lkp/linux/commit/104ec46b17d7bf278a05fca30b1cd513c4f7adac
        git remote add linux-review https://github.com/intel-lab-lkp/linux
        git fetch --no-tags linux-review Arun-Ramadoss/net-dsa-microchip-add-PTP-support-for-KSZ9563-KSZ8563-and-LAN937x/20221128-183731
        git checkout 104ec46b17d7bf278a05fca30b1cd513c4f7adac
        # save the config file
        mkdir build_dir && cp config build_dir/.config
        COMPILER_INSTALL_PATH=$HOME/0day COMPILER=gcc-12.1.0 make.cross W=1 O=build_dir ARCH=arm SHELL=/bin/bash net/dsa/

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

All errors (new ones prefixed by >>):

   net/dsa/tag_ksz.c: In function 'ksz_rcv_timestamp':
>> net/dsa/tag_ksz.c:225:9: error: implicit declaration of function 'ptp_header_update_correction' [-Werror=implicit-function-declaration]
     225 |         ptp_header_update_correction(skb, ptp_type, ptp_hdr, correction);
         |         ^~~~~~~~~~~~~~~~~~~~~~~~~~~~
   cc1: some warnings being treated as errors


vim +/ptp_header_update_correction +225 net/dsa/tag_ksz.c

   176	
   177	static void ksz_rcv_timestamp(struct sk_buff *skb, u8 *tag,
   178				      struct net_device *dev, unsigned int port)
   179	{
   180		struct skb_shared_hwtstamps *hwtstamps = skb_hwtstamps(skb);
   181		struct dsa_switch *ds = dev->dsa_ptr->ds;
   182		u8 *tstamp_raw = tag - KSZ_PTP_TAG_LEN;
   183		struct ksz_tagger_data *tagger_data;
   184		struct ptp_header *ptp_hdr;
   185		unsigned int ptp_type;
   186		u8 ptp_msg_type;
   187		ktime_t tstamp;
   188		s64 correction;
   189	
   190		tagger_data = ksz_tagger_data(ds);
   191		if (!tagger_data->meta_tstamp_handler)
   192			return;
   193	
   194		/* convert time stamp and write to skb */
   195		tstamp = ksz_decode_tstamp(get_unaligned_be32(tstamp_raw));
   196		memset(hwtstamps, 0, sizeof(*hwtstamps));
   197		hwtstamps->hwtstamp = tagger_data->meta_tstamp_handler(ds, tstamp);
   198	
   199		if (skb_headroom(skb) < ETH_HLEN)
   200			return;
   201	
   202		__skb_push(skb, ETH_HLEN);
   203		ptp_type = ptp_classify_raw(skb);
   204		__skb_pull(skb, ETH_HLEN);
   205	
   206		if (ptp_type == PTP_CLASS_NONE)
   207			return;
   208	
   209		ptp_hdr = ptp_parse_header(skb, ptp_type);
   210		if (!ptp_hdr)
   211			return;
   212	
   213		ptp_msg_type = ptp_get_msgtype(ptp_hdr, ptp_type);
   214		if (ptp_msg_type != PTP_MSGTYPE_PDELAY_REQ)
   215			return;
   216	
   217		/* Only subtract the partial time stamp from the correction field.  When
   218		 * the hardware adds the egress time stamp to the correction field of
   219		 * the PDelay_Resp message on tx, also only the partial time stamp will
   220		 * be added.
   221		 */
   222		correction = (s64)get_unaligned_be64(&ptp_hdr->correction);
   223		correction -= ktime_to_ns(tstamp) << 16;
   224	
 > 225		ptp_header_update_correction(skb, ptp_type, ptp_hdr, correction);
   226	}
   227
  

Patch

diff --git a/drivers/net/dsa/microchip/ksz_common.c b/drivers/net/dsa/microchip/ksz_common.c
index 9bfd7dd5cd31..306bdc1469d2 100644
--- a/drivers/net/dsa/microchip/ksz_common.c
+++ b/drivers/net/dsa/microchip/ksz_common.c
@@ -5,6 +5,7 @@ 
  * Copyright (C) 2017-2019 Microchip Technology Inc.
  */
 
+#include <linux/dsa/ksz_common.h>
 #include <linux/delay.h>
 #include <linux/export.h>
 #include <linux/gpio/consumer.h>
@@ -2453,6 +2454,17 @@  static enum dsa_tag_protocol ksz_get_tag_protocol(struct dsa_switch *ds,
 	return proto;
 }
 
+static int ksz_connect_tag_protocol(struct dsa_switch *ds,
+				    enum dsa_tag_protocol proto)
+{
+	struct ksz_tagger_data *tagger_data;
+
+	tagger_data = ksz_tagger_data(ds);
+	tagger_data->meta_tstamp_handler = ksz_tstamp_reconstruct;
+
+	return 0;
+}
+
 static int ksz_port_vlan_filtering(struct dsa_switch *ds, int port,
 				   bool flag, struct netlink_ext_ack *extack)
 {
@@ -2849,6 +2861,7 @@  static int ksz_switch_detect(struct ksz_device *dev)
 
 static const struct dsa_switch_ops ksz_switch_ops = {
 	.get_tag_protocol	= ksz_get_tag_protocol,
+	.connect_tag_protocol   = ksz_connect_tag_protocol,
 	.get_phy_flags		= ksz_get_phy_flags,
 	.setup			= ksz_setup,
 	.teardown		= ksz_teardown,
diff --git a/drivers/net/dsa/microchip/ksz_ptp.c b/drivers/net/dsa/microchip/ksz_ptp.c
index 515a40c9f107..d878e922c275 100644
--- a/drivers/net/dsa/microchip/ksz_ptp.c
+++ b/drivers/net/dsa/microchip/ksz_ptp.c
@@ -353,6 +353,34 @@  static int ksz_ptp_start_clock(struct ksz_device *dev)
 	return 0;
 }
 
+ktime_t ksz_tstamp_reconstruct(struct dsa_switch *ds, ktime_t tstamp)
+{
+	struct ksz_device *dev = ds->priv;
+	struct timespec64 ptp_clock_time;
+	struct ksz_ptp_data *ptp_data;
+	struct timespec64 diff;
+	struct timespec64 ts;
+
+	ptp_data = &dev->ptp_data;
+	ts = ktime_to_timespec64(tstamp);
+
+	spin_lock_bh(&ptp_data->clock_lock);
+	ptp_clock_time = ptp_data->clock_time;
+	spin_unlock_bh(&ptp_data->clock_lock);
+
+	/* calculate full time from partial time stamp */
+	ts.tv_sec = (ptp_clock_time.tv_sec & ~3) | ts.tv_sec;
+
+	/* find nearest possible point in time */
+	diff = timespec64_sub(ts, ptp_clock_time);
+	if (diff.tv_sec > 2)
+		ts.tv_sec -= 4;
+	else if (diff.tv_sec < -2)
+		ts.tv_sec += 4;
+
+	return timespec64_to_ktime(ts);
+}
+
 static const struct ptp_clock_info ksz_ptp_caps = {
 	.owner		= THIS_MODULE,
 	.name		= "Microchip Clock",
diff --git a/drivers/net/dsa/microchip/ksz_ptp.h b/drivers/net/dsa/microchip/ksz_ptp.h
index 5cec8f2e8420..13c06c24b488 100644
--- a/drivers/net/dsa/microchip/ksz_ptp.h
+++ b/drivers/net/dsa/microchip/ksz_ptp.h
@@ -30,6 +30,7 @@  int ksz_hwtstamp_get(struct dsa_switch *ds, int port, struct ifreq *ifr);
 int ksz_hwtstamp_set(struct dsa_switch *ds, int port, struct ifreq *ifr);
 int ksz_ptp_irq_setup(struct dsa_switch *ds, u8 p);
 void ksz_ptp_irq_free(struct dsa_switch *ds, u8 p);
+ktime_t ksz_tstamp_reconstruct(struct dsa_switch *ds, ktime_t tstamp);
 
 #else
 
@@ -66,6 +67,11 @@  static inline int ksz_ptp_irq_setup(struct dsa_switch *ds, u8 p)
 
 static inline void ksz_ptp_irq_free(struct dsa_switch *ds, u8 p) {}
 
+static inline ktime_t ksz_tstamp_reconstruct(struct dsa_switch *ds, ktime_t tstamp)
+{
+	return 0;
+}
+
 #endif	/* End of CONFIG_NET_DSA_MICROCHIP_KSZ_PTP */
 
 #endif
diff --git a/include/linux/dsa/ksz_common.h b/include/linux/dsa/ksz_common.h
index 62996860b887..0be0cd903727 100644
--- a/include/linux/dsa/ksz_common.h
+++ b/include/linux/dsa/ksz_common.h
@@ -9,9 +9,24 @@ 
 
 #include <net/dsa.h>
 
+/* All time stamps from the KSZ consist of 2 bits for seconds and 30 bits for
+ * nanoseconds. This is NOT the same as 32 bits for nanoseconds.
+ */
+#define KSZ_TSTAMP_SEC_MASK  GENMASK(31, 30)
+#define KSZ_TSTAMP_NSEC_MASK GENMASK(29, 0)
+
+static inline ktime_t ksz_decode_tstamp(u32 tstamp)
+{
+	u64 ns = FIELD_GET(KSZ_TSTAMP_SEC_MASK, tstamp) * NSEC_PER_SEC +
+		 FIELD_GET(KSZ_TSTAMP_NSEC_MASK, tstamp);
+
+	return ns_to_ktime(ns);
+}
+
 struct ksz_tagger_data {
 	bool (*hwtstamp_get_state)(struct dsa_switch *ds);
 	void (*hwtstamp_set_state)(struct dsa_switch *ds, bool on);
+	ktime_t (*meta_tstamp_handler)(struct dsa_switch *ds, ktime_t tstamp);
 };
 
 static inline struct ksz_tagger_data *
diff --git a/net/dsa/tag_ksz.c b/net/dsa/tag_ksz.c
index 828af38f0598..41ce5ff5ed41 100644
--- a/net/dsa/tag_ksz.c
+++ b/net/dsa/tag_ksz.c
@@ -7,6 +7,7 @@ 
 #include <linux/dsa/ksz_common.h>
 #include <linux/etherdevice.h>
 #include <linux/list.h>
+#include <linux/ptp_classify.h>
 #include <net/dsa.h>
 
 #include "tag.h"
@@ -157,10 +158,11 @@  MODULE_ALIAS_DSA_TAG_DRIVER(DSA_TAG_PROTO_KSZ8795, KSZ8795_NAME);
  * tag0 : Prioritization (not used now)
  * tag1 : each bit represents port (eg, 0x01=port1, 0x02=port2, 0x10=port5)
  *
- * For Egress (KSZ9477 -> Host), 1 byte is added before FCS.
+ * For Egress (KSZ9477 -> Host), 1/5 bytes is added before FCS.
  * ---------------------------------------------------------------------------
- * DA(6bytes)|SA(6bytes)|....|Data(nbytes)|tag0(1byte)|FCS(4bytes)
+ * DA(6bytes)|SA(6bytes)|....|Data(nbytes)|ts(4bytes)|tag0(1byte)|FCS(4bytes)
  * ---------------------------------------------------------------------------
+ * ts   : time stamp (Present only if bit 7 of tag0 is set)
  * tag0 : zero-based value represents port
  *	  (eg, 0x00=port1, 0x02=port3, 0x06=port7)
  */
@@ -172,6 +174,57 @@  MODULE_ALIAS_DSA_TAG_DRIVER(DSA_TAG_PROTO_KSZ8795, KSZ8795_NAME);
 #define KSZ9477_TAIL_TAG_OVERRIDE	BIT(9)
 #define KSZ9477_TAIL_TAG_LOOKUP		BIT(10)
 
+static void ksz_rcv_timestamp(struct sk_buff *skb, u8 *tag,
+			      struct net_device *dev, unsigned int port)
+{
+	struct skb_shared_hwtstamps *hwtstamps = skb_hwtstamps(skb);
+	struct dsa_switch *ds = dev->dsa_ptr->ds;
+	u8 *tstamp_raw = tag - KSZ_PTP_TAG_LEN;
+	struct ksz_tagger_data *tagger_data;
+	struct ptp_header *ptp_hdr;
+	unsigned int ptp_type;
+	u8 ptp_msg_type;
+	ktime_t tstamp;
+	s64 correction;
+
+	tagger_data = ksz_tagger_data(ds);
+	if (!tagger_data->meta_tstamp_handler)
+		return;
+
+	/* convert time stamp and write to skb */
+	tstamp = ksz_decode_tstamp(get_unaligned_be32(tstamp_raw));
+	memset(hwtstamps, 0, sizeof(*hwtstamps));
+	hwtstamps->hwtstamp = tagger_data->meta_tstamp_handler(ds, tstamp);
+
+	if (skb_headroom(skb) < ETH_HLEN)
+		return;
+
+	__skb_push(skb, ETH_HLEN);
+	ptp_type = ptp_classify_raw(skb);
+	__skb_pull(skb, ETH_HLEN);
+
+	if (ptp_type == PTP_CLASS_NONE)
+		return;
+
+	ptp_hdr = ptp_parse_header(skb, ptp_type);
+	if (!ptp_hdr)
+		return;
+
+	ptp_msg_type = ptp_get_msgtype(ptp_hdr, ptp_type);
+	if (ptp_msg_type != PTP_MSGTYPE_PDELAY_REQ)
+		return;
+
+	/* Only subtract the partial time stamp from the correction field.  When
+	 * the hardware adds the egress time stamp to the correction field of
+	 * the PDelay_Resp message on tx, also only the partial time stamp will
+	 * be added.
+	 */
+	correction = (s64)get_unaligned_be64(&ptp_hdr->correction);
+	correction -= ktime_to_ns(tstamp) << 16;
+
+	ptp_header_update_correction(skb, ptp_type, ptp_hdr, correction);
+}
+
 /* Time stamp tag is only inserted if PTP is enabled in hardware. */
 static void ksz_xmit_timestamp(struct dsa_port *dp, struct sk_buff *skb)
 {
@@ -220,8 +273,10 @@  static struct sk_buff *ksz9477_rcv(struct sk_buff *skb, struct net_device *dev)
 	unsigned int len = KSZ_EGRESS_TAG_LEN;
 
 	/* Extra 4-bytes PTP timestamp */
-	if (tag[0] & KSZ9477_PTP_TAG_INDICATION)
-		len += KSZ9477_PTP_TAG_LEN;
+	if (tag[0] & KSZ9477_PTP_TAG_INDICATION) {
+		ksz_rcv_timestamp(skb, tag, dev, port);
+		len += KSZ_PTP_TAG_LEN;
+	}
 
 	return ksz_common_rcv(skb, dev, port, len);
 }
@@ -287,10 +342,11 @@  MODULE_ALIAS_DSA_TAG_DRIVER(DSA_TAG_PROTO_KSZ9893, KSZ9893_NAME);
  * tag0 : represents tag override, lookup and valid
  * tag1 : each bit represents port (eg, 0x01=port1, 0x02=port2, 0x80=port8)
  *
- * For rcv, 1 byte is added before FCS.
+ * For rcv, 1/5 bytes is added before FCS.
  * ---------------------------------------------------------------------------
- * DA(6bytes)|SA(6bytes)|....|Data(nbytes)|tag0(1byte)|FCS(4bytes)
+ * DA(6bytes)|SA(6bytes)|....|Data(nbytes)|ts(4bytes)|tag0(1byte)|FCS(4bytes)
  * ---------------------------------------------------------------------------
+ * ts   : time stamp (Present only if bit 7 of tag0 is set)
  * tag0 : zero-based value represents port
  *	  (eg, 0x00=port1, 0x02=port3, 0x07=port8)
  */