1 /*
2 * Copyright (c) 2016-2018 Intel Corporation.
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <logging/log.h>
8 LOG_MODULE_REGISTER(net_ethernet, CONFIG_NET_L2_ETHERNET_LOG_LEVEL);
9
10 #include <net/net_core.h>
11 #include <net/net_l2.h>
12 #include <net/net_if.h>
13 #include <net/net_mgmt.h>
14 #include <net/ethernet.h>
15 #include <net/ethernet_mgmt.h>
16 #include <net/gptp.h>
17 #include <random/rand32.h>
18
19 #if defined(CONFIG_NET_LLDP)
20 #include <net/lldp.h>
21 #endif
22
23 #include <syscall_handler.h>
24 #if defined(CONFIG_NET_L2_CANBUS_ETH_TRANSLATOR)
25 #include <net/can.h>
26 #endif
27
28 #include "arp.h"
29 #include "eth_stats.h"
30 #include "net_private.h"
31 #include "ipv6.h"
32 #include "ipv4_autoconf_internal.h"
33 #include "bridge.h"
34
35 #define NET_BUF_TIMEOUT K_MSEC(100)
36
37 static const struct net_eth_addr multicast_eth_addr __unused = {
38 { 0x33, 0x33, 0x00, 0x00, 0x00, 0x00 } };
39
40 static const struct net_eth_addr broadcast_eth_addr = {
41 { 0xff, 0xff, 0xff, 0xff, 0xff, 0xff } };
42
net_eth_broadcast_addr(void)43 const struct net_eth_addr *net_eth_broadcast_addr(void)
44 {
45 return &broadcast_eth_addr;
46 }
47
net_eth_ipv4_mcast_to_mac_addr(const struct in_addr * ipv4_addr,struct net_eth_addr * mac_addr)48 void net_eth_ipv4_mcast_to_mac_addr(const struct in_addr *ipv4_addr,
49 struct net_eth_addr *mac_addr)
50 {
51 /* RFC 1112 6.4. Extensions to an Ethernet Local Network Module
52 * "An IP host group address is mapped to an Ethernet multicast
53 * address by placing the low-order 23-bits of the IP address into
54 * the low-order 23 bits of the Ethernet multicast address
55 * 01-00-5E-00-00-00 (hex)."
56 */
57 mac_addr->addr[0] = 0x01;
58 mac_addr->addr[1] = 0x00;
59 mac_addr->addr[2] = 0x5e;
60 mac_addr->addr[3] = ipv4_addr->s4_addr[1];
61 mac_addr->addr[4] = ipv4_addr->s4_addr[2];
62 mac_addr->addr[5] = ipv4_addr->s4_addr[3];
63
64 mac_addr->addr[3] &= 0x7f;
65 }
66
net_eth_ipv6_mcast_to_mac_addr(const struct in6_addr * ipv6_addr,struct net_eth_addr * mac_addr)67 void net_eth_ipv6_mcast_to_mac_addr(const struct in6_addr *ipv6_addr,
68 struct net_eth_addr *mac_addr)
69 {
70 /* RFC 2464 7. Address Mapping -- Multicast
71 * "An IPv6 packet with a multicast destination address DST,
72 * consisting of the sixteen octets DST[1] through DST[16],
73 * is transmitted to the Ethernet multicast address whose
74 * first two octets are the value 3333 hexadecimal and whose
75 * last four octets are the last four octets of DST."
76 */
77 mac_addr->addr[0] = mac_addr->addr[1] = 0x33;
78 memcpy(mac_addr->addr + 2, &ipv6_addr->s6_addr[12], 4);
79 }
80
81 #define print_ll_addrs(pkt, type, len, src, dst) \
82 if (CONFIG_NET_L2_ETHERNET_LOG_LEVEL >= LOG_LEVEL_DBG) { \
83 char out[sizeof("xx:xx:xx:xx:xx:xx")]; \
84 \
85 snprintk(out, sizeof(out), "%s", \
86 net_sprint_ll_addr((src)->addr, \
87 sizeof(struct net_eth_addr))); \
88 \
89 NET_DBG("iface %p src %s dst %s type 0x%x len %zu", \
90 net_pkt_iface(pkt), log_strdup(out), \
91 log_strdup(net_sprint_ll_addr((dst)->addr, \
92 sizeof(struct net_eth_addr))), \
93 type, (size_t)len); \
94 }
95
96 #ifdef CONFIG_NET_VLAN
97 #define print_vlan_ll_addrs(pkt, type, tci, len, src, dst, tagstrip) \
98 if (CONFIG_NET_L2_ETHERNET_LOG_LEVEL >= LOG_LEVEL_DBG) { \
99 char out[sizeof("xx:xx:xx:xx:xx:xx")]; \
100 \
101 snprintk(out, sizeof(out), "%s", \
102 net_sprint_ll_addr((src)->addr, \
103 sizeof(struct net_eth_addr))); \
104 \
105 NET_DBG("iface %p src %s dst %s type 0x%x " \
106 "tag %d %spri %d len %zu", \
107 net_pkt_iface(pkt), log_strdup(out), \
108 log_strdup(net_sprint_ll_addr((dst)->addr, \
109 sizeof(struct net_eth_addr))), \
110 type, net_eth_vlan_get_vid(tci), \
111 tagstrip ? "(stripped) " : "", \
112 net_eth_vlan_get_pcp(tci), (size_t)len); \
113 }
114 #else
115 #define print_vlan_ll_addrs(...)
116 #endif /* CONFIG_NET_VLAN */
117
ethernet_update_length(struct net_if * iface,struct net_pkt * pkt)118 static inline void ethernet_update_length(struct net_if *iface,
119 struct net_pkt *pkt)
120 {
121 uint16_t len;
122
123 /* Let's check IP payload's length. If it's smaller than 46 bytes,
124 * i.e. smaller than minimal Ethernet frame size minus ethernet
125 * header size,then Ethernet has padded so it fits in the minimal
126 * frame size of 60 bytes. In that case, we need to get rid of it.
127 */
128
129 if (net_pkt_family(pkt) == AF_INET) {
130 len = ntohs(NET_IPV4_HDR(pkt)->len);
131 } else {
132 len = ntohs(NET_IPV6_HDR(pkt)->len) + NET_IPV6H_LEN;
133 }
134
135 if (len < NET_ETH_MINIMAL_FRAME_SIZE - sizeof(struct net_eth_hdr)) {
136 struct net_buf *frag;
137
138 for (frag = pkt->frags; frag; frag = frag->frags) {
139 if (frag->len < len) {
140 len -= frag->len;
141 } else {
142 frag->len = len;
143 len = 0U;
144 }
145 }
146 }
147 }
148
ethernet_update_rx_stats(struct net_if * iface,struct net_pkt * pkt,size_t length)149 static void ethernet_update_rx_stats(struct net_if *iface,
150 struct net_pkt *pkt, size_t length)
151 {
152 #if defined(CONFIG_NET_STATISTICS_ETHERNET)
153 struct net_eth_hdr *hdr = NET_ETH_HDR(pkt);
154
155 eth_stats_update_bytes_rx(iface, length);
156 eth_stats_update_pkts_rx(iface);
157
158 if (net_eth_is_addr_broadcast(&hdr->dst)) {
159 eth_stats_update_broadcast_rx(iface);
160 } else if (net_eth_is_addr_multicast(&hdr->dst)) {
161 eth_stats_update_multicast_rx(iface);
162 }
163 #endif /* CONFIG_NET_STATISTICS_ETHERNET */
164 }
165
eth_is_vlan_tag_stripped(struct net_if * iface)166 static inline bool eth_is_vlan_tag_stripped(struct net_if *iface)
167 {
168 const struct device *dev = net_if_get_device(iface);
169 const struct ethernet_api *api = dev->api;
170
171 return (api->get_capabilities(dev) & ETHERNET_HW_VLAN_TAG_STRIP);
172 }
173
174 /* Drop packet if it has broadcast destination MAC address but the IP
175 * address is not multicast or broadcast address. See RFC 1122 ch 3.3.6
176 */
177 static inline
ethernet_check_ipv4_bcast_addr(struct net_pkt * pkt,struct net_eth_hdr * hdr)178 enum net_verdict ethernet_check_ipv4_bcast_addr(struct net_pkt *pkt,
179 struct net_eth_hdr *hdr)
180 {
181 if (net_eth_is_addr_broadcast(&hdr->dst) &&
182 !(net_ipv4_is_addr_mcast(&NET_IPV4_HDR(pkt)->dst) ||
183 net_ipv4_is_addr_bcast(net_pkt_iface(pkt),
184 &NET_IPV4_HDR(pkt)->dst))) {
185 return NET_DROP;
186 }
187
188 return NET_OK;
189 }
190
ethernet_recv(struct net_if * iface,struct net_pkt * pkt)191 static enum net_verdict ethernet_recv(struct net_if *iface,
192 struct net_pkt *pkt)
193 {
194 struct ethernet_context *ctx = net_if_l2_data(iface);
195 struct net_eth_hdr *hdr = NET_ETH_HDR(pkt);
196 uint8_t hdr_len = sizeof(struct net_eth_hdr);
197 uint16_t type;
198 struct net_linkaddr *lladdr;
199 sa_family_t family;
200
201 /* This expects that the Ethernet header is in the first net_buf
202 * fragment. This is a safe expectation here as it would not make
203 * any sense to split the Ethernet header to two net_buf's by the
204 * Ethernet driver.
205 */
206 if (hdr == NULL || pkt->buffer->len < hdr_len) {
207 goto drop;
208 }
209
210 if (IS_ENABLED(CONFIG_NET_ETHERNET_BRIDGE) &&
211 net_eth_iface_is_bridged(ctx)) {
212 net_pkt_set_l2_bridged(pkt, true);
213 net_pkt_lladdr_src(pkt)->addr = hdr->src.addr;
214 net_pkt_lladdr_src(pkt)->len = sizeof(struct net_eth_addr);
215 net_pkt_lladdr_src(pkt)->type = NET_LINK_ETHERNET;
216 net_pkt_lladdr_dst(pkt)->addr = hdr->dst.addr;
217 net_pkt_lladdr_dst(pkt)->len = sizeof(struct net_eth_addr);
218 net_pkt_lladdr_dst(pkt)->type = NET_LINK_ETHERNET;
219 ethernet_update_rx_stats(iface, pkt, net_pkt_get_len(pkt));
220 return net_eth_bridge_input(ctx, pkt);
221 }
222
223 type = ntohs(hdr->type);
224
225 if (net_eth_is_vlan_enabled(ctx, iface) &&
226 type == NET_ETH_PTYPE_VLAN &&
227 !eth_is_vlan_tag_stripped(iface)) {
228 struct net_eth_vlan_hdr *hdr_vlan =
229 (struct net_eth_vlan_hdr *)NET_ETH_HDR(pkt);
230
231 net_pkt_set_vlan_tci(pkt, ntohs(hdr_vlan->vlan.tci));
232 type = ntohs(hdr_vlan->type);
233 hdr_len = sizeof(struct net_eth_vlan_hdr);
234 }
235
236 switch (type) {
237 case NET_ETH_PTYPE_IP:
238 case NET_ETH_PTYPE_ARP:
239 net_pkt_set_family(pkt, AF_INET);
240 family = AF_INET;
241 break;
242 case NET_ETH_PTYPE_IPV6:
243 net_pkt_set_family(pkt, AF_INET6);
244 family = AF_INET6;
245 break;
246 #if defined(CONFIG_NET_GPTP)
247 case NET_ETH_PTYPE_PTP:
248 family = AF_UNSPEC;
249 break;
250 #endif
251 case NET_ETH_PTYPE_LLDP:
252 #if defined(CONFIG_NET_LLDP)
253 net_buf_pull(pkt->frags, hdr_len);
254 return net_lldp_recv(iface, pkt);
255 #else
256 NET_DBG("LLDP Rx agent not enabled");
257 goto drop;
258 #endif
259 default:
260 NET_DBG("Unknown hdr type 0x%04x iface %p", type, iface);
261 goto drop;
262 }
263
264 /* Set the pointers to ll src and dst addresses */
265 lladdr = net_pkt_lladdr_src(pkt);
266 lladdr->addr = hdr->src.addr;
267 lladdr->len = sizeof(struct net_eth_addr);
268 lladdr->type = NET_LINK_ETHERNET;
269
270 lladdr = net_pkt_lladdr_dst(pkt);
271 lladdr->addr = hdr->dst.addr;
272 lladdr->len = sizeof(struct net_eth_addr);
273 lladdr->type = NET_LINK_ETHERNET;
274
275 if (net_eth_is_vlan_enabled(ctx, iface)) {
276 if (type == NET_ETH_PTYPE_VLAN ||
277 (eth_is_vlan_tag_stripped(iface) &&
278 net_pkt_vlan_tci(pkt))) {
279 print_vlan_ll_addrs(pkt, type, net_pkt_vlan_tci(pkt),
280 net_pkt_get_len(pkt),
281 net_pkt_lladdr_src(pkt),
282 net_pkt_lladdr_dst(pkt),
283 eth_is_vlan_tag_stripped(iface));
284 } else {
285 print_ll_addrs(pkt, type, net_pkt_get_len(pkt),
286 net_pkt_lladdr_src(pkt),
287 net_pkt_lladdr_dst(pkt));
288 }
289 } else {
290 print_ll_addrs(pkt, type, net_pkt_get_len(pkt),
291 net_pkt_lladdr_src(pkt),
292 net_pkt_lladdr_dst(pkt));
293 }
294
295 #if defined(CONFIG_NET_L2_CANBUS_ETH_TRANSLATOR)
296 if (net_canbus_translate_eth_frame(iface, pkt) == NET_OK) {
297 return NET_OK;
298 }
299 #endif
300
301 if (!net_eth_is_addr_broadcast((struct net_eth_addr *)lladdr->addr) &&
302 !net_eth_is_addr_multicast((struct net_eth_addr *)lladdr->addr) &&
303 !net_eth_is_addr_lldp_multicast(
304 (struct net_eth_addr *)lladdr->addr) &&
305 !net_linkaddr_cmp(net_if_get_link_addr(iface), lladdr)) {
306 /* The ethernet frame is not for me as the link addresses
307 * are different.
308 */
309 NET_DBG("Dropping frame, not for me [%s]",
310 log_strdup(net_sprint_ll_addr(
311 net_if_get_link_addr(iface)->addr,
312 sizeof(struct net_eth_addr))));
313 goto drop;
314 }
315
316 net_buf_pull(pkt->frags, hdr_len);
317
318 if (IS_ENABLED(CONFIG_NET_IPV4) && type == NET_ETH_PTYPE_IP &&
319 ethernet_check_ipv4_bcast_addr(pkt, hdr) == NET_DROP) {
320 goto drop;
321 }
322
323 ethernet_update_rx_stats(iface, pkt, net_pkt_get_len(pkt) + hdr_len);
324
325 if (IS_ENABLED(CONFIG_NET_ARP) &&
326 family == AF_INET && type == NET_ETH_PTYPE_ARP) {
327 NET_DBG("ARP packet from %s received",
328 log_strdup(net_sprint_ll_addr(
329 (uint8_t *)hdr->src.addr,
330 sizeof(struct net_eth_addr))));
331
332 if (IS_ENABLED(CONFIG_NET_IPV4_AUTO) &&
333 net_ipv4_autoconf_input(iface, pkt) == NET_DROP) {
334 return NET_DROP;
335 }
336
337 return net_arp_input(pkt, hdr);
338 }
339
340 if (IS_ENABLED(CONFIG_NET_GPTP) && type == NET_ETH_PTYPE_PTP) {
341 return net_gptp_recv(iface, pkt);
342 }
343
344 ethernet_update_length(iface, pkt);
345
346 return NET_CONTINUE;
347 drop:
348 eth_stats_update_errors_rx(iface);
349 return NET_DROP;
350 }
351
352 #ifdef CONFIG_NET_IPV4
ethernet_ipv4_dst_is_broadcast_or_mcast(struct net_pkt * pkt)353 static inline bool ethernet_ipv4_dst_is_broadcast_or_mcast(struct net_pkt *pkt)
354 {
355 if (net_ipv4_is_addr_bcast(net_pkt_iface(pkt),
356 &NET_IPV4_HDR(pkt)->dst) ||
357 net_ipv4_is_addr_mcast(&NET_IPV4_HDR(pkt)->dst)) {
358 return true;
359 }
360
361 return false;
362 }
363
ethernet_fill_in_dst_on_ipv4_mcast(struct net_pkt * pkt,struct net_eth_addr * dst)364 static bool ethernet_fill_in_dst_on_ipv4_mcast(struct net_pkt *pkt,
365 struct net_eth_addr *dst)
366 {
367 if (net_pkt_family(pkt) == AF_INET &&
368 net_ipv4_is_addr_mcast(&NET_IPV4_HDR(pkt)->dst)) {
369 /* Multicast address */
370 net_eth_ipv4_mcast_to_mac_addr(&NET_IPV4_HDR(pkt)->dst, dst);
371
372 return true;
373 }
374
375 return false;
376 }
377
ethernet_ll_prepare_on_ipv4(struct net_if * iface,struct net_pkt * pkt)378 static struct net_pkt *ethernet_ll_prepare_on_ipv4(struct net_if *iface,
379 struct net_pkt *pkt)
380 {
381 if (ethernet_ipv4_dst_is_broadcast_or_mcast(pkt)) {
382 return pkt;
383 }
384
385 if (IS_ENABLED(CONFIG_NET_ARP)) {
386 struct net_pkt *arp_pkt;
387
388 arp_pkt = net_arp_prepare(pkt, &NET_IPV4_HDR(pkt)->dst, NULL);
389 if (!arp_pkt) {
390 return NULL;
391 }
392
393 if (pkt != arp_pkt) {
394 NET_DBG("Sending arp pkt %p (orig %p) to iface %p",
395 arp_pkt, pkt, iface);
396 net_pkt_unref(pkt);
397 return arp_pkt;
398 }
399
400 NET_DBG("Found ARP entry, sending pkt %p to iface %p",
401 pkt, iface);
402 }
403
404 return pkt;
405 }
406 #else
407 #define ethernet_ipv4_dst_is_broadcast_or_mcast(...) false
408 #define ethernet_fill_in_dst_on_ipv4_mcast(...) false
409 #define ethernet_ll_prepare_on_ipv4(...) NULL
410 #endif /* CONFIG_NET_IPV4 */
411
412 #ifdef CONFIG_NET_IPV6
ethernet_fill_in_dst_on_ipv6_mcast(struct net_pkt * pkt,struct net_eth_addr * dst)413 static bool ethernet_fill_in_dst_on_ipv6_mcast(struct net_pkt *pkt,
414 struct net_eth_addr *dst)
415 {
416 if (net_pkt_family(pkt) == AF_INET6 &&
417 net_ipv6_is_addr_mcast(&NET_IPV6_HDR(pkt)->dst)) {
418 memcpy(dst, (uint8_t *)multicast_eth_addr.addr,
419 sizeof(struct net_eth_addr) - 4);
420 memcpy((uint8_t *)dst + 2,
421 (uint8_t *)(&NET_IPV6_HDR(pkt)->dst) + 12,
422 sizeof(struct net_eth_addr) - 2);
423
424 return true;
425 }
426
427 return false;
428 }
429 #else
430 #define ethernet_fill_in_dst_on_ipv6_mcast(...) false
431 #endif /* CONFIG_NET_IPV6 */
432
433 #if defined(CONFIG_NET_VLAN)
set_vlan_tag(struct ethernet_context * ctx,struct net_if * iface,struct net_pkt * pkt)434 static enum net_verdict set_vlan_tag(struct ethernet_context *ctx,
435 struct net_if *iface,
436 struct net_pkt *pkt)
437 {
438 int i;
439
440 if (net_pkt_vlan_tag(pkt) != NET_VLAN_TAG_UNSPEC) {
441 return NET_OK;
442 }
443
444 #if defined(CONFIG_NET_IPV6)
445 if (net_pkt_family(pkt) == AF_INET6) {
446 struct net_if *target;
447
448 if (net_if_ipv6_addr_lookup(&NET_IPV6_HDR(pkt)->src,
449 &target)) {
450 if (target != iface) {
451 NET_DBG("Iface %p should be %p", iface,
452 target);
453
454 iface = target;
455 }
456 }
457 }
458 #endif
459
460 #if defined(CONFIG_NET_IPV4)
461 if (net_pkt_family(pkt) == AF_INET) {
462 struct net_if *target;
463
464 if (net_if_ipv4_addr_lookup(&NET_IPV4_HDR(pkt)->src,
465 &target)) {
466 if (target != iface) {
467 NET_DBG("Iface %p should be %p", iface,
468 target);
469 iface = target;
470 }
471 }
472 }
473 #endif
474
475 for (i = 0; i < CONFIG_NET_VLAN_COUNT; i++) {
476 if (ctx->vlan[i].tag == NET_VLAN_TAG_UNSPEC ||
477 ctx->vlan[i].iface != iface) {
478 continue;
479 }
480
481 /* Depending on source address, use the proper network
482 * interface when sending.
483 */
484 net_pkt_set_vlan_tag(pkt, ctx->vlan[i].tag);
485
486 return NET_OK;
487 }
488
489 return NET_DROP;
490 }
491
set_vlan_priority(struct ethernet_context * ctx,struct net_pkt * pkt)492 static void set_vlan_priority(struct ethernet_context *ctx,
493 struct net_pkt *pkt)
494 {
495 uint8_t vlan_priority;
496
497 vlan_priority = net_priority2vlan(net_pkt_priority(pkt));
498 net_pkt_set_vlan_priority(pkt, vlan_priority);
499 }
500 #else
501 #define set_vlan_tag(...) NET_DROP
502 #define set_vlan_priority(...)
503 #endif /* CONFIG_NET_VLAN */
504
ethernet_fill_header(struct ethernet_context * ctx,struct net_pkt * pkt,uint32_t ptype)505 static struct net_buf *ethernet_fill_header(struct ethernet_context *ctx,
506 struct net_pkt *pkt,
507 uint32_t ptype)
508 {
509 struct net_buf *hdr_frag;
510 struct net_eth_hdr *hdr;
511
512 hdr_frag = net_pkt_get_frag(pkt, NET_BUF_TIMEOUT);
513 if (!hdr_frag) {
514 return NULL;
515 }
516
517 if (IS_ENABLED(CONFIG_NET_VLAN) &&
518 net_eth_is_vlan_enabled(ctx, net_pkt_iface(pkt))) {
519 struct net_eth_vlan_hdr *hdr_vlan;
520
521 hdr_vlan = (struct net_eth_vlan_hdr *)(hdr_frag->data);
522
523 if (ptype == htons(NET_ETH_PTYPE_ARP) ||
524 (!ethernet_fill_in_dst_on_ipv4_mcast(pkt, &hdr_vlan->dst) &&
525 !ethernet_fill_in_dst_on_ipv6_mcast(pkt, &hdr_vlan->dst))) {
526 memcpy(&hdr_vlan->dst, net_pkt_lladdr_dst(pkt)->addr,
527 sizeof(struct net_eth_addr));
528 }
529
530 memcpy(&hdr_vlan->src, net_pkt_lladdr_src(pkt)->addr,
531 sizeof(struct net_eth_addr));
532
533 hdr_vlan->type = ptype;
534 hdr_vlan->vlan.tpid = htons(NET_ETH_PTYPE_VLAN);
535 hdr_vlan->vlan.tci = htons(net_pkt_vlan_tci(pkt));
536 net_buf_add(hdr_frag, sizeof(struct net_eth_vlan_hdr));
537
538 print_vlan_ll_addrs(pkt, ntohs(hdr_vlan->type),
539 net_pkt_vlan_tci(pkt),
540 hdr_frag->len,
541 &hdr_vlan->src, &hdr_vlan->dst, false);
542 } else {
543 hdr = (struct net_eth_hdr *)(hdr_frag->data);
544
545 if (ptype == htons(NET_ETH_PTYPE_ARP) ||
546 (!ethernet_fill_in_dst_on_ipv4_mcast(pkt, &hdr->dst) &&
547 !ethernet_fill_in_dst_on_ipv6_mcast(pkt, &hdr->dst))) {
548 memcpy(&hdr->dst, net_pkt_lladdr_dst(pkt)->addr,
549 sizeof(struct net_eth_addr));
550 }
551
552 memcpy(&hdr->src, net_pkt_lladdr_src(pkt)->addr,
553 sizeof(struct net_eth_addr));
554
555 hdr->type = ptype;
556 net_buf_add(hdr_frag, sizeof(struct net_eth_hdr));
557
558 print_ll_addrs(pkt, ntohs(hdr->type),
559 hdr_frag->len, &hdr->src, &hdr->dst);
560 }
561
562 net_pkt_frag_insert(pkt, hdr_frag);
563
564 return hdr_frag;
565 }
566
567 #if defined(CONFIG_NET_STATISTICS_ETHERNET)
ethernet_update_tx_stats(struct net_if * iface,struct net_pkt * pkt)568 static void ethernet_update_tx_stats(struct net_if *iface, struct net_pkt *pkt)
569 {
570 struct net_eth_hdr *hdr = NET_ETH_HDR(pkt);
571
572 eth_stats_update_bytes_tx(iface, net_pkt_get_len(pkt));
573 eth_stats_update_pkts_tx(iface);
574
575 if (net_eth_is_addr_multicast(&hdr->dst)) {
576 eth_stats_update_multicast_tx(iface);
577 } else if (net_eth_is_addr_broadcast(&hdr->dst)) {
578 eth_stats_update_broadcast_tx(iface);
579 }
580 }
581 #else
582 #define ethernet_update_tx_stats(...)
583 #endif /* CONFIG_NET_STATISTICS_ETHERNET */
584
ethernet_remove_l2_header(struct net_pkt * pkt)585 static void ethernet_remove_l2_header(struct net_pkt *pkt)
586 {
587 struct net_buf *buf;
588
589 /* Remove the buffer added in ethernet_fill_header() */
590 buf = pkt->buffer;
591 pkt->buffer = buf->frags;
592 buf->frags = NULL;
593
594 net_pkt_frag_unref(buf);
595 }
596
ethernet_send(struct net_if * iface,struct net_pkt * pkt)597 static int ethernet_send(struct net_if *iface, struct net_pkt *pkt)
598 {
599 const struct ethernet_api *api = net_if_get_device(iface)->api;
600 struct ethernet_context *ctx = net_if_l2_data(iface);
601 uint16_t ptype;
602 int ret;
603
604 if (!api) {
605 ret = -ENOENT;
606 goto error;
607 }
608
609 if (IS_ENABLED(CONFIG_NET_ETHERNET_BRIDGE) &&
610 net_pkt_is_l2_bridged(pkt)) {
611 net_pkt_cursor_init(pkt);
612 ret = net_l2_send(api->send, net_if_get_device(iface), iface, pkt);
613 if (ret != 0) {
614 eth_stats_update_errors_tx(iface);
615 goto error;
616 }
617 ethernet_update_tx_stats(iface, pkt);
618 ret = net_pkt_get_len(pkt);
619 net_pkt_unref(pkt);
620 return ret;
621 } else if (IS_ENABLED(CONFIG_NET_IPV4) &&
622 net_pkt_family(pkt) == AF_INET) {
623 struct net_pkt *tmp;
624
625 if (net_pkt_ipv4_auto(pkt)) {
626 ptype = htons(NET_ETH_PTYPE_ARP);
627 } else {
628 tmp = ethernet_ll_prepare_on_ipv4(iface, pkt);
629 if (!tmp) {
630 ret = -ENOMEM;
631 goto error;
632 } else if (IS_ENABLED(CONFIG_NET_ARP) && tmp != pkt) {
633 /* Original pkt got queued and is replaced
634 * by an ARP request packet.
635 */
636 pkt = tmp;
637 ptype = htons(NET_ETH_PTYPE_ARP);
638 net_pkt_set_family(pkt, AF_INET);
639 } else {
640 ptype = htons(NET_ETH_PTYPE_IP);
641 }
642 }
643 } else if (IS_ENABLED(CONFIG_NET_IPV6) &&
644 net_pkt_family(pkt) == AF_INET6) {
645 ptype = htons(NET_ETH_PTYPE_IPV6);
646 } else if (IS_ENABLED(CONFIG_NET_SOCKETS_PACKET) &&
647 net_pkt_family(pkt) == AF_PACKET) {
648 struct net_context *context = net_pkt_context(pkt);
649
650 if (context && net_context_get_type(context) == SOCK_DGRAM) {
651 struct sockaddr_ll *dst_addr;
652 struct sockaddr_ll_ptr *src_addr;
653
654 /* The destination address is set in remote for this
655 * socket type.
656 */
657 dst_addr = (struct sockaddr_ll *)&context->remote;
658 src_addr = (struct sockaddr_ll_ptr *)&context->local;
659
660 net_pkt_lladdr_dst(pkt)->addr = dst_addr->sll_addr;
661 net_pkt_lladdr_dst(pkt)->len =
662 sizeof(struct net_eth_addr);
663 net_pkt_lladdr_src(pkt)->addr = src_addr->sll_addr;
664 net_pkt_lladdr_src(pkt)->len =
665 sizeof(struct net_eth_addr);
666 ptype = dst_addr->sll_protocol;
667 } else {
668 goto send;
669 }
670 } else if (IS_ENABLED(CONFIG_NET_GPTP) && net_pkt_is_gptp(pkt)) {
671 ptype = htons(NET_ETH_PTYPE_PTP);
672 } else if (IS_ENABLED(CONFIG_NET_LLDP) && net_pkt_is_lldp(pkt)) {
673 ptype = htons(NET_ETH_PTYPE_LLDP);
674 } else if (IS_ENABLED(CONFIG_NET_ARP)) {
675 /* Unktown type: Unqueued pkt is an ARP reply.
676 */
677 ptype = htons(NET_ETH_PTYPE_ARP);
678 net_pkt_set_family(pkt, AF_INET);
679 } else {
680 ret = -ENOTSUP;
681 goto error;
682 }
683
684 /* If the ll dst addr has not been set before, let's assume
685 * temporarily it's a broadcast one. When filling the header,
686 * it might detect this should be multicast and act accordingly.
687 */
688 if (!net_pkt_lladdr_dst(pkt)->addr) {
689 net_pkt_lladdr_dst(pkt)->addr = (uint8_t *)broadcast_eth_addr.addr;
690 net_pkt_lladdr_dst(pkt)->len = sizeof(struct net_eth_addr);
691 }
692
693 if (IS_ENABLED(CONFIG_NET_VLAN) &&
694 net_eth_is_vlan_enabled(ctx, iface)) {
695 if (set_vlan_tag(ctx, iface, pkt) == NET_DROP) {
696 ret = -EINVAL;
697 goto error;
698 }
699
700 set_vlan_priority(ctx, pkt);
701 }
702
703 /* Then set the ethernet header.
704 */
705 if (!ethernet_fill_header(ctx, pkt, ptype)) {
706 ret = -ENOMEM;
707 goto error;
708 }
709
710 net_pkt_cursor_init(pkt);
711
712 send:
713 ret = net_l2_send(api->send, net_if_get_device(iface), iface, pkt);
714 if (ret != 0) {
715 eth_stats_update_errors_tx(iface);
716 ethernet_remove_l2_header(pkt);
717 goto error;
718 }
719
720 ethernet_update_tx_stats(iface, pkt);
721
722 ret = net_pkt_get_len(pkt);
723 ethernet_remove_l2_header(pkt);
724
725 net_pkt_unref(pkt);
726 error:
727 return ret;
728 }
729
ethernet_enable(struct net_if * iface,bool state)730 static inline int ethernet_enable(struct net_if *iface, bool state)
731 {
732 const struct ethernet_api *eth =
733 net_if_get_device(iface)->api;
734
735 if (!eth) {
736 return -ENOENT;
737 }
738
739 if (!state) {
740 net_arp_clear_cache(iface);
741
742 if (eth->stop) {
743 eth->stop(net_if_get_device(iface));
744 }
745 } else {
746 if (eth->start) {
747 eth->start(net_if_get_device(iface));
748 }
749 }
750
751 return 0;
752 }
753
ethernet_flags(struct net_if * iface)754 enum net_l2_flags ethernet_flags(struct net_if *iface)
755 {
756 struct ethernet_context *ctx = net_if_l2_data(iface);
757
758 return ctx->ethernet_l2_flags;
759 }
760
761 #if defined(CONFIG_NET_VLAN)
net_eth_get_vlan_iface(struct net_if * iface,uint16_t tag)762 struct net_if *net_eth_get_vlan_iface(struct net_if *iface, uint16_t tag)
763 {
764 struct ethernet_context *ctx = net_if_l2_data(iface);
765 struct net_if *first_non_vlan_iface = NULL;
766 int i;
767
768 for (i = 0; i < CONFIG_NET_VLAN_COUNT; i++) {
769 if (ctx->vlan[i].tag == NET_VLAN_TAG_UNSPEC) {
770 if (!first_non_vlan_iface) {
771 first_non_vlan_iface = ctx->vlan[i].iface;
772 }
773
774 continue;
775 }
776
777 if (ctx->vlan[i].tag != tag) {
778 continue;
779 }
780
781 NET_DBG("[%d] vlan tag %d -> iface %p", i, tag,
782 ctx->vlan[i].iface);
783
784 return ctx->vlan[i].iface;
785 }
786
787 return first_non_vlan_iface;
788 }
789
enable_vlan_iface(struct ethernet_context * ctx,struct net_if * iface)790 static bool enable_vlan_iface(struct ethernet_context *ctx,
791 struct net_if *iface)
792 {
793 int iface_idx = net_if_get_by_iface(iface);
794
795 if (iface_idx < 0) {
796 return false;
797 }
798
799 atomic_set_bit(ctx->interfaces, iface_idx);
800
801 return true;
802 }
803
disable_vlan_iface(struct ethernet_context * ctx,struct net_if * iface)804 static bool disable_vlan_iface(struct ethernet_context *ctx,
805 struct net_if *iface)
806 {
807 int iface_idx = net_if_get_by_iface(iface);
808
809 if (iface_idx < 0) {
810 return false;
811 }
812
813 atomic_clear_bit(ctx->interfaces, iface_idx);
814
815 return true;
816 }
817
is_vlan_enabled_for_iface(struct ethernet_context * ctx,struct net_if * iface)818 static bool is_vlan_enabled_for_iface(struct ethernet_context *ctx,
819 struct net_if *iface)
820 {
821 int iface_idx = net_if_get_by_iface(iface);
822
823 if (iface_idx < 0) {
824 return false;
825 }
826
827 return !!atomic_test_bit(ctx->interfaces, iface_idx);
828 }
829
net_eth_is_vlan_enabled(struct ethernet_context * ctx,struct net_if * iface)830 bool net_eth_is_vlan_enabled(struct ethernet_context *ctx,
831 struct net_if *iface)
832 {
833 if (ctx->vlan_enabled) {
834 if (ctx->vlan_enabled == NET_VLAN_MAX_COUNT) {
835 /* All network interface are using VLAN, no need
836 * to check further.
837 */
838 return true;
839 }
840
841 if (is_vlan_enabled_for_iface(ctx, iface)) {
842 return true;
843 }
844 }
845
846 return false;
847 }
848
net_eth_get_vlan_tag(struct net_if * iface)849 uint16_t net_eth_get_vlan_tag(struct net_if *iface)
850 {
851 struct ethernet_context *ctx = net_if_l2_data(iface);
852 int i;
853
854 for (i = 0; i < CONFIG_NET_VLAN_COUNT; i++) {
855 if (ctx->vlan[i].iface == iface) {
856 return ctx->vlan[i].tag;
857 }
858 }
859
860 return NET_VLAN_TAG_UNSPEC;
861 }
862
net_eth_get_vlan_status(struct net_if * iface)863 bool net_eth_get_vlan_status(struct net_if *iface)
864 {
865 struct ethernet_context *ctx = net_if_l2_data(iface);
866
867 if (ctx->vlan_enabled &&
868 net_eth_get_vlan_tag(iface) != NET_VLAN_TAG_UNSPEC) {
869 return true;
870 }
871
872 return false;
873 }
874
get_vlan(struct ethernet_context * ctx,struct net_if * iface,uint16_t vlan_tag)875 static struct ethernet_vlan *get_vlan(struct ethernet_context *ctx,
876 struct net_if *iface,
877 uint16_t vlan_tag)
878 {
879 int i;
880
881 for (i = 0; i < CONFIG_NET_VLAN_COUNT; i++) {
882 if (ctx->vlan[i].iface == iface &&
883 ctx->vlan[i].tag == vlan_tag) {
884 return &ctx->vlan[i];
885 }
886 }
887
888 return NULL;
889 }
890
setup_ipv6_link_local_addr(struct net_if * iface)891 static void setup_ipv6_link_local_addr(struct net_if *iface)
892 {
893 struct net_linkaddr link_addr;
894 struct net_if_addr *ifaddr;
895 struct in6_addr addr;
896 uint32_t entropy;
897 uint8_t mac_addr[6];
898
899 entropy = sys_rand32_get();
900 mac_addr[0] = entropy >> 0;
901 mac_addr[1] = entropy >> 8;
902 mac_addr[2] = entropy >> 16;
903
904 entropy = sys_rand32_get();
905 mac_addr[3] = entropy >> 0;
906 mac_addr[4] = entropy >> 8;
907 mac_addr[5] = entropy >> 16;
908
909 mac_addr[0] |= 0x02; /* force LAA bit */
910
911 link_addr.len = sizeof(mac_addr);
912 link_addr.type = NET_LINK_ETHERNET;
913 link_addr.addr = mac_addr;
914
915 net_ipv6_addr_create_iid(&addr, &link_addr);
916
917 ifaddr = net_if_ipv6_addr_add(iface, &addr, NET_ADDR_AUTOCONF, 0);
918 if (!ifaddr) {
919 NET_DBG("Cannot add %s address to VLAN interface %p",
920 log_strdup(net_sprint_ipv6_addr(&addr)), iface);
921 }
922 }
923
net_eth_vlan_enable(struct net_if * iface,uint16_t tag)924 int net_eth_vlan_enable(struct net_if *iface, uint16_t tag)
925 {
926 struct ethernet_context *ctx = net_if_l2_data(iface);
927 const struct ethernet_api *eth =
928 net_if_get_device(iface)->api;
929 struct ethernet_vlan *vlan;
930 int i;
931
932 if (!eth) {
933 return -ENOENT;
934 }
935
936 if (net_if_l2(iface) != &NET_L2_GET_NAME(ETHERNET)) {
937 return -EINVAL;
938 }
939
940 if (!ctx->is_init) {
941 return -EPERM;
942 }
943
944 if (tag == NET_VLAN_TAG_UNSPEC) {
945 return -EBADF;
946 }
947
948 vlan = get_vlan(ctx, iface, tag);
949 if (vlan) {
950 return -EALREADY;
951 }
952
953 for (i = 0; i < CONFIG_NET_VLAN_COUNT; i++) {
954 if (ctx->vlan[i].iface != iface) {
955 continue;
956 }
957
958 if (ctx->vlan[i].tag != NET_VLAN_TAG_UNSPEC) {
959 continue;
960 }
961
962 NET_DBG("[%d] Adding vlan tag %d to iface %p", i, tag, iface);
963
964 ctx->vlan[i].tag = tag;
965
966 /* Add a link local IPv6 address to VLAN interface here.
967 * Each network interface needs LL address, but as there is
968 * only one link (MAC) address defined for all the master and
969 * slave interfaces, the VLAN interface might be left without
970 * a LL address. In order to solve this issue, we create a
971 * random LL address and set it to the VLAN network interface.
972 */
973 if (IS_ENABLED(CONFIG_NET_IPV6)) {
974 setup_ipv6_link_local_addr(iface);
975 }
976
977 enable_vlan_iface(ctx, iface);
978
979 if (eth->vlan_setup) {
980 eth->vlan_setup(net_if_get_device(iface),
981 iface, tag, true);
982 }
983
984 ctx->vlan_enabled++;
985 if (ctx->vlan_enabled > NET_VLAN_MAX_COUNT) {
986 ctx->vlan_enabled = NET_VLAN_MAX_COUNT;
987 }
988
989 ethernet_mgmt_raise_vlan_enabled_event(iface, tag);
990
991 return 0;
992 }
993
994 return -ENOSPC;
995 }
996
net_eth_vlan_disable(struct net_if * iface,uint16_t tag)997 int net_eth_vlan_disable(struct net_if *iface, uint16_t tag)
998 {
999 struct ethernet_context *ctx = net_if_l2_data(iface);
1000 const struct ethernet_api *eth =
1001 net_if_get_device(iface)->api;
1002 struct ethernet_vlan *vlan;
1003
1004 if (!eth) {
1005 return -ENOENT;
1006 }
1007
1008 if (net_if_l2(iface) != &NET_L2_GET_NAME(ETHERNET)) {
1009 return -EINVAL;
1010 }
1011
1012 if (tag == NET_VLAN_TAG_UNSPEC) {
1013 return -EBADF;
1014 }
1015
1016 vlan = get_vlan(ctx, iface, tag);
1017 if (!vlan) {
1018 return -ESRCH;
1019 }
1020
1021 NET_DBG("Removing vlan tag %d from iface %p", vlan->tag, vlan->iface);
1022
1023 vlan->tag = NET_VLAN_TAG_UNSPEC;
1024
1025 disable_vlan_iface(ctx, iface);
1026
1027 if (eth->vlan_setup) {
1028 eth->vlan_setup(net_if_get_device(iface), iface, tag, false);
1029 }
1030
1031 ethernet_mgmt_raise_vlan_disabled_event(iface, tag);
1032
1033 ctx->vlan_enabled--;
1034 if (ctx->vlan_enabled < 0) {
1035 ctx->vlan_enabled = 0;
1036 }
1037
1038 return 0;
1039 }
1040 #endif /* CONFIG_NET_VLAN */
1041
1042 NET_L2_INIT(ETHERNET_L2, ethernet_recv, ethernet_send, ethernet_enable,
1043 ethernet_flags);
1044
carrier_on_off(struct k_work * work)1045 static void carrier_on_off(struct k_work *work)
1046 {
1047 struct ethernet_context *ctx = CONTAINER_OF(work, struct ethernet_context,
1048 carrier_work);
1049 bool eth_carrier_up;
1050
1051 if (ctx->iface == NULL) {
1052 return;
1053 }
1054
1055 eth_carrier_up = atomic_test_bit(&ctx->flags, ETH_CARRIER_UP);
1056
1057 if (eth_carrier_up == ctx->is_net_carrier_up) {
1058 return;
1059 }
1060
1061 ctx->is_net_carrier_up = eth_carrier_up;
1062
1063 NET_DBG("Carrier %s for interface %p", eth_carrier_up ? "ON" : "OFF",
1064 ctx->iface);
1065
1066 if (eth_carrier_up) {
1067 ethernet_mgmt_raise_carrier_on_event(ctx->iface);
1068 net_if_up(ctx->iface);
1069 } else {
1070 ethernet_mgmt_raise_carrier_off_event(ctx->iface);
1071 net_if_carrier_down(ctx->iface);
1072 }
1073 }
1074
net_eth_carrier_on(struct net_if * iface)1075 void net_eth_carrier_on(struct net_if *iface)
1076 {
1077 struct ethernet_context *ctx = net_if_l2_data(iface);
1078
1079 if (!atomic_test_and_set_bit(&ctx->flags, ETH_CARRIER_UP)) {
1080 k_work_submit(&ctx->carrier_work);
1081 }
1082 }
1083
net_eth_carrier_off(struct net_if * iface)1084 void net_eth_carrier_off(struct net_if *iface)
1085 {
1086 struct ethernet_context *ctx = net_if_l2_data(iface);
1087
1088 if (atomic_test_and_clear_bit(&ctx->flags, ETH_CARRIER_UP)) {
1089 k_work_submit(&ctx->carrier_work);
1090 }
1091 }
1092
1093 #if defined(CONFIG_PTP_CLOCK)
net_eth_get_ptp_clock(struct net_if * iface)1094 const struct device *net_eth_get_ptp_clock(struct net_if *iface)
1095 {
1096 const struct device *dev = net_if_get_device(iface);
1097 const struct ethernet_api *api = dev->api;
1098
1099 if (!api) {
1100 return NULL;
1101 }
1102
1103 if (net_if_l2(iface) != &NET_L2_GET_NAME(ETHERNET)) {
1104 return NULL;
1105 }
1106
1107 if (!(api->get_capabilities(dev) & ETHERNET_PTP)) {
1108 return NULL;
1109 }
1110
1111 if (!api->get_ptp_clock) {
1112 return NULL;
1113 }
1114
1115 return api->get_ptp_clock(net_if_get_device(iface));
1116 }
1117 #endif /* CONFIG_PTP_CLOCK */
1118
1119 #if defined(CONFIG_PTP_CLOCK)
z_impl_net_eth_get_ptp_clock_by_index(int index)1120 const struct device *z_impl_net_eth_get_ptp_clock_by_index(int index)
1121 {
1122 struct net_if *iface;
1123
1124 iface = net_if_get_by_index(index);
1125 if (!iface) {
1126 return NULL;
1127 }
1128
1129 return net_eth_get_ptp_clock(iface);
1130 }
1131
1132 #ifdef CONFIG_USERSPACE
z_vrfy_net_eth_get_ptp_clock_by_index(int index)1133 static inline const struct device *z_vrfy_net_eth_get_ptp_clock_by_index(int index)
1134 {
1135 return z_impl_net_eth_get_ptp_clock_by_index(index);
1136 }
1137 #include <syscalls/net_eth_get_ptp_clock_by_index_mrsh.c>
1138 #endif /* CONFIG_USERSPACE */
1139 #else /* CONFIG_PTP_CLOCK */
z_impl_net_eth_get_ptp_clock_by_index(int index)1140 const struct device *z_impl_net_eth_get_ptp_clock_by_index(int index)
1141 {
1142 ARG_UNUSED(index);
1143
1144 return NULL;
1145 }
1146 #endif /* CONFIG_PTP_CLOCK */
1147
1148 #if defined(CONFIG_NET_GPTP)
net_eth_get_ptp_port(struct net_if * iface)1149 int net_eth_get_ptp_port(struct net_if *iface)
1150 {
1151 struct ethernet_context *ctx = net_if_l2_data(iface);
1152
1153 return ctx->port;
1154 }
1155
net_eth_set_ptp_port(struct net_if * iface,int port)1156 void net_eth_set_ptp_port(struct net_if *iface, int port)
1157 {
1158 struct ethernet_context *ctx = net_if_l2_data(iface);
1159
1160 ctx->port = port;
1161 }
1162 #endif /* CONFIG_NET_GPTP */
1163
net_eth_promisc_mode(struct net_if * iface,bool enable)1164 int net_eth_promisc_mode(struct net_if *iface, bool enable)
1165 {
1166 struct ethernet_req_params params;
1167
1168 if (!(net_eth_get_hw_capabilities(iface) & ETHERNET_PROMISC_MODE)) {
1169 return -ENOTSUP;
1170 }
1171
1172 params.promisc_mode = enable;
1173
1174 return net_mgmt(NET_REQUEST_ETHERNET_SET_PROMISC_MODE, iface,
1175 ¶ms, sizeof(struct ethernet_req_params));
1176 }
1177
ethernet_init(struct net_if * iface)1178 void ethernet_init(struct net_if *iface)
1179 {
1180 struct ethernet_context *ctx = net_if_l2_data(iface);
1181
1182 #if defined(CONFIG_NET_VLAN)
1183 int i;
1184 #endif
1185
1186 NET_DBG("Initializing Ethernet L2 %p for iface %p", ctx, iface);
1187
1188 ctx->ethernet_l2_flags = NET_L2_MULTICAST;
1189 ctx->iface = iface;
1190 k_work_init(&ctx->carrier_work, carrier_on_off);
1191
1192 if (net_eth_get_hw_capabilities(iface) & ETHERNET_PROMISC_MODE) {
1193 ctx->ethernet_l2_flags |= NET_L2_PROMISC_MODE;
1194 }
1195
1196 #if defined(CONFIG_NET_VLAN)
1197 if (!(net_eth_get_hw_capabilities(iface) & ETHERNET_HW_VLAN)) {
1198 return;
1199 }
1200
1201 for (i = 0; i < CONFIG_NET_VLAN_COUNT; i++) {
1202 if (!ctx->vlan[i].iface) {
1203 NET_DBG("[%d] alloc ctx %p iface %p", i, ctx, iface);
1204 ctx->vlan[i].tag = NET_VLAN_TAG_UNSPEC;
1205 ctx->vlan[i].iface = iface;
1206
1207 if (!ctx->is_init) {
1208 atomic_clear(ctx->interfaces);
1209 }
1210
1211 break;
1212 }
1213 }
1214 #endif
1215
1216 net_arp_init();
1217
1218 ctx->is_init = true;
1219 }
1220
