1 // SPDX-License-Identifier: GPL-2.0-only
2 /*
3 * Copied from Linux Monitor (LiMon) - Networking.
4 *
5 * Copyright 1994 - 2000 Neil Russell.
6 * Copyright 2000 Roland Borde
7 * Copyright 2000 Paolo Scaffardi
8 * Copyright 2000-2002 Wolfgang Denk, wd@denx.de
9 */
10
11 #include <net.h>
12
13 #include "cdp.h"
14
15 /* Ethernet bcast address */
16 const u8 net_cdp_ethaddr[6] = { 0x01, 0x00, 0x0c, 0xcc, 0xcc, 0xcc };
17
18 #define CDP_DEVICE_ID_TLV 0x0001
19 #define CDP_ADDRESS_TLV 0x0002
20 #define CDP_PORT_ID_TLV 0x0003
21 #define CDP_CAPABILITIES_TLV 0x0004
22 #define CDP_VERSION_TLV 0x0005
23 #define CDP_PLATFORM_TLV 0x0006
24 #define CDP_NATIVE_VLAN_TLV 0x000a
25 #define CDP_APPLIANCE_VLAN_TLV 0x000e
26 #define CDP_TRIGGER_TLV 0x000f
27 #define CDP_POWER_CONSUMPTION_TLV 0x0010
28 #define CDP_SYSNAME_TLV 0x0014
29 #define CDP_SYSOBJECT_TLV 0x0015
30 #define CDP_MANAGEMENT_ADDRESS_TLV 0x0016
31
32 #define CDP_TIMEOUT 250UL /* one packet every 250ms */
33
34 static int cdp_seq;
35 static int cdp_ok;
36
37 ushort cdp_native_vlan;
38 ushort cdp_appliance_vlan;
39
40 static const uchar cdp_snap_hdr[8] = {
41 0xAA, 0xAA, 0x03, 0x00, 0x00, 0x0C, 0x20, 0x00 };
42
cdp_compute_csum(const uchar * buff,ushort len)43 static ushort cdp_compute_csum(const uchar *buff, ushort len)
44 {
45 ushort csum;
46 int odd;
47 ulong result = 0;
48 ushort leftover;
49 ushort *p;
50
51 if (len > 0) {
52 odd = 1 & (ulong)buff;
53 if (odd) {
54 result = *buff << 8;
55 len--;
56 buff++;
57 }
58 while (len > 1) {
59 p = (ushort *)buff;
60 result += *p++;
61 buff = (uchar *)p;
62 if (result & 0x80000000)
63 result = (result & 0xFFFF) + (result >> 16);
64 len -= 2;
65 }
66 if (len) {
67 leftover = (signed short)(*(const signed char *)buff);
68 /*
69 * CISCO SUCKS big time! (and blows too):
70 * CDP uses the IP checksum algorithm with a twist;
71 * for the last byte it *sign* extends and sums.
72 */
73 result = (result & 0xffff0000) |
74 ((result + leftover) & 0x0000ffff);
75 }
76 while (result >> 16)
77 result = (result & 0xFFFF) + (result >> 16);
78
79 if (odd)
80 result = ((result >> 8) & 0xff) |
81 ((result & 0xff) << 8);
82 }
83
84 /* add up 16-bit and 17-bit words for 17+c bits */
85 result = (result & 0xffff) + (result >> 16);
86 /* add up 16-bit and 2-bit for 16+c bit */
87 result = (result & 0xffff) + (result >> 16);
88 /* add up carry.. */
89 result = (result & 0xffff) + (result >> 16);
90
91 /* negate */
92 csum = ~(ushort)result;
93
94 /* run time endian detection */
95 if (csum != htons(csum)) /* little endian */
96 csum = htons(csum);
97
98 return csum;
99 }
100
cdp_send_trigger(void)101 static int cdp_send_trigger(void)
102 {
103 uchar *pkt;
104 ushort *s;
105 ushort *cp;
106 struct ethernet_hdr *et;
107 int len;
108 ushort chksum;
109 #if defined(CONFIG_CDP_DEVICE_ID) || defined(CONFIG_CDP_PORT_ID) || \
110 defined(CONFIG_CDP_VERSION) || defined(CONFIG_CDP_PLATFORM)
111 char buf[32];
112 #endif
113
114 pkt = net_tx_packet;
115 et = (struct ethernet_hdr *)pkt;
116
117 /* NOTE: trigger sent not on any VLAN */
118
119 /* form ethernet header */
120 memcpy(et->et_dest, net_cdp_ethaddr, 6);
121 memcpy(et->et_src, net_ethaddr, 6);
122
123 pkt += ETHER_HDR_SIZE;
124
125 /* SNAP header */
126 memcpy((uchar *)pkt, cdp_snap_hdr, sizeof(cdp_snap_hdr));
127 pkt += sizeof(cdp_snap_hdr);
128
129 /* CDP header */
130 *pkt++ = 0x02; /* CDP version 2 */
131 *pkt++ = 180; /* TTL */
132 s = (ushort *)pkt;
133 cp = s;
134 /* checksum (0 for later calculation) */
135 *s++ = htons(0);
136
137 /* CDP fields */
138 #ifdef CONFIG_CDP_DEVICE_ID
139 *s++ = htons(CDP_DEVICE_ID_TLV);
140 *s++ = htons(CONFIG_CDP_DEVICE_ID);
141 sprintf(buf, CONFIG_CDP_DEVICE_ID_PREFIX "%pm", net_ethaddr);
142 memcpy((uchar *)s, buf, 16);
143 s += 16 / 2;
144 #endif
145
146 #ifdef CONFIG_CDP_PORT_ID
147 *s++ = htons(CDP_PORT_ID_TLV);
148 memset(buf, 0, sizeof(buf));
149 sprintf(buf, CONFIG_CDP_PORT_ID, eth_get_dev_index());
150 len = strlen(buf);
151 if (len & 1) /* make it even */
152 len++;
153 *s++ = htons(len + 4);
154 memcpy((uchar *)s, buf, len);
155 s += len / 2;
156 #endif
157
158 #ifdef CONFIG_CDP_CAPABILITIES
159 *s++ = htons(CDP_CAPABILITIES_TLV);
160 *s++ = htons(8);
161 *(ulong *)s = htonl(CONFIG_CDP_CAPABILITIES);
162 s += 2;
163 #endif
164
165 #ifdef CONFIG_CDP_VERSION
166 *s++ = htons(CDP_VERSION_TLV);
167 memset(buf, 0, sizeof(buf));
168 strcpy(buf, CONFIG_CDP_VERSION);
169 len = strlen(buf);
170 if (len & 1) /* make it even */
171 len++;
172 *s++ = htons(len + 4);
173 memcpy((uchar *)s, buf, len);
174 s += len / 2;
175 #endif
176
177 #ifdef CONFIG_CDP_PLATFORM
178 *s++ = htons(CDP_PLATFORM_TLV);
179 memset(buf, 0, sizeof(buf));
180 strcpy(buf, CONFIG_CDP_PLATFORM);
181 len = strlen(buf);
182 if (len & 1) /* make it even */
183 len++;
184 *s++ = htons(len + 4);
185 memcpy((uchar *)s, buf, len);
186 s += len / 2;
187 #endif
188
189 #ifdef CONFIG_CDP_TRIGGER
190 *s++ = htons(CDP_TRIGGER_TLV);
191 *s++ = htons(8);
192 *(ulong *)s = htonl(CONFIG_CDP_TRIGGER);
193 s += 2;
194 #endif
195
196 #ifdef CONFIG_CDP_POWER_CONSUMPTION
197 *s++ = htons(CDP_POWER_CONSUMPTION_TLV);
198 *s++ = htons(6);
199 *s++ = htons(CONFIG_CDP_POWER_CONSUMPTION);
200 #endif
201
202 /* length of ethernet packet */
203 len = (uchar *)s - ((uchar *)net_tx_packet + ETHER_HDR_SIZE);
204 et->et_protlen = htons(len);
205
206 len = ETHER_HDR_SIZE + sizeof(cdp_snap_hdr);
207 chksum = cdp_compute_csum((uchar *)net_tx_packet + len,
208 (uchar *)s - (net_tx_packet + len));
209 if (chksum == 0)
210 chksum = 0xFFFF;
211 *cp = htons(chksum);
212
213 net_send_packet(net_tx_packet, (uchar *)s - net_tx_packet);
214 return 0;
215 }
216
cdp_timeout_handler(void)217 static void cdp_timeout_handler(void)
218 {
219 cdp_seq++;
220
221 if (cdp_seq < 3) {
222 net_set_timeout_handler(CDP_TIMEOUT, cdp_timeout_handler);
223 cdp_send_trigger();
224 return;
225 }
226
227 /* if not OK try again */
228 if (!cdp_ok)
229 net_start_again();
230 else
231 net_set_state(NETLOOP_SUCCESS);
232 }
233
cdp_receive(const uchar * pkt,unsigned len)234 void cdp_receive(const uchar *pkt, unsigned len)
235 {
236 const uchar *t;
237 const ushort *ss;
238 ushort type, tlen;
239 ushort vlan, nvlan;
240
241 /* minimum size? */
242 if (len < sizeof(cdp_snap_hdr) + 4)
243 goto pkt_short;
244
245 /* check for valid CDP SNAP header */
246 if (memcmp(pkt, cdp_snap_hdr, sizeof(cdp_snap_hdr)) != 0)
247 return;
248
249 pkt += sizeof(cdp_snap_hdr);
250 len -= sizeof(cdp_snap_hdr);
251
252 /* Version of CDP protocol must be >= 2 and TTL != 0 */
253 if (pkt[0] < 0x02 || pkt[1] == 0)
254 return;
255
256 /*
257 * if version is greater than 0x02 maybe we'll have a problem;
258 * output a warning
259 */
260 if (pkt[0] != 0x02)
261 printf("**WARNING: CDP packet received with a protocol version "
262 "%d > 2\n", pkt[0] & 0xff);
263
264 if (cdp_compute_csum(pkt, len) != 0)
265 return;
266
267 pkt += 4;
268 len -= 4;
269
270 vlan = htons(-1);
271 nvlan = htons(-1);
272 while (len > 0) {
273 if (len < 4)
274 goto pkt_short;
275
276 ss = (const ushort *)pkt;
277 type = ntohs(ss[0]);
278 tlen = ntohs(ss[1]);
279 if (tlen > len)
280 goto pkt_short;
281
282 pkt += tlen;
283 len -= tlen;
284
285 ss += 2; /* point ss to the data of the TLV */
286 tlen -= 4;
287
288 switch (type) {
289 case CDP_DEVICE_ID_TLV:
290 break;
291 case CDP_ADDRESS_TLV:
292 break;
293 case CDP_PORT_ID_TLV:
294 break;
295 case CDP_CAPABILITIES_TLV:
296 break;
297 case CDP_VERSION_TLV:
298 break;
299 case CDP_PLATFORM_TLV:
300 break;
301 case CDP_NATIVE_VLAN_TLV:
302 nvlan = *ss;
303 break;
304 case CDP_APPLIANCE_VLAN_TLV:
305 t = (const uchar *)ss;
306 while (tlen > 0) {
307 if (tlen < 3)
308 goto pkt_short;
309
310 ss = (const ushort *)(t + 1);
311
312 #ifdef CONFIG_CDP_APPLIANCE_VLAN_TYPE
313 if (t[0] == CONFIG_CDP_APPLIANCE_VLAN_TYPE)
314 vlan = *ss;
315 #else
316 /* XXX will this work; dunno */
317 vlan = ntohs(*ss);
318 #endif
319 t += 3; tlen -= 3;
320 }
321 break;
322 case CDP_TRIGGER_TLV:
323 break;
324 case CDP_POWER_CONSUMPTION_TLV:
325 break;
326 case CDP_SYSNAME_TLV:
327 break;
328 case CDP_SYSOBJECT_TLV:
329 break;
330 case CDP_MANAGEMENT_ADDRESS_TLV:
331 break;
332 }
333 }
334
335 cdp_appliance_vlan = vlan;
336 cdp_native_vlan = nvlan;
337
338 cdp_ok = 1;
339 return;
340
341 pkt_short:
342 printf("** CDP packet is too short\n");
343 return;
344 }
345
cdp_start(void)346 void cdp_start(void)
347 {
348 printf("Using %s device\n", eth_get_name());
349 cdp_seq = 0;
350 cdp_ok = 0;
351
352 cdp_native_vlan = htons(-1);
353 cdp_appliance_vlan = htons(-1);
354
355 net_set_timeout_handler(CDP_TIMEOUT, cdp_timeout_handler);
356
357 cdp_send_trigger();
358 }
359