blob: f9ccf5323183d98cf10435192d17502c7a8cd069 [file] [log] [blame]
Joe Hershbergerf575ae12012-05-23 07:57:59 +00001/*
2 * Copied from Linux Monitor (LiMon) - Networking.
3 *
4 * Copyright 1994 - 2000 Neil Russell.
5 * (See License)
6 * Copyright 2000 Roland Borde
7 * Copyright 2000 Paolo Scaffardi
8 * Copyright 2000-2002 Wolfgang Denk, wd@denx.de
Wolfgang Denk2ea91032014-09-30 10:44:01 +02009 * SPDX-License-Identifier: GPL-2.0
Joe Hershbergerf575ae12012-05-23 07:57:59 +000010 */
11
12#include <common.h>
13#include <net.h>
14#if defined(CONFIG_CDP_VERSION)
15#include <timestamp.h>
16#endif
17
18#include "cdp.h"
19
20/* Ethernet bcast address */
Joe Hershberger0adb5b72015-04-08 01:41:04 -050021const u8 net_cdp_ethaddr[6] = { 0x01, 0x00, 0x0c, 0xcc, 0xcc, 0xcc };
Joe Hershbergerf575ae12012-05-23 07:57:59 +000022
23#define CDP_DEVICE_ID_TLV 0x0001
24#define CDP_ADDRESS_TLV 0x0002
25#define CDP_PORT_ID_TLV 0x0003
26#define CDP_CAPABILITIES_TLV 0x0004
27#define CDP_VERSION_TLV 0x0005
28#define CDP_PLATFORM_TLV 0x0006
29#define CDP_NATIVE_VLAN_TLV 0x000a
30#define CDP_APPLIANCE_VLAN_TLV 0x000e
31#define CDP_TRIGGER_TLV 0x000f
32#define CDP_POWER_CONSUMPTION_TLV 0x0010
33#define CDP_SYSNAME_TLV 0x0014
34#define CDP_SYSOBJECT_TLV 0x0015
35#define CDP_MANAGEMENT_ADDRESS_TLV 0x0016
36
37#define CDP_TIMEOUT 250UL /* one packet every 250ms */
38
Joe Hershberger6aede5b2015-04-08 01:41:14 -050039static int cdp_seq;
40static int cdp_ok;
Joe Hershbergerf575ae12012-05-23 07:57:59 +000041
Joe Hershberger6aede5b2015-04-08 01:41:14 -050042ushort cdp_native_vlan;
43ushort cdp_appliance_vlan;
Joe Hershbergerf575ae12012-05-23 07:57:59 +000044
Joe Hershberger6aede5b2015-04-08 01:41:14 -050045static const uchar cdp_snap_hdr[8] = {
Joe Hershbergerf575ae12012-05-23 07:57:59 +000046 0xAA, 0xAA, 0x03, 0x00, 0x00, 0x0C, 0x20, 0x00 };
47
Joe Hershberger6aede5b2015-04-08 01:41:14 -050048static ushort cdp_compute_csum(const uchar *buff, ushort len)
Joe Hershbergerf575ae12012-05-23 07:57:59 +000049{
50 ushort csum;
51 int odd;
52 ulong result = 0;
53 ushort leftover;
54 ushort *p;
55
56 if (len > 0) {
57 odd = 1 & (ulong)buff;
58 if (odd) {
59 result = *buff << 8;
60 len--;
61 buff++;
62 }
63 while (len > 1) {
64 p = (ushort *)buff;
65 result += *p++;
66 buff = (uchar *)p;
67 if (result & 0x80000000)
68 result = (result & 0xFFFF) + (result >> 16);
69 len -= 2;
70 }
71 if (len) {
72 leftover = (signed short)(*(const signed char *)buff);
73 /*
74 * CISCO SUCKS big time! (and blows too):
75 * CDP uses the IP checksum algorithm with a twist;
76 * for the last byte it *sign* extends and sums.
77 */
78 result = (result & 0xffff0000) |
79 ((result + leftover) & 0x0000ffff);
80 }
81 while (result >> 16)
82 result = (result & 0xFFFF) + (result >> 16);
83
84 if (odd)
85 result = ((result >> 8) & 0xff) |
86 ((result & 0xff) << 8);
87 }
88
89 /* add up 16-bit and 17-bit words for 17+c bits */
90 result = (result & 0xffff) + (result >> 16);
91 /* add up 16-bit and 2-bit for 16+c bit */
92 result = (result & 0xffff) + (result >> 16);
93 /* add up carry.. */
94 result = (result & 0xffff) + (result >> 16);
95
96 /* negate */
97 csum = ~(ushort)result;
98
99 /* run time endian detection */
100 if (csum != htons(csum)) /* little endian */
101 csum = htons(csum);
102
103 return csum;
104}
105
Joe Hershberger6aede5b2015-04-08 01:41:14 -0500106static int cdp_send_trigger(void)
Joe Hershbergerf575ae12012-05-23 07:57:59 +0000107{
108 uchar *pkt;
109 ushort *s;
110 ushort *cp;
Joe Hershbergercb487f52012-05-23 07:58:06 +0000111 struct ethernet_hdr *et;
Joe Hershbergerf575ae12012-05-23 07:57:59 +0000112 int len;
113 ushort chksum;
114#if defined(CONFIG_CDP_DEVICE_ID) || defined(CONFIG_CDP_PORT_ID) || \
115 defined(CONFIG_CDP_VERSION) || defined(CONFIG_CDP_PLATFORM)
116 char buf[32];
117#endif
118
Joe Hershberger1203fcc2015-04-08 01:41:05 -0500119 pkt = net_tx_packet;
Joe Hershbergercb487f52012-05-23 07:58:06 +0000120 et = (struct ethernet_hdr *)pkt;
Joe Hershbergerf575ae12012-05-23 07:57:59 +0000121
122 /* NOTE: trigger sent not on any VLAN */
123
124 /* form ethernet header */
Joe Hershberger0adb5b72015-04-08 01:41:04 -0500125 memcpy(et->et_dest, net_cdp_ethaddr, 6);
126 memcpy(et->et_src, net_ethaddr, 6);
Joe Hershbergerf575ae12012-05-23 07:57:59 +0000127
128 pkt += ETHER_HDR_SIZE;
129
130 /* SNAP header */
Joe Hershberger6aede5b2015-04-08 01:41:14 -0500131 memcpy((uchar *)pkt, cdp_snap_hdr, sizeof(cdp_snap_hdr));
132 pkt += sizeof(cdp_snap_hdr);
Joe Hershbergerf575ae12012-05-23 07:57:59 +0000133
134 /* CDP header */
135 *pkt++ = 0x02; /* CDP version 2 */
136 *pkt++ = 180; /* TTL */
137 s = (ushort *)pkt;
138 cp = s;
139 /* checksum (0 for later calculation) */
140 *s++ = htons(0);
141
142 /* CDP fields */
143#ifdef CONFIG_CDP_DEVICE_ID
144 *s++ = htons(CDP_DEVICE_ID_TLV);
145 *s++ = htons(CONFIG_CDP_DEVICE_ID);
Joe Hershberger0adb5b72015-04-08 01:41:04 -0500146 sprintf(buf, CONFIG_CDP_DEVICE_ID_PREFIX "%pm", net_ethaddr);
Joe Hershbergerf575ae12012-05-23 07:57:59 +0000147 memcpy((uchar *)s, buf, 16);
148 s += 16 / 2;
149#endif
150
151#ifdef CONFIG_CDP_PORT_ID
152 *s++ = htons(CDP_PORT_ID_TLV);
153 memset(buf, 0, sizeof(buf));
154 sprintf(buf, CONFIG_CDP_PORT_ID, eth_get_dev_index());
155 len = strlen(buf);
156 if (len & 1) /* make it even */
157 len++;
158 *s++ = htons(len + 4);
159 memcpy((uchar *)s, buf, len);
160 s += len / 2;
161#endif
162
163#ifdef CONFIG_CDP_CAPABILITIES
164 *s++ = htons(CDP_CAPABILITIES_TLV);
165 *s++ = htons(8);
166 *(ulong *)s = htonl(CONFIG_CDP_CAPABILITIES);
167 s += 2;
168#endif
169
170#ifdef CONFIG_CDP_VERSION
171 *s++ = htons(CDP_VERSION_TLV);
172 memset(buf, 0, sizeof(buf));
173 strcpy(buf, CONFIG_CDP_VERSION);
174 len = strlen(buf);
175 if (len & 1) /* make it even */
176 len++;
177 *s++ = htons(len + 4);
178 memcpy((uchar *)s, buf, len);
179 s += len / 2;
180#endif
181
182#ifdef CONFIG_CDP_PLATFORM
183 *s++ = htons(CDP_PLATFORM_TLV);
184 memset(buf, 0, sizeof(buf));
185 strcpy(buf, CONFIG_CDP_PLATFORM);
186 len = strlen(buf);
187 if (len & 1) /* make it even */
188 len++;
189 *s++ = htons(len + 4);
190 memcpy((uchar *)s, buf, len);
191 s += len / 2;
192#endif
193
194#ifdef CONFIG_CDP_TRIGGER
195 *s++ = htons(CDP_TRIGGER_TLV);
196 *s++ = htons(8);
197 *(ulong *)s = htonl(CONFIG_CDP_TRIGGER);
198 s += 2;
199#endif
200
201#ifdef CONFIG_CDP_POWER_CONSUMPTION
202 *s++ = htons(CDP_POWER_CONSUMPTION_TLV);
203 *s++ = htons(6);
204 *s++ = htons(CONFIG_CDP_POWER_CONSUMPTION);
205#endif
206
207 /* length of ethernet packet */
Joe Hershberger1203fcc2015-04-08 01:41:05 -0500208 len = (uchar *)s - ((uchar *)net_tx_packet + ETHER_HDR_SIZE);
Joe Hershbergerf575ae12012-05-23 07:57:59 +0000209 et->et_protlen = htons(len);
210
Joe Hershberger6aede5b2015-04-08 01:41:14 -0500211 len = ETHER_HDR_SIZE + sizeof(cdp_snap_hdr);
212 chksum = cdp_compute_csum((uchar *)net_tx_packet + len,
Joe Hershberger1203fcc2015-04-08 01:41:05 -0500213 (uchar *)s - (net_tx_packet + len));
Joe Hershbergerf575ae12012-05-23 07:57:59 +0000214 if (chksum == 0)
215 chksum = 0xFFFF;
216 *cp = htons(chksum);
217
Joe Hershberger1203fcc2015-04-08 01:41:05 -0500218 net_send_packet(net_tx_packet, (uchar *)s - net_tx_packet);
Joe Hershbergerf575ae12012-05-23 07:57:59 +0000219 return 0;
220}
221
Joe Hershberger6aede5b2015-04-08 01:41:14 -0500222static void cdp_timeout_handler(void)
Joe Hershbergerf575ae12012-05-23 07:57:59 +0000223{
Joe Hershberger6aede5b2015-04-08 01:41:14 -0500224 cdp_seq++;
Joe Hershbergerf575ae12012-05-23 07:57:59 +0000225
Joe Hershberger6aede5b2015-04-08 01:41:14 -0500226 if (cdp_seq < 3) {
Joe Hershbergerbc0571f2015-04-08 01:41:21 -0500227 net_set_timeout_handler(CDP_TIMEOUT, cdp_timeout_handler);
Joe Hershberger6aede5b2015-04-08 01:41:14 -0500228 cdp_send_trigger();
Joe Hershbergerf575ae12012-05-23 07:57:59 +0000229 return;
230 }
231
232 /* if not OK try again */
Joe Hershberger6aede5b2015-04-08 01:41:14 -0500233 if (!cdp_ok)
Joe Hershbergerbc0571f2015-04-08 01:41:21 -0500234 net_start_again();
Joe Hershbergerf575ae12012-05-23 07:57:59 +0000235 else
Joe Hershberger22f6e992012-05-23 07:59:14 +0000236 net_set_state(NETLOOP_SUCCESS);
Joe Hershbergerf575ae12012-05-23 07:57:59 +0000237}
238
Joe Hershberger0b4c5ff2012-05-23 07:58:13 +0000239void cdp_receive(const uchar *pkt, unsigned len)
Joe Hershbergerf575ae12012-05-23 07:57:59 +0000240{
241 const uchar *t;
242 const ushort *ss;
243 ushort type, tlen;
244 ushort vlan, nvlan;
245
246 /* minimum size? */
Joe Hershberger6aede5b2015-04-08 01:41:14 -0500247 if (len < sizeof(cdp_snap_hdr) + 4)
Joe Hershbergerf575ae12012-05-23 07:57:59 +0000248 goto pkt_short;
249
250 /* check for valid CDP SNAP header */
Joe Hershberger6aede5b2015-04-08 01:41:14 -0500251 if (memcmp(pkt, cdp_snap_hdr, sizeof(cdp_snap_hdr)) != 0)
Joe Hershbergerf575ae12012-05-23 07:57:59 +0000252 return;
253
Joe Hershberger6aede5b2015-04-08 01:41:14 -0500254 pkt += sizeof(cdp_snap_hdr);
255 len -= sizeof(cdp_snap_hdr);
Joe Hershbergerf575ae12012-05-23 07:57:59 +0000256
257 /* Version of CDP protocol must be >= 2 and TTL != 0 */
258 if (pkt[0] < 0x02 || pkt[1] == 0)
259 return;
260
261 /*
262 * if version is greater than 0x02 maybe we'll have a problem;
263 * output a warning
264 */
265 if (pkt[0] != 0x02)
266 printf("**WARNING: CDP packet received with a protocol version "
267 "%d > 2\n", pkt[0] & 0xff);
268
Joe Hershberger6aede5b2015-04-08 01:41:14 -0500269 if (cdp_compute_csum(pkt, len) != 0)
Joe Hershbergerf575ae12012-05-23 07:57:59 +0000270 return;
271
272 pkt += 4;
273 len -= 4;
274
275 vlan = htons(-1);
276 nvlan = htons(-1);
277 while (len > 0) {
278 if (len < 4)
279 goto pkt_short;
280
281 ss = (const ushort *)pkt;
282 type = ntohs(ss[0]);
283 tlen = ntohs(ss[1]);
284 if (tlen > len)
285 goto pkt_short;
286
287 pkt += tlen;
288 len -= tlen;
289
290 ss += 2; /* point ss to the data of the TLV */
291 tlen -= 4;
292
293 switch (type) {
294 case CDP_DEVICE_ID_TLV:
295 break;
296 case CDP_ADDRESS_TLV:
297 break;
298 case CDP_PORT_ID_TLV:
299 break;
300 case CDP_CAPABILITIES_TLV:
301 break;
302 case CDP_VERSION_TLV:
303 break;
304 case CDP_PLATFORM_TLV:
305 break;
306 case CDP_NATIVE_VLAN_TLV:
307 nvlan = *ss;
308 break;
309 case CDP_APPLIANCE_VLAN_TLV:
310 t = (const uchar *)ss;
311 while (tlen > 0) {
312 if (tlen < 3)
313 goto pkt_short;
314
315 ss = (const ushort *)(t + 1);
316
317#ifdef CONFIG_CDP_APPLIANCE_VLAN_TYPE
318 if (t[0] == CONFIG_CDP_APPLIANCE_VLAN_TYPE)
319 vlan = *ss;
320#else
321 /* XXX will this work; dunno */
322 vlan = ntohs(*ss);
323#endif
324 t += 3; tlen -= 3;
325 }
326 break;
327 case CDP_TRIGGER_TLV:
328 break;
329 case CDP_POWER_CONSUMPTION_TLV:
330 break;
331 case CDP_SYSNAME_TLV:
332 break;
333 case CDP_SYSOBJECT_TLV:
334 break;
335 case CDP_MANAGEMENT_ADDRESS_TLV:
336 break;
337 }
338 }
339
Joe Hershberger6aede5b2015-04-08 01:41:14 -0500340 cdp_appliance_vlan = vlan;
341 cdp_native_vlan = nvlan;
Joe Hershbergerf575ae12012-05-23 07:57:59 +0000342
Joe Hershberger6aede5b2015-04-08 01:41:14 -0500343 cdp_ok = 1;
Joe Hershbergerf575ae12012-05-23 07:57:59 +0000344 return;
345
Joe Hershberger6aede5b2015-04-08 01:41:14 -0500346pkt_short:
Joe Hershbergerf575ae12012-05-23 07:57:59 +0000347 printf("** CDP packet is too short\n");
348 return;
349}
350
Joe Hershberger6aede5b2015-04-08 01:41:14 -0500351void cdp_start(void)
Joe Hershbergerf575ae12012-05-23 07:57:59 +0000352{
353 printf("Using %s device\n", eth_get_name());
Joe Hershberger6aede5b2015-04-08 01:41:14 -0500354 cdp_seq = 0;
355 cdp_ok = 0;
Joe Hershbergerf575ae12012-05-23 07:57:59 +0000356
Joe Hershberger6aede5b2015-04-08 01:41:14 -0500357 cdp_native_vlan = htons(-1);
358 cdp_appliance_vlan = htons(-1);
Joe Hershbergerf575ae12012-05-23 07:57:59 +0000359
Joe Hershbergerbc0571f2015-04-08 01:41:21 -0500360 net_set_timeout_handler(CDP_TIMEOUT, cdp_timeout_handler);
Joe Hershbergerf575ae12012-05-23 07:57:59 +0000361
Joe Hershberger6aede5b2015-04-08 01:41:14 -0500362 cdp_send_trigger();
Joe Hershbergerf575ae12012-05-23 07:57:59 +0000363}