drivers: eth: native_posix: Enable gPTP support

Allow gPTP code to be run as a linux process and communicate
with gPTP daemon running in linux host.

Signed-off-by: Jukka Rissanen <[email protected]>

drivers/ethernet/Kconfig.native_posix

@@ -40,6 +40,14 @@ config ETH_NATIVE_POSIX_DEV_NAME
 	help
 	  This option sets the TUN/TAP device name in your host system.
 
+config ETH_NATIVE_POSIX_PTP_CLOCK
+	bool "PTP clock driver support"
+	default n
+	select PTP_CLOCK
+	depends on NET_GPTP
+	help
+	  Enable PTP clock support.
+
 config ETH_NATIVE_POSIX_RANDOM_MAC
 	bool "Random MAC address"
 	depends on ENTROPY_GENERATOR

drivers/ethernet/eth_native_posix.c

@@ -28,6 +28,11 @@
 #include <net/net_if.h>
 #include <net/ethernet.h>
 
+#if defined(CONFIG_ETH_NATIVE_POSIX_PTP_CLOCK)
+#include <ptp_clock.h>
+#include <net/gptp.h>
+#endif
+
 #include "eth_native_posix_priv.h"
 #include "ethernet/eth_stats.h"
 
@@ -57,6 +62,9 @@ struct eth_context {
 #if defined(CONFIG_NET_STATISTICS_ETHERNET)
 	struct net_stats_eth stats;
 #endif
+#if defined(CONFIG_ETH_NATIVE_POSIX_PTP_CLOCK)
+	struct device *ptp_clock;
+#endif
 };
 
 NET_STACK_DEFINE(RX_ZETH, eth_rx_stack,
@@ -72,6 +80,99 @@ static struct eth_context *get_context(struct net_if *iface)
 	return net_if_get_device(iface)->driver_data;
 }
 
+#if defined(CONFIG_NET_GPTP)
+static bool need_timestamping(struct gptp_hdr *hdr)
+{
+	switch (hdr->message_type) {
+	case GPTP_SYNC_MESSAGE:
+	case GPTP_PATH_DELAY_RESP_MESSAGE:
+		return true;
+	default:
+		return false;
+	}
+}
+
+static struct gptp_hdr *check_gptp_msg(struct net_if *iface,
+				       struct net_pkt *pkt)
+{
+	struct gptp_hdr *gptp_hdr;
+	u8_t *msg_start;
+
+	if (net_pkt_ll_reserve(pkt)) {
+		msg_start = net_pkt_ll(pkt);
+	} else {
+		msg_start = net_pkt_ip_data(pkt);
+	}
+
+#if defined(CONFIG_NET_VLAN)
+	if (net_eth_get_vlan_status(iface)) {
+		struct net_eth_vlan_hdr *hdr_vlan;
+
+		hdr_vlan = (struct net_eth_vlan_hdr *)msg_start;
+		if (ntohs(hdr_vlan->type) != NET_ETH_PTYPE_PTP) {
+			return NULL;
+		}
+
+		gptp_hdr = (struct gptp_hdr *)(msg_start +
+					sizeof(struct net_eth_vlan_hdr));
+	} else
+#endif
+	{
+		struct net_eth_hdr *hdr;
+
+		hdr = (struct net_eth_hdr *)msg_start;
+		if (ntohs(hdr->type) != NET_ETH_PTYPE_PTP) {
+			return NULL;
+		}
+
+		gptp_hdr = (struct gptp_hdr *)(msg_start +
+					sizeof(struct net_eth_hdr));
+	}
+
+	return gptp_hdr;
+}
+
+static void update_pkt_priority(struct gptp_hdr *hdr, struct net_pkt *pkt)
+{
+	if (GPTP_IS_EVENT_MSG(hdr->message_type)) {
+		net_pkt_set_priority(pkt, NET_PRIORITY_CA);
+	} else {
+		net_pkt_set_priority(pkt, NET_PRIORITY_IC);
+	}
+}
+
+static void update_gptp(struct net_if *iface, struct net_pkt *pkt,
+			bool send)
+{
+	struct net_ptp_time timestamp;
+	struct gptp_hdr *hdr;
+	int ret;
+
+	ret = eth_clock_gettime(&timestamp);
+	if (ret < 0) {
+		return;
+	}
+
+	net_pkt_set_timestamp(pkt, &timestamp);
+
+	hdr = check_gptp_msg(iface, pkt);
+	if (!hdr) {
+		return;
+	}
+
+	if (send) {
+		ret = need_timestamping(hdr);
+		if (ret) {
+			net_if_add_tx_timestamp(pkt);
+		}
+	} else {
+		update_pkt_priority(hdr, pkt);
+	}
+}
+#else
+#define update_gptp(iface, pkt, send)
+#endif /* CONFIG_NET_GPTP */
+
 static int eth_send(struct net_if *iface, struct net_pkt *pkt)
 {
 	struct eth_context *ctx = get_context(iface);
@@ -105,6 +206,8 @@ static int eth_send(struct net_if *iface, struct net_pkt *pkt)
 		}
 	}
 
