net/cdp.c

   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 }