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  *      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 uchar NetCDPAddr[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 CDPSeq;
  40 static int CDPOK;
  41
  42 ushort CDPNativeVLAN;
  43 ushort CDPApplianceVLAN;
  44
  45 static const uchar CDP_SNAP_hdr[8] = {
  46         0xAA, 0xAA, 0x03, 0x00, 0x00, 0x0C, 0x20, 0x00 };
  47
  48 static ushort
  49 CDP_compute_csum(const uchar *buff, ushort len)
  50 {
  51         ushort csum;
  52         int     odd;
  53         ulong   result = 0;
  54         ushort  leftover;
  55         ushort *p;
  56
  57         if (len > 0) {
  58                 odd = 1 & (ulong)buff;
  59                 if (odd) {
  60                         result = *buff << 8;
  61                         len--;
  62                         buff++;
  63                 }
  64                 while (len > 1) {
  65                         p = (ushort *)buff;
  66                         result += *p++;
  67                         buff = (uchar *)p;
  68                         if (result & 0x80000000)
  69                                 result = (result & 0xFFFF) + (result >> 16);
  70                         len -= 2;
  71                 }
  72                 if (len) {
  73                         leftover = (signed short)(*(const signed char *)buff);
  74                         /*
  75                          * CISCO SUCKS big time! (and blows too):
  76                          * CDP uses the IP checksum algorithm with a twist;
  77                          * for the last byte it *sign* extends and sums.
  78                          */
  79                         result = (result & 0xffff0000) |
  80                                  ((result + leftover) & 0x0000ffff);
  81                 }
  82                 while (result >> 16)
  83                         result = (result & 0xFFFF) + (result >> 16);
  84
  85                 if (odd)
  86                         result = ((result >> 8) & 0xff) |
  87                                  ((result & 0xff) << 8);
  88         }
  89
  90         /* add up 16-bit and 17-bit words for 17+c bits */
  91         result = (result & 0xffff) + (result >> 16);
  92         /* add up 16-bit and 2-bit for 16+c bit */
  93         result = (result & 0xffff) + (result >> 16);
  94         /* add up carry.. */
  95         result = (result & 0xffff) + (result >> 16);
  96
  97         /* negate */
  98         csum = ~(ushort)result;
  99
 100         /* run time endian detection */
 101         if (csum != htons(csum))        /* little endian */
 102                 csum = htons(csum);
 103
 104         return csum;
 105 }
 106
 107 static int
 108 CDPSendTrigger(void)
 109 {
 110         uchar *pkt;
 111         ushort *s;
 112         ushort *cp;
 113         struct ethernet_hdr *et;
 114         int len;
 115         ushort chksum;
 116 #if     defined(CONFIG_CDP_DEVICE_ID) || defined(CONFIG_CDP_PORT_ID)   || \
 117         defined(CONFIG_CDP_VERSION)   || defined(CONFIG_CDP_PLATFORM)
 118         char buf[32];
 119 #endif
 120
 121         pkt = NetTxPacket;
 122         et = (struct ethernet_hdr *)pkt;
 123
 124         /* NOTE: trigger sent not on any VLAN */
 125
 126         /* form ethernet header */
 127         memcpy(et->et_dest, NetCDPAddr, 6);
 128         memcpy(et->et_src, NetOurEther, 6);
 129
 130         pkt += ETHER_HDR_SIZE;
 131
 132         /* SNAP header */
 133         memcpy((uchar *)pkt, CDP_SNAP_hdr, sizeof(CDP_SNAP_hdr));
 134         pkt += sizeof(CDP_SNAP_hdr);
 135
 136         /* CDP header */
 137         *pkt++ = 0x02;                          /* CDP version 2 */
 138         *pkt++ = 180;                           /* TTL */
 139         s = (ushort *)pkt;
 140         cp = s;
 141         /* checksum (0 for later calculation) */
 142         *s++ = htons(0);
 143
 144         /* CDP fields */
 145 #ifdef CONFIG_CDP_DEVICE_ID
 146         *s++ = htons(CDP_DEVICE_ID_TLV);
 147         *s++ = htons(CONFIG_CDP_DEVICE_ID);
 148         sprintf(buf, CONFIG_CDP_DEVICE_ID_PREFIX "%pm", NetOurEther);
 149         memcpy((uchar *)s, buf, 16);
 150         s += 16 / 2;
 151 #endif
 152
 153 #ifdef CONFIG_CDP_PORT_ID
 154         *s++ = htons(CDP_PORT_ID_TLV);
 155         memset(buf, 0, sizeof(buf));
 156         sprintf(buf, CONFIG_CDP_PORT_ID, eth_get_dev_index());
 157         len = strlen(buf);
 158         if (len & 1)    /* make it even */
 159                 len++;
 160         *s++ = htons(len + 4);
 161         memcpy((uchar *)s, buf, len);
 162         s += len / 2;
 163 #endif
 164
 165 #ifdef CONFIG_CDP_CAPABILITIES
 166         *s++ = htons(CDP_CAPABILITIES_TLV);
 167         *s++ = htons(8);
 168         *(ulong *)s = htonl(CONFIG_CDP_CAPABILITIES);
 169         s += 2;
 170 #endif
 171
 172 #ifdef CONFIG_CDP_VERSION
 173         *s++ = htons(CDP_VERSION_TLV);
 174         memset(buf, 0, sizeof(buf));
 175         strcpy(buf, CONFIG_CDP_VERSION);
 176         len = strlen(buf);
 177         if (len & 1)    /* make it even */
 178                 len++;
 179         *s++ = htons(len + 4);
 180         memcpy((uchar *)s, buf, len);
 181         s += len / 2;
 182 #endif
 183
 184 #ifdef CONFIG_CDP_PLATFORM
 185         *s++ = htons(CDP_PLATFORM_TLV);
 186         memset(buf, 0, sizeof(buf));
 187         strcpy(buf, CONFIG_CDP_PLATFORM);
 188         len = strlen(buf);
 189         if (len & 1)    /* make it even */
 190                 len++;
 191         *s++ = htons(len + 4);
 192         memcpy((uchar *)s, buf, len);
 193         s += len / 2;
 194 #endif
 195
 196 #ifdef CONFIG_CDP_TRIGGER
 197         *s++ = htons(CDP_TRIGGER_TLV);
 198         *s++ = htons(8);
 199         *(ulong *)s = htonl(CONFIG_CDP_TRIGGER);
 200         s += 2;
 201 #endif
 202
 203 #ifdef CONFIG_CDP_POWER_CONSUMPTION
 204         *s++ = htons(CDP_POWER_CONSUMPTION_TLV);
 205         *s++ = htons(6);
 206         *s++ = htons(CONFIG_CDP_POWER_CONSUMPTION);
 207 #endif
 208
 209         /* length of ethernet packet */
 210         len = (uchar *)s - ((uchar *)NetTxPacket + ETHER_HDR_SIZE);
 211         et->et_protlen = htons(len);
 212
 213         len = ETHER_HDR_SIZE + sizeof(CDP_SNAP_hdr);
 214         chksum = CDP_compute_csum((uchar *)NetTxPacket + len,
 215                                   (uchar *)s - (NetTxPacket + len));
 216         if (chksum == 0)
 217                 chksum = 0xFFFF;
 218         *cp = htons(chksum);
 219
 220         NetSendPacket(NetTxPacket, (uchar *)s - NetTxPacket);
 221         return 0;
 222 }
 223
 224 static void
 225 CDPTimeout(void)
 226 {
 227         CDPSeq++;
 228
 229         if (CDPSeq < 3) {
 230                 NetSetTimeout(CDP_TIMEOUT, CDPTimeout);
 231                 CDPSendTrigger();
 232                 return;
 233         }
 234
 235         /* if not OK try again */
 236         if (!CDPOK)
 237                 NetStartAgain();
 238         else
 239                 net_set_state(NETLOOP_SUCCESS);
 240 }
 241
 242 void cdp_receive(const uchar *pkt, unsigned len)
 243 {
 244         const uchar *t;
 245         const ushort *ss;
 246         ushort type, tlen;
 247         ushort vlan, nvlan;
 248
 249         /* minimum size? */
 250         if (len < sizeof(CDP_SNAP_hdr) + 4)
 251                 goto pkt_short;
 252
 253         /* check for valid CDP SNAP header */
 254         if (memcmp(pkt, CDP_SNAP_hdr, sizeof(CDP_SNAP_hdr)) != 0)
 255                 return;
 256
 257         pkt += sizeof(CDP_SNAP_hdr);
 258         len -= sizeof(CDP_SNAP_hdr);
 259
 260         /* Version of CDP protocol must be >= 2 and TTL != 0 */
 261         if (pkt[0] < 0x02 || pkt[1] == 0)
 262                 return;
 263
 264         /*
 265          * if version is greater than 0x02 maybe we'll have a problem;
 266          * output a warning
 267          */
 268         if (pkt[0] != 0x02)
 269                 printf("**WARNING: CDP packet received with a protocol version "
 270                                 "%d > 2\n", pkt[0] & 0xff);
 271
 272         if (CDP_compute_csum(pkt, len) != 0)
 273                 return;
 274
 275         pkt += 4;
 276         len -= 4;
 277
 278         vlan = htons(-1);
 279         nvlan = htons(-1);
 280         while (len > 0) {
 281                 if (len < 4)
 282                         goto pkt_short;
 283
 284                 ss = (const ushort *)pkt;
 285                 type = ntohs(ss[0]);
 286                 tlen = ntohs(ss[1]);
 287                 if (tlen > len)
 288                         goto pkt_short;
 289
 290                 pkt += tlen;
 291                 len -= tlen;
 292
 293                 ss += 2;        /* point ss to the data of the TLV */
 294                 tlen -= 4;
 295
 296                 switch (type) {
 297                 case CDP_DEVICE_ID_TLV:
 298                         break;
 299                 case CDP_ADDRESS_TLV:
 300                         break;
 301                 case CDP_PORT_ID_TLV:
 302                         break;
 303                 case CDP_CAPABILITIES_TLV:
 304                         break;
 305                 case CDP_VERSION_TLV:
 306                         break;
 307                 case CDP_PLATFORM_TLV:
 308                         break;
 309                 case CDP_NATIVE_VLAN_TLV:
 310                         nvlan = *ss;
 311                         break;
 312                 case CDP_APPLIANCE_VLAN_TLV:
 313                         t = (const uchar *)ss;
 314                         while (tlen > 0) {
 315                                 if (tlen < 3)
 316                                         goto pkt_short;
 317
 318                                 ss = (const ushort *)(t + 1);
 319
 320 #ifdef CONFIG_CDP_APPLIANCE_VLAN_TYPE
 321                                 if (t[0] == CONFIG_CDP_APPLIANCE_VLAN_TYPE)
 322                                         vlan = *ss;
 323 #else
 324                                 /* XXX will this work; dunno */
 325                                 vlan = ntohs(*ss);
 326 #endif
 327                                 t += 3; tlen -= 3;
 328                         }
 329                         break;
 330                 case CDP_TRIGGER_TLV:
 331                         break;
 332                 case CDP_POWER_CONSUMPTION_TLV:
 333                         break;
 334                 case CDP_SYSNAME_TLV:
 335                         break;
 336                 case CDP_SYSOBJECT_TLV:
 337                         break;
 338                 case CDP_MANAGEMENT_ADDRESS_TLV:
 339                         break;
 340                 }
 341         }
 342
 343         CDPApplianceVLAN = vlan;
 344         CDPNativeVLAN = nvlan;
 345
 346         CDPOK = 1;
 347         return;
 348
 349  pkt_short:
 350         printf("** CDP packet is too short\n");
 351         return;
 352 }
 353
 354 void
 355 CDPStart(void)
 356 {
 357         printf("Using %s device\n", eth_get_name());
 358         CDPSeq = 0;
 359         CDPOK = 0;
 360
 361         CDPNativeVLAN = htons(-1);
 362         CDPApplianceVLAN = htons(-1);
 363
 364         NetSetTimeout(CDP_TIMEOUT, CDPTimeout);
 365
 366         CDPSendTrigger();
 367 }