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