net: pkt_filter: Add VLAN support to filtering

The Ethernet matching needs tweaking so that it will also
work with VLAN packets.

Signed-off-by: Jukka Rissanen <jukka.rissanen@nordicsemi.no>

include/zephyr/net/net_pkt_filter.h

@@ -58,6 +58,8 @@ enum npf_test_type {
 	NPF_TEST_TYPE_ETH_DST_ADDR_MASK_MATCH,
 	NPF_TEST_TYPE_ETH_TYPE_MATCH,
 	NPF_TEST_TYPE_ETH_TYPE_UNMATCH,
+	NPF_TEST_TYPE_ETH_VLAN_TYPE_MATCH,
+	NPF_TEST_TYPE_ETH_VLAN_TYPE_UNMATCH,
 };
 
 #if defined(CONFIG_NET_PKT_FILTER_LOG_LEVEL_DBG) || \
@@ -604,6 +606,8 @@ struct npf_test_eth_type {
 
 extern npf_test_fn_t npf_eth_type_match;
 extern npf_test_fn_t npf_eth_type_unmatch;
+extern npf_test_fn_t npf_eth_vlan_type_match;
+extern npf_test_fn_t npf_eth_vlan_type_unmatch;
 
 /** @endcond */
 
@@ -637,6 +641,38 @@ extern npf_test_fn_t npf_eth_type_unmatch;
 			    .test.type = NPF_TEST_TYPE_ETH_TYPE_UNMATCH,)) \
 	}
 
+/**
+ * @brief Statically define an "Ethernet VLAN header type match" packet
+ *        filter condition.
+ *
+ * @param _name Name of the condition
+ * @param _type Ethernet VLAN header type to match
+ */
+#define NPF_ETH_VLAN_TYPE_MATCH(_name, _type)				\
+	struct npf_test_eth_type _name = {				\
+		.type = htons(_type),					\
+		.test.fn = npf_eth_vlan_type_match,			\
+		IF_ENABLED(NPF_TEST_ENABLE_NAME,			\
+			   (.test.name = "eth vlan type",		\
+			    .test.type = NPF_TEST_TYPE_ETH_VLAN_TYPE_MATCH,)) \
+	}
+
+/**
+ * @brief Statically define an "Ethernet VLAN header type unmatch" packet
+ *        filter condition.
+ *
+ * @param _name Name of the condition
+ * @param _type Ethernet VLAN header type to exclude
+ */
+#define NPF_ETH_VLAN_TYPE_UNMATCH(_name, _type)				\
+	struct npf_test_eth_type _name = {				\
+		.type = htons(_type),					\
+		.test.fn = npf_eth_vlan_type_unmatch,			\
+		IF_ENABLED(NPF_TEST_ENABLE_NAME,			\
+			   (.test.name = "!eth vlan type",		\
+			    .test.type = NPF_TEST_TYPE_ETH_VLAN_TYPE_UNMATCH,)) \
+	}
+
 /** Type of the packet filter rule. */
 enum npf_rule_type {
 	NPF_RULE_TYPE_UNKNOWN = 0,   /**< Unknown rule type */

subsys/net/pkt_filter/base.c

@@ -449,7 +449,9 @@ const char *npf_test_get_str(struct npf_test *test, char *buf, size_t len)
 		buf[pos] = ']';
 
 	} else if (test->type == NPF_TEST_TYPE_ETH_TYPE_MATCH ||
-		   test->type == NPF_TEST_TYPE_ETH_TYPE_UNMATCH) {
+		   test->type == NPF_TEST_TYPE_ETH_TYPE_UNMATCH ||
+		   test->type == NPF_TEST_TYPE_ETH_VLAN_TYPE_MATCH ||
+		   test->type == NPF_TEST_TYPE_ETH_VLAN_TYPE_UNMATCH) {
 		struct npf_test_eth_type *test_eth =
 			CONTAINER_OF(test, struct npf_test_eth_type, test);
 

subsys/net/pkt_filter/ethernet.c

@@ -83,3 +83,23 @@ bool npf_eth_type_unmatch(struct npf_test *test, struct net_pkt *pkt)
 {
 	return !npf_eth_type_match(test, pkt);
 }
+
+bool npf_eth_vlan_type_match(struct npf_test *test, struct net_pkt *pkt)
+{
+	struct npf_test_eth_type *test_eth_type =
+			CONTAINER_OF(test, struct npf_test_eth_type, test);
+	struct net_eth_vlan_hdr *eth_hdr =
+		(struct net_eth_vlan_hdr *)NET_ETH_HDR(pkt);
+
+	/* note: type_match->type is assumed to be in network order already */
+	NET_DBG("proto type 0x%04x pkt 0x%04x",
+		ntohs(test_eth_type->type),
+		ntohs(eth_hdr->type));
+
+	return eth_hdr->type == test_eth_type->type;
+}
+
+bool npf_eth_vlan_type_unmatch(struct npf_test *test, struct net_pkt *pkt)
+{
+	return !npf_eth_vlan_type_match(test, pkt);
+}