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; 112 Ethernet_t *et; 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; 121 et = (Ethernet_t *)pkt; 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 } 376