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