1 /*
2 * Copyright (c) 2020 DENX Software Engineering GmbH
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <zephyr/logging/log.h>
8 LOG_MODULE_REGISTER(net_dsa_lldp_sample, CONFIG_NET_DSA_LOG_LEVEL);
9
10 #include <zephyr/net/dsa.h>
11 #include "main.h"
12
iface_cb(struct net_if * iface,void * user_data)13 static void iface_cb(struct net_if *iface, void *user_data)
14 {
15
16 struct ud *ifaces = user_data;
17
18 if (net_if_l2(iface) != &NET_L2_GET_NAME(ETHERNET)) {
19 return;
20 }
21
22 if (net_eth_get_hw_capabilities(iface) & ETHERNET_DSA_MASTER_PORT) {
23 if (ifaces->master == NULL) {
24 ifaces->master = iface;
25
26 /* Get slave interfaces */
27 for (int i = 0; i < ARRAY_SIZE(ifaces->lan); i++) {
28 struct net_if *slave = dsa_get_slave_port(iface, i);
29
30 if (slave == NULL) {
31 LOG_ERR("Slave interface %d not found.", i);
32 break;
33 }
34
35 ifaces->lan[i] = slave;
36 }
37 return;
38 }
39 }
40 }
41
42 static const uint8_t eth_filter_l2_addr_base[][6] = {
43 /* MAC address of other device - for filtering testing */
44 { 0x01, 0x80, 0xc2, 0x00, 0x00, 0x03 }
45 };
46
dsa_ll_addr_switch_cb(struct net_if * iface,struct net_pkt * pkt)47 enum net_verdict dsa_ll_addr_switch_cb(struct net_if *iface,
48 struct net_pkt *pkt)
49 {
50 struct net_eth_hdr *hdr = NET_ETH_HDR(pkt);
51 struct net_linkaddr lladst;
52
53 net_pkt_cursor_init(pkt);
54 lladst.len = sizeof(hdr->dst.addr);
55 lladst.addr = &hdr->dst.addr[0];
56
57 /*
58 * Pass packet to lan1..3 when matching one from
59 * check_ll_ether_addr table
60 */
61 if (check_ll_ether_addr(lladst.addr, ð_filter_l2_addr_base[0][0])) {
62 return 1;
63 }
64
65 return 0;
66 }
67
start_slave_port_packet_socket(struct net_if * iface,struct instance_data * pd)68 int start_slave_port_packet_socket(struct net_if *iface,
69 struct instance_data *pd)
70 {
71 struct sockaddr_ll dst;
72 int ret;
73
74 pd->sock = socket(AF_PACKET, SOCK_RAW, htons(ETH_P_ALL));
75 if (pd->sock < 0) {
76 LOG_ERR("Failed to create RAW socket : %d", errno);
77 return -errno;
78 }
79
80 dst.sll_ifindex = net_if_get_by_iface(iface);
81 dst.sll_family = AF_PACKET;
82
83 ret = bind(pd->sock, (const struct sockaddr *)&dst,
84 sizeof(struct sockaddr_ll));
85 if (ret < 0) {
86 LOG_ERR("Failed to bind packet socket : %d", errno);
87 return -errno;
88 }
89
90 return 0;
91 }
92
93 struct ud user_data_ifaces;
init_dsa_ports(void)94 static int init_dsa_ports(void)
95 {
96 uint8_t tbl_buf[8];
97
98 /* Initialize interfaces - read them to user_data_ifaces */
99 (void)memset(&user_data_ifaces, 0, sizeof(user_data_ifaces));
100 net_if_foreach(iface_cb, &user_data_ifaces);
101
102 /*
103 * Set static table to forward LLDP protocol packets
104 * to master port.
105 */
106 dsa_switch_set_mac_table_entry(user_data_ifaces.lan[0],
107 ð_filter_l2_addr_base[0][0],
108 BIT(4), 0, 0);
109 dsa_switch_get_mac_table_entry(user_data_ifaces.lan[0], tbl_buf, 0);
110
111 LOG_INF("DSA static MAC address table entry [%d]:", 0);
112 LOG_INF("0x%x 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x",
113 tbl_buf[7], tbl_buf[6], tbl_buf[5], tbl_buf[4],
114 tbl_buf[3], tbl_buf[2], tbl_buf[1], tbl_buf[0]);
115
116 return 0;
117 }
118
main(void)119 int main(void)
120 {
121 init_dsa_ports();
122
123 LOG_INF("DSA ports init - OK");
124 return 0;
125 }
126
