net: ethernet: Set the ptype by the caller in send

Instead of setting the upper protocol type in ethernet_send()
by checking the protocol type bits, use the ptype that is already
set by the caller. This allows new protocol types to be supported
and makes the system extensible.

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

Author: jukkar

subsys/net/ip/ipv4.c

@@ -17,6 +17,7 @@ LOG_MODULE_REGISTER(net_ipv4, CONFIG_NET_IPV4_LOG_LEVEL);
 #include <zephyr/net/net_stats.h>
 #include <zephyr/net/net_context.h>
 #include <zephyr/net/virtual.h>
+#include <zephyr/net/ethernet.h>
 #include "net_private.h"
 #include "connection.h"
 #include "net_stats.h"
@@ -133,6 +134,7 @@ int net_ipv4_finalize(struct net_pkt *pkt, uint8_t next_header_proto)
 	}
 
 	net_pkt_set_data(pkt, &ipv4_access);
+	net_pkt_set_ll_proto_type(pkt, NET_ETH_PTYPE_IP);
 
 	if (IS_ENABLED(CONFIG_NET_UDP) &&
 	    next_header_proto == IPPROTO_UDP) {
@@ -452,6 +454,8 @@ enum net_verdict net_ipv4_input(struct net_pkt *pkt, bool is_loopback)
 
 enum net_verdict net_ipv4_prepare_for_send(struct net_pkt *pkt)
 {
+	net_pkt_set_ll_proto_type(pkt, NET_ETH_PTYPE_IP);
+
 	if (IS_ENABLED(CONFIG_NET_IPV4_PMTU)) {
 		struct net_pmtu_entry *entry;
 		struct sockaddr_in dst = {

subsys/net/ip/ipv6.c

@@ -30,6 +30,7 @@ LOG_MODULE_REGISTER(net_ipv6, CONFIG_NET_IPV6_LOG_LEVEL);
 #include <zephyr/net/net_context.h>
 #include <zephyr/net/net_mgmt.h>
 #include <zephyr/net/virtual.h>
+#include <zephyr/net/ethernet.h>
 #include "net_private.h"
 #include "connection.h"
 #include "icmpv6.h"
@@ -142,6 +143,8 @@ int net_ipv6_finalize(struct net_pkt *pkt, uint8_t next_header_proto)
 		return -ENOBUFS;
 	}
 
+	net_pkt_set_ll_proto_type(pkt, NET_ETH_PTYPE_IPV6);
+
 	if (IS_ENABLED(CONFIG_NET_UDP) &&
 	    next_header_proto == IPPROTO_UDP) {
 		return net_udp_finalize(pkt, false);

subsys/net/ip/ipv6_nbr.c

@@ -25,6 +25,7 @@ LOG_MODULE_REGISTER(net_ipv6_nd, CONFIG_NET_IPV6_ND_LOG_LEVEL);
 #include <zephyr/net/net_mgmt.h>
 #include <zephyr/net/dns_resolve.h>
 #include <zephyr/net/icmp.h>
+#include <zephyr/net/ethernet.h>
 #include "net_private.h"
 #include "connection.h"
 #include "icmpv6.h"
@@ -805,6 +806,8 @@ enum net_verdict net_ipv6_prepare_for_send(struct net_pkt *pkt)
 		return NET_DROP;
 	}
 
+	net_pkt_set_ll_proto_type(pkt, NET_ETH_PTYPE_IPV6);
+
 #if defined(CONFIG_NET_IPV6_FRAGMENT)
 	/* If we have already fragmented the packet, the fragment id will
 	 * contain a proper value and we can skip other checks.

subsys/net/ip/net_context.c

@@ -2566,23 +2566,49 @@ static int context_sendto(struct net_context *context,
 
 		if (net_context_get_proto(context) == IPPROTO_RAW) {
 			char type = (NET_IPV6_HDR(pkt)->vtc & 0xf0);
+			struct sockaddr_ll *ll_addr;
 
 			/* Set the family to pkt if detected */
 			switch (type) {
 			case 0x60:
 				net_pkt_set_family(pkt, AF_INET6);
+				net_pkt_set_ll_proto_type(pkt, NET_ETH_PTYPE_IPV6);
 				break;
 			case 0x40:
 				net_pkt_set_family(pkt, AF_INET);
+				net_pkt_set_ll_proto_type(pkt, NET_ETH_PTYPE_IP);
 				break;
 			default:
 				/* Not IP traffic, let it go forward as it is */
+				ll_addr = (struct sockaddr_ll *)dst_addr;
+
+				net_pkt_set_ll_proto_type(pkt,
+							  ntohs(ll_addr->sll_protocol));
 				break;
 			}
 
 			/* Pass to L2: */
 			ret = net_send_data(pkt);
 		} else {
+			struct sockaddr_ll_ptr *ll_src_addr;
+			struct sockaddr_ll *ll_dst_addr;
+
+			/* The destination address is set in remote for this
+			 * socket type.
+			 */
+			ll_dst_addr = (struct sockaddr_ll *)&context->remote;
+			ll_src_addr = (struct sockaddr_ll_ptr *)&context->local;
+
+			net_pkt_lladdr_dst(pkt)->addr = ll_dst_addr->sll_addr;
+			net_pkt_lladdr_dst(pkt)->len =
+						sizeof(struct net_eth_addr);
+			net_pkt_lladdr_src(pkt)->addr = ll_src_addr->sll_addr;
+			net_pkt_lladdr_src(pkt)->len =
+						sizeof(struct net_eth_addr);
+
+			net_pkt_set_ll_proto_type(pkt,
+						  ntohs(ll_dst_addr->sll_protocol));
+
 			net_if_queue_tx(net_pkt_iface(pkt), pkt);
 		}
 	} else if (IS_ENABLED(CONFIG_NET_SOCKETS_CAN) && family == AF_CAN &&

subsys/net/l2/ethernet/arp.c

@@ -270,6 +270,9 @@ static inline struct net_pkt *arp_prepare(struct net_if *iface,
 			return NULL;
 		}
 
+		net_pkt_set_ll_proto_type(pkt, NET_ETH_PTYPE_ARP);
+		net_pkt_set_family(pkt, AF_INET);
+
 		/* Avoid recursive loop with network packet capturing */
 		if (IS_ENABLED(CONFIG_NET_CAPTURE) && pending) {
 			net_pkt_set_captured(pkt, net_pkt_is_captured(pending));
@@ -498,6 +501,7 @@ static void arp_gratuitous_send(struct net_if *iface,
 
 	net_buf_add(pkt->buffer, sizeof(struct net_arp_hdr));
 	net_pkt_set_vlan_tag(pkt, net_eth_get_vlan_tag(iface));
+	net_pkt_set_ll_proto_type(pkt, NET_ETH_PTYPE_ARP);
 
 	hdr = NET_ARP_HDR(pkt);
 
@@ -751,6 +755,9 @@ static inline struct net_pkt *arp_prepare_reply(struct net_if *iface,
 	net_pkt_lladdr_dst(pkt)->addr = (uint8_t *)&hdr->dst_hwaddr.addr;
 	net_pkt_lladdr_dst(pkt)->len = sizeof(struct net_eth_addr);
 
+	net_pkt_set_ll_proto_type(pkt, NET_ETH_PTYPE_ARP);
+	net_pkt_set_family(pkt, AF_INET);
+
 	return pkt;
 }
 

subsys/net/l2/ethernet/ethernet.c

@@ -674,9 +674,9 @@ static int ethernet_send(struct net_if *iface, struct net_pkt *pkt)
 {
 	const struct ethernet_api *api = net_if_get_device(iface)->api;
 	struct ethernet_context *ctx = net_if_l2_data(iface);
-	uint16_t ptype = 0;
-	int ret;
+	uint16_t ptype = htons(net_pkt_ll_proto_type(pkt));
 	struct net_pkt *orig_pkt = pkt;
+	int ret;
 
 	if (!api) {
 		ret = -ENOENT;
@@ -696,65 +696,35 @@ static int ethernet_send(struct net_if *iface, struct net_pkt *pkt)
 		goto send;
 	}
 
-	if (IS_ENABLED(CONFIG_NET_IPV4) &&
-	    net_pkt_family(pkt) == AF_INET) {
-		struct net_pkt *tmp;
+	if (IS_ENABLED(CONFIG_NET_IPV4) && net_pkt_family(pkt) == AF_INET) {
+		if (!net_pkt_ipv4_acd(pkt)) {
+			struct net_pkt *tmp;
 
-		if (net_pkt_ipv4_acd(pkt)) {
-			ptype = htons(NET_ETH_PTYPE_ARP);
-		} else {
 			tmp = ethernet_ll_prepare_on_ipv4(iface, pkt);
-			if (!tmp) {
+			if (tmp == NULL) {
 				ret = -ENOMEM;
 				goto error;
 			} else if (IS_ENABLED(CONFIG_NET_ARP) && tmp != pkt) {
 				/* Original pkt got queued and is replaced
 				 * by an ARP request packet.
 				 */
 				pkt = tmp;
-				ptype = htons(NET_ETH_PTYPE_ARP);
-				net_pkt_set_family(pkt, AF_INET);
-			} else {
-				ptype = htons(NET_ETH_PTYPE_IP);
+				ptype = htons(net_pkt_ll_proto_type(pkt));
 			}
 		}
-	} else if (IS_ENABLED(CONFIG_NET_IPV6) &&
-		   net_pkt_family(pkt) == AF_INET6) {
-		ptype = htons(NET_ETH_PTYPE_IPV6);
 	} else if (IS_ENABLED(CONFIG_NET_SOCKETS_PACKET) &&
 		   net_pkt_family(pkt) == AF_PACKET) {
 		struct net_context *context = net_pkt_context(pkt);
 
-		if (context && net_context_get_type(context) == SOCK_DGRAM) {
-			struct sockaddr_ll *dst_addr;
-			struct sockaddr_ll_ptr *src_addr;
-
-			/* The destination address is set in remote for this
-			 * socket type.
-			 */
-			dst_addr = (struct sockaddr_ll *)&context->remote;
-			src_addr = (struct sockaddr_ll_ptr *)&context->local;
-
-			net_pkt_lladdr_dst(pkt)->addr = dst_addr->sll_addr;
-			net_pkt_lladdr_dst(pkt)->len =
-						sizeof(struct net_eth_addr);
-			net_pkt_lladdr_src(pkt)->addr = src_addr->sll_addr;
-			net_pkt_lladdr_src(pkt)->len =
-						sizeof(struct net_eth_addr);
-			ptype = dst_addr->sll_protocol;
-		} else {
+		if (!(context && net_context_get_type(context) == SOCK_DGRAM)) {
+			/* Raw packet, just send it */
 			goto send;
 		}
-	} else if (IS_ENABLED(CONFIG_NET_L2_PTP) && net_pkt_is_ptp(pkt)) {
-		ptype = htons(NET_ETH_PTYPE_PTP);
-	} else if (IS_ENABLED(CONFIG_NET_LLDP) && net_pkt_is_lldp(pkt)) {
-		ptype = htons(NET_ETH_PTYPE_LLDP);
-	} else if (IS_ENABLED(CONFIG_NET_ARP)) {
-		/* Unknown type: Unqueued pkt is an ARP reply.
-		 */
-		ptype = htons(NET_ETH_PTYPE_ARP);
-		net_pkt_set_family(pkt, AF_INET);
-	} else {
+	}
+
+	if (ptype == 0) {
+		/* Caller of this function has not set the ptype */
+		NET_ERR("No protocol set for pkt %p", pkt);
 		ret = -ENOTSUP;
 		goto error;
 	}

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

@@ -170,6 +170,7 @@ static struct net_pkt *setup_gptp_frame(struct net_if *iface,
 
 	net_buf_add(pkt->buffer, sizeof(struct gptp_hdr) + extra_header);
 	net_pkt_set_ptp(pkt, true);
+	net_pkt_set_ll_proto_type(pkt, NET_ETH_PTYPE_PTP);
 
 	net_pkt_lladdr_src(pkt)->addr = net_if_get_link_addr(iface)->addr;
 	net_pkt_lladdr_src(pkt)->len = net_if_get_link_addr(iface)->len;

subsys/net/l2/ethernet/lldp/lldp.c

@@ -114,6 +114,7 @@ static int lldp_send(struct ethernet_lldp *lldp)
 	}
 
 	net_pkt_set_lldp(pkt, true);
+	net_pkt_set_ll_proto_type(pkt, NET_ETH_PTYPE_LLDP);
 
 	ret = net_pkt_write(pkt, (uint8_t *)lldp->lldpdu,
 			    sizeof(struct net_lldpdu));

tests/net/arp/src/main.c

@@ -215,6 +215,8 @@ static inline struct net_pkt *prepare_arp_reply(struct net_if *iface,
 
 	net_buf_add(pkt->buffer, sizeof(struct net_arp_hdr));
 
+	net_pkt_set_ll_proto_type(pkt, NET_ETH_PTYPE_ARP);
+
 	return pkt;
 }
 
@@ -265,6 +267,8 @@ static inline struct net_pkt *prepare_arp_request(struct net_if *iface,
 
 	net_buf_add(pkt->buffer, sizeof(struct net_arp_hdr));
 
+	net_pkt_set_ll_proto_type(pkt, NET_ETH_PTYPE_ARP);
+
 	return pkt;
 }
 
@@ -373,6 +377,8 @@ ZTEST(arp_fn_tests, test_arp)
 	net_ipv4_addr_copy_raw(ipv4->src, (uint8_t *)&src);
 	net_ipv4_addr_copy_raw(ipv4->dst, (uint8_t *)&dst);
 
+	net_pkt_set_ll_proto_type(pkt, NET_ETH_PTYPE_IP);
+
 	memcpy(net_buf_add(pkt->buffer, len), app_data, len);
 
 	pkt2 = net_arp_prepare(pkt, &dst, NULL);
@@ -381,6 +387,9 @@ ZTEST(arp_fn_tests, test_arp)
 	 * stored in ARP table.
 	 */
 
+	zassert_equal(net_pkt_ll_proto_type(pkt2), NET_ETH_PTYPE_ARP,
+		      "ARP packet type is wrong");
+
 	/**TESTPOINTS: Check packets*/
 	zassert_not_equal((void *)(pkt2), (void *)(pkt),
 		/* The packets cannot be the same as the ARP cache has
@@ -530,6 +539,8 @@ ZTEST(arp_fn_tests, test_arp)
 	net_ipv4_addr_copy_raw(arp_hdr->dst_ipaddr, (uint8_t *)&dst);
 	net_ipv4_addr_copy_raw(arp_hdr->src_ipaddr, (uint8_t *)&src);
 
+	net_pkt_set_ll_proto_type(pkt, NET_ETH_PTYPE_ARP);
+
 	pkt2 = prepare_arp_reply(iface, pkt, &eth_hwaddr, &eth_hdr);
 
 	zassert_not_null(pkt2, "ARP reply generation failed.");
@@ -554,6 +565,9 @@ ZTEST(arp_fn_tests, test_arp)
 
 	net_pkt_unref(pkt);
 
+	/* Clear the ARP cache so that old entries do not confuse the tests */
+	net_arp_clear_cache(iface);
+
 	/* Then feed in ARP request */
 	pkt = net_pkt_alloc_with_buffer(iface, sizeof(struct net_eth_hdr) +
 					sizeof(struct net_arp_hdr),
@@ -571,6 +585,8 @@ ZTEST(arp_fn_tests, test_arp)
 	net_ipv4_addr_copy_raw(arp_hdr->dst_ipaddr, (uint8_t *)&src);
 	net_ipv4_addr_copy_raw(arp_hdr->src_ipaddr, (uint8_t *)&dst);
 
+	net_pkt_set_ll_proto_type(pkt, NET_ETH_PTYPE_ARP);
+
 	pkt2 = prepare_arp_request(iface, pkt, &eth_hwaddr, &eth_hdr);
 
 	/**TESTPOINT: Check if ARP request generation failed*/
@@ -633,6 +649,8 @@ ZTEST(arp_fn_tests, test_arp)
 
 		net_buf_add(pkt->buffer, sizeof(struct net_arp_hdr));
 
+		net_pkt_set_ll_proto_type(pkt, NET_ETH_PTYPE_ARP);
+
 		verdict = net_arp_input(pkt, eth_hdr);
 		zassert_not_equal(verdict, NET_DROP, "Gratuitous ARP failed");