blob: e8e7a67010b181eef59dc45165c82807360ebcb0 [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
39static int CDPSeq;
40static int CDPOK;
41
42ushort CDPNativeVLAN;
43ushort CDPApplianceVLAN;
44
45static const uchar CDP_SNAP_hdr[8] = {
46 0xAA, 0xAA, 0x03, 0x00, 0x00, 0x0C, 0x20, 0x00 };
47
48static ushort
49CDP_compute_csum(const uchar *buff, ushort len)
50{
51 ushort csum;
52 int odd;
53 ulong result = 0;
54 ushort leftover;
55 ushort *p;
56
57 if (len > 0) {
58 odd = 1 & (ulong)buff;
59 if (odd) {
60 result = *buff << 8;
61 len--;
62 buff++;
63 }
64 while (len > 1) {
65 p = (ushort *)buff;
66 result += *p++;
67 buff = (uchar *)p;
68 if (result & 0x80000000)
69 result = (result & 0xFFFF) + (result >> 16);
70 len -= 2;
71 }
72 if (len) {
73 leftover = (signed short)(*(const signed char *)buff);
74 /*
75 * CISCO SUCKS big time! (and blows too):
76 * CDP uses the IP checksum algorithm with a twist;
77 * for the last byte it *sign* extends and sums.
78 */
79 result = (result & 0xffff0000) |
80 ((result + leftover) & 0x0000ffff);
81 }
82 while (result >> 16)
83 result = (result & 0xFFFF) + (result >> 16);
84
85 if (odd)
86 result = ((result >> 8) & 0xff) |
87 ((result & 0xff) << 8);
88 }
89
90 /* add up 16-bit and 17-bit words for 17+c bits */
91 result = (result & 0xffff) + (result >> 16);
92 /* add up 16-bit and 2-bit for 16+c bit */
93 result = (result & 0xffff) + (result >> 16);
94 /* add up carry.. */
95 result = (result & 0xffff) + (result >> 16);
96
97 /* negate */
98 csum = ~(ushort)result;
99
100 /* run time endian detection */
101 if (csum != htons(csum)) /* little endian */
102 csum = htons(csum);
103
104 return csum;
105}
106
107static int
108CDPSendTrigger(void)
109{
110 uchar *pkt;
111 ushort *s;
112 ushort *cp;
Joe Hershbergercb487f52012-05-23 07:58:06 +0000113 struct ethernet_hdr *et;
Joe Hershbergerf575ae12012-05-23 07:57:59 +0000114 int len;
115 ushort chksum;
116#if defined(CONFIG_CDP_DEVICE_ID) || defined(CONFIG_CDP_PORT_ID) || \
117 defined(CONFIG_CDP_VERSION) || defined(CONFIG_CDP_PLATFORM)
118 char buf[32];
119#endif
120
121 pkt = NetTxPacket;
Joe Hershbergercb487f52012-05-23 07:58:06 +0000122 et = (struct ethernet_hdr *)pkt;
Joe Hershbergerf575ae12012-05-23 07:57:59 +0000123
124 /* NOTE: trigger sent not on any VLAN */
125
126 /* form ethernet header */
Joe Hershberger0adb5b72015-04-08 01:41:04 -0500127 memcpy(et->et_dest, net_cdp_ethaddr, 6);
128 memcpy(et->et_src, net_ethaddr, 6);
Joe Hershbergerf575ae12012-05-23 07:57:59 +0000129
130 pkt += ETHER_HDR_SIZE;
131
132 /* SNAP header */
133 memcpy((uchar *)pkt, CDP_SNAP_hdr, sizeof(CDP_SNAP_hdr));
134 pkt += sizeof(CDP_SNAP_hdr);
135
136 /* CDP header */
137 *pkt++ = 0x02; /* CDP version 2 */
138 *pkt++ = 180; /* TTL */
139 s = (ushort *)pkt;
140 cp = s;
141 /* checksum (0 for later calculation) */
142 *s++ = htons(0);
143
144 /* CDP fields */
145#ifdef CONFIG_CDP_DEVICE_ID
146 *s++ = htons(CDP_DEVICE_ID_TLV);
147 *s++ = htons(CONFIG_CDP_DEVICE_ID);
Joe Hershberger0adb5b72015-04-08 01:41:04 -0500148 sprintf(buf, CONFIG_CDP_DEVICE_ID_PREFIX "%pm", net_ethaddr);
Joe Hershbergerf575ae12012-05-23 07:57:59 +0000149 memcpy((uchar *)s, buf, 16);
150 s += 16 / 2;
151#endif
152
153#ifdef CONFIG_CDP_PORT_ID
154 *s++ = htons(CDP_PORT_ID_TLV);
155 memset(buf, 0, sizeof(buf));
156 sprintf(buf, CONFIG_CDP_PORT_ID, eth_get_dev_index());
157 len = strlen(buf);
158 if (len & 1) /* make it even */
159 len++;
160 *s++ = htons(len + 4);
161 memcpy((uchar *)s, buf, len);
162 s += len / 2;
163#endif
164
165#ifdef CONFIG_CDP_CAPABILITIES
166 *s++ = htons(CDP_CAPABILITIES_TLV);
167 *s++ = htons(8);
168 *(ulong *)s = htonl(CONFIG_CDP_CAPABILITIES);
169 s += 2;
170#endif
171
172#ifdef CONFIG_CDP_VERSION
173 *s++ = htons(CDP_VERSION_TLV);
174 memset(buf, 0, sizeof(buf));
175 strcpy(buf, CONFIG_CDP_VERSION);
176 len = strlen(buf);
177 if (len & 1) /* make it even */
178 len++;
179 *s++ = htons(len + 4);
180 memcpy((uchar *)s, buf, len);
181 s += len / 2;
182#endif
183
184#ifdef CONFIG_CDP_PLATFORM
185 *s++ = htons(CDP_PLATFORM_TLV);
186 memset(buf, 0, sizeof(buf));
187 strcpy(buf, CONFIG_CDP_PLATFORM);
188 len = strlen(buf);
189 if (len & 1) /* make it even */
190 len++;
191 *s++ = htons(len + 4);
192 memcpy((uchar *)s, buf, len);
193 s += len / 2;
194#endif
195
196#ifdef CONFIG_CDP_TRIGGER
197 *s++ = htons(CDP_TRIGGER_TLV);
198 *s++ = htons(8);
199 *(ulong *)s = htonl(CONFIG_CDP_TRIGGER);
200 s += 2;
201#endif
202
203#ifdef CONFIG_CDP_POWER_CONSUMPTION
204 *s++ = htons(CDP_POWER_CONSUMPTION_TLV);
205 *s++ = htons(6);
206 *s++ = htons(CONFIG_CDP_POWER_CONSUMPTION);
207#endif
208
209 /* length of ethernet packet */
210 len = (uchar *)s - ((uchar *)NetTxPacket + ETHER_HDR_SIZE);
211 et->et_protlen = htons(len);
212
213 len = ETHER_HDR_SIZE + sizeof(CDP_SNAP_hdr);
214 chksum = CDP_compute_csum((uchar *)NetTxPacket + len,
215 (uchar *)s - (NetTxPacket + len));
216 if (chksum == 0)
217 chksum = 0xFFFF;
218 *cp = htons(chksum);
219
Joe Hershbergeradf5d932012-05-23 07:59:13 +0000220 NetSendPacket(NetTxPacket, (uchar *)s - NetTxPacket);
Joe Hershbergerf575ae12012-05-23 07:57:59 +0000221 return 0;
222}
223
224static void
225CDPTimeout(void)
226{
227 CDPSeq++;
228
229 if (CDPSeq < 3) {
230 NetSetTimeout(CDP_TIMEOUT, CDPTimeout);
231 CDPSendTrigger();
232 return;
233 }
234
235 /* if not OK try again */
236 if (!CDPOK)
237 NetStartAgain();
238 else
Joe Hershberger22f6e992012-05-23 07:59:14 +0000239 net_set_state(NETLOOP_SUCCESS);
Joe Hershbergerf575ae12012-05-23 07:57:59 +0000240}
241
Joe Hershberger0b4c5ff2012-05-23 07:58:13 +0000242void cdp_receive(const uchar *pkt, unsigned len)
Joe Hershbergerf575ae12012-05-23 07:57:59 +0000243{
244 const uchar *t;
245 const ushort *ss;
246 ushort type, tlen;
247 ushort vlan, nvlan;
248
249 /* minimum size? */
250 if (len < sizeof(CDP_SNAP_hdr) + 4)
251 goto pkt_short;
252
253 /* check for valid CDP SNAP header */
254 if (memcmp(pkt, CDP_SNAP_hdr, sizeof(CDP_SNAP_hdr)) != 0)
255 return;
256
257 pkt += sizeof(CDP_SNAP_hdr);
258 len -= sizeof(CDP_SNAP_hdr);
259
260 /* Version of CDP protocol must be >= 2 and TTL != 0 */
261 if (pkt[0] < 0x02 || pkt[1] == 0)
262 return;
263
264 /*
265 * if version is greater than 0x02 maybe we'll have a problem;
266 * output a warning
267 */
268 if (pkt[0] != 0x02)
269 printf("**WARNING: CDP packet received with a protocol version "
270 "%d > 2\n", pkt[0] & 0xff);
271
272 if (CDP_compute_csum(pkt, len) != 0)
273 return;
274
275 pkt += 4;
276 len -= 4;
277
278 vlan = htons(-1);
279 nvlan = htons(-1);
280 while (len > 0) {
281 if (len < 4)
282 goto pkt_short;
283
284 ss = (const ushort *)pkt;
285 type = ntohs(ss[0]);
286 tlen = ntohs(ss[1]);
287 if (tlen > len)
288 goto pkt_short;
289
290 pkt += tlen;
291 len -= tlen;
292
293 ss += 2; /* point ss to the data of the TLV */
294 tlen -= 4;
295
296 switch (type) {
297 case CDP_DEVICE_ID_TLV:
298 break;
299 case CDP_ADDRESS_TLV:
300 break;
301 case CDP_PORT_ID_TLV:
302 break;
303 case CDP_CAPABILITIES_TLV:
304 break;
305 case CDP_VERSION_TLV:
306 break;
307 case CDP_PLATFORM_TLV:
308 break;
309 case CDP_NATIVE_VLAN_TLV:
310 nvlan = *ss;
311 break;
312 case CDP_APPLIANCE_VLAN_TLV:
313 t = (const uchar *)ss;
314 while (tlen > 0) {
315 if (tlen < 3)
316 goto pkt_short;
317
318 ss = (const ushort *)(t + 1);
319
320#ifdef CONFIG_CDP_APPLIANCE_VLAN_TYPE
321 if (t[0] == CONFIG_CDP_APPLIANCE_VLAN_TYPE)
322 vlan = *ss;
323#else
324 /* XXX will this work; dunno */
325 vlan = ntohs(*ss);
326#endif
327 t += 3; tlen -= 3;
328 }
329 break;
330 case CDP_TRIGGER_TLV:
331 break;
332 case CDP_POWER_CONSUMPTION_TLV:
333 break;
334 case CDP_SYSNAME_TLV:
335 break;
336 case CDP_SYSOBJECT_TLV:
337 break;
338 case CDP_MANAGEMENT_ADDRESS_TLV:
339 break;
340 }
341 }
342
343 CDPApplianceVLAN = vlan;
344 CDPNativeVLAN = nvlan;
345
346 CDPOK = 1;
347 return;
348
349 pkt_short:
350 printf("** CDP packet is too short\n");
351 return;
352}
353
354void
355CDPStart(void)
356{
357 printf("Using %s device\n", eth_get_name());
358 CDPSeq = 0;
359 CDPOK = 0;
360
361 CDPNativeVLAN = htons(-1);
362 CDPApplianceVLAN = htons(-1);
363
364 NetSetTimeout(CDP_TIMEOUT, CDPTimeout);
Joe Hershbergerf575ae12012-05-23 07:57:59 +0000365
366 CDPSendTrigger();
367}