net/cdp.c

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