drivers: native_posix: Add VLAN tag strip feature

This is mainly testing purpose from native_posize ethernet
driver. Enable CONFIG_ETH_NATIVE_POSIX_VLAN_TAG_STRIP to have
VLAN tag strip feature on ethernet Rx frames.

Signed-off-by: Ravi kumar Veeramally <[email protected]>

Author: rveerama1

drivers/ethernet/Kconfig.native_posix

@@ -94,6 +94,14 @@ config ETH_NATIVE_POSIX_RANDOM_MAC
 	help
 	  Generate a random MAC address dynamically.
 
+config ETH_NATIVE_POSIX_VLAN_TAG_STRIP
+	bool "Strip VLAN tag from Rx frames"
+	depends on NET_VLAN
+	help
+	  Native posix ethernet driver will strip of VLAN tag from
+	  Rx Ethernet frames and sets tag information in net packet
+	  metadata.
+
 if ! ETH_NATIVE_POSIX_RANDOM_MAC
 
 config	ETH_NATIVE_POSIX_MAC_ADDR

drivers/ethernet/eth_native_posix.c

@@ -231,55 +231,133 @@ static inline struct net_if *get_iface(struct eth_context *ctx,
 #endif
 }
 
-static int read_data(struct eth_context *ctx, int fd)
+#if defined(CONFIG_NET_VLAN)
+static struct net_pkt *prepare_vlan_pkt(struct eth_context *ctx,
+					int count, u16_t *vlan_tag, int *status)
 {
-	u16_t vlan_tag = NET_VLAN_TAG_UNSPEC;
-	struct net_if *iface;
+	struct net_eth_vlan_hdr *hdr = (struct net_eth_vlan_hdr *)ctx->recv;
 	struct net_pkt *pkt;
-	int count;
+	u8_t pos;
 
-	count = eth_read_data(fd, ctx->recv, sizeof(ctx->recv));
-	if (count <= 0) {
-		return 0;
+	if (IS_ENABLED(CONFIG_ETH_NATIVE_POSIX_VLAN_TAG_STRIP)) {
+		count -= NET_ETH_VLAN_HDR_SIZE;
 	}
 
 	pkt = net_pkt_rx_alloc_with_buffer(ctx->iface, count,
 					   AF_UNSPEC, 0, NET_BUF_TIMEOUT);
 	if (!pkt) {
-		return -ENOMEM;
+		*status = -ENOMEM;
+		return NULL;
+	}
+
+	net_pkt_set_vlan_tci(pkt, ntohs(hdr->vlan.tci));
+	*vlan_tag = net_pkt_vlan_tag(pkt);
+
+	pos = 0;
+
+	if (IS_ENABLED(CONFIG_ETH_NATIVE_POSIX_VLAN_TAG_STRIP)) {
+		if (net_pkt_write(pkt, ctx->recv,
+				  2 * sizeof(struct net_eth_addr))) {
+			goto error;
+		}
+
+		pos = (2 * sizeof(struct net_eth_addr)) + NET_ETH_VLAN_HDR_SIZE;
+		count -= (2 * sizeof(struct net_eth_addr));
+	}
+
+	if (net_pkt_write(pkt, ctx->recv + pos, count)) {
+		goto error;
+	}
+
+#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
+
+	*status = 0;
+
+	LOG_DBG("Recv pkt %p len %d", pkt, count);
+
+	return pkt;
+
+error:
+	net_pkt_unref(pkt);
+	*status = -ENOBUFS;
+	return NULL;
+}
+#endif
+
+static struct net_pkt *prepare_non_vlan_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)) {
-		return -ENOBUFS;
+		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)
+{
+	u16_t vlan_tag = NET_VLAN_TAG_UNSPEC;
+	struct net_if *iface;
+	struct net_pkt *pkt = NULL;
+	int status;
+	int count;
+
+	count = eth_read_data(fd, ctx->recv, sizeof(ctx->recv));
+	if (count <= 0) {
+		return 0;
 	}
 
 #if defined(CONFIG_NET_VLAN)
 	{
-		struct net_eth_hdr *hdr = NET_ETH_HDR(pkt);
+		struct net_eth_hdr *hdr = (struct net_eth_hdr *)(ctx->recv);
 
 		if (ntohs(hdr->type) == NET_ETH_PTYPE_VLAN) {
-			struct net_eth_vlan_hdr *hdr_vlan =
-				(struct net_eth_vlan_hdr *)NET_ETH_HDR(pkt);
+			pkt = prepare_vlan_pkt(ctx, count, &vlan_tag, &status);
+			if (!pkt) {
+				return status;
+			}
+		} else {
+			pkt = prepare_non_vlan_pkt(ctx, count, &status);
+			if (!pkt) {
+				return status;
+			}
 
-			net_pkt_set_vlan_tci(pkt, ntohs(hdr_vlan->vlan.tci));
-			vlan_tag = net_pkt_vlan_tag(pkt);
+			net_pkt_set_vlan_tci(pkt, 0);
 		}
-
-#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);
+	}
+#else
+	{
+		pkt = prepare_non_vlan_pkt(ctx, count, &status);
+		if (!pkt) {
+			return status;
 		}
-#endif
 	}
 #endif
 
 	iface = get_iface(ctx, vlan_tag);
 
-	LOG_DBG("Recv pkt %p len %d", pkt, count);
-
 	update_gptp(iface, pkt, false);
 
 	if (net_recv_data(iface, pkt) < 0) {
@@ -384,6 +462,9 @@ enum ethernet_hw_caps eth_posix_native_get_capabilities(struct device *dev)
 	ARG_UNUSED(dev);
 
 	return ETHERNET_HW_VLAN
+#if defined(CONFIG_ETH_NATIVE_POSIX_VLAN_TAG_STRIP)
+		| ETHERNET_HW_VLAN_TAG_STRIP
+#endif
 #if defined(CONFIG_ETH_NATIVE_POSIX_PTP_CLOCK)
 		| ETHERNET_PTP
 #endif

include/net/ethernet.h

@@ -75,6 +75,8 @@ struct net_eth_addr {
 #define NET_ETH_MTU			1500
 #define NET_ETH_MAX_FRAME_SIZE	(NET_ETH_MTU + sizeof(struct net_eth_hdr))
 
+#define NET_ETH_VLAN_HDR_SIZE	4
+
 /** @endcond */
 
 /** Ethernet hardware capabilities */