LXR uboot/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 */
  20const 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
  38static int CDPSeq;
  39static int CDPOK;
  40
  41ushort CDPNativeVLAN;
  42ushort CDPApplianceVLAN;
  43
  44static const uchar CDP_SNAP_hdr[8] = {
  45        0xAA, 0xAA, 0x03, 0x00, 0x00, 0x0C, 0x20, 0x00 };
  46
  47static ushort
  48CDP_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
 106static int
 107CDPSendTrigger(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
 223static void
 224CDPTimeout(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
 241void 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
 353void
 354CDPStart(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