xref: /openbmc/u-boot/net/cdp.c (revision 867a6ac86dd823de409752e9ca5dc8f4d4880f26)
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  }
364