[PATCH v10 net-next 2/3] netdevsim: Implement basic ptp support

@@ -200,7 +200,22 @@ static int nsim_get_ts_info(struct net_device *dev,
 {
 	struct netdevsim *ns = netdev_priv(dev);
 
+	ethtool_op_get_ts_info(dev, info);
+	if (!ns->phc) {
+		info->phc_index = -1;
+		return 0;
+	}
+
 	info->phc_index = mock_phc_index(ns->phc);
+	if (info->phc_index < 0)
+		return 0;
+
+	info->so_timestamping |= SOF_TIMESTAMPING_TX_HARDWARE |
+				 SOF_TIMESTAMPING_RX_HARDWARE |
+				 SOF_TIMESTAMPING_RAW_HARDWARE;
+
+	info->tx_types = BIT(HWTSTAMP_TX_OFF) | BIT(HWTSTAMP_TX_ON);
+	info->rx_filters = BIT(HWTSTAMP_FILTER_NONE) | BIT(HWTSTAMP_FILTER_ALL);
 
 	return 0;
 }

@@ -30,6 +30,8 @@
 #include <net/rtnetlink.h>
 #include <net/udp_tunnel.h>
 #include <net/busy_poll.h>
+#include <linux/ptp_clock_kernel.h>
+#include <linux/timecounter.h>
 
 #include "netdevsim.h"
 

@@ -122,7 +124,12 @@ static int nsim_forward_skb(struct net_device *tx_dev,
 
 static netdev_tx_t nsim_start_xmit(struct sk_buff *skb, struct net_device *dev)
 {
+	struct skb_shared_hwtstamps shhwtstamps = {};
+	struct ptp_clock_info *ptp_info_tx = NULL;
+	struct ptp_clock_info *ptp_info_rx = NULL;
 	struct netdevsim *ns = netdev_priv(dev);
+	struct timespec64 tx_ts, rx_ts;
+	struct sk_buff *skb_orig = skb;
 	struct skb_ext *psp_ext = NULL;
 	struct net_device *peer_dev;
 	unsigned int len = skb->len;

@@ -164,6 +171,44 @@ static netdev_tx_t nsim_start_xmit(struct sk_buff *skb, struct net_device *dev)
 		skb_linearize(skb);
 
 	skb_tx_timestamp(skb);
+
+	if (unlikely(READ_ONCE(peer_ns->tstamp_config.rx_filter) !=
+		     HWTSTAMP_FILTER_NONE))
+		ptp_info_rx = mock_phc_get_ptp_info(peer_ns->phc);
+
+	if (unlikely(skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP) &&
+	    READ_ONCE(ns->tstamp_config.tx_type) == HWTSTAMP_TX_ON)
+		ptp_info_tx = mock_phc_get_ptp_info(ns->phc);
+
+	if (unlikely(ptp_info_tx || ptp_info_rx)) {
+		/* Capture both timestamps closely */
+		if (ptp_info_tx)
+			ptp_info_tx->gettime64(ptp_info_tx, &tx_ts);
+		if (ptp_info_rx)
+			ptp_info_rx->gettime64(ptp_info_rx, &rx_ts);
+
+		/* If TX hardware timestamping is enabled report TX timestamp */
+		if (ptp_info_tx) {
+			shhwtstamps.hwtstamp = timespec64_to_ktime(tx_ts);
+			skb_tstamp_tx(skb_orig, &shhwtstamps);
+		}
+
+		/* Unshare the skb, stamping it must not corrupt cloned copies
+		 * and prevents returning single tstamp for Tx and Rx
+		 */
+		skb = skb_unshare(skb_orig, GFP_ATOMIC);
+		if (unlikely(!skb)) {
+			if (psp_ext)
+				__skb_ext_put(psp_ext);
+			goto out_drop_cnt;
+		}
+
+		/* If RX hardware timestamping is enabled report RX timestamp */
+		if (ptp_info_rx)
+			skb_hwtstamps(skb)->hwtstamp =
+				timespec64_to_ktime(rx_ts);
+	}
+
 	if (unlikely(nsim_forward_skb(dev, peer_dev,
 				      skb, rq, psp_ext) == NET_RX_DROP))
 		goto out_drop_cnt;

@@ -185,6 +230,63 @@ static netdev_tx_t nsim_start_xmit(struct sk_buff *skb, struct net_device *dev)
 	return NETDEV_TX_OK;
 }
 
+static int nsim_set_ts_config(struct net_device *netdev,
+			      struct kernel_hwtstamp_config *config,
+			      struct netlink_ext_ack *extack)
+{
+	struct netdevsim *ns = netdev_priv(netdev);
+	int rx_filter;
+
+	if (!ns->phc)
+		return -EOPNOTSUPP;
+
+	switch (config->rx_filter) {
+	case HWTSTAMP_FILTER_NONE:
+		rx_filter = HWTSTAMP_FILTER_NONE;
+		break;
+	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_PTP_V2_EVENT:
+	case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
+	case HWTSTAMP_FILTER_PTP_V2_SYNC:
+	case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
+	case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
+	case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
+	case HWTSTAMP_FILTER_NTP_ALL:
+	case HWTSTAMP_FILTER_ALL:
+		rx_filter = HWTSTAMP_FILTER_ALL;
+		break;
+	default:
+		return -ERANGE;
+	}
+
+	switch (config->tx_type) {
+	case HWTSTAMP_TX_OFF:
+		WRITE_ONCE(ns->tstamp_config.tx_type, HWTSTAMP_TX_OFF);
+		break;
+	case HWTSTAMP_TX_ON:
+		WRITE_ONCE(ns->tstamp_config.tx_type, HWTSTAMP_TX_ON);
+		break;
+	default:
+		return -ERANGE;
+	}
+
+	WRITE_ONCE(ns->tstamp_config.rx_filter, rx_filter);
+	config->rx_filter = rx_filter;
+
+	return 0;
+}
+
+static int nsim_get_ts_config(struct net_device *netdev,
+			      struct kernel_hwtstamp_config *config)
+{
+	struct netdevsim *ns = netdev_priv(netdev);
+
+	*config = ns->tstamp_config;
+	return 0;
+}
+
 static int nsim_set_rx_mode(struct net_device *dev,
 			    struct netdev_hw_addr_list *uc,
 			    struct netdev_hw_addr_list *mc)

@@ -647,6 +749,8 @@ static const struct net_device_ops nsim_netdev_ops = {
 	.ndo_vlan_rx_add_vid	= nsim_vlan_rx_add_vid,
 	.ndo_vlan_rx_kill_vid	= nsim_vlan_rx_kill_vid,
 	.net_shaper_ops		= &nsim_shaper_ops,
+	.ndo_hwtstamp_get	= nsim_get_ts_config,
+	.ndo_hwtstamp_set	= nsim_set_ts_config,
 };
 
 static const struct net_device_ops nsim_vf_netdev_ops = {

@@ -108,6 +108,7 @@ struct netdevsim {
 	struct net_device *netdev;
 	struct nsim_dev *nsim_dev;
 	struct nsim_dev_port *nsim_dev_port;
+	struct kernel_hwtstamp_config tstamp_config;
 	struct mock_phc *phc;
 	struct nsim_rq **rq;