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