+	update_gptp(iface, pkt, true);
+
 	SYS_LOG_DBG("Send pkt %p len %d", pkt, count);
 
 	eth_write_data(ctx->dev_fd, ctx->send, count);
@@ -193,6 +296,15 @@ static int read_data(struct eth_context *ctx, int fd)
 			net_pkt_set_vlan_tci(pkt, ntohs(hdr_vlan->vlan.tci));
 			vlan_tag = net_pkt_vlan_tag(pkt);
 		}
+
+#if CONFIG_NET_TC_RX_COUNT > 1
+		{
+			enum net_priority prio;
+
+			prio = net_vlan2priority(net_pkt_vlan_priority(pkt));
+			net_pkt_set_priority(pkt, prio);
+		}
+#endif
 	}
 #endif
 
@@ -215,6 +327,8 @@ static int read_data(struct eth_context *ctx, int fd)
 
 	SYS_LOG_DBG("Recv pkt %p len %d", pkt, pkt_len);
 
+	update_gptp(iface, pkt, false);
+
 	if (net_recv_data(iface, pkt) < 0) {
 		net_pkt_unref(pkt);
 	}
@@ -311,9 +425,22 @@ enum ethernet_hw_caps eth_posix_native_get_capabilities(struct device *dev)
 {
 	ARG_UNUSED(dev);
 
-	return ETHERNET_HW_VLAN;
+	return ETHERNET_HW_VLAN
+#if defined(CONFIG_ETH_NATIVE_POSIX_PTP_CLOCK)
+		| ETHERNET_PTP
+#endif
+		;
 }
 
