1 /*
2  * Copyright (c) 2020 DENX Software Engineering GmbH
3  *
4  * SPDX-License-Identifier: Apache-2.0
5  */
6 
7 #include <stdlib.h>
8 #include <stdio.h>
9 #include <string.h>
10 
11 #include <zephyr/net/socket.h>
12 #include <zephyr/net/net_core.h>
13 #include <zephyr/net/net_l2.h>
14 #include <zephyr/net/net_if.h>
15 #include <zephyr/net/socket.h>
16 #include <zephyr/net/ethernet.h>
17 #include <zephyr/net/lldp.h>
18 #include <errno.h>
19 
20 #include <zephyr/logging/log.h>
21 
22 /* Loglevel of dsa_lldp function */
23 LOG_MODULE_DECLARE(net_dsa_sample, CONFIG_NET_DSA_LOG_LEVEL);
24 
25 #include "dsa_lldp.h"
26 
27 #define LLDP_SYSTEM_NAME_SIZE    24
28 #define LLDP_ETHER_TYPE          0x88CC
29 #define LLDP_INPUT_DATA_BUF_SIZE 512
30 #define DSA_BUF_SIZ              128
31 
32 static const uint8_t eth_filter_l2_addr_base[][6] = {
33 	/* MAC address of other device - for filtering testing */
34 	{0x01, 0x80, 0xc2, 0x00, 0x00, 0x03}};
35 
dsa_ll_addr_switch_cb(struct net_if * iface,struct net_pkt * pkt)36 enum net_verdict dsa_ll_addr_switch_cb(struct net_if *iface, struct net_pkt *pkt)
37 {
38 	struct net_eth_hdr *hdr = NET_ETH_HDR(pkt);
39 	struct net_linkaddr lladst = { 0 };
40 
41 	net_pkt_cursor_init(pkt);
42 
43 	(void)net_linkaddr_set(&lladst, (const uint8_t *)hdr->dst.addr,
44 			       sizeof(hdr->dst.addr));
45 
46 	/*
47 	 * Pass packet to lan1..3 when matching one from
48 	 * check_ll_ether_addr table
49 	 */
50 	if (check_ll_ether_addr(lladst.addr, &eth_filter_l2_addr_base[0][0])) {
51 		return NET_CONTINUE;
52 	}
53 
54 	return NET_OK;
55 }
56 
start_user_port_packet_socket(struct net_if * iface,struct instance_data * pd)57 int start_user_port_packet_socket(struct net_if *iface, struct instance_data *pd)
58 {
59 	struct sockaddr_ll dst;
60 	int ret;
61 
62 	pd->sock = socket(AF_PACKET, SOCK_RAW, htons(ETH_P_ALL));
63 	if (pd->sock < 0) {
64 		LOG_ERR("Failed to create RAW socket : %d", errno);
65 		return -errno;
66 	}
67 
68 	dst.sll_ifindex = net_if_get_by_iface(iface);
69 	dst.sll_family = AF_PACKET;
70 
71 	ret = bind(pd->sock, (const struct sockaddr *)&dst, sizeof(struct sockaddr_ll));
72 	if (ret < 0) {
73 		LOG_ERR("Failed to bind packet socket : %d", errno);
74 		return -errno;
75 	}
76 
77 	return 0;
78 }
79 
dsa_lldp(struct ud * user_data)80 void dsa_lldp(struct ud *user_data)
81 {
82 	uint8_t tbl_buf[8];
83 
84 	/*
85 	 * Set static table to forward LLDP protocol packets
86 	 * to conduit port.
87 	 */
88 	dsa_switch_set_mac_table_entry(user_data->lan[0], &eth_filter_l2_addr_base[0][0], BIT(4), 0,
89 				       0);
90 	dsa_switch_get_mac_table_entry(user_data->lan[0], tbl_buf, 0);
91 
92 	LOG_INF("DSA static MAC address table entry [%d]:", 0);
93 	LOG_INF("0x%x 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x", tbl_buf[7], tbl_buf[6], tbl_buf[5],
94 		tbl_buf[4], tbl_buf[3], tbl_buf[2], tbl_buf[1], tbl_buf[0]);
95 }
96 
dsa_lldp_send(struct net_if * iface,struct instance_data * pd,uint16_t lan,int src_port,int origin_port,int cmd,struct eth_addr * origin_addr)97 static int dsa_lldp_send(struct net_if *iface, struct instance_data *pd, uint16_t lan, int src_port,
98 			 int origin_port, int cmd, struct eth_addr *origin_addr)
99 {
100 	int ret, len;
101 	char buffer[DSA_BUF_SIZ], sys_name[LLDP_SYSTEM_NAME_SIZE];
102 	struct sockaddr_ll dst;
103 	struct ethernet_context *ctx = net_if_l2_data(iface);
104 	struct net_eth_hdr *eth_hdr = (struct net_eth_hdr *)buffer;
105 	uint8_t *p = &buffer[sizeof(struct net_eth_hdr)];
106 	uint8_t *pb = p;
107 
108 	lan = ctx->dsa_port_idx;
109 
110 	dst.sll_ifindex = net_if_get_by_iface(iface);
111 	/* Construct the Ethernet header */
112 	memset(buffer, 0, DSA_BUF_SIZ);
113 	/* Ethernet header */
114 	/* Take MAC address assigned to LAN port */
115 	memcpy(eth_hdr->src.addr, net_if_get_link_addr(iface)->addr, ETH_ALEN);
116 	eth_hdr->dst.addr[0] = MCAST_DEST_MAC0;
117 	eth_hdr->dst.addr[1] = MCAST_DEST_MAC1;
118 	eth_hdr->dst.addr[2] = MCAST_DEST_MAC2;
119 	eth_hdr->dst.addr[3] = MCAST_DEST_MAC3;
120 	eth_hdr->dst.addr[4] = MCAST_DEST_MAC4;
121 	eth_hdr->dst.addr[5] = MCAST_DEST_MAC5;
122 
123 	/* Ethertype field */
124 	eth_hdr->type = htons(LLDP_ETHER_TYPE);
125 
126 	/* LLDP packet data */
127 	/* Chassis ID */
128 	dsa_buf_write_be16((LLDP_TLV_CHASSIS_ID << 9) | (ETH_ALEN + 1), &p);
129 	*p++ = 4; /* subtype */
130 	memcpy(p, net_if_get_link_addr(iface)->addr, ETH_ALEN);
131 	p += ETH_ALEN;
132 
133 	/* PORT ID */
134 	dsa_buf_write_be16((LLDP_TLV_PORT_ID << 9) | (ETH_ALEN + 1), &p);
135 	*p++ = 3; /* subtype */
136 	memcpy(p, net_if_get_link_addr(iface)->addr, ETH_ALEN);
137 	p += ETH_ALEN;
138 
139 	/* TTL ID */
140 	dsa_buf_write_be16((LLDP_TLV_TTL << 9) | 2, &p);
141 	*p++ = 0; /* TTL field is 2 bytes long */
142 	*p++ = 120;
143 
144 	/* SYSTEM NAME */
145 	memset(sys_name, 0, sizeof(sys_name));
146 	sprintf(sys_name, "ip_k66f LAN:%d", lan);
147 	dsa_buf_write_be16((LLDP_TLV_SYSTEM_NAME << 9) | strlen(sys_name), &p);
148 	memcpy(p, sys_name, strlen(sys_name));
149 	p += strlen(sys_name);
150 
151 	len = sizeof(struct net_eth_hdr) + (p - pb);
152 	ret = sendto(pd->sock, buffer, len, 0, (const struct sockaddr *)&dst,
153 		     sizeof(struct sockaddr_ll));
154 	if (ret < 0) {
155 		LOG_ERR("Failed to send, errno %d", errno);
156 	}
157 
158 	return 0;
159 }
160 
dsa_lldp_print_info(uint8_t * lldp_p,uint8_t lanid)161 static void dsa_lldp_print_info(uint8_t *lldp_p, uint8_t lanid)
162 {
163 	uint16_t tl, length;
164 	uint8_t type, subtype;
165 	uint8_t *p, t1, t2;
166 	char t[LLDP_INPUT_DATA_BUF_SIZE];
167 
168 	LOG_INF("LLDP pkt recv -> lan%d", lanid);
169 	do {
170 		/* In-buffer data is stored as big endian */
171 		t1 = *lldp_p++;
172 		t2 = *lldp_p++;
173 		tl = (uint16_t)t1 << 8 | t2;
174 
175 		/* Get type and length */
176 		type = tl >> 9;
177 		length = tl & 0x1FF;
178 
179 		switch (type) {
180 		case LLDP_TLV_CHASSIS_ID:
181 		case LLDP_TLV_PORT_ID:
182 			/* Extract subtype */
183 			subtype = *lldp_p++;
184 			length--;
185 			break;
186 		}
187 
188 		p = lldp_p;
189 
190 		switch (type) {
191 		case LLDP_TLV_END_LLDPDU:
192 			return;
193 		case LLDP_TLV_CHASSIS_ID:
194 			LOG_INF("\tCHASSIS ID:\t%02x:%02x:%02x:%02x:%02x:%02x", p[0], p[1], p[2],
195 				p[3], p[4], p[5]);
196 			break;
197 		case LLDP_TLV_PORT_ID:
198 			LOG_INF("\tPORT ID:\t%02x:%02x:%02x:%02x:%02x:%02x", p[0], p[1], p[2], p[3],
199 				p[4], p[5]);
200 			break;
201 		case LLDP_TLV_TTL:
202 			/* TTL field has 2 bytes in BE */
203 			LOG_INF("\tTTL:\t\t%ds", (uint16_t)p[0] << 8 | p[1]);
204 			break;
205 		case LLDP_TLV_SYSTEM_NAME:
206 			memset(t, 0, length + 1);
207 			memcpy(t, p, length);
208 			LOG_INF("\tSYSTEM NAME:\t%s", t);
209 			break;
210 		}
211 		lldp_p += length;
212 	} while (1);
213 }
214 
dsa_lldp_recv(struct net_if * iface,struct instance_data * pd,uint16_t * lan,int * origin_port,struct eth_addr * origin_addr)215 static int dsa_lldp_recv(struct net_if *iface, struct instance_data *pd, uint16_t *lan,
216 			 int *origin_port, struct eth_addr *origin_addr)
217 {
218 	struct ethernet_context *ctx = net_if_l2_data(iface);
219 	struct net_eth_hdr *eth_hdr = (struct net_eth_hdr *)pd->recv_buffer;
220 	uint8_t *lldp_p = &pd->recv_buffer[sizeof(struct net_eth_hdr)];
221 	int received;
222 
223 	*lan = ctx->dsa_port_idx;
224 
225 	/* Receive data */
226 	received = recv(pd->sock, pd->recv_buffer, sizeof(pd->recv_buffer), 0);
227 	if (received < 0) {
228 		LOG_ERR("RAW : recv error %d", errno);
229 		return -1;
230 	}
231 
232 	if (eth_hdr->type != 0xCC88) {
233 		LOG_ERR("Wrong LLDP packet type value [0x%x]", eth_hdr->type);
234 		return -1;
235 	}
236 
237 	dsa_lldp_print_info(lldp_p, *lan);
238 	return 0;
239 }
240 
241 DSA_THREAD(1, dsa_lldp_recv, dsa_lldp_send)
242 DSA_THREAD(2, dsa_lldp_recv, dsa_lldp_send)
243 DSA_THREAD(3, dsa_lldp_recv, dsa_lldp_send)
244