]>
Commit | Line | Data |
---|---|---|
f575ae1f JH |
1 | /* |
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, [email protected] | |
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 */ | |
20 | const 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 | ||
38 | static int CDPSeq; | |
39 | static int CDPOK; | |
40 | ||
41 | ushort CDPNativeVLAN; | |
42 | ushort CDPApplianceVLAN; | |
43 | ||
44 | static const uchar CDP_SNAP_hdr[8] = { | |
45 | 0xAA, 0xAA, 0x03, 0x00, 0x00, 0x0C, 0x20, 0x00 }; | |
46 | ||
47 | static ushort | |
48 | CDP_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 | ||
106 | static int | |
107 | CDPSendTrigger(void) | |
108 | { | |
109 | uchar *pkt; | |
110 | ushort *s; | |
111 | ushort *cp; | |
cb487f56 | 112 | struct ethernet_hdr *et; |
f575ae1f JH |
113 | 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; | |
cb487f56 | 121 | et = (struct ethernet_hdr *)pkt; |
f575ae1f JH |
122 | |
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 | ||
adf5d93e | 219 | NetSendPacket(NetTxPacket, (uchar *)s - NetTxPacket); |
f575ae1f JH |
220 | return 0; |
221 | } | |
222 | ||
223 | static void | |
224 | CDPTimeout(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 | |
22f6e99d | 238 | net_set_state(NETLOOP_SUCCESS); |
f575ae1f JH |
239 | } |
240 | ||
0b4c5ff4 | 241 | void cdp_receive(const uchar *pkt, unsigned len) |
f575ae1f JH |
242 | { |
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 | ||
353 | void | |
354 | CDPStart(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); | |
f575ae1f JH |
364 | |
365 | CDPSendTrigger(); | |
366 | } |