1 /*
2 * Copyright (c) 2021 BayLibre SAS
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <zephyr/logging/log.h>
8 LOG_MODULE_REGISTER(npf_ethernet, CONFIG_NET_PKT_FILTER_LOG_LEVEL);
9
10 #include <zephyr/net/ethernet.h>
11 #include <zephyr/net/net_pkt_filter.h>
12
addr_mask_compare(struct net_eth_addr * addr1,struct net_eth_addr * addr2,struct net_eth_addr * mask)13 static bool addr_mask_compare(struct net_eth_addr *addr1,
14 struct net_eth_addr *addr2,
15 struct net_eth_addr *mask)
16 {
17 for (int i = 0; i < 6; i++) {
18 if ((addr1->addr[i] & mask->addr[i]) !=
19 (addr2->addr[i] & mask->addr[i])) {
20 return false;
21 }
22 }
23 return true;
24 }
25
addr_match(struct npf_test * test,struct net_eth_addr * pkt_addr)26 static bool addr_match(struct npf_test *test, struct net_eth_addr *pkt_addr)
27 {
28 struct npf_test_eth_addr *test_eth_addr =
29 CONTAINER_OF(test, struct npf_test_eth_addr, test);
30 struct net_eth_addr *addr = test_eth_addr->addresses;
31 struct net_eth_addr *mask = &test_eth_addr->mask;
32 unsigned int nb_addr = test_eth_addr->nb_addresses;
33
34 while (nb_addr) {
35 if (addr_mask_compare(addr, pkt_addr, mask)) {
36 return true;
37 }
38 addr++;
39 nb_addr--;
40 }
41 return false;
42 }
43
npf_eth_src_addr_match(struct npf_test * test,struct net_pkt * pkt)44 bool npf_eth_src_addr_match(struct npf_test *test, struct net_pkt *pkt)
45 {
46 struct net_eth_hdr *eth_hdr = NET_ETH_HDR(pkt);
47
48 return addr_match(test, ð_hdr->src);
49 }
50
npf_eth_src_addr_unmatch(struct npf_test * test,struct net_pkt * pkt)51 bool npf_eth_src_addr_unmatch(struct npf_test *test, struct net_pkt *pkt)
52 {
53 return !npf_eth_src_addr_match(test, pkt);
54 }
55
npf_eth_dst_addr_match(struct npf_test * test,struct net_pkt * pkt)56 bool npf_eth_dst_addr_match(struct npf_test *test, struct net_pkt *pkt)
57 {
58 struct net_eth_hdr *eth_hdr = NET_ETH_HDR(pkt);
59
60 return addr_match(test, ð_hdr->dst);
61 }
62
npf_eth_dst_addr_unmatch(struct npf_test * test,struct net_pkt * pkt)63 bool npf_eth_dst_addr_unmatch(struct npf_test *test, struct net_pkt *pkt)
64 {
65 return !npf_eth_dst_addr_match(test, pkt);
66 }
67
npf_eth_type_match(struct npf_test * test,struct net_pkt * pkt)68 bool npf_eth_type_match(struct npf_test *test, struct net_pkt *pkt)
69 {
70 struct npf_test_eth_type *test_eth_type =
71 CONTAINER_OF(test, struct npf_test_eth_type, test);
72 struct net_eth_hdr *eth_hdr = NET_ETH_HDR(pkt);
73
74 /* note: type_match->type is assumed to be in network order already */
75 return eth_hdr->type == test_eth_type->type;
76 }
77
npf_eth_type_unmatch(struct npf_test * test,struct net_pkt * pkt)78 bool npf_eth_type_unmatch(struct npf_test *test, struct net_pkt *pkt)
79 {
80 return !npf_eth_type_match(test, pkt);
81 }
82
