[Patch net-next v2 12/13] net: dsa: microchip: ptp: lan937x: add 2 step timestamping

[Arun Ramadoss <arun.ramadoss@microchip.com>]

LAN937x series of switches support 2 step timestamping mechanism. There
are timestamp correction calculation performed in ksz_rcv_timestamp and
ksz_xmit_timestamp which are applicable only for p2p1step. To check
whether the 2 step is enabled or not in tag_ksz.c introduced the helper
function in taggger_data to query it from ksz_ptp.c. Based on whether 2
step is enabled or not, timestamp calculation are performed.

Signed-off-by: Arun Ramadoss <arun.ramadoss@microchip.com>
---
v1 -> v2
- declard is_ptp_twostep as macro NULL for ptp disabled case
- Moved the patch in series to have continuity for lan937x updates 9/11
  to 12/13
- enable PTP_1STEP bit based on tx timestamping

Patch v1
- Patch is new.
---
 drivers/net/dsa/microchip/ksz_common.c |  1 +
 drivers/net/dsa/microchip/ksz_ptp.c    | 36 ++++++++++++++++++++++++++
 drivers/net/dsa/microchip/ksz_ptp.h    |  3 +++
 include/linux/dsa/ksz_common.h         |  1 +
 net/dsa/tag_ksz.c                      | 14 ++++++++++
 5 files changed, 55 insertions(+)

diff --git a/drivers/net/dsa/microchip/ksz_common.c b/drivers/net/dsa/microchip/ksz_common.c
index 3b578a4a33de..91f998fab231 100644
--- a/drivers/net/dsa/microchip/ksz_common.c
+++ b/drivers/net/dsa/microchip/ksz_common.c
@@ -2462,6 +2462,7 @@ static int ksz_connect_tag_protocol(struct dsa_switch *ds,
 	tagger_data = ksz_tagger_data(ds);
 	tagger_data->xmit_work_fn = ksz_port_deferred_xmit;
 	tagger_data->meta_tstamp_handler = ksz_tstamp_reconstruct;
+	tagger_data->is_ptp_twostep = ksz_is_ptp_twostep;
 
 	return 0;
 }
diff --git a/drivers/net/dsa/microchip/ksz_ptp.c b/drivers/net/dsa/microchip/ksz_ptp.c
index 25b0f09753ce..8392962311e2 100644
--- a/drivers/net/dsa/microchip/ksz_ptp.c
+++ b/drivers/net/dsa/microchip/ksz_ptp.c
@@ -329,6 +329,9 @@ int ksz_get_ts_info(struct dsa_switch *ds, int port, struct ethtool_ts_info *ts)
 
 	ts->tx_types = BIT(HWTSTAMP_TX_OFF) | BIT(HWTSTAMP_TX_ONESTEP_P2P);
 
+	if (is_lan937x(dev))
+		ts->tx_types |= BIT(HWTSTAMP_TX_ON);
+
 	ts->rx_filters = BIT(HWTSTAMP_FILTER_NONE) |
 			 BIT(HWTSTAMP_FILTER_PTP_V2_L4_EVENT) |
 			 BIT(HWTSTAMP_FILTER_PTP_V2_L2_EVENT) |
