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 #include "main.h"
22
23 /* Loglevel of dsa_lldp function */
24 LOG_MODULE_DECLARE(net_dsa_lldp_sample, CONFIG_NET_DSA_LOG_LEVEL);
25
26 #define LLDP_SYSTEM_NAME_SIZE 24
27 #define LLDP_ETHER_TYPE 0x88CC
28 #define LLDP_INPUT_DATA_BUF_SIZE 512
29 #define DSA_BUF_SIZ 128
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)30 int dsa_lldp_send(struct net_if *iface, struct instance_data *pd,
31 uint16_t lan, int src_port, int origin_port, int cmd,
32 struct eth_addr *origin_addr)
33 {
34 int ret, len;
35 char buffer[DSA_BUF_SIZ], sys_name[LLDP_SYSTEM_NAME_SIZE];
36 struct sockaddr_ll dst;
37 struct ethernet_context *ctx = net_if_l2_data(iface);
38 struct net_eth_hdr *eth_hdr = (struct net_eth_hdr *) buffer;
39 uint8_t *p = &buffer[sizeof(struct net_eth_hdr)];
40 uint8_t *pb = p;
41
42 lan = ctx->dsa_port_idx;
43
44 dst.sll_ifindex = net_if_get_by_iface(iface);
45 /* Construct the Ethernet header */
46 memset(buffer, 0, DSA_BUF_SIZ);
47 /* Ethernet header */
48 /* Take MAC address assigned to LAN port */
49 memcpy(eth_hdr->src.addr, net_if_get_link_addr(iface)->addr, ETH_ALEN);
50 eth_hdr->dst.addr[0] = MCAST_DEST_MAC0;
51 eth_hdr->dst.addr[1] = MCAST_DEST_MAC1;
52 eth_hdr->dst.addr[2] = MCAST_DEST_MAC2;
53 eth_hdr->dst.addr[3] = MCAST_DEST_MAC3;
54 eth_hdr->dst.addr[4] = MCAST_DEST_MAC4;
55 eth_hdr->dst.addr[5] = MCAST_DEST_MAC5;
56
57 /* Ethertype field */
58 eth_hdr->type = htons(LLDP_ETHER_TYPE);
59
60 /* LLDP packet data */
61 /* Chassis ID */
62 dsa_buf_write_be16((LLDP_TLV_CHASSIS_ID << 9) | (ETH_ALEN + 1), &p);
63 *p++ = 4; /* subtype */
64 memcpy(p, net_if_get_link_addr(iface)->addr, ETH_ALEN);
65 p += ETH_ALEN;
66
67 /* PORT ID */
68 dsa_buf_write_be16((LLDP_TLV_PORT_ID << 9) | (ETH_ALEN + 1), &p);
69 *p++ = 3; /* subtype */
70 memcpy(p, net_if_get_link_addr(iface)->addr, ETH_ALEN);
71 p += ETH_ALEN;
72
73 /* TTL ID */
74 dsa_buf_write_be16((LLDP_TLV_TTL << 9) | 2, &p);
75 *p++ = 0; /* TTL field is 2 bytes long */
76 *p++ = 120;
77
78 /* SYSTEM NAME */
79 memset(sys_name, 0, sizeof(sys_name));
80 sprintf(sys_name, "ip_k66f LAN:%d", lan);
81 dsa_buf_write_be16((LLDP_TLV_SYSTEM_NAME << 9) | strlen(sys_name), &p);
82 memcpy(p, sys_name, strlen(sys_name));
83 p += strlen(sys_name);
84
85 len = sizeof(struct net_eth_hdr) + (p - pb);
86 ret = sendto(pd->sock, buffer, len, 0, (const struct sockaddr *)&dst,
87 sizeof(struct sockaddr_ll));
88 if (ret < 0) {
89 LOG_ERR("Failed to send, errno %d", errno);
90 }
91
92 return 0;
93 }
94
dsa_lldp_print_info(uint8_t * lldp_p,uint8_t lanid)95 void dsa_lldp_print_info(uint8_t *lldp_p, uint8_t lanid)
96 {
97 uint16_t tl, length;
98 uint8_t type, subtype;
99 uint8_t *p, t1, t2;
100 char t[LLDP_INPUT_DATA_BUF_SIZE];
101
102 LOG_INF("LLDP pkt recv -> lan%d", lanid);
103 do {
104 /* In-buffer data is stored as big endian */
105 t1 = *lldp_p++;
106 t2 = *lldp_p++;
107 tl = (uint16_t) t1 << 8 | t2;
108
109 /* Get type and length */
110 type = tl >> 9;
111 length = tl & 0x1FF;
112
113 switch (type) {
114 case LLDP_TLV_CHASSIS_ID:
115 case LLDP_TLV_PORT_ID:
116 /* Extract subtype */
117 subtype = *lldp_p++;
118 length--;
119 break;
120 }
121
122 p = lldp_p;
123
124 switch (type) {
125 case LLDP_TLV_END_LLDPDU:
126 return;
127 case LLDP_TLV_CHASSIS_ID:
128 LOG_INF("\tCHASSIS ID:\t%02x:%02x:%02x:%02x:%02x:%02x",
129 p[0], p[1], p[2], p[3], p[4], p[5]);
130 break;
131 case LLDP_TLV_PORT_ID:
132 LOG_INF("\tPORT ID:\t%02x:%02x:%02x:%02x:%02x:%02x",
133 p[0], p[1], p[2], p[3], p[4], p[5]);
134 break;
135 case LLDP_TLV_TTL:
136 /* TTL field has 2 bytes in BE */
137 LOG_INF("\tTTL:\t\t%ds", (uint16_t) p[0] << 8 | p[1]);
138 break;
139 case LLDP_TLV_SYSTEM_NAME:
140 memset(t, 0, length + 1);
141 memcpy(t, p, length);
142 LOG_INF("\tSYSTEM NAME:\t%s", t);
143 break;
144 }
145 lldp_p += length;
146 } while (1);
147 }
148
dsa_lldp_recv(struct net_if * iface,struct instance_data * pd,uint16_t * lan,int * origin_port,struct eth_addr * origin_addr)149 int dsa_lldp_recv(struct net_if *iface, struct instance_data *pd,
150 uint16_t *lan, int *origin_port,
151 struct eth_addr *origin_addr)
152 {
153 struct ethernet_context *ctx = net_if_l2_data(iface);
154 struct net_eth_hdr *eth_hdr =
155 (struct net_eth_hdr *) pd->recv_buffer;
156 uint8_t *lldp_p = &pd->recv_buffer[sizeof(struct net_eth_hdr)];
157 int received;
158
159 *lan = ctx->dsa_port_idx;
160
161 /* Receive data */
162 received = recv(pd->sock, pd->recv_buffer,
163 sizeof(pd->recv_buffer), 0);
164 if (received < 0) {
165 LOG_ERR("RAW : recv error %d", errno);
166 return -1;
167 }
168
169 if (eth_hdr->type != 0xCC88) {
170 LOG_ERR("Wrong LLDP packet type value [0x%x]", eth_hdr->type);
171 return -1;
172 }
173
174 dsa_lldp_print_info(lldp_p, *lan);
175 return 0;
176 }
177
178 DSA_THREAD(1, dsa_lldp_recv, dsa_lldp_send)
179 DSA_THREAD(2, dsa_lldp_recv, dsa_lldp_send)
180 DSA_THREAD(3, dsa_lldp_recv, dsa_lldp_send)
181
