zephyr/drivers/ethernet/eth_native_tap.c

/*
 * Copyright (c) 2018 Intel Corporation
 *
 * SPDX-License-Identifier: Apache-2.0
 */

/**
 * @file
 *
 * TAP Ethernet driver for the native_sim board. This is meant for network
 * connectivity between the host and Zephyr.
 *
 * Note this driver is divided in two files. This one, built in the embedded code context,
 * with whichever libC is used in that context, and eth_native_tap_adapt.c built with the host
 * libC.
 */

#define LOG_MODULE_NAME eth_tap
#define LOG_LEVEL CONFIG_ETHERNET_LOG_LEVEL

#include <zephyr/logging/log.h>
LOG_MODULE_REGISTER(LOG_MODULE_NAME);

#include <stdio.h>

#include <zephyr/kernel.h>
#include <stdbool.h>
#include <errno.h>
#include <stddef.h>
#include <cmdline.h>
#include <posix_native_task.h>

#include <zephyr/net/net_pkt.h>
#include <zephyr/net/net_core.h>
#include <zephyr/net/net_if.h>
#include <zephyr/net/ethernet.h>
#include <ethernet/eth_stats.h>

#include <zephyr/drivers/ptp_clock.h>
#include <zephyr/net/gptp.h>
#include <zephyr/net/lldp.h>

#include "eth_native_tap_priv.h"
#include "nsi_host_trampolines.h"
#include "eth.h"

#define NET_BUF_TIMEOUT K_MSEC(100)

#if defined(CONFIG_NET_VLAN)
#define ETH_HDR_LEN sizeof(struct net_eth_vlan_hdr)
#else
#define ETH_HDR_LEN sizeof(struct net_eth_hdr)
#endif

struct eth_context {
	uint8_t recv[NET_ETH_MTU + ETH_HDR_LEN];
	uint8_t send[NET_ETH_MTU + ETH_HDR_LEN];
	uint8_t mac_addr[6];
	struct net_linkaddr ll_addr;
	struct net_if *iface;
	const char *if_name;
	k_tid_t rx_thread;
	struct z_thread_stack_element *rx_stack;
	size_t rx_stack_size;
	int dev_fd;
	bool init_done;
	bool status;
	bool promisc_mode;

#if defined(CONFIG_NET_STATISTICS_ETHERNET)
	struct net_stats_eth stats;
#endif
#if defined(CONFIG_ETH_NATIVE_TAP_PTP_CLOCK)
	const struct device *ptp_clock;
#endif
};

static const char *if_name_cmd_opt;
static const char *mac_addr_cmd_opt;
#ifdef CONFIG_NET_IPV4
static const char *ipv4_addr_cmd_opt;
static const char *ipv4_nm_cmd_opt;
static const char *ipv4_gw_cmd_opt;
#endif


#define DEFINE_RX_THREAD(x, _)						\
	K_KERNEL_STACK_DEFINE(rx_thread_stack_##x,			\
			      CONFIG_ARCH_POSIX_RECOMMENDED_STACK_SIZE);\
	static struct k_thread rx_thread_data_##x

LISTIFY(CONFIG_ETH_NATIVE_TAP_INTERFACE_COUNT, DEFINE_RX_THREAD, (;), _);

#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,
				       bool is_tx)
{
	uint8_t *msg_start = net_pkt_data(pkt);
	struct gptp_hdr *ghdr;
	int eth_hlen;
	struct net_eth_hdr *hdr;

	hdr = (struct net_eth_hdr *)msg_start;
	if (ntohs(hdr->type) != NET_ETH_PTYPE_PTP) {
		return NULL;
	}

	eth_hlen = sizeof(struct net_eth_hdr);

	/* In TX, the first net_buf contains the Ethernet header
	 * and the actual gPTP header is in the second net_buf.
	 * In RX, the Ethernet header + other headers are in the
	 * first net_buf.
	 */
	if (is_tx) {
		if (pkt->frags->frags == NULL) {
			return false;
		}

		ghdr = (struct gptp_hdr *)pkt->frags->frags->data;
	} else {
		ghdr = (struct gptp_hdr *)(pkt->frags->data + eth_hlen);
	}

	return ghdr;
}

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.second, &timestamp.nanosecond);
	if (ret < 0) {
		return;
	}

	net_pkt_set_timestamp(pkt, &timestamp);

	hdr = check_gptp_msg(iface, pkt, send);
	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(const struct device *dev, struct net_pkt *pkt)
{
	struct eth_context *ctx = dev->data;
	int count = net_pkt_get_len(pkt);
	int ret;

	ret = net_pkt_read(pkt, ctx->send, count);
	if (ret) {
		return ret;
	}

	update_gptp(net_pkt_iface(pkt), pkt, true);

	LOG_DBG("Send pkt %p len %d", pkt, count);

	ret = nsi_host_write(ctx->dev_fd, ctx->send, count);
	if (ret < 0) {
		LOG_DBG("Cannot send pkt %p (%d)", pkt, ret);
	}

	return ret < 0 ? ret : 0;
}