+#if defined(CONFIG_ETH_NATIVE_POSIX_PTP_CLOCK)
+static struct device *eth_get_ptp_clock(struct device *dev)
+{
+	struct eth_context *context = dev->driver_data;
+
+	return context->ptp_clock;
+}
+#endif
+
 static const struct ethernet_api eth_if_api = {
 	.iface_api.init = eth_iface_init,
 	.iface_api.send = eth_send,
@@ -323,9 +450,92 @@ static const struct ethernet_api eth_if_api = {
 #if defined(CONFIG_NET_STATISTICS_ETHERNET)
 	.stats = &eth_context_data.stats,
 #endif
+#if defined(CONFIG_ETH_NATIVE_POSIX_PTP_CLOCK)
+	.get_ptp_clock = eth_get_ptp_clock,
+#endif
 };
 
 ETH_NET_DEVICE_INIT(eth_native_posix, CONFIG_ETH_NATIVE_POSIX_DRV_NAME,
 		    eth_init, &eth_context_data, NULL,
 		    CONFIG_KERNEL_INIT_PRIORITY_DEFAULT, &eth_if_api,
 		    _ETH_MTU);
+
+#if defined(CONFIG_ETH_NATIVE_POSIX_PTP_CLOCK)
+struct ptp_context {
+	struct eth_context *eth_context;
+};
+
+static struct ptp_context ptp_0_context;
+
+
+static int ptp_clock_set_native_posix(struct device *clk,
+				      struct net_ptp_time *tm)
+{
+	ARG_UNUSED(clk);
+	ARG_UNUSED(tm);
+
+	/* We cannot set the host device time so this function
+	 * does nothing.
+	 */
+
+	return 0;
+}
+
+static int ptp_clock_get_native_posix(struct device *clk,
+				      struct net_ptp_time *tm)
+{
+	ARG_UNUSED(clk);
+
+	return eth_clock_gettime(tm);
+}
+
+static int ptp_clock_adjust_native_posix(struct device *clk,
+					 int increment)
+{
+	ARG_UNUSED(clk);
+	ARG_UNUSED(increment);
+
+	/* We cannot adjust the host device time so this function
+	 * does nothing.
+	 */
+
+	return 0;
+}
+
+static int ptp_clock_rate_adjust_native_posix(struct device *clk,
+					      float ratio)
+{
+	ARG_UNUSED(clk);
+	ARG_UNUSED(ratio);
+
+	/* We cannot adjust the host device time so this function
+	 * does nothing.
+	 */
+
+	return 0;
+}
+
+static const struct ptp_clock_driver_api api = {
+	.set = ptp_clock_set_native_posix,
+	.get = ptp_clock_get_native_posix,
+	.adjust = ptp_clock_adjust_native_posix,
+	.rate_adjust = ptp_clock_rate_adjust_native_posix,
+};
+
+static int ptp_init(struct device *port)
+{
+	struct device *eth_dev = DEVICE_GET(eth_native_posix);
+	struct eth_context *context = eth_dev->driver_data;
+	struct ptp_context *ptp_context = port->driver_data;
+
+	context->ptp_clock = port;
+	ptp_context->eth_context = context;
+
+	return 0;
+}
+
+DEVICE_AND_API_INIT(eth_native_posix_ptp_clock_0, PTP_CLOCK_NAME,
+		    ptp_init, &ptp_0_context, NULL, POST_KERNEL,
+		    CONFIG_APPLICATION_INIT_PRIORITY, &api);
+
+#endif /* CONFIG_ETH_NATIVE_POSIX_PTP_CLOCK */

drivers/ethernet/eth_native_posix_adapt.c

@@ -23,6 +23,7 @@
 #include <sys/ioctl.h>
 #include <sys/socket.h>
 #include <net/if.h>
+#include <time.h>
 
 #ifdef __linux
 #include <linux/if_tun.h>
@@ -38,6 +39,10 @@
 #include <sys_clock.h>
 #include <logging/sys_log.h>
 
+#if defined(CONFIG_NET_GPTP)
+#include <net/gptp.h>
+#endif
+
 #include "eth_native_posix_priv.h"
 
 /* Note that we cannot create the TUN/TAP device from the setup script
@@ -138,3 +143,21 @@ ssize_t eth_write_data(int fd, void *buf, size_t buf_len)
 {
 	return write(fd, buf, buf_len);
 }
+
+#if defined(CONFIG_NET_GPTP)
+int eth_clock_gettime(struct net_ptp_time *time)
+{
+	struct timespec tp;
+	int ret;
+
+	ret = clock_gettime(CLOCK_MONOTONIC_RAW, &tp);
+	if (ret < 0) {
+		return -errno;
+	}
+
+	time->second = tp.tv_sec;
+	time->nanosecond = tp.tv_nsec;
+
+	return 0;
+}
+#endif /* CONFIG_NET_GPTP */

drivers/ethernet/eth_native_posix_priv.h

@@ -18,4 +18,8 @@ int eth_wait_data(int fd);
 ssize_t eth_read_data(int fd, void *buf, size_t buf_len);
 ssize_t eth_write_data(int fd, void *buf, size_t buf_len);
 
+#if defined(CONFIG_NET_GPTP)
+int eth_clock_gettime(struct net_ptp_time *time);
+#endif
+
 #endif /* _ETH_NATIVE_POSIX_PRIV_H */

samples/net/gptp/boards/native_posix.conf

@@ -0,0 +1,6 @@
+# Settings for native_posix ethernet driver
+CONFIG_SYS_LOG_ETHERNET_LEVEL=1
+CONFIG_ETH_NATIVE_POSIX_PTP_CLOCK=y
+
+#CONFIG_ETH_NATIVE_POSIX_RANDOM_MAC=y
+CONFIG_ETH_NATIVE_POSIX_MAC_ADDR="00:00:5e:00:53:2a"