1 /*
2 * Copyright (c) 2024 Nordic Semiconductor
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <zephyr/logging/log.h>
8 LOG_MODULE_REGISTER(net_cooked, CONFIG_NET_CAPTURE_LOG_LEVEL);
9
10 #include <zephyr/kernel.h>
11 #include <stdlib.h>
12 #include <zephyr/net/net_core.h>
13 #include <zephyr/net/net_ip.h>
14 #include <zephyr/net/net_if.h>
15 #include <zephyr/net/net_pkt.h>
16 #include <zephyr/net/ethernet.h>
17 #include <zephyr/net/virtual.h>
18 #include <zephyr/net/virtual_mgmt.h>
19 #include <zephyr/net/capture.h>
20 #include <zephyr/net/net_l2.h>
21
22 #include "sll.h"
23
24 #define BUF_ALLOC_TIMEOUT 100 /* ms */
25
26 /* Use our own slabs for temporary pkts */
27 NET_PKT_SLAB_DEFINE(cooked_pkts, CONFIG_NET_CAPTURE_PKT_COUNT);
28
29 #if defined(CONFIG_NET_BUF_FIXED_DATA_SIZE)
30 NET_BUF_POOL_FIXED_DEFINE(cooked_bufs, CONFIG_NET_CAPTURE_BUF_COUNT,
31 CONFIG_NET_BUF_DATA_SIZE, 4, NULL);
32 #else
33 NET_BUF_POOL_VAR_DEFINE(cooked_bufs, CONFIG_NET_CAPTURE_BUF_COUNT,
34 CONFIG_NET_PKT_BUF_RX_DATA_POOL_SIZE, 4, NULL);
35 #endif
36
37 #define COOKED_MTU 1024
38 #define COOKED_DEVICE "NET_COOKED"
39
40 struct cooked_context {
41 struct net_if *iface;
42 struct net_if *attached_to;
43
44 /* -1 is used as a not configured link type */
45 int link_types[CONFIG_NET_CAPTURE_COOKED_MODE_MAX_LINK_TYPES];
46 int link_type_count;
47 int mtu;
48 bool init_done;
49 bool status;
50 };
51
iface_init(struct net_if * iface)52 static void iface_init(struct net_if *iface)
53 {
54 struct cooked_context *ctx = net_if_get_device(iface)->data;
55 struct net_if *any_iface;
56 int ifindex;
57 int ret;
58
59 ifindex = net_if_get_by_name("any");
60 if (ifindex < 0) {
61 NET_DBG("No such interface \"any\", cannot init interface %d",
62 net_if_get_by_iface(iface));
63 return;
64 }
65
66 if (net_if_l2(net_if_get_by_index(ifindex)) != &NET_L2_GET_NAME(DUMMY)) {
67 NET_DBG("The \"any\" interface %d is wrong type", ifindex);
68 return;
69 }
70
71 if (ctx->init_done) {
72 return;
73 }
74
75 ctx->iface = iface;
76 any_iface = net_if_get_by_index(ifindex);
77
78 (void)net_if_set_name(iface,
79 CONFIG_NET_CAPTURE_COOKED_MODE_INTERFACE_NAME);
80 (void)net_virtual_set_name(iface, "Cooked mode capture");
81
82 net_if_flag_set(iface, NET_IF_NO_AUTO_START);
83 net_if_flag_set(iface, NET_IF_POINTOPOINT);
84 net_if_flag_clear(iface, NET_IF_IPV4);
85 net_if_flag_clear(iface, NET_IF_IPV6);
86
87 /* Hook into "any" interface so that we can receive the
88 * captured data.
89 */
90 ret = net_virtual_interface_attach(ctx->iface, any_iface);
91 if (ret < 0) {
92 NET_DBG("Cannot hook into interface %d (%d)",
93 net_if_get_by_iface(any_iface), ret);
94 return;
95 }
96
97 NET_DBG("Interface %d attached on top of %d",
98 net_if_get_by_iface(ctx->iface),
99 net_if_get_by_iface(any_iface));
100
101 ctx->init_done = true;
102 }
103
dev_init(const struct device * dev)104 static int dev_init(const struct device *dev)
105 {
106 struct cooked_context *ctx = dev->data;
107
108 memset(ctx->link_types, -1, sizeof(ctx->link_types));
109
110 return 0;
111 }
112
interface_start(const struct device * dev)113 static int interface_start(const struct device *dev)
114 {
115 struct cooked_context *ctx = dev->data;
116 int ret = 0;
117
118 if (ctx->status) {
119 return -EALREADY;
120 }
121
122 ctx->status = true;
123
124 NET_DBG("Starting iface %d", net_if_get_by_iface(ctx->iface));
125
126 return ret;
127 }
128
interface_stop(const struct device * dev)129 static int interface_stop(const struct device *dev)
130 {
131 struct cooked_context *ctx = dev->data;
132
133 if (!ctx->status) {
134 return -EALREADY;
135 }
136
137 ctx->status = false;
138
139 NET_DBG("Stopping iface %d", net_if_get_by_iface(ctx->iface));
140
141 return 0;
142 }
143
interface_recv(struct net_if * iface,struct net_pkt * pkt)144 static enum net_verdict interface_recv(struct net_if *iface, struct net_pkt *pkt)
145 {
146 struct cooked_context *ctx = net_if_get_device(iface)->data;
147 bool found = false;
148 uint16_t ptype;
149
150 /* Feed the packet to capture system after verifying that we are capturing
151 * these types of packets.
152 * The packet will be freed by capture API after it has been processed.
153 */
154
155 ptype = net_pkt_ll_proto_type(pkt);
156
157 NET_DBG("Capture pkt %p for interface %d", pkt, net_if_get_by_iface(iface));
158
159 for (int i = 0; i < ctx->link_type_count; i++) {
160 if (ctx->link_types[i] == ptype) {
161 found = true;
162 break;
163 }
164 }
165
166 if (found) {
167 int ret;
168
169 NET_DBG("Handler found for packet type 0x%04x", ptype);
170
171 /* Normally capture API will clone the net_pkt and we would
172 * always need to unref it. But for cooked packets, we can avoid
173 * the cloning so need to unref only if there was an error with
174 * capturing.
175 */
176 ret = net_capture_pkt_with_status(iface, pkt);
177 if (ret < 0) {
178 net_pkt_unref(pkt);
179 }
180
181 return NET_OK;
182 }
183
184 NET_DBG("No handler found for packet type 0x%04x", ptype);
185
186 return NET_DROP;
187 }
188
interface_attach(struct net_if * iface,struct net_if * lower_iface)189 static int interface_attach(struct net_if *iface, struct net_if *lower_iface)
190 {
191 struct cooked_context *ctx;
192
193 if (net_if_get_by_iface(iface) < 0) {
194 return -ENOENT;
195 }
196
197 ctx = net_if_get_device(iface)->data;
198 ctx->attached_to = lower_iface;
199
200 return 0;
201 }
202
interface_set_config(struct net_if * iface,enum virtual_interface_config_type type,const struct virtual_interface_config * config)203 static int interface_set_config(struct net_if *iface,
204 enum virtual_interface_config_type type,
205 const struct virtual_interface_config *config)
206 {
207 struct cooked_context *ctx = net_if_get_device(iface)->data;
208
209 switch (type) {
210 case VIRTUAL_INTERFACE_CONFIG_TYPE_LINK_TYPE:
211 if (config->link_types.count > ARRAY_SIZE(ctx->link_types)) {
212 return -ERANGE;
213 }
214
215 for (int i = 0; i < config->link_types.count; i++) {
216 NET_DBG("Adding link type %u", config->link_types.type[i]);
217
218 ctx->link_types[i] = (int)config->link_types.type[i];
219 }
220
221 ctx->link_type_count = config->link_types.count;
222
223 /* Mark the rest of the types as invalid */
224 for (int i = ctx->link_type_count; i < ARRAY_SIZE(ctx->link_types); i++) {
225 ctx->link_types[i] = -1;
226 }
227
228 return 0;
229
230 case VIRTUAL_INTERFACE_CONFIG_TYPE_MTU:
231 NET_DBG("Interface %d MTU set to %d",
232 net_if_get_by_iface(iface), config->mtu);
233 net_if_set_mtu(iface, config->mtu);
234 return 0;
235
236 default:
237 break;
238 }
239
240 return -ENOTSUP;
241 }
242
interface_get_config(struct net_if * iface,enum virtual_interface_config_type type,struct virtual_interface_config * config)243 static int interface_get_config(struct net_if *iface,
244 enum virtual_interface_config_type type,
245 struct virtual_interface_config *config)
246 {
247 struct cooked_context *ctx = net_if_get_device(iface)->data;
248 int i;
249
250 switch (type) {
251 case VIRTUAL_INTERFACE_CONFIG_TYPE_LINK_TYPE:
252 for (i = 0; i < ctx->link_type_count; i++) {
253 if (ctx->link_types[i] < 0) {
254 break;
255 }
256
257 config->link_types.type[i] = (uint16_t)ctx->link_types[i];
258 }
259
260 config->link_types.count = i;
261 NET_ASSERT(config->link_types.count == ctx->link_type_count);
262 return 0;
263
264 case VIRTUAL_INTERFACE_CONFIG_TYPE_MTU:
265 config->mtu = net_if_get_mtu(iface);
266 return 0;
267
268 default:
269 break;
270 }
271
272 return -ENOTSUP;
273 }
274
275 static const struct virtual_interface_api cooked_api = {
276 .iface_api.init = iface_init,
277
278 .start = interface_start,
279 .stop = interface_stop,
280 .recv = interface_recv,
281 .attach = interface_attach,
282 .set_config = interface_set_config,
283 .get_config = interface_get_config,
284 };
285
286 static struct cooked_context cooked_context_data;
287
288 NET_VIRTUAL_INTERFACE_INIT(cooked, COOKED_DEVICE, dev_init,
289 NULL, &cooked_context_data, NULL,
290 CONFIG_KERNEL_INIT_PRIORITY_DEFAULT,
291 &cooked_api, COOKED_MTU);
292
net_capture_cooked_setup(struct net_capture_cooked * ctx,uint16_t hatype,uint16_t halen,uint8_t * addr)293 int net_capture_cooked_setup(struct net_capture_cooked *ctx,
294 uint16_t hatype,
295 uint16_t halen,
296 uint8_t *addr)
297 {
298 if (halen == 0 || halen > NET_CAPTURE_LL_ADDRLEN) {
299 return -EINVAL;
300 }
301
302 memset(ctx, 0, sizeof(*ctx));
303
304 ctx->hatype = hatype;
305 ctx->halen = halen;
306
307 memcpy(ctx->addr, addr, halen);
308
309 return 0;
310 }
311
create_sll_header(struct net_if * iface,struct net_pkt * pkt,struct net_capture_cooked * ctx,enum net_capture_packet_type type,uint16_t ptype)312 static int create_sll_header(struct net_if *iface,
313 struct net_pkt *pkt,
314 struct net_capture_cooked *ctx,
315 enum net_capture_packet_type type,
316 uint16_t ptype)
317 {
318 struct sll2_header hdr2;
319 struct sll_header hdr1;
320 size_t hdr_len;
321 uint8_t *hdr;
322 int ret;
323
324 if (IS_ENABLED(CONFIG_NET_CAPTURE_COOKED_MODE_SLLV1)) {
325 hdr1.sll_pkttype = htons(type);
326 hdr1.sll_hatype = htons(ctx->hatype);
327 hdr1.sll_halen = htons(ctx->halen);
328 memcpy(hdr1.sll_addr, ctx->addr, sizeof(ctx->addr));
329 hdr1.sll_protocol = htons(ptype);
330
331 hdr = (uint8_t *)&hdr1;
332 hdr_len = sizeof(hdr1);
333
334 } else {
335 hdr2.sll2_protocol = htons(ptype);
336 hdr2.sll2_reserved_mbz = 0;
337 hdr2.sll2_if_index = net_if_get_by_iface(iface);
338 hdr2.sll2_hatype = htons(ctx->hatype);
339 hdr2.sll2_pkttype = htons(type);
340 hdr2.sll2_halen = htons(ctx->halen);
341 memcpy(hdr2.sll2_addr, ctx->addr, sizeof(ctx->addr));
342
343 hdr = (uint8_t *)&hdr2;
344 hdr_len = sizeof(hdr2);
345 }
346
347 ret = net_pkt_write(pkt, hdr, hdr_len);
348 if (ret < 0) {
349 NET_DBG("Cannot write sll%s header (%d)",
350 IS_ENABLED(CONFIG_NET_CAPTURE_COOKED_MODE_SLLV1) ?
351 "" : "2", ret);
352 }
353
354 return ret;
355 }
356
get_net_pkt(void)357 static struct k_mem_slab *get_net_pkt(void)
358 {
359 return &cooked_pkts;
360 }
361
get_net_buf(void)362 static struct net_buf_pool *get_net_buf(void)
363 {
364 return &cooked_bufs;
365 }
366
net_capture_data(struct net_capture_cooked * ctx,const uint8_t * data,size_t data_len,enum net_capture_packet_type type,uint16_t ptype)367 void net_capture_data(struct net_capture_cooked *ctx,
368 const uint8_t *data, size_t data_len,
369 enum net_capture_packet_type type,
370 uint16_t ptype)
371 {
372 static struct net_context context;
373 const struct device *dev;
374 struct net_if *iface;
375 struct net_pkt *pkt;
376 int ret;
377
378 net_context_setup_pools(&context, get_net_pkt, get_net_buf);
379
380 pkt = net_pkt_alloc_from_slab(&cooked_pkts, K_MSEC(BUF_ALLOC_TIMEOUT));
381 if (pkt == NULL) {
382 NET_DBG("Cannot allocate %s", "net_pkt");
383 return;
384 }
385
386 net_pkt_set_context(pkt, &context);
387
388 ret = net_pkt_alloc_buffer_raw(pkt,
389 sizeof(COND_CODE_1(CONFIG_NET_CAPTURE_COOKED_MODE_SLLV1,
390 (struct sll_header),
391 (struct sll2_header))) + data_len,
392 K_MSEC(BUF_ALLOC_TIMEOUT));
393 if (ret < 0) {
394 NET_DBG("Cannot allocate %s %zd bytes (%d)", "net_buf for",
395 sizeof(struct sll_header) + data_len, ret);
396 net_pkt_unref(pkt);
397 return;
398 }
399
400 /* Write the packet to "any" interface which will pass it to
401 * the virtual interface that does the actual capturing of the packet.
402 * The reason for this trickery is that we do not have a network
403 * interface in use in this API, and want to have the packet routed
404 * via the any interface which can then deliver it to registered
405 * virtual interfaces.
406 */
407 dev = device_get_binding(COOKED_DEVICE);
408 if (dev == NULL) {
409 NET_DBG("No such device %s found, data not captured!",
410 COOKED_DEVICE);
411 net_pkt_unref(pkt);
412 return;
413 }
414
415 /* Next we feed the data to the any interface, which
416 * will pass the data to the virtual interface.
417 * When using capture API or net-shell capture command, one
418 * should capture packets from the virtual interface.
419 */
420 iface = ((struct cooked_context *)dev->data)->attached_to;
421
422 ret = create_sll_header(iface, pkt, ctx, type, ptype);
423 if (ret < 0) {
424 NET_DBG("Cannot write %s %zd bytes (%d)", "header",
425 sizeof(struct sll_header), ret);
426 net_pkt_unref(pkt);
427 return;
428 }
429
430 ret = net_pkt_write(pkt, data, data_len);
431 if (ret < 0) {
432 NET_DBG("Cannot write %s %zd bytes (%d)", "payload",
433 data_len, ret);
434 net_pkt_unref(pkt);
435 return;
436 }
437
438 /* Mark that this packet came from cooked capture mode.
439 * This will prevent the capture API from cloning the packet,
440 * so that the net_pkt will be passed as is to capture interface.
441 */
442 net_pkt_set_cooked_mode(pkt, true);
443
444 /* The protocol type is used by virtual cooked interface to decide
445 * whether we capture the packet or not.
446 */
447 net_pkt_set_ll_proto_type(pkt, ptype);
448
449 net_pkt_lladdr_src(pkt)->addr = NULL;
450 net_pkt_lladdr_src(pkt)->len = 0U;
451 net_pkt_lladdr_src(pkt)->type = NET_LINK_DUMMY;
452 net_pkt_lladdr_dst(pkt)->addr = NULL;
453 net_pkt_lladdr_dst(pkt)->len = 0U;
454 net_pkt_lladdr_dst(pkt)->type = NET_LINK_DUMMY;
455
456 ret = net_recv_data(iface, pkt);
457 if (ret < 0) {
458 net_pkt_unref(pkt);
459 }
460 }
461