static struct net_linkaddr *eth_get_mac(struct eth_context *ctx)
{
	(void)net_linkaddr_set(&ctx->ll_addr, ctx->mac_addr,
			       sizeof(ctx->mac_addr));

	return &ctx->ll_addr;
}

static struct net_pkt *prepare_pkt(struct eth_context *ctx,
				   int count, int *status)
{
	struct net_pkt *pkt;

	pkt = net_pkt_rx_alloc_with_buffer(ctx->iface, count,
					   AF_UNSPEC, 0, NET_BUF_TIMEOUT);
	if (!pkt) {
		*status = -ENOMEM;
		return NULL;
	}

	if (net_pkt_write(pkt, ctx->recv, count)) {
		net_pkt_unref(pkt);
		*status = -ENOBUFS;
		return NULL;
	}

	*status = 0;

	LOG_DBG("Recv pkt %p len %d", pkt, count);

	return pkt;
}

static int read_data(struct eth_context *ctx, int fd)
{
	struct net_if *iface = ctx->iface;
	struct net_pkt *pkt = NULL;
	int status;
	int count;

	count = nsi_host_read(fd, ctx->recv, sizeof(ctx->recv));
	if (count <= 0) {
		return 0;
	}

	pkt = prepare_pkt(ctx, count, &status);
	if (!pkt) {
		return status;
	}

	update_gptp(iface, pkt, false);

	if (net_recv_data(iface, pkt) < 0) {
		net_pkt_unref(pkt);
	}

	return 0;
}

static void eth_rx(void *p1, void *p2, void *p3)
{
	ARG_UNUSED(p2);
	ARG_UNUSED(p3);

	struct eth_context *ctx = p1;
	LOG_DBG("Starting ZETH RX thread");

	while (1) {
		if (net_if_is_up(ctx->iface)) {
			while (!eth_wait_data(ctx->dev_fd)) {
				read_data(ctx, ctx->dev_fd);
				k_yield();
			}
		}

		k_sleep(K_MSEC(CONFIG_ETH_NATIVE_TAP_RX_TIMEOUT));
	}
}

#if defined(CONFIG_THREAD_MAX_NAME_LEN)
#define THREAD_MAX_NAME_LEN CONFIG_THREAD_MAX_NAME_LEN
#else
#define THREAD_MAX_NAME_LEN 1
#endif

static void create_rx_handler(struct eth_context *ctx)
{
	k_thread_create(ctx->rx_thread,
			ctx->rx_stack,
			ctx->rx_stack_size,
			eth_rx,
			ctx, NULL, NULL, K_PRIO_COOP(14),
			0, K_NO_WAIT);

	if (IS_ENABLED(CONFIG_THREAD_NAME)) {
		char name[THREAD_MAX_NAME_LEN];

		snprintk(name, sizeof(name), "eth_native_tap_rx-%s",
			 ctx->if_name);
		k_thread_name_set(ctx->rx_thread, name);
	}
}

