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