net: Extend the protocol handling in Ethernet

Allow user to specify protocol extensions when receiving data
from Ethernet network. This means that user can register L3
protocol handler using NET_L3_REGISTER() with the desired
protocol type. Ethernet code will then call the handler if
such a protocol type packet is received. This is currently
only implemented for Ethernet. The original IPv4 and IPv6
handling is left intact even if they can be considered to
be L3 layer protocol. This could be changed in the future
if needed so that IPv4 and IPv6 handling could be made
pluggable protocols.

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

cmake/linker_script/common/common-rom.cmake

@@ -95,6 +95,7 @@ if(CONFIG_NET_SOCKETS)
   zephyr_iterable_section(NAME net_socket_register KVMA RAM_REGION GROUP RODATA_REGION SUBALIGN ${CONFIG_LINKER_ITERABLE_SUBALIGN})
 endif()
 
+zephyr_iterable_section(NAME net_l3_register KVMA RAM_REGION GROUP RODATA_REGION SUBALIGN ${CONFIG_LINKER_ITERABLE_SUBALIGN})
 
 if(CONFIG_NET_L2_PPP)
   zephyr_iterable_section(NAME ppp_protocol_handler KVMA RAM_REGION GROUP RODATA_REGION SUBALIGN ${CONFIG_LINKER_ITERABLE_SUBALIGN})

include/zephyr/linker/common-rom/common-rom-net.ld

@@ -2,6 +2,8 @@
 
 #include <zephyr/linker/iterable_sections.h>
 
+ITERABLE_SECTION_ROM(net_l3_register, Z_LINK_ITERABLE_SUBALIGN)
+
 #if defined(CONFIG_NET_SOCKETS)
 	ITERABLE_SECTION_ROM(net_socket_register, Z_LINK_ITERABLE_SUBALIGN)
 #endif

include/zephyr/net/net_core.h

@@ -21,6 +21,7 @@
 #include <zephyr/kernel.h>
 
 #include <zephyr/net/net_timeout.h>
+#include <zephyr/net/net_linkaddr.h>
 
 #ifdef __cplusplus
 extern "C" {
@@ -154,6 +155,42 @@ int net_send_data(struct net_pkt *pkt);
 #define NET_TC_COUNT 0
 #endif /* CONFIG_NET_TC_TX_COUNT && CONFIG_NET_TC_RX_COUNT */
 
+/**
+ * @brief Registration information for a given L3 handler. Note that
+ *        the layer number (L3) just refers to something that is on top
+ *        of L2. So for example IPv6 is L3 and IPv4 is L3, but Ethernet
+ *	  based LLDP, gPTP are more in the layer 2.5 but we consider them
+ *        as L3 here for simplicity.
+ */
+struct net_l3_register {
+	/** Store also the name of the L3 type in order to be able to
+	 * print it later.
+	 */
+	const char * const name;
+	/** Handler function for the given protocol type */
+	enum net_verdict (*handler)(struct net_if *iface,
+				    uint16_t ptype,
+				    struct net_pkt *pkt);
+	/** Is L2 address correct */
+	bool (*l2_addr_check)(struct net_if *iface,
+			      uint16_t ptype,
+			      struct net_pkt *pkt,
+			      struct net_linkaddr *dst_lladdr);
+	/** Protocol type */
+	uint16_t ptype;
+};
+
+#define NET_L3_GET_NAME(l3_name) __net_l3_register_##l3_name
+
+#define NET_L3_REGISTER(_name, _ptype, _handler, _is_addr_ok)		 \
+	static const STRUCT_SECTION_ITERABLE(net_l3_register,		 \
+					     NET_L3_GET_NAME(_name)) = { \
+		.ptype = _ptype,					 \
+		.handler = _handler,					 \
+		.l2_addr_check = _is_addr_ok,				 \
+		.name = STRINGIFY(_name),				 \
+	}
+
 /* @endcond */
 
 /**

include/zephyr/net/net_pkt.h

@@ -179,9 +179,12 @@ struct net_pkt {
 	struct net_linkaddr lladdr_dst;
 	uint16_t ll_proto_type;
 
+	union {
+		uint8_t hdr_len;	/* length of L2 header */
 #if defined(CONFIG_NET_IP)
-	uint8_t ip_hdr_len;	/* pre-filled in order to avoid func call */
+		uint8_t ip_hdr_len;	/* pre-filled in order to avoid func call */
 #endif
+	};
 
 	uint8_t overwrite : 1;	 /* Is packet content being overwritten? */
 	uint8_t eof : 1;	 /* Last packet before EOF */
