From 8c26e40f47e6bf3cba93f6c0f724c16afdee798c Mon Sep 17 00:00:00 2001 From: Jukka Rissanen Date: Mon, 12 Mar 2018 21:54:19 +0200 Subject: [PATCH] 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 --- drivers/ethernet/Kconfig.native_posix | 8 + drivers/ethernet/eth_native_posix.c | 212 +++++++++++++++++++++- drivers/ethernet/eth_native_posix_adapt.c | 23 +++ drivers/ethernet/eth_native_posix_priv.h | 4 + samples/net/gptp/boards/native_posix.conf | 6 + 5 files changed, 252 insertions(+), 1 deletion(-) create mode 100644 samples/net/gptp/boards/native_posix.conf diff --git a/drivers/ethernet/Kconfig.native_posix b/drivers/ethernet/Kconfig.native_posix index 65dc1e3924e0c..a91b5713eed1d 100644 --- a/drivers/ethernet/Kconfig.native_posix +++ b/drivers/ethernet/Kconfig.native_posix @@ -41,6 +41,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 index 0402185af0ed0..0a06dc35c630d 100644 --- a/drivers/ethernet/eth_native_posix.c +++ b/drivers/ethernet/eth_native_posix.c @@ -28,6 +28,11 @@ #include #include +#if defined(CONFIG_ETH_NATIVE_POSIX_PTP_CLOCK) +#include +#include +#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(×tamp); + if (ret < 0) { + return; + } + + net_pkt_set_timestamp(pkt, ×tamp); + + 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 = ð_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, ð_context_data, NULL, CONFIG_KERNEL_INIT_PRIORITY_DEFAULT, ð_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 */ diff --git a/drivers/ethernet/eth_native_posix_adapt.c b/drivers/ethernet/eth_native_posix_adapt.c index a7e2039d4eeb9..e9b7661fbd085 100644 --- a/drivers/ethernet/eth_native_posix_adapt.c +++ b/drivers/ethernet/eth_native_posix_adapt.c @@ -23,6 +23,7 @@ #include #include #include +#include #ifdef __linux #include @@ -38,6 +39,10 @@ #include #include +#if defined(CONFIG_NET_GPTP) +#include +#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 index ffcbd62d2d623..17f0ecb926673 100644 --- 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/boards/native_posix.conf b/samples/net/gptp/boards/native_posix.conf new file mode 100644 index 0000000000000..8f8994613dca5 --- /dev/null +++ b/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"