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 u8 net_cdp_ethaddr[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 cdp_seq; 40 static int cdp_ok; 41 42 ushort cdp_native_vlan; 43 ushort cdp_appliance_vlan; 44 45 static const uchar cdp_snap_hdr[8] = { 46 0xAA, 0xAA, 0x03, 0x00, 0x00, 0x0C, 0x20, 0x00 }; 47 48 static ushort 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 cdp_send_trigger(void) 107 { 108 uchar *pkt; 109 ushort *s; 110 ushort *cp; 111 struct ethernet_hdr *et; 112 int len; 113 ushort chksum; 114 #if defined(CONFIG_CDP_DEVICE_ID) || defined(CONFIG_CDP_PORT_ID) || \ 115 defined(CONFIG_CDP_VERSION) || defined(CONFIG_CDP_PLATFORM) 116 char buf[32]; 117 #endif 118 119 pkt = net_tx_packet; 120 et = (struct ethernet_hdr *)pkt; 121 122 /* NOTE: trigger sent not on any VLAN */ 123 124 /* form ethernet header */ 125 memcpy(et->et_dest, net_cdp_ethaddr, 6); 126 memcpy(et->et_src, net_ethaddr, 6); 127 128 pkt += ETHER_HDR_SIZE; 129 130 /* SNAP header */ 131 memcpy((uchar *)pkt, cdp_snap_hdr, sizeof(cdp_snap_hdr)); 132 pkt += sizeof(cdp_snap_hdr); 133 134 /* CDP header */ 135 *pkt++ = 0x02; /* CDP version 2 */ 136 *pkt++ = 180; /* TTL */ 137 s = (ushort *)pkt; 138 cp = s; 139 /* checksum (0 for later calculation) */ 140 *s++ = htons(0); 141 142 /* CDP fields */ 143 #ifdef CONFIG_CDP_DEVICE_ID 144 *s++ = htons(CDP_DEVICE_ID_TLV); 145 *s++ = htons(CONFIG_CDP_DEVICE_ID); 146 sprintf(buf, CONFIG_CDP_DEVICE_ID_PREFIX "%pm", net_ethaddr); 147 memcpy((uchar *)s, buf, 16); 148 s += 16 / 2; 149 #endif 150 151 #ifdef CONFIG_CDP_PORT_ID 152 *s++ = htons(CDP_PORT_ID_TLV); 153 memset(buf, 0, sizeof(buf)); 154 sprintf(buf, CONFIG_CDP_PORT_ID, eth_get_dev_index()); 155 len = strlen(buf); 156 if (len & 1) /* make it even */ 157 len++; 158 *s++ = htons(len + 4); 159 memcpy((uchar *)s, buf, len); 160 s += len / 2; 161 #endif 162 163 #ifdef CONFIG_CDP_CAPABILITIES 164 *s++ = htons(CDP_CAPABILITIES_TLV); 165 *s++ = htons(8); 166 *(ulong *)s = htonl(CONFIG_CDP_CAPABILITIES); 167 s += 2; 168 #endif 169 170 #ifdef CONFIG_CDP_VERSION 171 *s++ = htons(CDP_VERSION_TLV); 172 memset(buf, 0, sizeof(buf)); 173 strcpy(buf, CONFIG_CDP_VERSION); 174 len = strlen(buf); 175 if (len & 1) /* make it even */ 176 len++; 177 *s++ = htons(len + 4); 178 memcpy((uchar *)s, buf, len); 179 s += len / 2; 180 #endif 181 182 #ifdef CONFIG_CDP_PLATFORM 183 *s++ = htons(CDP_PLATFORM_TLV); 184 memset(buf, 0, sizeof(buf)); 185 strcpy(buf, CONFIG_CDP_PLATFORM); 186 len = strlen(buf); 187 if (len & 1) /* make it even */ 188 len++; 189 *s++ = htons(len + 4); 190 memcpy((uchar *)s, buf, len); 191 s += len / 2; 192 #endif 193 194 #ifdef CONFIG_CDP_TRIGGER 195 *s++ = htons(CDP_TRIGGER_TLV); 196 *s++ = htons(8); 197 *(ulong *)s = htonl(CONFIG_CDP_TRIGGER); 198 s += 2; 199 #endif 200 201 #ifdef CONFIG_CDP_POWER_CONSUMPTION 202 *s++ = htons(CDP_POWER_CONSUMPTION_TLV); 203 *s++ = htons(6); 204 *s++ = htons(CONFIG_CDP_POWER_CONSUMPTION); 205 #endif 206 207 /* length of ethernet packet */ 208 len = (uchar *)s - ((uchar *)net_tx_packet + ETHER_HDR_SIZE); 209 et->et_protlen = htons(len); 210 211 len = ETHER_HDR_SIZE + sizeof(cdp_snap_hdr); 212 chksum = cdp_compute_csum((uchar *)net_tx_packet + len, 213 (uchar *)s - (net_tx_packet + len)); 214 if (chksum == 0) 215 chksum = 0xFFFF; 216 *cp = htons(chksum); 217 218 net_send_packet(net_tx_packet, (uchar *)s - net_tx_packet); 219 return 0; 220 } 221 222 static void cdp_timeout_handler(void) 223 { 224 cdp_seq++; 225 226 if (cdp_seq < 3) { 227 net_set_timeout_handler(CDP_TIMEOUT, cdp_timeout_handler); 228 cdp_send_trigger(); 229 return; 230 } 231 232 /* if not OK try again */ 233 if (!cdp_ok) 234 net_start_again(); 235 else 236 net_set_state(NETLOOP_SUCCESS); 237 } 238 239 void cdp_receive(const uchar *pkt, unsigned len) 240 { 241 const uchar *t; 242 const ushort *ss; 243 ushort type, tlen; 244 ushort vlan, nvlan; 245 246 /* minimum size? */ 247 if (len < sizeof(cdp_snap_hdr) + 4) 248 goto pkt_short; 249 250 /* check for valid CDP SNAP header */ 251 if (memcmp(pkt, cdp_snap_hdr, sizeof(cdp_snap_hdr)) != 0) 252 return; 253 254 pkt += sizeof(cdp_snap_hdr); 255 len -= sizeof(cdp_snap_hdr); 256 257 /* Version of CDP protocol must be >= 2 and TTL != 0 */ 258 if (pkt[0] < 0x02 || pkt[1] == 0) 259 return; 260 261 /* 262 * if version is greater than 0x02 maybe we'll have a problem; 263 * output a warning 264 */ 265 if (pkt[0] != 0x02) 266 printf("**WARNING: CDP packet received with a protocol version " 267 "%d > 2\n", pkt[0] & 0xff); 268 269 if (cdp_compute_csum(pkt, len) != 0) 270 return; 271 272 pkt += 4; 273 len -= 4; 274 275 vlan = htons(-1); 276 nvlan = htons(-1); 277 while (len > 0) { 278 if (len < 4) 279 goto pkt_short; 280 281 ss = (const ushort *)pkt; 282 type = ntohs(ss[0]); 283 tlen = ntohs(ss[1]); 284 if (tlen > len) 285 goto pkt_short; 286 287 pkt += tlen; 288 len -= tlen; 289 290 ss += 2; /* point ss to the data of the TLV */ 291 tlen -= 4; 292 293 switch (type) { 294 case CDP_DEVICE_ID_TLV: 295 break; 296 case CDP_ADDRESS_TLV: 297 break; 298 case CDP_PORT_ID_TLV: 299 break; 300 case CDP_CAPABILITIES_TLV: 301 break; 302 case CDP_VERSION_TLV: 303 break; 304 case CDP_PLATFORM_TLV: 305 break; 306 case CDP_NATIVE_VLAN_TLV: 307 nvlan = *ss; 308 break; 309 case CDP_APPLIANCE_VLAN_TLV: 310 t = (const uchar *)ss; 311 while (tlen > 0) { 312 if (tlen < 3) 313 goto pkt_short; 314 315 ss = (const ushort *)(t + 1); 316 317 #ifdef CONFIG_CDP_APPLIANCE_VLAN_TYPE 318 if (t[0] == CONFIG_CDP_APPLIANCE_VLAN_TYPE) 319 vlan = *ss; 320 #else 321 /* XXX will this work; dunno */ 322 vlan = ntohs(*ss); 323 #endif 324 t += 3; tlen -= 3; 325 } 326 break; 327 case CDP_TRIGGER_TLV: 328 break; 329 case CDP_POWER_CONSUMPTION_TLV: 330 break; 331 case CDP_SYSNAME_TLV: 332 break; 333 case CDP_SYSOBJECT_TLV: 334 break; 335 case CDP_MANAGEMENT_ADDRESS_TLV: 336 break; 337 } 338 } 339 340 cdp_appliance_vlan = vlan; 341 cdp_native_vlan = nvlan; 342 343 cdp_ok = 1; 344 return; 345 346 pkt_short: 347 printf("** CDP packet is too short\n"); 348 return; 349 } 350 351 void cdp_start(void) 352 { 353 printf("Using %s device\n", eth_get_name()); 354 cdp_seq = 0; 355 cdp_ok = 0; 356 357 cdp_native_vlan = htons(-1); 358 cdp_appliance_vlan = htons(-1); 359 360 net_set_timeout_handler(CDP_TIMEOUT, cdp_timeout_handler); 361 362 cdp_send_trigger(); 363 } 364