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