blob: 3d9559eb3b66b67c3214b1f65165e42ae3dc1062 [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
9 */
10
11#include <common.h>
12#include <net.h>
13#if defined(CONFIG_CDP_VERSION)
14#include <timestamp.h>
15#endif
16
17#include "cdp.h"
18
19/* Ethernet bcast address */
20const uchar NetCDPAddr[6] = { 0x01, 0x00, 0x0c, 0xcc, 0xcc, 0xcc };
21
22#define CDP_DEVICE_ID_TLV 0x0001
23#define CDP_ADDRESS_TLV 0x0002
24#define CDP_PORT_ID_TLV 0x0003
25#define CDP_CAPABILITIES_TLV 0x0004
26#define CDP_VERSION_TLV 0x0005
27#define CDP_PLATFORM_TLV 0x0006
28#define CDP_NATIVE_VLAN_TLV 0x000a
29#define CDP_APPLIANCE_VLAN_TLV 0x000e
30#define CDP_TRIGGER_TLV 0x000f
31#define CDP_POWER_CONSUMPTION_TLV 0x0010
32#define CDP_SYSNAME_TLV 0x0014
33#define CDP_SYSOBJECT_TLV 0x0015
34#define CDP_MANAGEMENT_ADDRESS_TLV 0x0016
35
36#define CDP_TIMEOUT 250UL /* one packet every 250ms */
37
38static int CDPSeq;
39static int CDPOK;
40
41ushort CDPNativeVLAN;
42ushort CDPApplianceVLAN;
43
44static const uchar CDP_SNAP_hdr[8] = {
45 0xAA, 0xAA, 0x03, 0x00, 0x00, 0x0C, 0x20, 0x00 };
46
47static ushort
48CDP_compute_csum(const uchar *buff, ushort len)
49{
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
106static int
107CDPSendTrigger(void)
108{
109 uchar *pkt;
110 ushort *s;
111 ushort *cp;
Joe Hershbergercb487f52012-05-23 07:58:06 +0000112 struct ethernet_hdr *et;
Joe Hershbergerf575ae12012-05-23 07:57:59 +0000113 int len;
114 ushort chksum;
115#if defined(CONFIG_CDP_DEVICE_ID) || defined(CONFIG_CDP_PORT_ID) || \
116 defined(CONFIG_CDP_VERSION) || defined(CONFIG_CDP_PLATFORM)
117 char buf[32];
118#endif
119
120 pkt = NetTxPacket;
Joe Hershbergercb487f52012-05-23 07:58:06 +0000121 et = (struct ethernet_hdr *)pkt;
Joe Hershbergerf575ae12012-05-23 07:57:59 +0000122
123 /* NOTE: trigger sent not on any VLAN */
124
125 /* form ethernet header */
126 memcpy(et->et_dest, NetCDPAddr, 6);
127 memcpy(et->et_src, NetOurEther, 6);
128
129 pkt += ETHER_HDR_SIZE;
130
131 /* SNAP header */
132 memcpy((uchar *)pkt, CDP_SNAP_hdr, sizeof(CDP_SNAP_hdr));
133 pkt += sizeof(CDP_SNAP_hdr);
134
135 /* CDP header */
136 *pkt++ = 0x02; /* CDP version 2 */
137 *pkt++ = 180; /* TTL */
138 s = (ushort *)pkt;
139 cp = s;
140 /* checksum (0 for later calculation) */
141 *s++ = htons(0);
142
143 /* CDP fields */
144#ifdef CONFIG_CDP_DEVICE_ID
145 *s++ = htons(CDP_DEVICE_ID_TLV);
146 *s++ = htons(CONFIG_CDP_DEVICE_ID);
147 sprintf(buf, CONFIG_CDP_DEVICE_ID_PREFIX "%pm", NetOurEther);
148 memcpy((uchar *)s, buf, 16);
149 s += 16 / 2;
150#endif
151
152#ifdef CONFIG_CDP_PORT_ID
153 *s++ = htons(CDP_PORT_ID_TLV);
154 memset(buf, 0, sizeof(buf));
155 sprintf(buf, CONFIG_CDP_PORT_ID, eth_get_dev_index());
156 len = strlen(buf);
157 if (len & 1) /* make it even */
158 len++;
159 *s++ = htons(len + 4);
160 memcpy((uchar *)s, buf, len);
161 s += len / 2;
162#endif
163
164#ifdef CONFIG_CDP_CAPABILITIES
165 *s++ = htons(CDP_CAPABILITIES_TLV);
166 *s++ = htons(8);
167 *(ulong *)s = htonl(CONFIG_CDP_CAPABILITIES);
168 s += 2;
169#endif
170
171#ifdef CONFIG_CDP_VERSION
172 *s++ = htons(CDP_VERSION_TLV);
173 memset(buf, 0, sizeof(buf));
174 strcpy(buf, CONFIG_CDP_VERSION);
175 len = strlen(buf);
176 if (len & 1) /* make it even */
177 len++;
178 *s++ = htons(len + 4);
179 memcpy((uchar *)s, buf, len);
180 s += len / 2;
181#endif
182
183#ifdef CONFIG_CDP_PLATFORM
184 *s++ = htons(CDP_PLATFORM_TLV);
185 memset(buf, 0, sizeof(buf));
186 strcpy(buf, CONFIG_CDP_PLATFORM);
187 len = strlen(buf);
188 if (len & 1) /* make it even */
189 len++;
190 *s++ = htons(len + 4);
191 memcpy((uchar *)s, buf, len);
192 s += len / 2;
193#endif
194
195#ifdef CONFIG_CDP_TRIGGER
196 *s++ = htons(CDP_TRIGGER_TLV);
197 *s++ = htons(8);
198 *(ulong *)s = htonl(CONFIG_CDP_TRIGGER);
199 s += 2;
200#endif
201
202#ifdef CONFIG_CDP_POWER_CONSUMPTION
203 *s++ = htons(CDP_POWER_CONSUMPTION_TLV);
204 *s++ = htons(6);
205 *s++ = htons(CONFIG_CDP_POWER_CONSUMPTION);
206#endif
207
208 /* length of ethernet packet */
209 len = (uchar *)s - ((uchar *)NetTxPacket + ETHER_HDR_SIZE);
210 et->et_protlen = htons(len);
211
212 len = ETHER_HDR_SIZE + sizeof(CDP_SNAP_hdr);
213 chksum = CDP_compute_csum((uchar *)NetTxPacket + len,
214 (uchar *)s - (NetTxPacket + len));
215 if (chksum == 0)
216 chksum = 0xFFFF;
217 *cp = htons(chksum);
218
Joe Hershbergeradf5d932012-05-23 07:59:13 +0000219 NetSendPacket(NetTxPacket, (uchar *)s - NetTxPacket);
Joe Hershbergerf575ae12012-05-23 07:57:59 +0000220 return 0;
221}
222
223static void
224CDPTimeout(void)
225{
226 CDPSeq++;
227
228 if (CDPSeq < 3) {
229 NetSetTimeout(CDP_TIMEOUT, CDPTimeout);
230 CDPSendTrigger();
231 return;
232 }
233
234 /* if not OK try again */
235 if (!CDPOK)
236 NetStartAgain();
237 else
Joe Hershberger22f6e992012-05-23 07:59:14 +0000238 net_set_state(NETLOOP_SUCCESS);
Joe Hershbergerf575ae12012-05-23 07:57:59 +0000239}
240
Joe Hershberger0b4c5ff2012-05-23 07:58:13 +0000241void cdp_receive(const uchar *pkt, unsigned len)
Joe Hershbergerf575ae12012-05-23 07:57:59 +0000242{
243 const uchar *t;
244 const ushort *ss;
245 ushort type, tlen;
246 ushort vlan, nvlan;
247
248 /* minimum size? */
249 if (len < sizeof(CDP_SNAP_hdr) + 4)
250 goto pkt_short;
251
252 /* check for valid CDP SNAP header */
253 if (memcmp(pkt, CDP_SNAP_hdr, sizeof(CDP_SNAP_hdr)) != 0)
254 return;
255
256 pkt += sizeof(CDP_SNAP_hdr);
257 len -= sizeof(CDP_SNAP_hdr);
258
259 /* Version of CDP protocol must be >= 2 and TTL != 0 */
260 if (pkt[0] < 0x02 || pkt[1] == 0)
261 return;
262
263 /*
264 * if version is greater than 0x02 maybe we'll have a problem;
265 * output a warning
266 */
267 if (pkt[0] != 0x02)
268 printf("**WARNING: CDP packet received with a protocol version "
269 "%d > 2\n", pkt[0] & 0xff);
270
271 if (CDP_compute_csum(pkt, len) != 0)
272 return;
273
274 pkt += 4;
275 len -= 4;
276
277 vlan = htons(-1);
278 nvlan = htons(-1);
279 while (len > 0) {
280 if (len < 4)
281 goto pkt_short;
282
283 ss = (const ushort *)pkt;
284 type = ntohs(ss[0]);
285 tlen = ntohs(ss[1]);
286 if (tlen > len)
287 goto pkt_short;
288
289 pkt += tlen;
290 len -= tlen;
291
292 ss += 2; /* point ss to the data of the TLV */
293 tlen -= 4;
294
295 switch (type) {
296 case CDP_DEVICE_ID_TLV:
297 break;
298 case CDP_ADDRESS_TLV:
299 break;
300 case CDP_PORT_ID_TLV:
301 break;
302 case CDP_CAPABILITIES_TLV:
303 break;
304 case CDP_VERSION_TLV:
305 break;
306 case CDP_PLATFORM_TLV:
307 break;
308 case CDP_NATIVE_VLAN_TLV:
309 nvlan = *ss;
310 break;
311 case CDP_APPLIANCE_VLAN_TLV:
312 t = (const uchar *)ss;
313 while (tlen > 0) {
314 if (tlen < 3)
315 goto pkt_short;
316
317 ss = (const ushort *)(t + 1);
318
319#ifdef CONFIG_CDP_APPLIANCE_VLAN_TYPE
320 if (t[0] == CONFIG_CDP_APPLIANCE_VLAN_TYPE)
321 vlan = *ss;
322#else
323 /* XXX will this work; dunno */
324 vlan = ntohs(*ss);
325#endif
326 t += 3; tlen -= 3;
327 }
328 break;
329 case CDP_TRIGGER_TLV:
330 break;
331 case CDP_POWER_CONSUMPTION_TLV:
332 break;
333 case CDP_SYSNAME_TLV:
334 break;
335 case CDP_SYSOBJECT_TLV:
336 break;
337 case CDP_MANAGEMENT_ADDRESS_TLV:
338 break;
339 }
340 }
341
342 CDPApplianceVLAN = vlan;
343 CDPNativeVLAN = nvlan;
344
345 CDPOK = 1;
346 return;
347
348 pkt_short:
349 printf("** CDP packet is too short\n");
350 return;
351}
352
353void
354CDPStart(void)
355{
356 printf("Using %s device\n", eth_get_name());
357 CDPSeq = 0;
358 CDPOK = 0;
359
360 CDPNativeVLAN = htons(-1);
361 CDPApplianceVLAN = htons(-1);
362
363 NetSetTimeout(CDP_TIMEOUT, CDPTimeout);
Joe Hershbergerf575ae12012-05-23 07:57:59 +0000364
365 CDPSendTrigger();
366}