static void eth_iface_init(struct net_if *iface)
{
	struct eth_context *ctx = net_if_get_device(iface)->data;
	struct net_linkaddr *ll_addr;
#if !defined(CONFIG_ETH_NATIVE_TAP_RANDOM_MAC)
	const char *mac_addr =
		mac_addr_cmd_opt ? mac_addr_cmd_opt : CONFIG_ETH_NATIVE_TAP_MAC_ADDR;
#endif
#ifdef CONFIG_NET_IPV4
	struct in_addr addr, netmask;
#endif

	ctx->iface = iface;

	ethernet_init(iface);

	if (ctx->init_done) {
		return;
	}

	net_lldp_set_lldpdu(iface);

	ctx->init_done = true;

#if defined(CONFIG_ETH_NATIVE_TAP_RANDOM_MAC)
	/* 00-00-5E-00-53-xx Documentation RFC 7042 */
	gen_random_mac(ctx->mac_addr, 0x00, 0x00, 0x5E);

	ctx->mac_addr[3] = 0x00;
	ctx->mac_addr[4] = 0x53;

	/* The TUN/TAP setup script will by default set the MAC address of host
	 * interface to 00:00:5E:00:53:FF so do not allow that.
	 */
	if (ctx->mac_addr[5] == 0xff) {
		ctx->mac_addr[5] = 0x01;
	}
#else
	/* Difficult to configure MAC addresses any sane way if we have more
	 * than one network interface.
	 */
	BUILD_ASSERT(CONFIG_ETH_NATIVE_TAP_INTERFACE_COUNT == 1,
		     "Cannot have static MAC if interface count > 1");

	if (mac_addr[0] != 0) {
		if (net_bytes_from_str(ctx->mac_addr, sizeof(ctx->mac_addr), mac_addr) < 0) {
			LOG_ERR("Invalid MAC address %s", mac_addr);
		}
	}
#endif

	ll_addr = eth_get_mac(ctx);

	/* If we have only one network interface, then use the name
	 * defined in the Kconfig directly. This way there is no need to
	 * change the documentation etc. and break things.
	 */
	if (CONFIG_ETH_NATIVE_TAP_INTERFACE_COUNT == 1) {
		ctx->if_name = CONFIG_ETH_NATIVE_TAP_DRV_NAME;
	}

	if (if_name_cmd_opt != NULL) {
		ctx->if_name = if_name_cmd_opt;
	}

	LOG_DBG("Interface %p using \"%s\"", iface, ctx->if_name);

	net_if_set_link_addr(iface, ll_addr->addr, ll_addr->len,
			     NET_LINK_ETHERNET);

#ifdef CONFIG_NET_IPV4
	if (ipv4_addr_cmd_opt != NULL) {
		if (net_addr_pton(AF_INET, ipv4_addr_cmd_opt, &addr) == 0) {
			net_if_ipv4_addr_add(iface, &addr, NET_ADDR_MANUAL, 0);

			if (ipv4_nm_cmd_opt != NULL) {
				if (net_addr_pton(AF_INET, ipv4_nm_cmd_opt, &netmask) == 0) {
					net_if_ipv4_set_netmask_by_addr(iface, &addr, &netmask);
				} else {
					NET_ERR("Invalid netmask: %s", ipv4_nm_cmd_opt);
				}
			}
		} else {
			NET_ERR("Invalid address: %s", ipv4_addr_cmd_opt);
		}
	}

	if (ipv4_gw_cmd_opt != NULL) {
		if (net_addr_pton(AF_INET, ipv4_gw_cmd_opt, &addr) == 0) {
			net_if_ipv4_set_gw(iface, &addr);
		} else {
			NET_ERR("Invalid gateway: %s", ipv4_gw_cmd_opt);
		}
	}
#endif

	ctx->dev_fd = eth_iface_create(CONFIG_ETH_NATIVE_POSIX_DEV_NAME, ctx->if_name, false);
	if (ctx->dev_fd < 0) {
		LOG_ERR("Cannot create %s (%d/%s)", ctx->if_name, ctx->dev_fd,
			strerror(-ctx->dev_fd));
	} else {
		/* Create a thread that will handle incoming data from host */
		create_rx_handler(ctx);
	}
}

static enum ethernet_hw_caps eth_native_tap_get_capabilities(const struct device *dev)
{
	ARG_UNUSED(dev);