@@ -516,6 +519,16 @@ static inline void net_pkt_set_chksum_done(struct net_pkt *pkt,
 	pkt->chksum_done = is_chksum_done;
 }
 
+static inline uint8_t net_pkt_hdr_len(struct net_pkt *pkt)
+{
+	return pkt->hdr_len;
+}
+
+static inline void net_pkt_set_hdr_len(struct net_pkt *pkt, uint8_t len)
+{
+	pkt->hdr_len = len;
+}
+
 static inline uint8_t net_pkt_ip_hdr_len(struct net_pkt *pkt)
 {
 #if defined(CONFIG_NET_IP)

modules/hostap/src/supp_main.c

@@ -1219,3 +1219,16 @@ static int init(void)
 }
 
 SYS_INIT(init, APPLICATION, 0);
+
+static enum net_verdict eapol_recv(struct net_if *iface, uint16_t ptype,
+				   struct net_pkt *pkt)
+{
+       ARG_UNUSED(iface);
+       ARG_UNUSED(ptype);
+
+       net_pkt_set_family(pkt, AF_UNSPEC);
+
+       return NET_CONTINUE;
+}
+
+NET_L3_REGISTER(eapol, NET_ETH_PTYPE_EAPOL, eapol_recv);

subsys/net/ip/net_private.h