@@ -356,6 +359,8 @@ static int ksz_set_hwtstamp_config(struct ksz_device *dev,
 				   struct ksz_port *prt,
 				   struct hwtstamp_config *config)
 {
+	int ret;
+
 	if (config->flags)
 		return -EINVAL;
 
@@ -371,6 +376,25 @@ static int ksz_set_hwtstamp_config(struct ksz_device *dev,
 		prt->ptpmsg_irq[KSZ_XDREQ_MSG].ts_en = 1;
 		prt->ptpmsg_irq[KSZ_PDRES_MSG].ts_en = 0;
 		prt->hwts_tx_en = true;
+
+		ret = ksz_rmw16(dev, REG_PTP_MSG_CONF1, PTP_1STEP, PTP_1STEP);
+		if (ret)
+			return ret;
+
+		break;
+	case HWTSTAMP_TX_ON:
+		if (!is_lan937x(dev))
+			return -ERANGE;
+
+		prt->ptpmsg_irq[KSZ_SYNC_MSG].ts_en  = 1;
+		prt->ptpmsg_irq[KSZ_XDREQ_MSG].ts_en = 1;
+		prt->ptpmsg_irq[KSZ_PDRES_MSG].ts_en = 1;
+		prt->hwts_tx_en = true;
+
+		ret = ksz_rmw16(dev, REG_PTP_MSG_CONF1, PTP_1STEP, 0);
+		if (ret)
+			return ret;
+
 		break;
 	default:
 		return -ERANGE;
@@ -403,6 +427,14 @@ static int ksz_set_hwtstamp_config(struct ksz_device *dev,
 	return ksz_ptp_enable_mode(dev);
 }
 
+bool ksz_is_ptp_twostep(struct dsa_switch *ds, unsigned int port)
+{
+	struct ksz_device *dev	= ds->priv;
+	struct ksz_port *prt = &dev->ports[port];
+
+	return (prt->tstamp_config.tx_type == HWTSTAMP_TX_ON);
+}
+
 int ksz_hwtstamp_set(struct dsa_switch *ds, int port, struct ifreq *ifr)
 {
 	struct ksz_device *dev = ds->priv;
@@ -451,6 +483,10 @@ void ksz_port_txtstamp(struct dsa_switch *ds, int port,
 	ptp_msg_type = ptp_get_msgtype(hdr, type);
 
 	switch (ptp_msg_type) {
+	case PTP_MSGTYPE_SYNC:
+		if (prt->tstamp_config.tx_type == HWTSTAMP_TX_ONESTEP_P2P)
+			return;
+		 fallthrough;
 	case PTP_MSGTYPE_PDELAY_REQ:
 		 fallthrough;
 	case PTP_MSGTYPE_PDELAY_RESP:
diff --git a/drivers/net/dsa/microchip/ksz_ptp.h b/drivers/net/dsa/microchip/ksz_ptp.h
index 5a71c826c7d8..abad072b7032 100644
--- a/drivers/net/dsa/microchip/ksz_ptp.h
+++ b/drivers/net/dsa/microchip/ksz_ptp.h
@@ -46,6 +46,7 @@ int ksz_ptp_irq_setup(struct dsa_switch *ds, u8 p);
 void ksz_ptp_irq_free(struct dsa_switch *ds, u8 p);
 void ksz_port_deferred_xmit(struct kthread_work *work);
 ktime_t ksz_tstamp_reconstruct(struct dsa_switch *ds, ktime_t tstamp);
+bool ksz_is_ptp_twostep(struct dsa_switch *ds, unsigned int port);
 
 #else
 
@@ -80,6 +81,8 @@ static inline void ksz_ptp_irq_free(struct dsa_switch *ds, u8 p) {}
 
 #define ksz_port_deferred_xmit NULL
 
+#define ksz_is_ptp_twostep NULL
+
 #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 b180eb3429bd..be1a850c82fb 100644
--- a/include/linux/dsa/ksz_common.h
+++ b/include/linux/dsa/ksz_common.h
@@ -33,6 +33,7 @@ struct ksz_tagger_data {
 	void (*xmit_work_fn)(struct kthread_work *work);
 	void (*hwtstamp_set_state)(struct dsa_switch *ds, bool on);
 	ktime_t (*meta_tstamp_handler)(struct dsa_switch *ds, ktime_t tstamp);
+	bool (*is_ptp_twostep)(struct dsa_switch *ds, unsigned int port);
 };
 
 struct ksz_skb_cb {
diff --git a/net/dsa/tag_ksz.c b/net/dsa/tag_ksz.c
index e57066f3947e..e2aa438437a0 100644
--- a/net/dsa/tag_ksz.c
+++ b/net/dsa/tag_ksz.c
@@ -216,6 +216,12 @@ static void ksz_rcv_timestamp(struct sk_buff *skb, u8 *tag,
 	if (!ptp_hdr)
 		return;
 
+	if (!tagger_data->is_ptp_twostep)
+		return;
+
+	if (tagger_data->is_ptp_twostep(ds, port))
+		return;
+
 	ptp_msg_type = ptp_get_msgtype(ptp_hdr, ptp_type);
 	if (ptp_msg_type != PTP_MSGTYPE_PDELAY_REQ)
 		return;
@@ -237,6 +243,7 @@ static void ksz_rcv_timestamp(struct sk_buff *skb, u8 *tag,
 static void ksz_xmit_timestamp(struct dsa_port *dp, struct sk_buff *skb)
 {
 	struct sk_buff *clone = KSZ_SKB_CB(skb)->clone;
+	struct ksz_tagger_data *tagger_data;
 	struct ksz_tagger_private *priv;
 	struct ptp_header *ptp_hdr;
 	unsigned int ptp_type;
@@ -252,6 +259,13 @@ static void ksz_xmit_timestamp(struct dsa_port *dp, struct sk_buff *skb)
 	if (!clone)
 		goto output_tag;
 
+	tagger_data = ksz_tagger_data(dp->ds);
+	if (!tagger_data->is_ptp_twostep)
+		goto output_tag;
+
+	if (tagger_data->is_ptp_twostep(dp->ds, dp->index))
+		goto output_tag;
+
 	ptp_type = KSZ_SKB_CB(clone)->ptp_type;
 	if (ptp_type == PTP_CLASS_NONE)
 		goto output_tag;
-- 
2.36.1