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 <jukka.rissanen@nordicsemi.no>

Author: jukkar

cmake/linker_script/common/common-rom.cmake

@@ -91,6 +91,10 @@ zephyr_linker_section_configure(
   KEEP SORT NAME
 )
 
+if(CONFIG_NETWORKING)
+  zephyr_iterable_section(NAME net_l3_register KVMA RAM_REGION GROUP RODATA_REGION SUBALIGN ${CONFIG_LINKER_ITERABLE_SUBALIGN})
+endif()
+
 if(CONFIG_NET_SOCKETS)
   zephyr_iterable_section(NAME net_socket_register KVMA RAM_REGION GROUP RODATA_REGION SUBALIGN ${CONFIG_LINKER_ITERABLE_SUBALIGN})
 endif()

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

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

include/zephyr/net/ethernet.h

@@ -1261,6 +1261,16 @@ static inline bool net_eth_is_vlan_interface(struct net_if *iface)
 #define ETH_NET_DEVICE_DT_INST_DEFINE(inst, ...) \
 	ETH_NET_DEVICE_DT_DEFINE(DT_DRV_INST(inst), __VA_ARGS__)
 
+/**
+ * @brief Ethernet L3 protocol register macro.
+ *
+ * @param name Name of the L3 protocol.
+ * @param ptype Ethernet protocol type.
+ * @param handler Handler function for this protocol type.
+ */
+#define ETH_NET_L3_REGISTER(name, ptype, handler) \
+	NET_L3_REGISTER(&NET_L2_GET_NAME(ETHERNET), name, ptype, handler)
+
 /**
  * @brief Inform ethernet L2 driver that ethernet carrier is detected.
  * This happens when cable is connected.

include/zephyr/net/lldp.h

@@ -225,16 +225,6 @@ typedef enum net_verdict (*net_lldp_recv_cb_t)(struct net_if *iface,
  */
 int net_lldp_register_callback(struct net_if *iface, net_lldp_recv_cb_t cb);
 
-/**
- * @brief Parse LLDP packet
- *
- * @param iface Network interface
- * @param pkt Network packet
- *
- * @return Return the policy for network buffer
- */
-enum net_verdict net_lldp_recv(struct net_if *iface, struct net_pkt *pkt);
-
 /**
  * @brief Set LLDP protocol data unit (LLDPDU) for the network interface.
  *

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,40 @@ 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;
+	/** What L2 layer this is for */
+	const struct net_l2 * const l2;
+	/** Handler function for the given protocol type */
+	enum net_verdict (*handler)(struct net_if *iface,
+				    uint16_t ptype,
+				    struct net_pkt *pkt);
+	/** Protocol type */
+	uint16_t ptype;
+};
+
+#define NET_L3_GET_NAME(l3_name, ptype) __net_l3_register_##l3_name##_##ptype
+
+#define NET_L3_REGISTER(_l2_type, _name, _ptype, _handler)		\
+	static const STRUCT_SECTION_ITERABLE(net_l3_register,		\
+				    NET_L3_GET_NAME(_name, _ptype)) = { \
+		.ptype = _ptype,					\
+		.handler = _handler,					\
+		.name = STRINGIFY(_name),				\
+		.l2 = _l2_type,						\
+	};								\
+	BUILD_ASSERT((_handler) != NULL, "Handler is not defined")
+
 /* @endcond */
 
 /**

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;
+}
+
+ETH_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/arp.c

@@ -19,6 +19,7 @@ LOG_MODULE_REGISTER(net_arp, CONFIG_NET_ARP_LOG_LEVEL);
 #include <zephyr/net/net_mgmt.h>
 
 #include "arp.h"
+#include "ipv4.h"
 #include "net_private.h"
 
 #define NET_BUF_TIMEOUT K_MSEC(100)
@@ -1013,3 +1014,28 @@ void net_arp_init(void)
 			  K_SECONDS(CONFIG_NET_ARP_GRATUITOUS_INTERVAL));
 #endif /* defined(CONFIG_NET_ARP_GRATUITOUS_TRANSMISSION) */
 }
+
+static enum net_verdict 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);
+}
+
+ETH_NET_L3_REGISTER(ARP, NET_ETH_PTYPE_ARP, arp_recv);

subsys/net/l2/ethernet/ethernet.c