@@ -239,18 +239,8 @@ void net_if_ipv6_start_dad(struct net_if *iface,
  * @brief Initialize Precision Time Protocol Layer.
  */
 void net_gptp_init(void);
-
-/**
- * @brief Process a ptp message.
- *
- * @param buf Buffer with a valid PTP Ethernet type.
- *
- * @return Return the policy for network buffer.
- */
-enum net_verdict net_gptp_recv(struct net_if *iface, struct net_pkt *pkt);
 #else
 #define net_gptp_init()
-#define net_gptp_recv(iface, pkt) NET_DROP
 #endif /* CONFIG_NET_GPTP */
 
 #if defined(CONFIG_NET_IPV4_FRAGMENT)

subsys/net/l2/ethernet/ethernet.c

@@ -240,8 +240,9 @@ static enum net_verdict ethernet_recv(struct net_if *iface,
 	uint8_t hdr_len = sizeof(struct net_eth_hdr);
 	uint16_t type;
 	struct net_linkaddr *lladdr;
-	sa_family_t family = AF_UNSPEC;
 	bool is_vlan_pkt = false;
+	bool handled = false;
+	enum net_verdict verdict;
 
 	/* This expects that the Ethernet header is in the first net_buf
 	 * fragment. This is a safe expectation here as it would not make
@@ -280,7 +281,6 @@ static enum net_verdict ethernet_recv(struct net_if *iface,
 		    !eth_is_vlan_tag_stripped(iface)) {
 			struct net_eth_vlan_hdr *hdr_vlan =
 				(struct net_eth_vlan_hdr *)NET_ETH_HDR(pkt);
-			enum net_verdict verdict;
 
 			net_pkt_set_vlan_tci(pkt, ntohs(hdr_vlan->vlan.tci));
 			type = ntohs(hdr_vlan->type);
@@ -311,43 +311,7 @@ static enum net_verdict ethernet_recv(struct net_if *iface,
 		}
 	}
 
-	switch (type) {
-	case NET_ETH_PTYPE_IP:
-	case NET_ETH_PTYPE_ARP:
-		net_pkt_set_family(pkt, AF_INET);
-		family = AF_INET;
-		break;
-	case NET_ETH_PTYPE_IPV6:
-		net_pkt_set_family(pkt, AF_INET6);
-		family = AF_INET6;
-		break;
-	case NET_ETH_PTYPE_EAPOL:
-		family = AF_UNSPEC;
-		break;
-#if defined(CONFIG_NET_L2_PTP)
-	case NET_ETH_PTYPE_PTP:
-		family = AF_UNSPEC;
-		break;
-#endif
-	case NET_ETH_PTYPE_LLDP:
-#if defined(CONFIG_NET_LLDP)
-		net_buf_pull(pkt->frags, hdr_len);
-		return net_lldp_recv(iface, pkt);
-#else
-		NET_DBG("LLDP Rx agent not enabled");
-		goto drop;
-#endif
-	default:
-		if (IS_ENABLED(CONFIG_NET_ETHERNET_FORWARD_UNRECOGNISED_ETHERTYPE)) {
-			family = AF_UNSPEC;
-			break;
-		}
-
-		NET_DBG("Unknown hdr type 0x%04x iface %d (%p)", type,
-			net_if_get_by_iface(iface), iface);
-		eth_stats_update_unknown_protocol(iface);
-		return NET_DROP;
-	}
+	net_pkt_set_hdr_len(pkt, hdr_len);
 
 	/* Set the pointers to ll src and dst addresses */
 	lladdr = net_pkt_lladdr_src(pkt);
@@ -374,11 +338,27 @@ static enum net_verdict ethernet_recv(struct net_if *iface,
 			       net_pkt_lladdr_dst(pkt));
 	}
 
+	STRUCT_SECTION_FOREACH(net_l3_register, l3) {
+		bool is_addr_ok;
+
+		if (l3->ptype != type || l3->l2_addr_check == NULL) {
+			continue;
+		}
+
+		NET_DBG("Calling L3 %s address check for type 0x%04x iface %d (%p)",
+			l3->name, type, net_if_get_by_iface(iface), iface);
+
+		is_addr_ok = l3->l2_addr_check(iface, type, pkt, lladdr);
+		if (!is_addr_ok) {
+			NET_DBG("Dropping frame, not for me [%s]",
+				net_sprint_ll_addr(lladdr->addr,
+						   sizeof(struct net_eth_addr)));
+			goto drop;
+		}
+	}
+
 	if (!net_eth_is_addr_broadcast((struct net_eth_addr *)lladdr->addr) &&
 	    !net_eth_is_addr_multicast((struct net_eth_addr *)lladdr->addr) &&
-	    !net_eth_is_addr_lldp_multicast(
-		    (struct net_eth_addr *)lladdr->addr) &&
-	    !net_eth_is_addr_ptp_multicast((struct net_eth_addr *)lladdr->addr) &&
 	    !net_linkaddr_cmp(net_if_get_link_addr(iface), lladdr)) {
 		/* The ethernet frame is not for me as the link addresses
 		 * are different.
@@ -389,44 +369,136 @@ static enum net_verdict ethernet_recv(struct net_if *iface,
 		goto drop;
 	}
 
+	/* Get rid of the Ethernet header. */
 	net_buf_pull(pkt->frags, hdr_len);
 
-	if (IS_ENABLED(CONFIG_NET_IPV4) && type == NET_ETH_PTYPE_IP &&
-	    ethernet_check_ipv4_bcast_addr(pkt, hdr) == NET_DROP) {
-		goto drop;
-	}
+	STRUCT_SECTION_FOREACH(net_l3_register, l3) {
+		if (l3->ptype != type) {
+			continue;
+		}
 
-	ethernet_update_rx_stats(iface, hdr, net_pkt_get_len(pkt) + hdr_len);
+		NET_DBG("Calling L3 %s handler for type 0x%04x iface %d (%p)",
+			l3->name, type, net_if_get_by_iface(iface), iface);
 
-	if (IS_ENABLED(CONFIG_NET_ARP) &&
-	    family == AF_INET && type == NET_ETH_PTYPE_ARP) {
-		NET_DBG("ARP packet from %s received",
-			net_sprint_ll_addr((uint8_t *)hdr->src.addr,
-					   sizeof(struct net_eth_addr)));
+		verdict = l3->handler(iface, type, pkt);
+		if (verdict == NET_OK || verdict == NET_CONTINUE) {
+			handled = true;
+			break;
+		} else {
+			goto drop;
+		}
+	}
 
-		if (IS_ENABLED(CONFIG_NET_IPV4_ACD) &&
-		    net_ipv4_acd_input(iface, pkt) == NET_DROP) {
+	if (!handled) {
+		if (IS_ENABLED(CONFIG_NET_ETHERNET_FORWARD_UNRECOGNISED_ETHERTYPE)) {
+			net_pkt_set_family(pkt, AF_UNSPEC);
+		} else {
+			NET_DBG("Unknown hdr type 0x%04x iface %d (%p)", type,
+				net_if_get_by_iface(iface), iface);
+			eth_stats_update_unknown_protocol(iface);
 			return NET_DROP;
 		}
-
-		return net_arp_input(pkt, hdr);
 	}
 
-	if (IS_ENABLED(CONFIG_NET_GPTP) && type == NET_ETH_PTYPE_PTP) {
-		return net_gptp_recv(iface, pkt);
+	/* FIXME: ARP eats the packet and the pkt is no longer a valid one,
+	 * so we cannot update the stats here. Fix the code to be more generic
+	 * so that we do not need this check.
+	 */
+	if (type == NET_ETH_PTYPE_ARP) {
+		return verdict;
 	}
 
+	ethernet_update_rx_stats(iface, hdr, net_pkt_get_len(pkt) + hdr_len);
+
 	if (type != NET_ETH_PTYPE_EAPOL) {
 		ethernet_update_length(iface, pkt);
 	}
 
-	return NET_CONTINUE;
+	return verdict;
 drop:
 	eth_stats_update_errors_rx(iface);
 	return NET_DROP;
 }
 
+#if defined(CONFIG_NET_IPV4)
+static bool is_ipv4_addr_ok(struct net_if *iface,
+			    uint16_t ptype,
+			    struct net_pkt *pkt,
+			    struct net_linkaddr *dst_lladdr)
+{
+	struct net_eth_hdr *hdr = NET_ETH_HDR(pkt);
+
+	ARG_UNUSED(iface);
+	ARG_UNUSED(ptype);
+	ARG_UNUSED(dst_lladdr);
+
+	if (ethernet_check_ipv4_bcast_addr(pkt, hdr) == NET_DROP) {
+		return false;
+	}
+
+	return true;
+}
+#endif
+
+#if defined(CONFIG_NET_IPV4) || defined(CONFIG_NET_IPV6)
+static enum net_verdict ethernet_ip_recv(struct net_if *iface,
+					 uint16_t ptype,
+					 struct net_pkt *pkt)
+{
+	ARG_UNUSED(iface);
+
+	switch (ptype) {
+	case NET_ETH_PTYPE_IP:
+	case NET_ETH_PTYPE_ARP:
+		net_pkt_set_family(pkt, AF_INET);
+		break;
+	case NET_ETH_PTYPE_IPV6:
+		net_pkt_set_family(pkt, AF_INET6);
+		break;
+	default:
+		return NET_DROP;
+	}
+
+	return NET_CONTINUE;
+}
+#endif /* CONFIG_NET_IPV4 || CONFIG_NET_IPV6 */
+
 #ifdef CONFIG_NET_IPV4
+NET_L3_REGISTER(ipv4, NET_ETH_PTYPE_IP, ethernet_ip_recv, is_ipv4_addr_ok);
+#endif
+
+#ifdef CONFIG_NET_ARP
+static enum net_verdict ethernet_arp_recv(struct net_if *iface,
+					  uint16_t ptype,
+					  struct net_pkt *pkt)
+{
+	struct net_eth_hdr *hdr = NET_ETH_HDR(pkt);
+
+	ARG_UNUSED(iface);
+	ARG_UNUSED(ptype);
+
+	net_pkt_set_family(pkt, AF_INET);
+
+	NET_DBG("ARP packet from %s received",
+		net_sprint_ll_addr((uint8_t *)hdr->src.addr,
+				   sizeof(struct net_eth_addr)));
+
+	if (IS_ENABLED(CONFIG_NET_IPV4_ACD) &&
+	    net_ipv4_acd_input(iface, pkt) == NET_DROP) {
+		return NET_DROP;
+	}
+
+	return net_arp_input(pkt, hdr);
+}
+
+NET_L3_REGISTER(arp, NET_ETH_PTYPE_ARP, ethernet_arp_recv, NULL);
+#endif /* CONFIG_NET_ARP */
+
+#if defined(CONFIG_NET_IPV6)
+NET_L3_REGISTER(ipv6, NET_ETH_PTYPE_IPV6, ethernet_ip_recv, NULL);
+#endif /* CONFIG_NET_IPV6 */
+
+#if defined(CONFIG_NET_IPV4)
 static inline bool ethernet_ipv4_dst_is_broadcast_or_mcast(struct net_pkt *pkt)
 {
 	if (net_ipv4_is_addr_bcast(net_pkt_iface(pkt),