text: /*
   * The MIT License (MIT)
   *
   * Copyright (c) 2013 Keichi Takahashi keichi.t@me.com
   *
   * Permission is hereby granted, free of charge, to any person obtaining a copy
   * of this software and associated documentation files (the "Software"), to deal
   * in the Software without restriction, including without limitation the rights
   * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
   * copies of the Software, and to permit persons to whom the Software is
   * furnished to do so, subject to the following conditions:
   *
   * The above copyright notice and this permission notice shall be included in
   * all copies or substantial portions of the Software.
   *
   * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
   * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
   * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
   * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
   * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
   * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
   * SOFTWARE.
   */
  
  /* Substantially derived from * https://github.com/keichi/tiny-lldpd
   * also under the MIT license */
  
  #include <signal.h>
  #include <stdint.h>
  #include <stdio.h>
  #include <stdlib.h>
  #include <string.h>
  #include <syslog.h>
  #include <unistd.h>
  
  #include <net/ethernet.h>
  #include <net/if.h>
  #include <netinet/if_ether.h>
  #include <netinet/in.h>
  #include <netpacket/packet.h>
  #include <sys/socket.h>
  
  #define MAXINTERFACES 8
  const char *ifnames[MAXINTERFACES] = {0};
  int ninterfaces = 0;
  
  uint8_t sendbuf[1024];
  
  const uint8_t lldpaddr[ETH_ALEN] = {0x01, 0x80, 0xc2, 0x00, 0x00, 0x0e};
  #define ETH_P_LLDP            0x88cc
  #define TLV_END               0
  #define TLV_CHASSIS_ID        1
  #define TLV_PORT_ID           2
  #define TLV_TTL               3
  #define TLV_PORT_DESCRIPTION  4
  #define TLV_SYSTEM_NAME       5
  
  #define CHASSIS_ID_MAC_ADDRESS  4
  #define PORT_ID_MAC_ADDRESS     3
  
  static int write_lldp_tlv_header(void *p, int type, int length)
  {
    *((uint16_t *)p) = htons((type & 0x7f) << 9 | (length & 0x1ff));
    return 2;
  }
  
  
  static int write_lldp_type_subtype_tlv(size_t offset,
      uint8_t type, uint8_t subtype, int length, const void *data)
  {
    uint8_t *p = sendbuf + offset;
  
    if ((offset + 2 + 1 + length) > sizeof(sendbuf)) {
      fprintf(stderr, "LLDP frame too large %zd > %zd\n",
          (offset + 2 + 1 + length), sizeof(sendbuf));
      exit(1);
    }
  
    p += write_lldp_tlv_header(p, type, length + 1);
    *p++ = subtype;
    memcpy(p, data, length);
    p += length;
  
    return (p - sendbuf);
  }
  
  
  static int write_lldp_type_tlv(size_t offset, uint8_t type,
      int length, const void *data)
  {
    uint8_t *p = sendbuf + offset;
  
    if ((offset + 2 + length) > sizeof(sendbuf)) {
      fprintf(stderr, "LLDP frame too large %zd > %zd\n",
          (offset + 2 + length), sizeof(sendbuf));
      exit(1);
    }
  
    p += write_lldp_tlv_header(p, type, length);
    memcpy(p, data, length);
    p += length;
  
    return (p - sendbuf);
  }
  
  
  static int write_lldp_end_tlv(size_t offset)
  {
    uint8_t *p = sendbuf + offset;
  
    if ((offset + 2) > sizeof(sendbuf)) {
      fprintf(stderr, "LLDP frame too large %zd > %zd\n",
          (offset + 2), sizeof(sendbuf));
      exit(1);
    }
  
    offset += write_lldp_tlv_header(p, TLV_END, 0);
    return offset;
  }
  
  
  static void mac_str_to_bytes(const char *macstr, uint8_t *mac)
  {
    if (sscanf(macstr, "%hhx:%hhx:%hhx:%hhx:%hhx:%hhx",
          &mac[0], &mac[1], &mac[2], &mac[3], &mac[4], &mac[5]) != 6) {
      fprintf(stderr, "Invalid MAC address: %s\n", macstr);
      exit(1);
    }
  }
  
  
  static size_t format_lldp_packet(const char *macaddr, const char *ifname,
      const char *serial)
  {
    uint8_t saddr[ETH_ALEN];
    size_t offset = 0;
    struct ether_header *eh = (struct ether_header *)sendbuf;
    uint16_t ttl;
  
    mac_str_to_bytes(macaddr, saddr);
    memset(sendbuf, 0, sizeof(sendbuf));
  
    eh = (struct ether_header *)sendbuf;
    memcpy(eh->ether_shost, saddr, sizeof(eh->ether_shost));
    memcpy(eh->ether_dhost, lldpaddr, sizeof(eh->ether_dhost));
    eh->ether_type = htons(ETH_P_LLDP);
    offset = sizeof(*eh);
  
    offset = write_lldp_type_subtype_tlv(offset,
        TLV_CHASSIS_ID, CHASSIS_ID_MAC_ADDRESS, ETH_ALEN, saddr);
    offset = write_lldp_type_subtype_tlv(offset,
        TLV_PORT_ID, PORT_ID_MAC_ADDRESS, ETH_ALEN, saddr);
  
    ttl = htons(120);
    offset = write_lldp_type_tlv(offset, TLV_TTL, sizeof(ttl), &ttl);
  
    offset = write_lldp_type_tlv(offset,
        TLV_PORT_DESCRIPTION, strlen(ifname), ifname);
    offset = write_lldp_type_tlv(offset,
        TLV_SYSTEM_NAME, strlen(serial), serial);
    offset = write_lldp_end_tlv(offset);
  
    return offset;
  }
  
  
  #ifndef UNIT_TESTS
  static void send_lldp_packet(int s, size_t len, const char *ifname)
  {
    struct sockaddr_ll sll;
  
    memset(&sll, 0, sizeof(sll));
    sll.sll_family = PF_PACKET;
    sll.sll_ifindex = if_nametoindex(ifname);
    sll.sll_hatype = ARPHRD_ETHER;
    sll.sll_halen = ETH_ALEN;
    sll.sll_pkttype = PACKET_OTHERHOST;
    memcpy(sll.sll_addr, lldpaddr, ETH_ALEN);
    if (sendto(s, sendbuf, len, 0, (struct sockaddr*)&sll, sizeof(sll)) < 0) {
      fprintf(stderr, "LLDP sendto failed\n");
      exit(1);
    }
  }
  
  
  static void usage(const char *progname)
  {
    fprintf(stderr, "usage: %s -i eth# -m 00:11:22:33:44:55 -s G0123456789\n",
        progname);
    exit(1);
  }
  
  
  int main(int argc, char *argv[])
  {
    const char *macaddr = NULL;
    const char *serial = NULL;
    int c;
    int s;
  
    while ((c = getopt(argc, argv, "i:m:s:")) != -1) {
      switch (c) {
        case 'i':
          if (ninterfaces == (MAXINTERFACES - 1)) {
            usage(argv[0]);
          }
          ifnames[ninterfaces++] = optarg;
          break;
        case 'm':
          macaddr = optarg;
          break;
        case 's':
          serial = optarg;
          break;
        default:
          usage(argv[0]);
          break;
      }
    }
  
    if (ninterfaces == 0 || macaddr == NULL || serial == NULL) {
      usage(argv[0]);
    }
  
    if ((s = socket(PF_PACKET, SOCK_RAW, htons(ETH_P_ALL))) < 0) {
      fprintf(stderr, "socket(PF_PACKET) failed\n");
      exit(1);
    }
  
    while (1) {
      int i;
  
      for (i = 0; i < ninterfaces; ++i) {
        if (ifnames[i] != NULL) {
          size_t len = format_lldp_packet(macaddr, ifnames[i], serial);
          send_lldp_packet(s, len, ifnames[i]);
        }
        usleep(10000 + (rand() % 80000));
      }
  
      usleep(500000 + (rand() % 1000000));
    }
  
    return 0;
  }
  #endif  /* UNIT_TESTS */
  