networkd: Introduce Link Layer Discovery Protocol (LLDP)

This patch introduces LLDP support to networkd. it implements the
receiver side of the protocol.

The Link Layer Discovery Protocol (LLDP) is an industry-standard,
vendor-neutral method to allow networked devices to advertise
capabilities, identity, and other information onto a LAN. The Layer 2
protocol, detailed in IEEE 802.1AB-2005.LLDP allows network devices
that operate at the lower layers of a protocol stack (such as
Layer 2 bridges and switches) to learn some of the capabilities
and characteristics of LAN devices available to higher
layer protocols.

1 files changed, 321 insertions, 0 deletions

diff --git a/src/libsystemd-network/lldp-tlv.c b/src/libsystemd-network/lldp-tlv.c
new file mode 100644
index 0000000000..e43d70d3cf
--- /dev/null
+++ b/src/libsystemd-network/lldp-tlv.c
@@ -0,0 +1,321 @@
+/*-*- Mode: C; c-basic-offset: 8; indent-tabs-mode: nil -*-*/
+
+/***
+  This file is part of systemd.
+
+  Copyright (C) 2014 Tom Gundersen
+  Copyright (C) 2014 Susant Sahani
+
+  systemd is free software; you can redistribute it and/or modify it
+  under the terms of the GNU Lesser General Public License as published by
+  the Free Software Foundation; either version 2.1 of the License, or
+  (at your option) any later version.
+
+  systemd is distributed in the hope that it will be useful, but
+  WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+  Lesser General Public License for more details.
+
+  You should have received a copy of the GNU Lesser General Public License
+  along with systemd; If not, see <http://www.gnu.org/licenses/>.
+***/
+
+#include <net/ethernet.h>
+#include <arpa/inet.h>
+
+#include "macro.h"
+#include "lldp-tlv.h"
+
+int tlv_section_new(tlv_section **ret) {
+        tlv_section *s;
+
+        s = new0(tlv_section, 1);
+        if (!s)
+                return -ENOMEM;
+
+        *ret = s;
+
+        return 0;
+}
+
+void tlv_section_free(tlv_section *m) {
+
+        if (!m)
+                return;
+
+        free(m);
+}
+
+int tlv_packet_new(tlv_packet **ret) {
+        tlv_packet *m;
+
+        m = new0(tlv_packet, 1);
+        if (!m)
+                return -ENOMEM;
+
+        LIST_HEAD_INIT(m->sections);
+
+        *ret = m;
+
+        return 0;
+}
+
+void tlv_packet_free(tlv_packet *m) {
+        tlv_section *s, *n;
+
+        if (!m)
+                return;
+
+        LIST_FOREACH_SAFE(section, s, n, m->sections)
+                tlv_section_free(s);
+
+        free(m);
+}
+
+int tlv_packet_append_bytes(tlv_packet *m, const void *data, size_t data_length) {
+        uint8_t *p;
+
+        assert_return(m, -EINVAL);
+        assert_return(data, -EINVAL);
+        assert_return(data_length, -EINVAL);
+
+        if (m->length + data_length > ETHER_MAX_LEN)
+                return -ENOMEM;
+
+        p = m->pdu + m->length;
+        memcpy(p, data, data_length);
+        m->length += data_length;
+
+        return 0;
+}
+
+int tlv_packet_append_u8(tlv_packet *m, uint8_t data) {
+
+        assert_return(m, -EINVAL);
+
+        return tlv_packet_append_bytes(m, &data, sizeof(uint8_t));
+}
+
+int tlv_packet_append_u16(tlv_packet *m, uint16_t data) {
+        uint16_t type;
+
+        assert_return(m, -EINVAL);
+
+        type = htons(data);
+
+        return tlv_packet_append_bytes(m, &type, sizeof(uint16_t));
+}
+
+int tlv_packet_append_u32(tlv_packet *m, uint32_t data) {
+        uint32_t type;
+
+        assert_return(m, -EINVAL);
+
+        type = htonl(data);
+
+        return tlv_packet_append_bytes(m, &type, sizeof(uint32_t));
+}
+
+int tlv_packet_append_string(tlv_packet *m, char *data, uint16_t size) {
+
+        assert_return(m, -EINVAL);
+
+        return tlv_packet_append_bytes(m, data, size);
+}
+
+int lldp_tlv_packet_open_container(tlv_packet *m, uint16_t type) {
+
+        assert_return(m, -EINVAL);
+
+        m->container_pos = m->pdu + m->length;
+
+        return tlv_packet_append_u16(m, type << 9);
+}
+
+int lldp_tlv_packet_close_container(tlv_packet *m) {
+        uint16_t type;
+
+        assert_return(m, -EINVAL);
+        assert_return(m->container_pos, -EINVAL);
+
+        memcpy(&type, m->container_pos, sizeof(uint16_t));
+
+        type |= htons(((m->pdu + m->length) - (m->container_pos + 2)) & 0x01ff);
+        memcpy(m->container_pos, &type, sizeof(uint16_t));
+
+        return 0;
+}
+
+static inline int tlv_packet_read_internal(tlv_section *m, void **data) {
+
+        assert_return(m->read_pos, -EINVAL);
+
+        *data = m->read_pos;
+
+        return 0;
+}
+
+int tlv_packet_read_u8(tlv_packet *m, uint8_t *data) {
+        void *val;
+        int r;
+
+        assert_return(m, -EINVAL);
+
+        r = tlv_packet_read_internal(m->container,  &val);
+        if (r < 0)
+                return r;
+
+        memcpy(data, val, sizeof(uint8_t));
+
+        m->container->read_pos ++;
+
+        return 0;
+}
+
+int tlv_packet_read_u16(tlv_packet *m, uint16_t *data) {
+        uint16_t t;
+        void *val;
+        int r;
+
+        assert_return(m, -EINVAL);
+
+        r = tlv_packet_read_internal(m->container, &val);
+        if (r < 0)
+                return r;
+
+        memcpy(&t, val, sizeof(uint16_t));
+        *data = ntohs(t);
+
+        m->container->read_pos += 2;
+
+        return 0;
+}
+
+int tlv_packet_read_u32(tlv_packet *m, uint32_t *data) {
+        uint32_t t;
+        void *val;
+        int r;
+
+        assert_return(m, -EINVAL);
+
+        r = tlv_packet_read_internal(m->container, &val);
+        if (r < 0)
+                return r;
+
+        memcpy(&t, val, sizeof(uint32_t));
+        *data = ntohl(t);
+
+        m->container->read_pos += 4;
+
+        return r;
+}
+
+int tlv_packet_read_string(tlv_packet *m, char **data, uint16_t *data_length) {
+        void *val;
+        int r;
+
+        assert_return(m, -EINVAL);
+
+        r = tlv_packet_read_internal(m->container, &val);
+        if (r < 0)
+                return r;
+
+        *data = (char *) val;
+        *data_length =  m->container->length;
+
+        m->container->read_pos += m->container->length;
+
+        return 0;
+}
+
+int tlv_packet_read_bytes(tlv_packet *m, uint8_t **data, uint16_t *data_length) {
+        void *val;
+        int r;
+
+        assert_return(m, -EINVAL);
+
+        r = tlv_packet_read_internal(m->container, &val);
+        if (r < 0)
+                return r;
+
+        *data = (uint8_t *) val;
+        *data_length = m->container->length;
+
+        m->container->read_pos += m->container->length;
+
+        return 0;
+}
+
+/* parse raw TLV packet */
+int tlv_packet_parse_pdu(tlv_packet *m, uint16_t size) {
+        tlv_section *section, *tail;
+        uint16_t t, l;
+        uint8_t *p;
+        int r;
+
+        assert_return(m, -EINVAL);
+        assert_return(size, -EINVAL);
+
+        p = m->pdu;
+
+        /* extract ethernet herader */
+        memcpy(&m->mac, p, ETH_ALEN);
+        p += sizeof(struct ether_header);
+
+        for (l = 0; l <= size; ) {
+                r = tlv_section_new(&section);
+                if (r < 0)
+                        return r;
+
+                memcpy(&t, p, sizeof(uint16_t));
+
+                section->type = ntohs(t) >> 9;
+                section->length = ntohs(t) & 0x01ff;
+
+                if (section->type == LLDP_TYPE_END || section->type >=_LLDP_TYPE_MAX) {
+                        tlv_section_free(section);
+                        break;
+                }
+
+                p += 2;
+                section->data = p;
+
+                LIST_FIND_TAIL(section, m->sections, tail);
+                LIST_INSERT_AFTER(section, m->sections, tail, section);
+
+                p += section->length;
+                l += (section->length + 2);
+        }
+
+        return 0;
+}
+
+int lldp_tlv_packet_enter_container(tlv_packet *m, uint16_t type) {
+        tlv_section *s;
+
+        assert_return(m, -EINVAL);
+
+        LIST_FOREACH(section, s, m->sections)
+                if (s->type == type)
+                        break;
+        if (!s)
+                return -1;
+
+        m->container = s;
+
+        m->container->read_pos = s->data;
+        if (!m->container->read_pos) {
+                m->container = 0;
+                return -1;
+        }
+
+        return 0;
+}
+
+int lldp_tlv_packet_exit_container(tlv_packet *m) {
+        assert_return(m, -EINVAL);
+
+        m->container = 0;
+
+        return 0;
+}