	return ETHERNET_TXTIME
#if defined(CONFIG_NET_VLAN)
		| ETHERNET_HW_VLAN
#endif
#if defined(CONFIG_ETH_NATIVE_TAP_VLAN_TAG_STRIP)
		| ETHERNET_HW_VLAN_TAG_STRIP
#endif
#if defined(CONFIG_ETH_NATIVE_TAP_PTP_CLOCK)
		| ETHERNET_PTP
#endif
#if defined(CONFIG_NET_PROMISCUOUS_MODE)
		| ETHERNET_PROMISC_MODE
#endif
#if defined(CONFIG_NET_LLDP)
		| ETHERNET_LLDP
#endif
		;
}

#if defined(CONFIG_ETH_NATIVE_TAP_PTP_CLOCK)
static const struct device *eth_get_ptp_clock(const struct device *dev)
{
	struct eth_context *context = dev->data;

	return context->ptp_clock;
}
#endif

#if defined(CONFIG_NET_STATISTICS_ETHERNET)
static struct net_stats_eth *get_stats(const struct device *dev)
{
	struct eth_context *context = dev->data;

	return &(context->stats);
}
#endif

static int set_config(const struct device *dev,
		      enum ethernet_config_type type,
		      const struct ethernet_config *config)
{
	int ret = 0;

	if (IS_ENABLED(CONFIG_NET_PROMISCUOUS_MODE) &&
	    type == ETHERNET_CONFIG_TYPE_PROMISC_MODE) {
		struct eth_context *context = dev->data;

		if (config->promisc_mode) {
			if (context->promisc_mode) {
				return -EALREADY;
			}

			context->promisc_mode = true;
		} else {
			if (!context->promisc_mode) {
				return -EALREADY;
			}

			context->promisc_mode = false;
		}

		ret = eth_promisc_mode(context->if_name,
				       context->promisc_mode);
	} else if (type == ETHERNET_CONFIG_TYPE_MAC_ADDRESS) {
		struct eth_context *context = dev->data;

		memcpy(context->mac_addr, config->mac_address.addr,
		       sizeof(context->mac_addr));
	}

	return ret;
}

#if defined(CONFIG_NET_VLAN)
static int vlan_setup(const struct device *dev, struct net_if *iface,
		      uint16_t tag, bool enable)
{
	if (enable) {
		net_lldp_set_lldpdu(iface);
	} else {
		net_lldp_unset_lldpdu(iface);
	}

	return 0;
}
#endif /* CONFIG_NET_VLAN */

static const struct ethernet_api eth_if_api = {
	.iface_api.init = eth_iface_init,

	.get_capabilities = eth_native_tap_get_capabilities,
	.set_config = set_config,
	.send = eth_send,

#if defined(CONFIG_NET_VLAN)
	.vlan_setup = vlan_setup,
#endif
#if defined(CONFIG_NET_STATISTICS_ETHERNET)
	.get_stats = get_stats,
#endif
#if defined(CONFIG_ETH_NATIVE_TAP_PTP_CLOCK)
	.get_ptp_clock = eth_get_ptp_clock,
#endif
};

#define DEFINE_ETH_DEV_DATA(x, _)					     \
	static struct eth_context eth_context_data_##x = {		     \
		.if_name = CONFIG_ETH_NATIVE_TAP_DRV_NAME #x,		     \
		.rx_thread = &rx_thread_data_##x,			     \
		.rx_stack = rx_thread_stack_##x,			     \
		.rx_stack_size = K_KERNEL_STACK_SIZEOF(rx_thread_stack_##x), \
	}

LISTIFY(CONFIG_ETH_NATIVE_TAP_INTERFACE_COUNT, DEFINE_ETH_DEV_DATA, (;), _);

#define DEFINE_ETH_DEVICE(x, _)						\
	ETH_NET_DEVICE_INIT(eth_native_tap_##x,				\
			    CONFIG_ETH_NATIVE_TAP_DRV_NAME #x,		\
			    NULL, NULL,	&eth_context_data_##x, NULL,	\
			    CONFIG_KERNEL_INIT_PRIORITY_DEFAULT,	\
			    &eth_if_api,				\
			    NET_ETH_MTU)

LISTIFY(CONFIG_ETH_NATIVE_TAP_INTERFACE_COUNT, DEFINE_ETH_DEVICE, (;), _);

#if defined(CONFIG_ETH_NATIVE_TAP_PTP_CLOCK)

