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