[PATCH v2 2/2] net/af_xdp: add read_clock support to AF_XDP PMD

[Mark Blasko <blasko@google.com>]

Implement the ethdev read_clock operation in the AF_XDP Poll Mode
Driver. This allows DPDK applications to query the current time of the
NIC hardware clock.

At device start, the PMD queries ethtool for the interface's PTP
Hardware Clock index and opens the corresponding PTP device node. The
read_clock operation queries the current time via clock_gettime.

Signed-off-by: Mark Blasko <blasko@google.com>
Reviewed-by: Joshua Washington <joshwash@google.com>
---
v2:
  - New patch introduced in v2 to support read_clock ethdev operation.
---
 drivers/net/af_xdp/rte_eth_af_xdp.c | 127 ++++++++++++++++++++++++++++
 1 file changed, 127 insertions(+)

diff --git a/drivers/net/af_xdp/rte_eth_af_xdp.c b/drivers/net/af_xdp/rte_eth_af_xdp.c
index 2f9ad7d180..410be570a0 100644
--- a/drivers/net/af_xdp/rte_eth_af_xdp.c
+++ b/drivers/net/af_xdp/rte_eth_af_xdp.c
@@ -15,6 +15,7 @@
 #include <linux/if_link.h>
 #include <linux/ethtool.h>
 #include <linux/sockios.h>
+#include <linux/net_tstamp.h>
 #include "af_xdp_deps.h"
 
 #include <rte_ethdev.h>
@@ -28,6 +29,7 @@
 #include <dev_driver.h>
 #include <rte_eal.h>
 #include <rte_ether.h>
+#include <rte_time.h>
 #include <rte_lcore.h>
 #include <rte_log.h>
 #include <rte_memory.h>
@@ -62,6 +64,9 @@
 #define PF_XDP AF_XDP
 #endif
 
+#include <fcntl.h>
+#include <time.h>
+
 
 static int timestamp_dynfield_offset = -1;
 static uint64_t timestamp_dynflag;
@@ -194,6 +199,7 @@ struct pmd_internals {
 	int rx_timestamp_offset;
 	int rx_timestamp_valid_offset;
 	uint8_t rx_timestamp_valid_mask;
+	int ptp_fd;
 };
 
 struct pmd_process_private {
@@ -795,9 +801,72 @@ eth_af_xdp_tx(void *queue, struct rte_mbuf **bufs, uint16_t nb_pkts)
 #endif
 }
 
+static int
+eth_af_xdp_enable_hw_timestamping(const char *if_name)
+{
+	struct hwtstamp_config config = {0};
+	struct ifreq ifr = {0};
+	int fd, ret;
+
+	fd = socket(AF_INET, SOCK_DGRAM, 0);
+	if (fd < 0)
+		return -1;
+
+	ifr.ifr_data = (caddr_t)&config;
+	strlcpy(ifr.ifr_name, if_name, IFNAMSIZ);
+
+	ret = ioctl(fd, SIOCGHWTSTAMP, &ifr);
+	if (ret == 0) {
+		if (config.rx_filter != HWTSTAMP_FILTER_NONE) {
+			close(fd);
+			return 0;
+		}
+	}
+
+	config.flags = 0;
+	config.tx_type = HWTSTAMP_TX_OFF;
+	config.rx_filter = HWTSTAMP_FILTER_ALL;
+
+	ret = ioctl(fd, SIOCSHWTSTAMP, &ifr);
+	close(fd);
+
+	if (ret < 0)
+		return -errno;
+
+	if (config.rx_filter == HWTSTAMP_FILTER_NONE)
+		return -ENOTSUP;
+
+	return 0;
+}
+
+static int
+eth_af_xdp_get_ptp_index(const char *if_name)
+{
+	struct ethtool_ts_info info = {0};
+	struct ifreq ifr = {0};
+	int fd, ret;
+
+	fd = socket(AF_INET, SOCK_DGRAM, 0);
+	if (fd < 0)
+		return -1;
+
+	ifr.ifr_data = (caddr_t)&info;
+	info.cmd = ETHTOOL_GET_TS_INFO;
+	strlcpy(ifr.ifr_name, if_name, IFNAMSIZ);
+
+	ret = ioctl(fd, SIOCETHTOOL, &ifr);
+	close(fd);
+
+	if (ret < 0)
+		return -1;
+
+	return info.phc_index;
+}
+
 static int
 eth_dev_start(struct rte_eth_dev *dev)
 {
+	struct pmd_internals *internals = dev->data->dev_private;
 	uint16_t i;
 
 	if (dev->data->dev_conf.rxmode.offloads &
@@ -814,6 +883,25 @@ eth_dev_start(struct rte_eth_dev *dev)
 		AF_XDP_LOG_LINE(INFO,
 			"Registered mbuf timestamp field, offset: %d",
 			timestamp_dynfield_offset);
+
+		rc = eth_af_xdp_enable_hw_timestamping(internals->if_name);
+		if (rc < 0) {
+			AF_XDP_LOG_LINE(WARNING,
+				"Could not enable HW timestamping on %s: %s",
+				internals->if_name, strerror(-rc));
+		}
+
+		int phc_index = eth_af_xdp_get_ptp_index(internals->if_name);
+		if (phc_index >= 0) {
+			char ptp_dev[32];
+			snprintf(ptp_dev, sizeof(ptp_dev), "/dev/ptp%d", phc_index);
+			internals->ptp_fd = open(ptp_dev, O_RDONLY);
+			if (internals->ptp_fd >= 0) {
+				AF_XDP_LOG_LINE(INFO,
+					"Opened PTP device %s for read_clock",
+					ptp_dev);
+			}
+		}
 	}
 
 	dev->data->dev_link.link_status = RTE_ETH_LINK_UP;
@@ -829,6 +917,7 @@ eth_dev_start(struct rte_eth_dev *dev)
 static int
 eth_dev_stop(struct rte_eth_dev *dev)
 {
+	struct pmd_internals *internals = dev->data->dev_private;
 	uint16_t i;
 
 	dev->data->dev_link.link_status = RTE_ETH_LINK_DOWN;
@@ -837,6 +926,11 @@ eth_dev_stop(struct rte_eth_dev *dev)
 		dev->data->tx_queue_state[i] = RTE_ETH_QUEUE_STATE_STOPPED;
 	}
 
+	if (internals->ptp_fd >= 0) {
+		close(internals->ptp_fd);
+		internals->ptp_fd = -1;
+	}
+
 	return 0;
 }
 