#if defined(CONFIG_NET_GPTP)
BUILD_ASSERT(								\
	CONFIG_ETH_NATIVE_TAP_INTERFACE_COUNT == CONFIG_NET_GPTP_NUM_PORTS, \
	"Number of network interfaces must match gPTP port count");
#endif

struct ptp_context {
	struct eth_context *eth_context;
};

#define DEFINE_PTP_DEV_DATA(x, _) \
	static struct ptp_context ptp_context_##x

LISTIFY(CONFIG_ETH_NATIVE_TAP_INTERFACE_COUNT, DEFINE_PTP_DEV_DATA, (;), _);

static int ptp_clock_set_native_tap(const 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_tap(const struct device *clk, struct net_ptp_time *tm)
{
	ARG_UNUSED(clk);

	return eth_clock_gettime(&tm->second, &tm->nanosecond);
}

static int ptp_clock_adjust_native_tap(const 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_tap(const struct device *clk, double ratio)
{
	ARG_UNUSED(clk);
	ARG_UNUSED(ratio);

	/* We cannot adjust the host device time so this function
	 * does nothing.
	 */

	return 0;
}

static DEVICE_API(ptp_clock, api) = {
	.set = ptp_clock_set_native_tap,
	.get = ptp_clock_get_native_tap,
	.adjust = ptp_clock_adjust_native_tap,
	.rate_adjust = ptp_clock_rate_adjust_native_tap,
};

#define PTP_INIT_FUNC(x, _)						\
	static int ptp_init_##x(const struct device *port)			\
	{								\
		const struct device *const eth_dev = DEVICE_GET(eth_native_tap_##x); \
		struct eth_context *context = eth_dev->data;	\
		struct ptp_context *ptp_context = port->data;	\
									\
		context->ptp_clock = port;				\
		ptp_context->eth_context = context;			\
									\
		return 0;						\
	}

LISTIFY(CONFIG_ETH_NATIVE_TAP_INTERFACE_COUNT, PTP_INIT_FUNC, (), _)

#define DEFINE_PTP_DEVICE(x, _)						\
	DEVICE_DEFINE(eth_native_tap_ptp_clock_##x,			\
			    PTP_CLOCK_NAME "_" #x,			\
			    ptp_init_##x,				\
			    NULL,					\
			    &ptp_context_##x,				\
			    NULL,					\
			    POST_KERNEL,				\
			    CONFIG_KERNEL_INIT_PRIORITY_DEFAULT,	\
			    &api)

LISTIFY(CONFIG_ETH_NATIVE_TAP_INTERFACE_COUNT, DEFINE_PTP_DEVICE, (;), _);

#endif /* CONFIG_ETH_NATIVE_TAP_PTP_CLOCK */

static void add_native_tap_options(void)
{
	static struct args_struct_t eth_native_tap_options[] = {
		{
			.is_mandatory = false,
			.option = "eth-if",
			.name = "name",
			.type = 's',
			.dest = (void *)&if_name_cmd_opt,
			.descript = "Name of the eth interface to use",
		},
		{
			.is_mandatory = false,
			.option = "mac-addr",
			.name = "mac",
			.type = 's',
			.dest = (void *)&mac_addr_cmd_opt,
			.descript = "MAC address",
		},
#ifdef CONFIG_NET_IPV4
		{
			.is_mandatory = false,
			.option = "ipv4-addr",
			.name = "ipv4",
			.type = 's',
			.dest = (void *)&ipv4_addr_cmd_opt,
			.descript = "IPv4 address",
		},
		{
			.is_mandatory = false,
			.option = "ipv4-gw",
			.name = "ipv4",
			.type = 's',
			.dest = (void *)&ipv4_gw_cmd_opt,
			.descript = "IPv4 gateway",
		},
		{
			.is_mandatory = false,
			.option = "ipv4-nm",
			.name = "ipv4",
			.type = 's',
			.dest = (void *)&ipv4_nm_cmd_opt,
			.descript = "IPv4 netmask",
		},
#endif
		ARG_TABLE_ENDMARKER,
	};

	native_add_command_line_opts(eth_native_tap_options);
}

NATIVE_TASK(add_native_tap_options, PRE_BOOT_1, 10);