[people/ms/u-boot.git] / net / cdp.c

/*
 *	Copied from Linux Monitor (LiMon) - Networking.
 *
 *	Copyright 1994 - 2000 Neil Russell.
 *	(See License)
 *	Copyright 2000 Roland Borde
 *	Copyright 2000 Paolo Scaffardi
 *	Copyright 2000-2002 Wolfgang Denk, wd@denx.de
 */

#include <common.h>
#include <net.h>
#if defined(CONFIG_CDP_VERSION)
#include <timestamp.h>
#endif

#include "cdp.h"

/* Ethernet bcast address */
const uchar NetCDPAddr[6] = { 0x01, 0x00, 0x0c, 0xcc, 0xcc, 0xcc };

#define CDP_DEVICE_ID_TLV		0x0001
#define CDP_ADDRESS_TLV			0x0002
#define CDP_PORT_ID_TLV			0x0003
#define CDP_CAPABILITIES_TLV		0x0004
#define CDP_VERSION_TLV			0x0005
#define CDP_PLATFORM_TLV		0x0006
#define CDP_NATIVE_VLAN_TLV		0x000a
#define CDP_APPLIANCE_VLAN_TLV		0x000e
#define CDP_TRIGGER_TLV			0x000f
#define CDP_POWER_CONSUMPTION_TLV	0x0010
#define CDP_SYSNAME_TLV			0x0014
#define CDP_SYSOBJECT_TLV		0x0015
#define CDP_MANAGEMENT_ADDRESS_TLV	0x0016

#define CDP_TIMEOUT			250UL	/* one packet every 250ms */

static int CDPSeq;
static int CDPOK;

ushort CDPNativeVLAN;
ushort CDPApplianceVLAN;

static const uchar CDP_SNAP_hdr[8] = {
	0xAA, 0xAA, 0x03, 0x00, 0x00, 0x0C, 0x20, 0x00 };

static ushort
CDP_compute_csum(const uchar *buff, ushort len)
{
	ushort csum;
	int     odd;
	ulong   result = 0;
	ushort  leftover;
	ushort *p;

	if (len > 0) {
		odd = 1 & (ulong)buff;
		if (odd) {
			result = *buff << 8;
			len--;
			buff++;
		}
		while (len > 1) {
			p = (ushort *)buff;
			result += *p++;
			buff = (uchar *)p;
			if (result & 0x80000000)
				result = (result & 0xFFFF) + (result >> 16);
			len -= 2;
		}
		if (len) {
			leftover = (signed short)(*(const signed char *)buff);
			/*
			 * CISCO SUCKS big time! (and blows too):
			 * CDP uses the IP checksum algorithm with a twist;
			 * for the last byte it *sign* extends and sums.
			 */
			result = (result & 0xffff0000) |
				 ((result + leftover) & 0x0000ffff);
		}
		while (result >> 16)
			result = (result & 0xFFFF) + (result >> 16);

		if (odd)
			result = ((result >> 8) & 0xff) |
				 ((result & 0xff) << 8);
	}

	/* add up 16-bit and 17-bit words for 17+c bits */
	result = (result & 0xffff) + (result >> 16);
	/* add up 16-bit and 2-bit for 16+c bit */
	result = (result & 0xffff) + (result >> 16);
	/* add up carry.. */
	result = (result & 0xffff) + (result >> 16);

	/* negate */
	csum = ~(ushort)result;

	/* run time endian detection */
	if (csum != htons(csum))	/* little endian */
		csum = htons(csum);

	return csum;
}