@@ -1170,6 +1264,11 @@ eth_dev_close(struct rte_eth_dev *dev)
 		}
 	}
 
+	if (internals->ptp_fd >= 0) {
+		close(internals->ptp_fd);
+		internals->ptp_fd = -1;
+	}
+
 out:
 	rte_free(dev->process_private);
 
@@ -2039,6 +2138,31 @@ eth_dev_promiscuous_disable(struct rte_eth_dev *dev)
 	return eth_dev_change_flags(internals->if_name, 0, ~IFF_PROMISC);
 }
 
+/*
+ * In Linux, dynamic POSIX clock IDs from file descriptors (such as /dev/ptpX)
+ * are encoded with CLOCKFD (3) in the lower 3 bits and ~fd in the upper bits.
+ * As this is not defined in user-space UAPI headers, define the macro here.
+ */
+#define CLOCKFD 3
+#define FD_TO_CLOCKID(fd)	((clockid_t)(~(unsigned int)(fd) << 3 | CLOCKFD))
+
+static int
+eth_af_xdp_read_clock(struct rte_eth_dev *dev, uint64_t *timestamp)
+{
+	struct pmd_internals *internals = dev->data->dev_private;
+	struct timespec ts;
+
+	if (internals->ptp_fd < 0)
+		return -ENOTSUP;
+
+	clockid_t clkid = FD_TO_CLOCKID(internals->ptp_fd);
+	if (clock_gettime(clkid, &ts) < 0)
+		return -1;
+
+	*timestamp = rte_timespec_to_ns(&ts);
+	return 0;
+}
+
 static const struct eth_dev_ops ops = {
 	.dev_start = eth_dev_start,
 	.dev_stop = eth_dev_stop,
@@ -2054,6 +2178,7 @@ static const struct eth_dev_ops ops = {
 	.stats_get = eth_stats_get,
 	.stats_reset = eth_stats_reset,
 	.get_monitor_addr = eth_get_monitor_addr,
+	.read_clock = eth_af_xdp_read_clock,
 };
 
 /* AF_XDP Device Plugin option works in unprivileged
@@ -2075,6 +2200,7 @@ static const struct eth_dev_ops ops_afxdp_dp = {
 	.stats_get = eth_stats_get,
 	.stats_reset = eth_stats_reset,
 	.get_monitor_addr = eth_get_monitor_addr,
+	.read_clock = eth_af_xdp_read_clock,
 };
 
 /** parse busy_budget argument */
@@ -2399,6 +2525,7 @@ init_internals(struct rte_vdev_device *dev, const char *if_name,
 	internals->rx_timestamp_offset = rx_timestamp_offset;
 	internals->rx_timestamp_valid_offset = rx_timestamp_valid_offset;
 	internals->rx_timestamp_valid_mask = (uint8_t)rx_timestamp_valid_mask;
+	internals->ptp_fd = -1;
 
 	if (xdp_get_channels_info(if_name, &internals->max_queue_cnt,
 				  &internals->configured_queue_cnt)) {
-- 
2.55.0.141.g00534a21ce-goog