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 };
40
iface_cb(struct net_if * iface,void * user_data)41 static void iface_cb(struct net_if *iface, void *user_data)
42 {
43 struct ud *ud = user_data;
44
45 if (net_if_l2(iface) != &NET_L2_GET_NAME(VIRTUAL)) {
46 return;
47 }
48
49 if (!ud->first) {
50 ud->first = iface;
51 return;
52 }
53
54 if (!ud->second) {
55 ud->second = iface;
56 return;
57 }
58 }
59
setup_iface(struct net_if * eth_iface,struct net_if * iface,const char * ipv6_addr,const char * ipv4_addr,uint16_t vlan_tag)60 static int setup_iface(struct net_if *eth_iface,
61 struct net_if *iface,
62 const char *ipv6_addr,
63 const char *ipv4_addr,
64 uint16_t vlan_tag)
65 {
66 struct net_if_addr *ifaddr;
67 struct in_addr addr4;
68 struct in6_addr addr6;
69 int ret;
70
71 ret = net_eth_vlan_enable(eth_iface, vlan_tag);
72 if (ret < 0) {
73 LOG_ERR("Cannot enable VLAN for tag %d (%d)", vlan_tag, ret);
74 }
75
76 if (net_addr_pton(AF_INET6, ipv6_addr, &addr6)) {
77 LOG_ERR("Invalid address: %s", ipv6_addr);
78 return -EINVAL;
79 }
80
81 ifaddr = net_if_ipv6_addr_add(iface, &addr6, NET_ADDR_MANUAL, 0);
82 if (!ifaddr) {
83 LOG_ERR("Cannot add %s to interface %p", ipv6_addr, iface);
84 return -EINVAL;
85 }
86
87 if (net_addr_pton(AF_INET, ipv4_addr, &addr4)) {
88 LOG_ERR("Invalid address: %s", ipv4_addr);
89 return -EINVAL;
90 }
91
92 ifaddr = net_if_ipv4_addr_add(iface, &addr4, NET_ADDR_MANUAL, 0);
93 if (!ifaddr) {
94 LOG_ERR("Cannot add %s to interface %p", ipv4_addr, iface);
95 return -EINVAL;
96 }
97
98 LOG_DBG("Interface %p VLAN tag %d setup done.", iface, vlan_tag);
99
100 return 0;
101 }
102
103 static struct ud ud;
104
init_vlan(struct net_if * iface)105 static int init_vlan(struct net_if *iface)
106 {
107 enum ethernet_hw_caps caps;
108 int ret;
109
110 (void)memset(&ud, 0, sizeof(ud));
111
112 net_if_foreach(iface_cb, &ud);
113
114 caps = net_eth_get_hw_capabilities(iface);
115 if (!(caps & ETHERNET_HW_VLAN)) {
116 LOG_DBG("Interface %p does not support %s", iface, "VLAN");
117 return -ENOENT;
118 }
119
120 ret = setup_iface(iface, ud.first,
121 CONFIG_NET_SAMPLE_IFACE2_MY_IPV6_ADDR,
122 CONFIG_NET_SAMPLE_IFACE2_MY_IPV4_ADDR,
123 CONFIG_NET_SAMPLE_IFACE2_VLAN_TAG);
124 if (ret < 0) {
125 return ret;
126 }
127
128 ret = setup_iface(iface, ud.second,
129 CONFIG_NET_SAMPLE_IFACE3_MY_IPV6_ADDR,
130 CONFIG_NET_SAMPLE_IFACE3_MY_IPV4_ADDR,
131 CONFIG_NET_SAMPLE_IFACE3_VLAN_TAG);
132 if (ret < 0) {
133 return ret;
134 }
135
136 /* Bring up the VLAN interface automatically */
137 net_if_up(ud.first);
138 net_if_up(ud.second);
139
140 return 0;
141 }
142
parse_lldp(struct net_if * iface,struct net_pkt * pkt)143 static enum net_verdict parse_lldp(struct net_if *iface, struct net_pkt *pkt)
144 {
145 LOG_DBG("iface %p Parsing LLDP, len %zu", iface, net_pkt_get_len(pkt));
146
147 net_pkt_cursor_init(pkt);
148
149 while (1) {
150 uint16_t type_length;
151 uint16_t length;
152 uint8_t type;
153
154 if (net_pkt_read_be16(pkt, &type_length)) {
155 LOG_DBG("End LLDP DU TLV");
156 break;
157 }
158
159 length = type_length & 0x1FF;
160 type = (uint8_t)(type_length >> 9);
161
162 /* Skip for now data */
163 if (net_pkt_skip(pkt, length)) {
164 LOG_DBG("");
165 break;
166 }
167
168 switch (type) {
169 case LLDP_TLV_CHASSIS_ID:
170 LOG_DBG("Chassis ID");
171 break;
172 case LLDP_TLV_PORT_ID:
173 LOG_DBG("Port ID");
174 break;
175 case LLDP_TLV_TTL:
176 LOG_DBG("TTL");
177 break;
178 default:
179 LOG_DBG("TLV Not parsed");
180 break;
181 }
182
183 LOG_DBG("type_length %u type %u length %u",
184 type_length, type, length);
185 }
186
187 /* Let stack to free the packet */
188 return NET_DROP;
189 }
190
init_app(void)191 static int init_app(void)
192 {
193 enum ethernet_hw_caps caps;
194 struct net_if *iface;
195 int ret;
196
197 iface = net_if_get_first_by_type(&NET_L2_GET_NAME(ETHERNET));
198 if (!iface) {
199 LOG_ERR("No ethernet interfaces found.");
200 return -ENOENT;
201 }
202
203 ret = init_vlan(iface);
204 if (ret < 0) {
205 LOG_WRN("Cannot setup VLAN (%d)", ret);
206 }
207
208 caps = net_eth_get_hw_capabilities(iface);
209 if (!(caps & ETHERNET_LLDP)) {
210 LOG_ERR("Interface %p does not support %s", iface, "LLDP");
211 LOG_ERR("Cannot continue!");
212 return -ENOENT;
213 }
214
215 set_optional_tlv(iface);
216 net_lldp_register_callback(iface, parse_lldp);
217
218 return 0;
219 }
220
main(void)221 int main(void)
222 {
223 /* The application will setup VLAN but does nothing meaningful.
224 * The configuration will enable LLDP support so you should see
225 * LLDPDU messages sent to the network interface.
226 */
227 init_app();
228 return 0;
229 }
230