static int
CDPSendTrigger(void)
{
	uchar *pkt;
	ushort *s;
	ushort *cp;
	struct ethernet_hdr *et;
	int len;
	ushort chksum;
#if	defined(CONFIG_CDP_DEVICE_ID) || defined(CONFIG_CDP_PORT_ID)   || \
	defined(CONFIG_CDP_VERSION)   || defined(CONFIG_CDP_PLATFORM)
	char buf[32];
#endif

	pkt = NetTxPacket;
	et = (struct ethernet_hdr *)pkt;

	/* NOTE: trigger sent not on any VLAN */

	/* form ethernet header */
	memcpy(et->et_dest, NetCDPAddr, 6);
	memcpy(et->et_src, NetOurEther, 6);

	pkt += ETHER_HDR_SIZE;

	/* SNAP header */
	memcpy((uchar *)pkt, CDP_SNAP_hdr, sizeof(CDP_SNAP_hdr));
	pkt += sizeof(CDP_SNAP_hdr);

	/* CDP header */
	*pkt++ = 0x02;				/* CDP version 2 */
	*pkt++ = 180;				/* TTL */
	s = (ushort *)pkt;
	cp = s;
	/* checksum (0 for later calculation) */
	*s++ = htons(0);

	/* CDP fields */
#ifdef CONFIG_CDP_DEVICE_ID
	*s++ = htons(CDP_DEVICE_ID_TLV);
	*s++ = htons(CONFIG_CDP_DEVICE_ID);
	sprintf(buf, CONFIG_CDP_DEVICE_ID_PREFIX "%pm", NetOurEther);
	memcpy((uchar *)s, buf, 16);
	s += 16 / 2;
#endif

#ifdef CONFIG_CDP_PORT_ID
	*s++ = htons(CDP_PORT_ID_TLV);
	memset(buf, 0, sizeof(buf));
	sprintf(buf, CONFIG_CDP_PORT_ID, eth_get_dev_index());
	len = strlen(buf);
	if (len & 1)	/* make it even */
		len++;
	*s++ = htons(len + 4);
	memcpy((uchar *)s, buf, len);
	s += len / 2;
#endif

#ifdef CONFIG_CDP_CAPABILITIES
	*s++ = htons(CDP_CAPABILITIES_TLV);
	*s++ = htons(8);
	*(ulong *)s = htonl(CONFIG_CDP_CAPABILITIES);
	s += 2;
#endif

#ifdef CONFIG_CDP_VERSION
	*s++ = htons(CDP_VERSION_TLV);
	memset(buf, 0, sizeof(buf));
	strcpy(buf, CONFIG_CDP_VERSION);
	len = strlen(buf);
	if (len & 1)	/* make it even */
		len++;
	*s++ = htons(len + 4);
	memcpy((uchar *)s, buf, len);
	s += len / 2;
#endif

#ifdef CONFIG_CDP_PLATFORM
	*s++ = htons(CDP_PLATFORM_TLV);
	memset(buf, 0, sizeof(buf));
	strcpy(buf, CONFIG_CDP_PLATFORM);
	len = strlen(buf);
	if (len & 1)	/* make it even */
		len++;
	*s++ = htons(len + 4);
	memcpy((uchar *)s, buf, len);
	s += len / 2;
#endif

#ifdef CONFIG_CDP_TRIGGER
	*s++ = htons(CDP_TRIGGER_TLV);
	*s++ = htons(8);
	*(ulong *)s = htonl(CONFIG_CDP_TRIGGER);
	s += 2;
#endif

#ifdef CONFIG_CDP_POWER_CONSUMPTION
	*s++ = htons(CDP_POWER_CONSUMPTION_TLV);
	*s++ = htons(6);
	*s++ = htons(CONFIG_CDP_POWER_CONSUMPTION);
#endif

	/* length of ethernet packet */
	len = (uchar *)s - ((uchar *)NetTxPacket + ETHER_HDR_SIZE);
	et->et_protlen = htons(len);

	len = ETHER_HDR_SIZE + sizeof(CDP_SNAP_hdr);
	chksum = CDP_compute_csum((uchar *)NetTxPacket + len,
				  (uchar *)s - (NetTxPacket + len));
	if (chksum == 0)
		chksum = 0xFFFF;
	*cp = htons(chksum);

	(void) eth_send(NetTxPacket, (uchar *)s - NetTxPacket);
	return 0;
}

static void
CDPTimeout(void)
{
	CDPSeq++;

	if (CDPSeq < 3) {
		NetSetTimeout(CDP_TIMEOUT, CDPTimeout);
		CDPSendTrigger();
		return;
	}

	/* if not OK try again */
	if (!CDPOK)
		NetStartAgain();
	else
		NetState = NETLOOP_SUCCESS;
}

