Add gflldp, a minimal LLDP implementation
This might be used as part of an auto-activation service
for non-GPON networks, serving a similar role to OMCI and
Ranging in GPON.
Change-Id: Ic3cf9b2cefb3ff1a731145ceb7ece8c8746d4481
diff --git a/cmds/Makefile b/cmds/Makefile
index 4778036..45580b2 100644
--- a/cmds/Makefile
+++ b/cmds/Makefile
@@ -33,6 +33,7 @@
dnsck \
freemegs \
gfhd254_reboot \
+ gflldpd \
gstatic \
http_bouncer \
ionice \
@@ -52,6 +53,7 @@
LIB_TARGETS=\
stdoutline.so
HOST_TEST_TARGETS=\
+ host-gflldpd_test \
host-netusage_test \
host-utils_test \
host-isoping_test
@@ -272,6 +274,10 @@
anonid: anonid.o
host-anonid: host-anonid.o
anonid host-anonid: LIBS += -lcrypto
+host-gflldpd_test.o: CXXFLAGS += -D WVTEST_CONFIGURED -I ../wvtest/cpp
+host-gflldpd_test.o: gflldpd.c
+host-gflldpd_test: LIBS+=$(HOST_LIBS) -lm -lstdc++
+host-gflldpd_test: host-gflldpd_test.o host-wvtestmain.o host-wvtest.o
TESTS = $(wildcard test-*.sh) $(wildcard test-*.py) $(wildcard *_test.py) $(TEST_TARGETS)
diff --git a/cmds/gflldpd.c b/cmds/gflldpd.c
new file mode 100644
index 0000000..44c2dd2
--- /dev/null
+++ b/cmds/gflldpd.c
@@ -0,0 +1,246 @@
+/*
+ * 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 */
diff --git a/cmds/gflldpd_test.cc b/cmds/gflldpd_test.cc
new file mode 100644
index 0000000..21f33df
--- /dev/null
+++ b/cmds/gflldpd_test.cc
@@ -0,0 +1,61 @@
+/*
+ * 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.
+ */
+
+#include <netinet/if_ether.h>
+#include <stdio.h>
+#include <string.h>
+
+#include <wvtest.h>
+
+
+#define UNIT_TESTS
+#include "gflldpd.c"
+
+
+WVTEST_MAIN("mac_str_to_bytes") {
+ uint8_t expected_mac[] = { 0x00, 0x11, 0x22, 0x33, 0x44, 0x55 };
+ uint8_t mac[ETH_ALEN];
+
+ mac_str_to_bytes("00:11:22:33:44:55", mac);
+ WVPASSEQ(memcmp(mac, expected_mac, ETH_ALEN), 0);
+}
+
+
+WVTEST_MAIN("format_lldp_packet") {
+ size_t siz;
+ uint8_t expected[] = {
+ 0x01, 0x80, 0xc2, 0x00, 0x00, 0x0e, 0x00, 0x11,
+ 0x22, 0x33, 0x44, 0x55, 0x88, 0xcc, 0x02, 0x07,
+ 0x04, 0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x04,
+ 0x07, 0x03, 0x00, 0x11, 0x22, 0x33, 0x44, 0x55,
+ 0x06, 0x02, 0x00, 0x78, 0x08, 0x04, 0x65, 0x74,
+ 0x68, 0x30, 0x0a, 0x0b, 0x47, 0x30, 0x31, 0x32,
+ 0x33, 0x34, 0x35, 0x36, 0x37, 0x38, 0x39, 0x00,
+ 0x00
+ };
+
+ siz = format_lldp_packet("00:11:22:33:44:55", "eth0", "G0123456789");
+ WVPASSEQ(siz, sizeof(expected));
+ WVPASSEQ(memcmp(sendbuf, expected, siz), 0);
+}
