]>
Commit | Line | Data |
---|---|---|
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, wd@denx.de | |
9 | * SPDX-License-Identifier: GPL-2.0 | |
10 | */ | |
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 */ | |
21 | const u8 net_cdp_ethaddr[6] = { 0x01, 0x00, 0x0c, 0xcc, 0xcc, 0xcc }; | |
22 | ||
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 | ||
39 | static int cdp_seq; | |
40 | static int cdp_ok; | |
41 | ||
42 | ushort cdp_native_vlan; | |
43 | ushort cdp_appliance_vlan; | |
44 | ||
45 | static const uchar cdp_snap_hdr[8] = { | |
46 | 0xAA, 0xAA, 0x03, 0x00, 0x00, 0x0C, 0x20, 0x00 }; | |
47 | ||
48 | static ushort 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 cdp_send_trigger(void) | |
107 | { | |
108 | uchar *pkt; | |
109 | ushort *s; | |
110 | ushort *cp; | |
111 | struct ethernet_hdr *et; | |
112 | 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 | ||
119 | pkt = net_tx_packet; | |
120 | et = (struct ethernet_hdr *)pkt; | |
121 | ||
122 | /* NOTE: trigger sent not on any VLAN */ | |
123 | ||
124 | /* form ethernet header */ | |
125 | memcpy(et->et_dest, net_cdp_ethaddr, 6); | |
126 | memcpy(et->et_src, net_ethaddr, 6); | |
127 | ||
128 | pkt += ETHER_HDR_SIZE; | |
129 | ||
130 | /* SNAP header */ | |
131 | memcpy((uchar *)pkt, cdp_snap_hdr, sizeof(cdp_snap_hdr)); | |
132 | pkt += sizeof(cdp_snap_hdr); | |
133 | ||
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); | |
146 | sprintf(buf, CONFIG_CDP_DEVICE_ID_PREFIX "%pm", net_ethaddr); | |
147 | 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 */ | |
208 | len = (uchar *)s - ((uchar *)net_tx_packet + ETHER_HDR_SIZE); | |
209 | et->et_protlen = htons(len); | |
210 | ||
211 | len = ETHER_HDR_SIZE + sizeof(cdp_snap_hdr); | |
212 | chksum = cdp_compute_csum((uchar *)net_tx_packet + len, | |
213 | (uchar *)s - (net_tx_packet + len)); | |
214 | if (chksum == 0) | |
215 | chksum = 0xFFFF; | |
216 | *cp = htons(chksum); | |
217 | ||
218 | net_send_packet(net_tx_packet, (uchar *)s - net_tx_packet); | |
219 | return 0; | |
220 | } | |
221 | ||
222 | static void cdp_timeout_handler(void) | |
223 | { | |
224 | cdp_seq++; | |
225 | ||
226 | if (cdp_seq < 3) { | |
227 | net_set_timeout_handler(CDP_TIMEOUT, cdp_timeout_handler); | |
228 | cdp_send_trigger(); | |
229 | return; | |
230 | } | |
231 | ||
232 | /* if not OK try again */ | |
233 | if (!cdp_ok) | |
234 | net_start_again(); | |
235 | else | |
236 | net_set_state(NETLOOP_SUCCESS); | |
237 | } | |
238 | ||
239 | void cdp_receive(const uchar *pkt, unsigned len) | |
240 | { | |
241 | const uchar *t; | |
242 | const ushort *ss; | |
243 | ushort type, tlen; | |
244 | ushort vlan, nvlan; | |
245 | ||
246 | /* minimum size? */ | |
247 | if (len < sizeof(cdp_snap_hdr) + 4) | |
248 | goto pkt_short; | |
249 | ||
250 | /* check for valid CDP SNAP header */ | |
251 | if (memcmp(pkt, cdp_snap_hdr, sizeof(cdp_snap_hdr)) != 0) | |
252 | return; | |
253 | ||
254 | pkt += sizeof(cdp_snap_hdr); | |
255 | len -= sizeof(cdp_snap_hdr); | |
256 | ||
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 | ||
269 | if (cdp_compute_csum(pkt, len) != 0) | |
270 | 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 | ||
340 | cdp_appliance_vlan = vlan; | |
341 | cdp_native_vlan = nvlan; | |
342 | ||
343 | cdp_ok = 1; | |
344 | return; | |
345 | ||
346 | pkt_short: | |
347 | printf("** CDP packet is too short\n"); | |
348 | return; | |
349 | } | |
350 | ||
351 | void cdp_start(void) | |
352 | { | |
353 | printf("Using %s device\n", eth_get_name()); | |
354 | cdp_seq = 0; | |
355 | cdp_ok = 0; | |
356 | ||
357 | cdp_native_vlan = htons(-1); | |
358 | cdp_appliance_vlan = htons(-1); | |
359 | ||
360 | net_set_timeout_handler(CDP_TIMEOUT, cdp_timeout_handler); | |
361 | ||
362 | cdp_send_trigger(); | |
363 | } |