static void
CDPDummyHandler(uchar *pkt, unsigned dest, IPaddr_t sip, unsigned src,
		unsigned len)
{
	/* nothing */
}

void
CDPHandler(const uchar *pkt, unsigned len)
{
	const uchar *t;
	const ushort *ss;
	ushort type, tlen;
	ushort vlan, nvlan;

	/* minimum size? */
	if (len < sizeof(CDP_SNAP_hdr) + 4)
		goto pkt_short;

	/* check for valid CDP SNAP header */
	if (memcmp(pkt, CDP_SNAP_hdr, sizeof(CDP_SNAP_hdr)) != 0)
		return;

	pkt += sizeof(CDP_SNAP_hdr);
	len -= sizeof(CDP_SNAP_hdr);

	/* Version of CDP protocol must be >= 2 and TTL != 0 */
	if (pkt[0] < 0x02 || pkt[1] == 0)
		return;

	/*
	 * if version is greater than 0x02 maybe we'll have a problem;
	 * output a warning
	 */
	if (pkt[0] != 0x02)
		printf("**WARNING: CDP packet received with a protocol version "
				"%d > 2\n", pkt[0] & 0xff);

	if (CDP_compute_csum(pkt, len) != 0)
		return;

	pkt += 4;
	len -= 4;

	vlan = htons(-1);
	nvlan = htons(-1);
	while (len > 0) {
		if (len < 4)
			goto pkt_short;

		ss = (const ushort *)pkt;
		type = ntohs(ss[0]);
		tlen = ntohs(ss[1]);
		if (tlen > len)
			goto pkt_short;

		pkt += tlen;
		len -= tlen;

		ss += 2;	/* point ss to the data of the TLV */
		tlen -= 4;

		switch (type) {
		case CDP_DEVICE_ID_TLV:
			break;
		case CDP_ADDRESS_TLV:
			break;
		case CDP_PORT_ID_TLV:
			break;
		case CDP_CAPABILITIES_TLV:
			break;
		case CDP_VERSION_TLV:
			break;
		case CDP_PLATFORM_TLV:
			break;
		case CDP_NATIVE_VLAN_TLV:
			nvlan = *ss;
			break;
		case CDP_APPLIANCE_VLAN_TLV:
			t = (const uchar *)ss;
			while (tlen > 0) {
				if (tlen < 3)
					goto pkt_short;

				ss = (const ushort *)(t + 1);

#ifdef CONFIG_CDP_APPLIANCE_VLAN_TYPE
				if (t[0] == CONFIG_CDP_APPLIANCE_VLAN_TYPE)
					vlan = *ss;
#else
				/* XXX will this work; dunno */
				vlan = ntohs(*ss);
#endif
				t += 3; tlen -= 3;
			}
			break;
		case CDP_TRIGGER_TLV:
			break;
		case CDP_POWER_CONSUMPTION_TLV:
			break;
		case CDP_SYSNAME_TLV:
			break;
		case CDP_SYSOBJECT_TLV:
			break;
		case CDP_MANAGEMENT_ADDRESS_TLV:
			break;
		}
	}

	CDPApplianceVLAN = vlan;
	CDPNativeVLAN = nvlan;

	CDPOK = 1;
	return;

 pkt_short:
	printf("** CDP packet is too short\n");
	return;
}

void
CDPStart(void)
{
	printf("Using %s device\n", eth_get_name());
	CDPSeq = 0;
	CDPOK = 0;

	CDPNativeVLAN = htons(-1);
	CDPApplianceVLAN = htons(-1);

	NetSetTimeout(CDP_TIMEOUT, CDPTimeout);
	NetSetHandler(CDPDummyHandler);

	CDPSendTrigger();
}
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, wd@denx.de
	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
