1 /*
2 * Copyright (c) 2018 Intel Corporation.
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <zephyr/logging/log.h>
8 LOG_MODULE_REGISTER(net_lldp_sample, LOG_LEVEL_DBG);
9
10 #include <zephyr/kernel.h>
11 #include <errno.h>
12
13 #include <zephyr/net/net_core.h>
14 #include <zephyr/net/net_l2.h>
15 #include <zephyr/net/net_if.h>
16 #include <zephyr/net/ethernet.h>
17
18 static struct lldp_system_name_tlv {
19 uint16_t type_length;
20 uint8_t name[4];
21 } __packed tlv = {
22 .name = { 't', 'e', 's', 't' },
23 };
24
set_optional_tlv(struct net_if * iface)25 static void set_optional_tlv(struct net_if *iface)
26 {
27 NET_DBG("");
28
29 tlv.type_length = htons((LLDP_TLV_SYSTEM_NAME << 9) |
30 ((sizeof(tlv) - sizeof(uint16_t)) & 0x01ff));
31
32 net_lldp_config_optional(iface, (uint8_t *)&tlv, sizeof(tlv));
33 }
34
35 /* User data for the interface callback */
36 struct ud {
37 struct net_if *first;
38 struct net_if *second;
39 struct net_if *third;
40 };
41
iface_cb(struct net_if * iface,void * user_data)42 static void iface_cb(struct net_if *iface, void *user_data)
43 {
44 struct ud *ud = user_data;
45
46 if (net_if_l2(iface) != &NET_L2_GET_NAME(ETHERNET)) {
47 return;
48 }
49
50 if (!ud->first) {
51 ud->first = iface;
52 return;
53 }
54
55 if (!ud->second) {
56 ud->second = iface;
57 return;
58 }
59
60 if (!ud->third) {
61 ud->third = iface;
62 return;
63 }
64 }
65
setup_iface(struct net_if * iface,const char * ipv6_addr,const char * ipv4_addr,uint16_t vlan_tag)66 static int setup_iface(struct net_if *iface, const char *ipv6_addr,
67 const char *ipv4_addr, uint16_t vlan_tag)
68 {
69 struct net_if_addr *ifaddr;
70 struct in_addr addr4;
71 struct in6_addr addr6;
72 int ret;
73
74 ret = net_eth_vlan_enable(iface, vlan_tag);
75 if (ret < 0) {
76 LOG_ERR("Cannot enable VLAN for tag %d (%d)", vlan_tag, ret);
77 }
78
79 if (net_addr_pton(AF_INET6, ipv6_addr, &addr6)) {
80 LOG_ERR("Invalid address: %s", ipv6_addr);
81 return -EINVAL;
82 }
83
84 ifaddr = net_if_ipv6_addr_add(iface, &addr6, NET_ADDR_MANUAL, 0);
85 if (!ifaddr) {
86 LOG_ERR("Cannot add %s to interface %p", ipv6_addr, iface);
87 return -EINVAL;
88 }
89
90 if (net_addr_pton(AF_INET, ipv4_addr, &addr4)) {
91 LOG_ERR("Invalid address: %s", ipv4_addr);
92 return -EINVAL;
93 }
94
95 ifaddr = net_if_ipv4_addr_add(iface, &addr4, NET_ADDR_MANUAL, 0);
96 if (!ifaddr) {
97 LOG_ERR("Cannot add %s to interface %p", ipv4_addr, iface);
98 return -EINVAL;
99 }
100
101 LOG_DBG("Interface %p VLAN tag %d setup done.", iface, vlan_tag);
102
103 return 0;
104 }
105
106 static struct ud ud;
107
init_vlan(void)108 static int init_vlan(void)
109 {
110 enum ethernet_hw_caps caps;
111 int ret;
112
113 (void)memset(&ud, 0, sizeof(ud));
114
115 net_if_foreach(iface_cb, &ud);
116
117 caps = net_eth_get_hw_capabilities(ud.first);
118 if (!(caps & ETHERNET_HW_VLAN)) {
119 LOG_DBG("Interface %p does not support %s", ud.first, "VLAN");
120 return -ENOENT;
121 }
122
123 /* This sample has two VLANs. For the second one we need to manually
124 * create IP address for this test. But first the VLAN needs to be
125 * added to the interface so that IPv6 DAD can work properly.
126 */
127 ret = setup_iface(ud.second,
128 CONFIG_NET_SAMPLE_IFACE2_MY_IPV6_ADDR,
129 CONFIG_NET_SAMPLE_IFACE2_MY_IPV4_ADDR,
130 CONFIG_NET_SAMPLE_IFACE2_VLAN_TAG);
131 if (ret < 0) {
132 return ret;
133 }
134
135 ret = setup_iface(ud.third,
136 CONFIG_NET_SAMPLE_IFACE3_MY_IPV6_ADDR,
137 CONFIG_NET_SAMPLE_IFACE3_MY_IPV4_ADDR,
138 CONFIG_NET_SAMPLE_IFACE3_VLAN_TAG);
139 if (ret < 0) {
140 return ret;
141 }
142
143 return 0;
144 }
145
parse_lldp(struct net_if * iface,struct net_pkt * pkt)146 static enum net_verdict parse_lldp(struct net_if *iface, struct net_pkt *pkt)
147 {
148 LOG_DBG("iface %p Parsing LLDP, len %zu", iface, net_pkt_get_len(pkt));
149
150 net_pkt_cursor_init(pkt);
151
152 while (1) {
153 uint16_t type_length;
154 uint16_t length;
155 uint8_t type;
156
157 if (net_pkt_read_be16(pkt, &type_length)) {
158 LOG_DBG("End LLDP DU TLV");
159 break;
160 }
161
162 length = type_length & 0x1FF;
163 type = (uint8_t)(type_length >> 9);
164
165 /* Skip for now data */
166 if (net_pkt_skip(pkt, length)) {
167 LOG_DBG("");
168 break;
169 }
170
171 switch (type) {
172 case LLDP_TLV_CHASSIS_ID:
173 LOG_DBG("Chassis ID");
174 break;
175 case LLDP_TLV_PORT_ID:
176 LOG_DBG("Port ID");
177 break;
178 case LLDP_TLV_TTL:
179 LOG_DBG("TTL");
180 break;
181 default:
182 LOG_DBG("TLV Not parsed");
183 break;
184 }
185
186 LOG_DBG("type_length %u type %u length %u",
187 type_length, type, length);
188 }
189
190 /* Let stack to free the packet */
191 return NET_DROP;
192 }
193
init_app(void)194 static int init_app(void)
195 {
196 enum ethernet_hw_caps caps;
197 int ret;
198
199 ret = init_vlan();
200 if (ret < 0) {
201 LOG_WRN("Cannot setup VLAN (%d)", ret);
202 }
203
204 caps = net_eth_get_hw_capabilities(ud.first);
205 if (!(caps & ETHERNET_LLDP)) {
206 LOG_ERR("Interface %p does not support %s", ud.first, "LLDP");
207 LOG_ERR("Cannot continue!");
208 return -ENOENT;
209 }
210
211 set_optional_tlv(ud.first);
212 net_lldp_register_callback(ud.first, parse_lldp);
213
214 return 0;
215 }
216
main(void)217 int main(void)
218 {
219 /* The application will setup VLAN but does nothing meaningful.
220 * The configuration will enable LLDP support so you should see
221 * LLDPDU messages sent to the network interface.
222 */
223 init_app();
224 return 0;
225 }
226
