1 // SPDX-License-Identifier: GPL-2.0
2 /*
3 * Copied from Linux Monitor (LiMon) - Networking.
4 *
5 * Copyright 1994 - 2000 Neil Russell.
6 * (See License)
7 * Copyright 2000 Roland Borde
8 * Copyright 2000 Paolo Scaffardi
9 * Copyright 2000-2002 Wolfgang Denk, wd@denx.de
10 */
11
12 #include <common.h>
13 #include <net.h>
14
15 #include "cdp.h"
16
17 /* Ethernet bcast address */
18 const u8 net_cdp_ethaddr[6] = { 0x01, 0x00, 0x0c, 0xcc, 0xcc, 0xcc };
19
20 #define CDP_DEVICE_ID_TLV 0x0001
21 #define CDP_ADDRESS_TLV 0x0002
22 #define CDP_PORT_ID_TLV 0x0003
23 #define CDP_CAPABILITIES_TLV 0x0004
24 #define CDP_VERSION_TLV 0x0005
25 #define CDP_PLATFORM_TLV 0x0006
26 #define CDP_NATIVE_VLAN_TLV 0x000a
27 #define CDP_APPLIANCE_VLAN_TLV 0x000e
28 #define CDP_TRIGGER_TLV 0x000f
29 #define CDP_POWER_CONSUMPTION_TLV 0x0010
30 #define CDP_SYSNAME_TLV 0x0014
31 #define CDP_SYSOBJECT_TLV 0x0015
32 #define CDP_MANAGEMENT_ADDRESS_TLV 0x0016
33
34 #define CDP_TIMEOUT 250UL /* one packet every 250ms */
35
36 static int cdp_seq;
37 static int cdp_ok;
38
39 ushort cdp_native_vlan;
40 ushort cdp_appliance_vlan;
41
42 static const uchar cdp_snap_hdr[8] = {
43 0xAA, 0xAA, 0x03, 0x00, 0x00, 0x0C, 0x20, 0x00 };
44
cdp_compute_csum(const uchar * buff,ushort len)45 static ushort cdp_compute_csum(const uchar *buff, ushort len)
46 {
47 ushort csum;
48 int odd;
49 ulong result = 0;
50 ushort leftover;
51 ushort *p;
52
53 if (len > 0) {
54 odd = 1 & (ulong)buff;
55 if (odd) {
56 result = *buff << 8;
57 len--;
58 buff++;
59 }
60 while (len > 1) {
61 p = (ushort *)buff;
62 result += *p++;
63 buff = (uchar *)p;
64 if (result & 0x80000000)
65 result = (result & 0xFFFF) + (result >> 16);
66 len -= 2;
67 }
68 if (len) {
69 leftover = (signed short)(*(const signed char *)buff);
70 /*
71 * CISCO SUCKS big time! (and blows too):
72 * CDP uses the IP checksum algorithm with a twist;
73 * for the last byte it *sign* extends and sums.
74 */
75 result = (result & 0xffff0000) |
76 ((result + leftover) & 0x0000ffff);
77 }
78 while (result >> 16)
79 result = (result & 0xFFFF) + (result >> 16);
80
81 if (odd)
82 result = ((result >> 8) & 0xff) |
83 ((result & 0xff) << 8);
84 }
85
86 /* add up 16-bit and 17-bit words for 17+c bits */
87 result = (result & 0xffff) + (result >> 16);
88 /* add up 16-bit and 2-bit for 16+c bit */
89 result = (result & 0xffff) + (result >> 16);
90 /* add up carry.. */
91 result = (result & 0xffff) + (result >> 16);
92
93 /* negate */
94 csum = ~(ushort)result;
95
96 /* run time endian detection */
97 if (csum != htons(csum)) /* little endian */
98 csum = htons(csum);
99
100 return csum;
101 }
102
cdp_send_trigger(void)103 static int cdp_send_trigger(void)
104 {
105 uchar *pkt;
106 ushort *s;
107 ushort *cp;
108 struct ethernet_hdr *et;
109 int len;
110 ushort chksum;
111 #if defined(CONFIG_CDP_DEVICE_ID) || defined(CONFIG_CDP_PORT_ID) || \
112 defined(CONFIG_CDP_VERSION) || defined(CONFIG_CDP_PLATFORM)
113 char buf[32];
114 #endif
115
116 pkt = net_tx_packet;
117 et = (struct ethernet_hdr *)pkt;
118
119 /* NOTE: trigger sent not on any VLAN */
120
121 /* form ethernet header */
122 memcpy(et->et_dest, net_cdp_ethaddr, 6);
123 memcpy(et->et_src, net_ethaddr, 6);
124
125 pkt += ETHER_HDR_SIZE;
126
127 /* SNAP header */
128 memcpy((uchar *)pkt, cdp_snap_hdr, sizeof(cdp_snap_hdr));
129 pkt += sizeof(cdp_snap_hdr);
130
131 /* CDP header */
132 *pkt++ = 0x02; /* CDP version 2 */
133 *pkt++ = 180; /* TTL */
134 s = (ushort *)pkt;
135 cp = s;
136 /* checksum (0 for later calculation) */
137 *s++ = htons(0);
138
139 /* CDP fields */
140 #ifdef CONFIG_CDP_DEVICE_ID
141 *s++ = htons(CDP_DEVICE_ID_TLV);
142 *s++ = htons(CONFIG_CDP_DEVICE_ID);
143 sprintf(buf, CONFIG_CDP_DEVICE_ID_PREFIX "%pm", net_ethaddr);
144 memcpy((uchar *)s, buf, 16);
145 s += 16 / 2;
146 #endif
147
148 #ifdef CONFIG_CDP_PORT_ID
149 *s++ = htons(CDP_PORT_ID_TLV);
150 memset(buf, 0, sizeof(buf));
151 sprintf(buf, CONFIG_CDP_PORT_ID, eth_get_dev_index());
152 len = strlen(buf);
153 if (len & 1) /* make it even */
154 len++;
155 *s++ = htons(len + 4);
156 memcpy((uchar *)s, buf, len);
157 s += len / 2;
158 #endif
159
160 #ifdef CONFIG_CDP_CAPABILITIES
161 *s++ = htons(CDP_CAPABILITIES_TLV);
162 *s++ = htons(8);
163 *(ulong *)s = htonl(CONFIG_CDP_CAPABILITIES);
164 s += 2;
165 #endif
166
167 #ifdef CONFIG_CDP_VERSION
168 *s++ = htons(CDP_VERSION_TLV);
169 memset(buf, 0, sizeof(buf));
170 strcpy(buf, CONFIG_CDP_VERSION);
171 len = strlen(buf);
172 if (len & 1) /* make it even */
173 len++;
174 *s++ = htons(len + 4);
175 memcpy((uchar *)s, buf, len);
176 s += len / 2;
177 #endif
178
179 #ifdef CONFIG_CDP_PLATFORM
180 *s++ = htons(CDP_PLATFORM_TLV);
181 memset(buf, 0, sizeof(buf));
182 strcpy(buf, CONFIG_CDP_PLATFORM);
183 len = strlen(buf);
184 if (len & 1) /* make it even */
185 len++;
186 *s++ = htons(len + 4);
187 memcpy((uchar *)s, buf, len);
188 s += len / 2;
189 #endif
190
191 #ifdef CONFIG_CDP_TRIGGER
192 *s++ = htons(CDP_TRIGGER_TLV);
193 *s++ = htons(8);
194 *(ulong *)s = htonl(CONFIG_CDP_TRIGGER);
195 s += 2;
196 #endif
197
198 #ifdef CONFIG_CDP_POWER_CONSUMPTION
199 *s++ = htons(CDP_POWER_CONSUMPTION_TLV);
200 *s++ = htons(6);
201 *s++ = htons(CONFIG_CDP_POWER_CONSUMPTION);
202 #endif
203
204 /* length of ethernet packet */
205 len = (uchar *)s - ((uchar *)net_tx_packet + ETHER_HDR_SIZE);
206 et->et_protlen = htons(len);
207
208 len = ETHER_HDR_SIZE + sizeof(cdp_snap_hdr);
209 chksum = cdp_compute_csum((uchar *)net_tx_packet + len,
210 (uchar *)s - (net_tx_packet + len));
211 if (chksum == 0)
212 chksum = 0xFFFF;
213 *cp = htons(chksum);
214
215 net_send_packet(net_tx_packet, (uchar *)s - net_tx_packet);
216 return 0;
217 }
218
cdp_timeout_handler(void)219 static void cdp_timeout_handler(void)
220 {
221 cdp_seq++;
222
223 if (cdp_seq < 3) {
224 net_set_timeout_handler(CDP_TIMEOUT, cdp_timeout_handler);
225 cdp_send_trigger();
226 return;
227 }
228
229 /* if not OK try again */
230 if (!cdp_ok)
231 net_start_again();
232 else
233 net_set_state(NETLOOP_SUCCESS);
234 }
235
cdp_receive(const uchar * pkt,unsigned len)236 void cdp_receive(const uchar *pkt, unsigned len)
237 {
238 const uchar *t;
239 const ushort *ss;
240 ushort type, tlen;
241 ushort vlan, nvlan;
242
243 /* minimum size? */
244 if (len < sizeof(cdp_snap_hdr) + 4)
245 goto pkt_short;
246
247 /* check for valid CDP SNAP header */
248 if (memcmp(pkt, cdp_snap_hdr, sizeof(cdp_snap_hdr)) != 0)
249 return;
250
251 pkt += sizeof(cdp_snap_hdr);
252 len -= sizeof(cdp_snap_hdr);
253
254 /* Version of CDP protocol must be >= 2 and TTL != 0 */
255 if (pkt[0] < 0x02 || pkt[1] == 0)
256 return;
257
258 /*
259 * if version is greater than 0x02 maybe we'll have a problem;
260 * output a warning
261 */
262 if (pkt[0] != 0x02)
263 printf("**WARNING: CDP packet received with a protocol version "
264 "%d > 2\n", pkt[0] & 0xff);
265
266 if (cdp_compute_csum(pkt, len) != 0)
267 return;
268
269 pkt += 4;
270 len -= 4;
271
272 vlan = htons(-1);
273 nvlan = htons(-1);
274 while (len > 0) {
275 if (len < 4)
276 goto pkt_short;
277
278 ss = (const ushort *)pkt;
279 type = ntohs(ss[0]);
280 tlen = ntohs(ss[1]);
281 if (tlen > len)
282 goto pkt_short;
283
284 pkt += tlen;
285 len -= tlen;
286
287 ss += 2; /* point ss to the data of the TLV */
288 tlen -= 4;
289
290 switch (type) {
291 case CDP_DEVICE_ID_TLV:
292 break;
293 case CDP_ADDRESS_TLV:
294 break;
295 case CDP_PORT_ID_TLV:
296 break;
297 case CDP_CAPABILITIES_TLV:
298 break;
299 case CDP_VERSION_TLV:
300 break;
301 case CDP_PLATFORM_TLV:
302 break;
303 case CDP_NATIVE_VLAN_TLV:
304 nvlan = *ss;
305 break;
306 case CDP_APPLIANCE_VLAN_TLV:
307 t = (const uchar *)ss;
308 while (tlen > 0) {
309 if (tlen < 3)
310 goto pkt_short;
311
312 ss = (const ushort *)(t + 1);
313
314 #ifdef CONFIG_CDP_APPLIANCE_VLAN_TYPE
315 if (t[0] == CONFIG_CDP_APPLIANCE_VLAN_TYPE)
316 vlan = *ss;
317 #else
318 /* XXX will this work; dunno */
319 vlan = ntohs(*ss);
320 #endif
321 t += 3; tlen -= 3;
322 }
323 break;
324 case CDP_TRIGGER_TLV:
325 break;
326 case CDP_POWER_CONSUMPTION_TLV:
327 break;
328 case CDP_SYSNAME_TLV:
329 break;
330 case CDP_SYSOBJECT_TLV:
331 break;
332 case CDP_MANAGEMENT_ADDRESS_TLV:
333 break;
334 }
335 }
336
337 cdp_appliance_vlan = vlan;
338 cdp_native_vlan = nvlan;
339
340 cdp_ok = 1;
341 return;
342
343 pkt_short:
344 printf("** CDP packet is too short\n");
345 return;
346 }
347
cdp_start(void)348 void cdp_start(void)
349 {
350 printf("Using %s device\n", eth_get_name());
351 cdp_seq = 0;
352 cdp_ok = 0;
353
354 cdp_native_vlan = htons(-1);
355 cdp_appliance_vlan = htons(-1);
356
357 net_set_timeout_handler(CDP_TIMEOUT, cdp_timeout_handler);
358
359 cdp_send_trigger();
360 }
361