219	(void) eth_send(NetTxPacket, (uchar *)s - NetTxPacket);
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
238	NetState = NETLOOP_SUCCESS;
239	}
240
241	static void
242	CDPDummyHandler(uchar *pkt, unsigned dest, IPaddr_t sip, unsigned src,
243	unsigned len)
244	{
245	/* nothing */
246	}
247
248	void
249	CDPHandler(const uchar *pkt, unsigned len)
250	{
251	const uchar *t;
252	const ushort *ss;
253	ushort type, tlen;
254	ushort vlan, nvlan;
255
256	/* minimum size? */
257	if (len < sizeof(CDP_SNAP_hdr) + 4)
258	goto pkt_short;
259
260	/* check for valid CDP SNAP header */
261	if (memcmp(pkt, CDP_SNAP_hdr, sizeof(CDP_SNAP_hdr)) != 0)
262	return;
263
264	pkt += sizeof(CDP_SNAP_hdr);
265	len -= sizeof(CDP_SNAP_hdr);
266
267	/* Version of CDP protocol must be >= 2 and TTL != 0 */
268	if (pkt[0] < 0x02 \|\| pkt[1] == 0)
269	return;
270
271	/*
272	* if version is greater than 0x02 maybe we'll have a problem;
273	* output a warning
274	*/
275	if (pkt[0] != 0x02)
276	printf("**WARNING: CDP packet received with a protocol version "
277	"%d > 2\n", pkt[0] & 0xff);
278
279	if (CDP_compute_csum(pkt, len) != 0)
280	return;
281
282	pkt += 4;
283	len -= 4;
284
285	vlan = htons(-1);
286	nvlan = htons(-1);
287	while (len > 0) {
288	if (len < 4)
289	goto pkt_short;
290
291	ss = (const ushort *)pkt;
292	type = ntohs(ss[0]);
293	tlen = ntohs(ss[1]);
294	if (tlen > len)
295	goto pkt_short;
296
297	pkt += tlen;
298	len -= tlen;
299
300	ss += 2; /* point ss to the data of the TLV */
301	tlen -= 4;
302
303	switch (type) {
304	case CDP_DEVICE_ID_TLV:
305	break;
306	case CDP_ADDRESS_TLV:
307	break;
308	case CDP_PORT_ID_TLV:
309	break;
310	case CDP_CAPABILITIES_TLV:
311	break;
312	case CDP_VERSION_TLV:
313	break;
314	case CDP_PLATFORM_TLV:
315	break;
316	case CDP_NATIVE_VLAN_TLV:
317	nvlan = *ss;
318	break;
319	case CDP_APPLIANCE_VLAN_TLV:
320	t = (const uchar *)ss;
321	while (tlen > 0) {
322	if (tlen < 3)
323	goto pkt_short;
324
325	ss = (const ushort *)(t + 1);
326
327	#ifdef CONFIG_CDP_APPLIANCE_VLAN_TYPE
328	if (t[0] == CONFIG_CDP_APPLIANCE_VLAN_TYPE)
329	vlan = *ss;
330	#else
331	/* XXX will this work; dunno */
332	vlan = ntohs(*ss);
333	#endif
334	t += 3; tlen -= 3;
335	}
336	break;
337	case CDP_TRIGGER_TLV:
338	break;
339	case CDP_POWER_CONSUMPTION_TLV:
340	break;
341	case CDP_SYSNAME_TLV:
342	break;
343	case CDP_SYSOBJECT_TLV:
344	break;
345	case CDP_MANAGEMENT_ADDRESS_TLV:
346	break;
347	}
348	}
349
350	CDPApplianceVLAN = vlan;
351	CDPNativeVLAN = nvlan;
352
353	CDPOK = 1;
354	return;
355
356	pkt_short:
357	printf("** CDP packet is too short\n");
358	return;
359	}
360
361	void
362	CDPStart(void)
363	{
364	printf("Using %s device\n", eth_get_name());
365	CDPSeq = 0;
366	CDPOK = 0;
367
368	CDPNativeVLAN = htons(-1);
369	CDPApplianceVLAN = htons(-1);
370
371	NetSetTimeout(CDP_TIMEOUT, CDPTimeout);
372	NetSetHandler(CDPDummyHandler);
373
374	CDPSendTrigger();
375	}