net: gptp: Initial support for native-posix board

jukkar · jukkar · commit 37593324a36a · 2018-03-19T11:21:37.000+02:00
Allow gPTP code to be run as a linux process and communicate
with gPTP daemon running in linux host.

Signed-off-by: Jukka Rissanen &lt;jukka.rissanen@linux.intel.com&gt;
diff --git a/drivers/ethernet/Kconfig.native_posix b/drivers/ethernet/Kconfig.native_posix
@@ -43,6 +43,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
diff --git a/drivers/ethernet/eth_native_posix.c b/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_messages.h>
+#endif
+
 #include "eth_native_posix_priv.h"
 
 #if defined(CONFIG_NET_L2_ETHERNET)
@@ -67,6 +72,107 @@ 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 ethernet_context *eth_ctx;
+	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)
+	eth_ctx = net_if_l2_data(iface);
+	if (net_eth_is_vlan_enabled(eth_ctx, 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)
+{
+	switch (hdr->message_type) {
+	case GPTP_SYNC_MESSAGE:
+	case GPTP_DELAY_REQ_MESSAGE:
+	case GPTP_PATH_DELAY_REQ_MESSAGE:
+	case GPTP_PATH_DELAY_RESP_MESSAGE:
+		net_pkt_set_priority(pkt, NET_PRIORITY_CA);
+		break;
+	default:
+		net_pkt_set_priority(pkt, NET_PRIORITY_IC);
+		break;
+	}
+}
+
+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);
@@ -86,6 +192,8 @@ static int eth_send(struct net_if *iface, struct net_pkt *pkt)
 		frag = frag->frags;
 	}
 
+	update_gptp(iface, pkt, true);
+
 	net_pkt_unref(pkt);
 
 	SYS_LOG_DBG("Send pkt %p len %d", pkt, count);
@@ -130,6 +238,7 @@ static struct net_if *get_iface(struct eth_context *context,
 static int read_data(struct eth_context *ctx, int fd)
 {
 	u16_t vlan_tag = NET_VLAN_TAG_UNSPEC;
+	struct net_if *iface;
 	struct net_pkt *pkt;
 	struct net_buf *frag;
 	int ret;
@@ -172,12 +281,25 @@ 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
 
 	SYS_LOG_DBG("Recv pkt %p len %d", pkt, net_pkt_get_len(pkt));
 
-	if (net_recv_data(get_iface(ctx, vlan_tag), pkt) < 0) {
+	iface = get_iface(ctx, vlan_tag);
+
+	update_gptp(iface, pkt, false);
+
+	if (net_recv_data(iface, pkt) < 0) {
 		net_pkt_unref(pkt);
 	}
 
@@ -283,3 +405,60 @@ 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)
+static int ptp_clock_set_native_posix(struct ptp_clock *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 ptp_clock *clk,
+				      struct net_ptp_time *tm)
+{
+	return eth_clock_gettime(tm);
+}
+
+static int ptp_clock_adjust_native_posix(struct ptp_clock *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 ptp_clock *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,
+};
+
+PTP_CLOCK_DEVICE_INIT(eth_native_posix, api);
+
+#endif /* CONFIG_ETH_NATIVE_POSIX_PTP_CLOCK */
diff --git a/drivers/ethernet/eth_native_posix_adapt.c b/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 */
diff --git a/drivers/ethernet/eth_native_posix_priv.h b/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 */
diff --git a/samples/net/gptp/prj.conf b/samples/net/gptp/prj.conf
@@ -85,3 +85,10 @@ CONFIG_NET_TC_RX_COUNT=4
 
 # Enable priority support in net_context
 CONFIG_NET_CONTEXT_PRIORITY=y
+
+# Settings for native_posix ethernet driver (if compiled for that board)
+CONFIG_ETH_NATIVE_POSIX=y
+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"
