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 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 }