1 /*
2 * Copyright (c) 2018 Intel Corporation.
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <zephyr/logging/log.h>
8 LOG_MODULE_REGISTER(net_promisc_sample, LOG_LEVEL_INF);
9
10 #include <zephyr/kernel.h>
11 #include <errno.h>
12 #include <stdlib.h>
13 #include <zephyr/shell/shell.h>
14
15 #include <zephyr/net/net_core.h>
16 #include <zephyr/net/promiscuous.h>
17 #include <zephyr/net/udp.h>
18
net_pkt_hexdump(struct net_pkt * pkt,const char * str)19 static void net_pkt_hexdump(struct net_pkt *pkt, const char *str)
20 {
21 struct net_buf *buf = pkt->buffer;
22
23 while (buf) {
24 LOG_HEXDUMP_DBG(buf->data, buf->len, str);
25 buf = buf->frags;
26 }
27 }
28
iface_cb(struct net_if * iface,void * user_data)29 static void iface_cb(struct net_if *iface, void *user_data)
30 {
31 int ret;
32
33 ret = net_promisc_mode_on(iface);
34 if (ret < 0) {
35 LOG_INF("Cannot set promiscuous mode for interface %p (%d)",
36 iface, ret);
37 return;
38 }
39
40 LOG_INF("Promiscuous mode enabled for interface %p", iface);
41 }
42
get_ports(struct net_pkt * pkt,uint16_t * src,uint16_t * dst)43 static int get_ports(struct net_pkt *pkt, uint16_t *src, uint16_t *dst)
44 {
45 struct net_udp_hdr hdr, *udp_hdr;
46
47 udp_hdr = net_udp_get_hdr(pkt, &hdr);
48 if (!udp_hdr) {
49 return -EINVAL;
50 }
51
52 *src = ntohs(udp_hdr->src_port);
53 *dst = ntohs(udp_hdr->dst_port);
54
55 return 0;
56 }
57
print_info(struct net_pkt * pkt)58 static void print_info(struct net_pkt *pkt)
59 {
60 char src_addr_buf[NET_IPV6_ADDR_LEN], *src_addr;
61 char dst_addr_buf[NET_IPV6_ADDR_LEN], *dst_addr;
62 uint16_t dst_port = 0U, src_port = 0U;
63 sa_family_t family = AF_UNSPEC;
64 void *dst, *src;
65 uint8_t next_hdr;
66 const char *proto;
67 size_t len;
68 int ret;
69
70 /* Enable hexdump by setting the log level to LOG_LEVEL_DBG */
71 net_pkt_hexdump(pkt, "Network packet");
72
73 switch (NET_IPV6_HDR(pkt)->vtc & 0xf0) {
74 case 0x60:
75 family = AF_INET6;
76 net_pkt_set_family(pkt, AF_INET6);
77 dst = &NET_IPV6_HDR(pkt)->dst;
78 src = &NET_IPV6_HDR(pkt)->src;
79 next_hdr = NET_IPV6_HDR(pkt)->nexthdr;
80 net_pkt_set_ip_hdr_len(pkt, sizeof(struct net_ipv6_hdr));
81 break;
82 case 0x40:
83 family = AF_INET;
84 net_pkt_set_family(pkt, AF_INET);
85 dst = &NET_IPV4_HDR(pkt)->dst;
86 src = &NET_IPV4_HDR(pkt)->src;
87 next_hdr = NET_IPV4_HDR(pkt)->proto;
88 net_pkt_set_ip_hdr_len(pkt, sizeof(struct net_ipv4_hdr));
89 break;
90 }
91
92 if (family == AF_UNSPEC) {
93 LOG_INF("Recv %p len %zd (unknown address family)",
94 pkt, net_pkt_get_len(pkt));
95 return;
96 }
97
98 ret = 0;
99
100 switch (next_hdr) {
101 case IPPROTO_TCP:
102 proto = "TCP";
103 ret = get_ports(pkt, &src_port, &dst_port);
104 break;
105 case IPPROTO_UDP:
106 proto = "UDP";
107 ret = get_ports(pkt, &src_port, &dst_port);
108 break;
109 case IPPROTO_ICMPV6:
110 case IPPROTO_ICMP:
111 proto = "ICMP";
112 break;
113 default:
114 proto = "<unknown>";
115 break;
116 }
117
118 if (ret < 0) {
119 LOG_ERR("Cannot get port numbers for pkt %p", pkt);
120 return;
121 }
122
123 src_addr = net_addr_ntop(family, src,
124 src_addr_buf, sizeof(src_addr_buf));
125 dst_addr = net_addr_ntop(family, dst,
126 dst_addr_buf, sizeof(dst_addr_buf));
127
128 len = net_pkt_get_len(pkt);
129
130 if (family == AF_INET) {
131 if (next_hdr == IPPROTO_TCP || next_hdr == IPPROTO_UDP) {
132 LOG_INF("%s %s (%zd) %s:%u -> %s:%u",
133 "IPv4", proto, len,
134 src_addr, src_port,
135 dst_addr, dst_port);
136 } else {
137 LOG_INF("%s %s (%zd) %s -> %s", "IPv4", proto,
138 len, src_addr,
139 dst_addr);
140 }
141 } else {
142 if (next_hdr == IPPROTO_TCP || next_hdr == IPPROTO_UDP) {
143 LOG_INF("%s %s (%zd) [%s]:%u -> [%s]:%u",
144 "IPv6", proto, len,
145 src_addr, src_port,
146 dst_addr, dst_port);
147 } else {
148 LOG_INF("%s %s (%zd) %s -> %s", "IPv6", proto,
149 len, src_addr,
150 dst_addr);
151 }
152 }
153 }
154
set_promisc_mode(const struct shell * sh,size_t argc,char * argv[],bool enable)155 static int set_promisc_mode(const struct shell *sh,
156 size_t argc, char *argv[], bool enable)
157 {
158 struct net_if *iface;
159 char *endptr;
160 int idx, ret;
161
162 if (argc < 2) {
163 shell_fprintf(sh, SHELL_ERROR, "Invalid arguments.\n");
164 return -ENOEXEC;
165 }
166
167 idx = strtol(argv[1], &endptr, 10);
168
169 iface = net_if_get_by_index(idx);
170 if (!iface) {
171 shell_fprintf(sh, SHELL_ERROR,
172 "Cannot find network interface for index %d\n",
173 idx);
174 return -ENOEXEC;
175 }
176
177 shell_fprintf(sh, SHELL_INFO, "Promiscuous mode %s...\n",
178 enable ? "ON" : "OFF");
179
180 if (enable) {
181 ret = net_promisc_mode_on(iface);
182 } else {
183 ret = net_promisc_mode_off(iface);
184 }
185
186 if (ret < 0) {
187 if (ret == -EALREADY) {
188 shell_fprintf(sh, SHELL_INFO,
189 "Promiscuous mode already %s\n",
190 enable ? "enabled" : "disabled");
191 } else {
192 shell_fprintf(sh, SHELL_ERROR,
193 "Cannot %s promiscuous mode for "
194 "interface %p (%d)\n",
195 enable ? "set" : "unset", iface, ret);
196 }
197
198 return -ENOEXEC;
199 }
200
201 return 0;
202 }
203
cmd_promisc_on(const struct shell * sh,size_t argc,char * argv[])204 static int cmd_promisc_on(const struct shell *sh,
205 size_t argc, char *argv[])
206 {
207 return set_promisc_mode(sh, argc, argv, true);
208 }
209
cmd_promisc_off(const struct shell * sh,size_t argc,char * argv[])210 static int cmd_promisc_off(const struct shell *sh,
211 size_t argc, char *argv[])
212 {
213 return set_promisc_mode(sh, argc, argv, false);
214 }
215
216 SHELL_STATIC_SUBCMD_SET_CREATE(promisc_commands,
217 SHELL_CMD(on, NULL,
218 "Turn promiscuous mode on\n"
219 "promisc on <interface index> "
220 "Turn on promiscuous mode for the interface\n",
221 cmd_promisc_on),
222 SHELL_CMD(off, NULL, "Turn promiscuous mode off\n"
223 "promisc off <interface index> "
224 "Turn off promiscuous mode for the interface\n",
225 cmd_promisc_off),
226 SHELL_SUBCMD_SET_END
227 );
228
229 SHELL_CMD_REGISTER(promisc, &promisc_commands,
230 "Promiscuous mode commands", NULL);
231
main(void)232 int main(void)
233 {
234 struct net_pkt *pkt;
235
236 net_if_foreach(iface_cb, NULL);
237
238 while (1) {
239 pkt = net_promisc_mode_wait_data(K_FOREVER);
240 if (pkt) {
241 print_info(pkt);
242 }
243
244 net_pkt_unref(pkt);
245 }
246 return 0;
247 }
248