@@ -236,11 +236,13 @@ static enum net_verdict ethernet_recv(struct net_if *iface,
 				      struct net_pkt *pkt)
 {
 	struct ethernet_context *ctx = net_if_l2_data(iface);
-	struct net_eth_hdr *hdr = NET_ETH_HDR(pkt);
 	uint8_t hdr_len = sizeof(struct net_eth_hdr);
-	uint16_t type;
-	struct net_linkaddr *lladdr;
+	struct net_eth_hdr *hdr = NET_ETH_HDR(pkt);
+	enum net_verdict verdict = NET_CONTINUE;
 	bool is_vlan_pkt = false;
+	bool handled = false;
+	struct net_linkaddr *lladdr;
+	uint16_t type;
 
 	/* This expects that the Ethernet header is in the first net_buf
 	 * fragment. This is a safe expectation here as it would not make
@@ -279,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);
@@ -310,39 +311,6 @@ 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);
-		break;
-	case NET_ETH_PTYPE_IPV6:
-		net_pkt_set_family(pkt, AF_INET6);
-		break;
-	case NET_ETH_PTYPE_EAPOL:
-		break;
-#if defined(CONFIG_NET_L2_PTP)
-	case NET_ETH_PTYPE_PTP:
-		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)) {
-			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;
-	}
-
 	/* Set the pointers to ll src and dst addresses */
 	lladdr = net_pkt_lladdr_src(pkt);
 	lladdr->addr = hdr->src.addr;
@@ -370,9 +338,7 @@ static enum net_verdict ethernet_recv(struct net_if *iface,
 
 	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_eth_is_addr_group((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.
@@ -383,43 +349,92 @@ 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 || l3->l2 != &NET_L2_GET_NAME(ETHERNET)) {
+			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) && 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_DROP) {
+			NET_DBG("Dropping frame, packet rejected by %s", l3->name);
+			goto drop;
+		}
 
-		if (IS_ENABLED(CONFIG_NET_IPV4_ACD) &&
-		    net_ipv4_acd_input(iface, pkt) == NET_DROP) {
+		handled = true;
+		break;
+	}
+
+	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) || 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);
+
+	if (ptype == NET_ETH_PTYPE_IP) {
+		struct net_eth_hdr *hdr = NET_ETH_HDR(pkt);
+
+		if (ethernet_check_ipv4_bcast_addr(pkt, hdr) == NET_DROP) {
+			return NET_DROP;
+		}
+
+		net_pkt_set_family(pkt, AF_INET);
+	} else if (ptype == NET_ETH_PTYPE_IPV6) {
+		net_pkt_set_family(pkt, AF_INET6);
+	} else {
+		return NET_DROP;
+	}
+
+	return NET_CONTINUE;
+}
+#endif /* CONFIG_NET_IPV4 || CONFIG_NET_IPV6 */
+
 #ifdef CONFIG_NET_IPV4
+ETH_NET_L3_REGISTER(IPv4, NET_ETH_PTYPE_IP, ethernet_ip_recv);
+#endif
+
+#if defined(CONFIG_NET_IPV6)
+ETH_NET_L3_REGISTER(IPv6, NET_ETH_PTYPE_IPV6, ethernet_ip_recv);
+#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),

subsys/net/l2/ethernet/gptp/gptp.c

@@ -320,10 +320,20 @@ static void gptp_handle_msg(struct net_pkt *pkt)
 	}
 }
 
-enum net_verdict net_gptp_recv(struct net_if *iface, struct net_pkt *pkt)
+static enum net_verdict net_gptp_recv(struct net_if *iface, uint16_t ptype,
+				      struct net_pkt *pkt)
 {
 	struct gptp_hdr *hdr = GPTP_HDR(pkt);
 
+	ARG_UNUSED(ptype);
+
+	if (!(net_eth_is_addr_ptp_multicast(
+		      (struct net_eth_addr *)net_pkt_lladdr_dst(pkt)->addr) ||
+	      net_eth_is_addr_lldp_multicast(
+		      (struct net_eth_addr *)net_pkt_lladdr_dst(pkt)->addr))) {
+		return NET_DROP;
+	}
+
 	if ((hdr->ptp_version != GPTP_VERSION) ||
 			(hdr->transport_specific != GPTP_TRANSPORT_802_1_AS)) {
 		/* The stack only supports PTP V2 and transportSpecific set
@@ -346,6 +356,8 @@ enum net_verdict net_gptp_recv(struct net_if *iface, struct net_pkt *pkt)
 	return NET_DROP;
 }
 
+ETH_NET_L3_REGISTER(gPTP, NET_ETH_PTYPE_PTP, net_gptp_recv);
+
 static void gptp_init_clock_ds(void)
 {
 	struct gptp_global_ds *global_ds;