[thirdparty/systemd.git] / src / libsystemd-network / sd-lldp.c

/***
    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 <arpa/inet.h>

#include "sd-lldp.h"

#include "alloc-util.h"
#include "fd-util.h"
#include "fileio.h"
#include "hashmap.h"
#include "lldp-internal.h"
#include "lldp-port.h"
#include "lldp-tlv.h"
#include "prioq.h"
#include "siphash24.h"
#include "string-util.h"

/* Section 10.5.2.2 Reception counters */
struct lldp_agent_statistics {
        uint64_t stats_ageouts_total;
        uint64_t stats_frames_discarded_total;
        uint64_t stats_frames_in_errors_total;
        uint64_t stats_frames_in_total;
        uint64_t stats_tlvs_discarded_total;
        uint64_t stats_tlvs_unrecognized_total;
};

struct sd_lldp {
        lldp_port *port;

        Prioq *by_expiry;
        Hashmap *neighbour_mib;

        sd_lldp_cb_t cb;

        void *userdata;

        lldp_agent_statistics statistics;
};

static void chassis_id_hash_func(const void *p, struct siphash *state) {
        const lldp_chassis_id *id = p;

        assert(id);
        assert(id->data);

        siphash24_compress(&id->length, sizeof(id->length), state);
        siphash24_compress(id->data, id->length, state);
}

static int chassis_id_compare_func(const void *_a, const void *_b) {
        const lldp_chassis_id *a, *b;

        a = _a;
        b = _b;

        assert(!a->length || a->data);
        assert(!b->length || b->data);

        if (a->type != b->type)
                return -1;

        if (a->length != b->length)
                return a->length < b->length ? -1 : 1;

        return memcmp(a->data, b->data, a->length);
}

static const struct hash_ops chassis_id_hash_ops = {
        .hash = chassis_id_hash_func,
        .compare = chassis_id_compare_func
};

static void lldp_mib_delete_objects(sd_lldp *lldp);

static int lldp_receive_frame(sd_lldp *lldp, tlv_packet *tlv) {
        int r;

        assert(lldp);
        assert(tlv);

        /* Remove expired packets */
        if (prioq_size(lldp->by_expiry) > 0)
                lldp_mib_delete_objects(lldp);

        r = lldp_mib_add_objects(lldp->by_expiry, lldp->neighbour_mib, tlv);
        if (r < 0)
                goto out;

        if (lldp->cb)
                lldp->cb(lldp, SD_LLDP_EVENT_UPDATE_INFO, lldp->userdata);

        log_lldp("Packet added. MIB size: %d , PQ size: %d",
                 hashmap_size(lldp->neighbour_mib),
                 prioq_size(lldp->by_expiry));

        lldp->statistics.stats_frames_in_total ++;

 out:
        if (r < 0)
                log_lldp("Receive frame failed: %s", strerror(-r));

        return 0;
}

/* 10.3.2 LLDPDU validation: rxProcessFrame() */
int lldp_handle_packet(tlv_packet *tlv, uint16_t length) {
        bool system_description = false, system_name = false, chassis_id = false;
        bool malformed = false, port_id = false, ttl = false, end = false;
        uint16_t type, len, i, l, t;
        lldp_port *port;
        uint8_t *p, *q;
        sd_lldp *lldp;
        int r;

        assert(tlv);
        assert(length > 0);

        port = (lldp_port *) tlv->userdata;
        lldp = (sd_lldp *) port->userdata;

        if (lldp->port->status == LLDP_PORT_STATUS_DISABLED) {
                log_lldp("Port: %s is disabled. Dropping.", lldp->port->ifname);
                goto out;
        }

        p = tlv->pdu;
        p += sizeof(struct ether_header);

        for (i = 1, l = 0; l <= length; i++) {

                memcpy(&t, p, sizeof(uint16_t));

                type = ntohs(t) >> 9;
                len = ntohs(t) & 0x01ff;

                if (type == LLDP_TYPE_END) {
                        if (len != 0) {
                                log_lldp("TLV type end must be length 0 (not %d). Dropping.", len);

                                malformed = true;
                                goto out;
                        }

                        end = true;

                        break;
                } else if (type >=_LLDP_TYPE_MAX) {
                        log_lldp("TLV type: %d not recognized. Dropping.", type);

                        malformed = true;
                        goto out;
                }

                /* skip type and length encoding */
                p += 2;
                q = p;

                p += len;
                l += (len + 2);

                if (i <= 3) {
                        if (i != type) {
                                log_lldp("TLV missing or out of order. Dropping.");

                                malformed = true;
                                goto out;
                        }
                }

                switch(type) {
                case LLDP_TYPE_CHASSIS_ID:

                        if (len < 2) {
                                log_lldp("Received malformed Chassis ID TLV length: %d. Dropping.", len);

                                malformed = true;
                                goto out;
                        }

                        if (chassis_id) {
                                log_lldp("Duplicate Chassis ID TLV found. Dropping.");

                                malformed = true;
                                goto out;
                        }

                        /* Look what subtype it has */
                        if (*q == LLDP_CHASSIS_SUBTYPE_RESERVED || *q > LLDP_CHASSIS_SUBTYPE_LOCALLY_ASSIGNED) {
                                log_lldp("Unknown subtype: %d found in Chassis ID TLV. Dropping.", *q);

                                malformed = true;
                                goto out;

                        }

                        chassis_id = true;

                        break;
                case LLDP_TYPE_PORT_ID:

                        if (len < 2) {
                                log_lldp("Received malformed Port ID TLV length: %d. Dropping.", len);

                                malformed = true;
                                goto out;
                        }

                        if (port_id) {
                                log_lldp("Duplicate Port ID TLV found. Dropping.");

                                malformed = true;
                                goto out;
                        }

                        /* Look what subtype it has */
                        if (*q == LLDP_PORT_SUBTYPE_RESERVED || *q > LLDP_PORT_SUBTYPE_LOCALLY_ASSIGNED) {
                                log_lldp("Unknown subtype: %d found in Port ID TLV. Dropping.", *q);

                                malformed = true;
                                goto out;

                        }

                        port_id = true;

                        break;
                case LLDP_TYPE_TTL:

                        if(len != 2) {
                                log_lldp("Received invalid TTL TLV lenth: %d. Dropping.", len);

                                malformed = true;
                                goto out;
                        }

                        if (ttl) {
                                log_lldp("Duplicate TTL TLV found. Dropping.");

                                malformed = true;
                                goto out;
                        }

                        ttl = true;

                        break;
                case LLDP_TYPE_SYSTEM_NAME:

                        /* According to RFC 1035 the length of a FQDN is limited to 255 characters */
                        if (len > 255) {
                                log_lldp("Received invalid system name length: %d. Dropping.", len);
                                malformed = true;
                                goto out;
                        }

                        if (system_name) {
                                log_lldp("Duplicate system name found. Dropping.");
                                malformed = true;
                                goto out;
                        }

                        system_name = true;

                        break;
                case LLDP_TYPE_SYSTEM_DESCRIPTION:

                        /* 0 <= n <= 255 octets */
                        if (len > 255) {
                                log_lldp("Received invalid system description length: %d. Dropping.", len);
                                malformed = true;
                                goto out;
                        }

                        if (system_description) {
                                log_lldp("Duplicate system description found. Dropping.");
                                malformed = true;
                                goto out;
                        }

                        system_description = true;
                        break;
                default:

                        if (len == 0) {
                                log_lldp("TLV type: %d length 0 received. Dropping.", type);

                                malformed = true;
                                goto out;
                        }
                        break;
                }
        }

        if(!chassis_id || !port_id || !ttl || !end) {
                log_lldp("One or more mandatory TLV missing. Dropping.");

                malformed = true;
                goto out;

        }

        r = tlv_packet_parse_pdu(tlv, length);
        if (r < 0) {
                log_lldp("Failed to parse the TLV. Dropping.");

                malformed = true;
                goto out;
        }

        return lldp_receive_frame(lldp, tlv);

 out:
        if (malformed) {
                lldp->statistics.stats_frames_discarded_total ++;
                lldp->statistics.stats_frames_in_errors_total ++;
        }

        sd_lldp_packet_unref(tlv);

        return 0;
}

static int ttl_expiry_item_prioq_compare_func(const void *a, const void *b) {
        const lldp_neighbour_port *p = a, *q = b;

        if (p->until < q->until)
                return -1;

        if (p->until > q->until)
                return 1;

        return 0;
}

/* 10.5.5.2.1 mibDeleteObjects ()
 * The mibDeleteObjects () procedure deletes all information in the LLDP remote
 * systems MIB associated with the MSAP identifier if an LLDPDU is received with
 * an rxTTL value of zero (see 10.3.2) or the timing counter rxInfoTTL expires. */

static void lldp_mib_delete_objects(sd_lldp *lldp) {
        lldp_neighbour_port *p;
        usec_t t = 0;

        /* Remove all entries that are past their TTL */
        for (;;) {

                if (prioq_size(lldp->by_expiry) <= 0)
                        break;

                p = prioq_peek(lldp->by_expiry);
                if (!p)
                        break;

                if (t <= 0)
                        t = now(clock_boottime_or_monotonic());

                if (p->until > t)
                        break;

                lldp_neighbour_port_remove_and_free(p);

                lldp->statistics.stats_ageouts_total ++;
        }
}

static void lldp_mib_objects_flush(sd_lldp *lldp) {
        lldp_neighbour_port *p, *q;
        lldp_chassis *c;

        assert(lldp);
        assert(lldp->neighbour_mib);
        assert(lldp->by_expiry);

        /* Drop all packets */
        while ((c = hashmap_steal_first(lldp->neighbour_mib))) {

                LIST_FOREACH_SAFE(port, p, q, c->ports) {
                        lldp_neighbour_port_remove_and_free(p);
                }
        }

        assert(hashmap_size(lldp->neighbour_mib) == 0);
        assert(prioq_size(lldp->by_expiry) == 0);
}

int sd_lldp_save(sd_lldp *lldp, const char *lldp_file) {
        _cleanup_free_ char *temp_path = NULL;
        _cleanup_fclose_ FILE *f = NULL;
        uint8_t *mac, *port_id, type;
        lldp_neighbour_port *p;
        uint16_t data = 0, length = 0;
        char buf[LINE_MAX];
        lldp_chassis *c;
        usec_t time;
        Iterator i;
        int r;

        assert(lldp);
        assert(lldp_file);

        r = fopen_temporary(lldp_file, &f, &temp_path);
        if (r < 0)
                goto fail;

        fchmod(fileno(f), 0644);

        HASHMAP_FOREACH(c, lldp->neighbour_mib, i) {
                LIST_FOREACH(port, p, c->ports) {
                        _cleanup_free_ char *s = NULL;
                        char *k, *t;

                        r = sd_lldp_packet_read_chassis_id(p->packet, &type, &mac, &length);
                        if (r < 0)
                                continue;

                        sprintf(buf, "'_Chassis=%02x:%02x:%02x:%02x:%02x:%02x' '_CType=%d' ",
                                mac[0], mac[1], mac[2], mac[3], mac[4], mac[5], type);

                        s = strdup(buf);
                        if (!s) {
                                r = -ENOMEM;
                                goto fail;
                        }

                        r = sd_lldp_packet_read_port_id(p->packet, &type, &port_id, &length);
                        if (r < 0)
                                continue;

                        if (type != LLDP_PORT_SUBTYPE_MAC_ADDRESS) {
                                k = strndup((char *) port_id, length -1);
                                if (!k) {
                                        r = -ENOMEM;
                                        goto fail;
                                }

                                sprintf(buf, "'_Port=%s' '_PType=%d' ", k , type);
                                free(k);
                        } else {
                                mac = port_id;
                                sprintf(buf, "'_Port=%02x:%02x:%02x:%02x:%02x:%02x' '_PType=%d' ",
                                        mac[0], mac[1], mac[2], mac[3], mac[4], mac[5], type);
                        }

                        k = strappend(s, buf);
                        if (!k) {
                                r = -ENOMEM;
                                goto fail;
                        }

                        free(s);
                        s = k;

                        time = now(clock_boottime_or_monotonic());

                        /* Don't write expired packets */
                        if (time - p->until <= 0)
                                continue;

                        sprintf(buf, "'_TTL="USEC_FMT"' ", p->until);

                        k = strappend(s, buf);
                        if (!k) {
                                r = -ENOMEM;
                                goto fail;
                        }

                        free(s);
                        s = k;

                        r = sd_lldp_packet_read_system_name(p->packet, &k, &length);
                        if (r < 0)
                                k = strappend(s, "'_NAME=N/A' ");
                        else {
                                t = strndup(k, length);
                                if (!t) {
                                        r = -ENOMEM;
                                        goto fail;
                                }

                                k = strjoin(s, "'_NAME=", t, "' ", NULL);
                                free(t);
                        }

                        if (!k) {
                                r = -ENOMEM;
                                goto fail;
                        }

                        free(s);
                        s = k;

                        (void) sd_lldp_packet_read_system_capability(p->packet, &data);

                        sprintf(buf, "'_CAP=%x'", data);

                        k = strappend(s, buf);
                        if (!k) {
                                r = -ENOMEM;
                                goto fail;
                        }

                        free(s);
                        s = k;

                        fprintf(f, "%s\n", s);
                }
        }

        r = fflush_and_check(f);
        if (r < 0)
                goto fail;

        if (rename(temp_path, lldp_file) < 0) {
                r = -errno;
                goto fail;
        }

        return 0;

 fail:
        if (temp_path)
                (void) unlink(temp_path);

        return log_error_errno(r, "Failed to save lldp data %s: %m", lldp_file);
}

int sd_lldp_start(sd_lldp *lldp) {
        int r;

        assert_return(lldp, -EINVAL);
        assert_return(lldp->port, -EINVAL);

        lldp->port->status = LLDP_PORT_STATUS_ENABLED;

        r = lldp_port_start(lldp->port);
        if (r < 0) {
                log_lldp("Failed to start Port : %s , %s",
                         lldp->port->ifname,
                         strerror(-r));

                return r;
        }

        return 0;
}

int sd_lldp_stop(sd_lldp *lldp) {
        int r;

        assert_return(lldp, -EINVAL);
        assert_return(lldp->port, -EINVAL);

        lldp->port->status = LLDP_PORT_STATUS_DISABLED;

        r = lldp_port_stop(lldp->port);
        if (r < 0)
                return r;

        lldp_mib_objects_flush(lldp);

        return 0;
}

int sd_lldp_attach_event(sd_lldp *lldp, sd_event *event, int priority) {
        int r;

        assert_return(lldp, -EINVAL);
        assert_return(!lldp->port->event, -EBUSY);

        if (event)
                lldp->port->event = sd_event_ref(event);
        else {
                r = sd_event_default(&lldp->port->event);
                if (r < 0)
                        return r;
        }

        lldp->port->event_priority = priority;

        return 0;
}

int sd_lldp_detach_event(sd_lldp *lldp) {

        assert_return(lldp, -EINVAL);

        lldp->port->event = sd_event_unref(lldp->port->event);

        return 0;
}

int sd_lldp_set_callback(sd_lldp *lldp, sd_lldp_cb_t cb, void *userdata) {
        assert_return(lldp, -EINVAL);

        lldp->cb = cb;
        lldp->userdata = userdata;

        return 0;
}

sd_lldp* sd_lldp_unref(sd_lldp *lldp) {

        if (!lldp)
                return NULL;

        /* Drop all packets */
        lldp_mib_objects_flush(lldp);

        lldp_port_free(lldp->port);

        hashmap_free(lldp->neighbour_mib);
        prioq_free(lldp->by_expiry);

        free(lldp);
        return NULL;
}

int sd_lldp_new(int ifindex,
                const char *ifname,
                const struct ether_addr *mac,
                sd_lldp **ret) {
        _cleanup_(sd_lldp_unrefp) sd_lldp *lldp = NULL;
        int r;

        assert_return(ret, -EINVAL);
        assert_return(ifindex > 0, -EINVAL);
        assert_return(ifname, -EINVAL);
        assert_return(mac, -EINVAL);

        lldp = new0(sd_lldp, 1);
        if (!lldp)
                return -ENOMEM;

        r = lldp_port_new(ifindex, ifname, mac, lldp, &lldp->port);
        if (r < 0)
                return r;

        lldp->neighbour_mib = hashmap_new(&chassis_id_hash_ops);
        if (!lldp->neighbour_mib)
                return -ENOMEM;

        r = prioq_ensure_allocated(&lldp->by_expiry,
                                   ttl_expiry_item_prioq_compare_func);
        if (r < 0)
                return r;

        *ret = lldp;
        lldp = NULL;

        return 0;
}

int sd_lldp_get_packets(sd_lldp *lldp, sd_lldp_packet ***tlvs) {
        lldp_neighbour_port *p;
        lldp_chassis *c;
        Iterator iter;
        unsigned count = 0, i;

        assert_return(lldp, -EINVAL);
        assert_return(tlvs, -EINVAL);

        HASHMAP_FOREACH(c, lldp->neighbour_mib, iter) {
                LIST_FOREACH(port, p, c->ports)
                        count++;
        }

        if (!count) {
                *tlvs = NULL;
                return 0;
        }

        *tlvs = new(sd_lldp_packet *, count);
        if (!*tlvs)
                return -ENOMEM;

        i = 0;
        HASHMAP_FOREACH(c, lldp->neighbour_mib, iter) {
                LIST_FOREACH(port, p, c->ports)
                        (*tlvs)[i++] = sd_lldp_packet_ref(p->packet);
        }

        return count;
}
Commit	Line	Data
ad1ad5c8 SS	1	/***
	2	This file is part of systemd.
	3
	4	Copyright (C) 2014 Tom Gundersen
	5	Copyright (C) 2014 Susant Sahani
	6
	7	systemd is free software; you can redistribute it and/or modify it
	8	under the terms of the GNU Lesser General Public License as published by
	9	the Free Software Foundation; either version 2.1 of the License, or
	10	(at your option) any later version.
	11
	12	systemd is distributed in the hope that it will be useful, but
	13	WITHOUT ANY WARRANTY; without even the implied warranty of
	14	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
	15	Lesser General Public License for more details.
	16
	17	You should have received a copy of the GNU Lesser General Public License
	18	along with systemd; If not, see <http://www.gnu.org/licenses/>.
	19	***/
	20
	21	#include <arpa/inet.h>
	22
ad1ad5c8	23	#include "sd-lldp.h"
07630cea	24
b5efdb8a	25	#include "alloc-util.h"
3ffd4af2	26	#include "fd-util.h"
0d39fa9c	27	#include "fileio.h"
07630cea	28	#include "hashmap.h"
ad1ad5c8	29	#include "lldp-internal.h"
07630cea LP	30	#include "lldp-port.h"
07630cea LP	31	#include "lldp-tlv.h"
07630cea LP	32	#include "prioq.h"
	33	#include "siphash24.h"
	34	#include "string-util.h"
49699bac	35
ad1ad5c8	36	/* Section 10.5.2.2 Reception counters */
6b3fd9a1	37	struct lldp_agent_statistics {
ad1ad5c8 SS	38	uint64_t stats_ageouts_total;
	39	uint64_t stats_frames_discarded_total;
	40	uint64_t stats_frames_in_errors_total;
	41	uint64_t stats_frames_in_total;
	42	uint64_t stats_tlvs_discarded_total;
	43	uint64_t stats_tlvs_unrecognized_total;
	44	};
	45
	46	struct sd_lldp {
	47	lldp_port *port;
	48
	49	Prioq *by_expiry;
	50	Hashmap *neighbour_mib;
	51
49699bac SS	52	sd_lldp_cb_t cb;
	53
	54	void *userdata;
	55
6b3fd9a1	56	lldp_agent_statistics statistics;
ad1ad5c8 SS	57	};
ad1ad5c8 SS	58
b826ab58	59	static void chassis_id_hash_func(const void p, struct siphash state) {
ad1ad5c8 SS	60	const lldp_chassis_id *id = p;
	61
	62	assert(id);
b826ab58	63	assert(id->data);
ad1ad5c8	64
1e2527a6	65	siphash24_compress(&id->length, sizeof(id->length), state);
b826ab58	66	siphash24_compress(id->data, id->length, state);
ad1ad5c8 SS	67	}
	68
	69	static int chassis_id_compare_func(const void _a, const void _b) {
	70	const lldp_chassis_id a, b;
	71
	72	a = _a;
	73	b = _b;
	74
	75	assert(!a->length \|\| a->data);
	76	assert(!b->length \|\| b->data);
	77
	78	if (a->type != b->type)
	79	return -1;
	80
	81	if (a->length != b->length)
	82	return a->length < b->length ? -1 : 1;
	83
	84	return memcmp(a->data, b->data, a->length);
	85	}
	86
	87	static const struct hash_ops chassis_id_hash_ops = {
	88	.hash = chassis_id_hash_func,
	89	.compare = chassis_id_compare_func
	90	};
	91
	92	static void lldp_mib_delete_objects(sd_lldp *lldp);
	93
	94	static int lldp_receive_frame(sd_lldp lldp, tlv_packet tlv) {
	95	int r;
	96
	97	assert(lldp);
	98	assert(tlv);
	99
	100	/* Remove expired packets */
c7d264ff	101	if (prioq_size(lldp->by_expiry) > 0)
ad1ad5c8 SS	102	lldp_mib_delete_objects(lldp);
	103
	104	r = lldp_mib_add_objects(lldp->by_expiry, lldp->neighbour_mib, tlv);
	105	if (r < 0)
	106	goto out;
	107
c7d264ff LP	108	if (lldp->cb)
c7d264ff LP	109	lldp->cb(lldp, SD_LLDP_EVENT_UPDATE_INFO, lldp->userdata);
49699bac	110
ad1ad5c8 SS	111	log_lldp("Packet added. MIB size: %d , PQ size: %d",
	112	hashmap_size(lldp->neighbour_mib),
	113	prioq_size(lldp->by_expiry));
	114
6b3fd9a1	115	lldp->statistics.stats_frames_in_total ++;
ad1ad5c8	116
ad1ad5c8 SS	117	out:
	118	if (r < 0)
	119	log_lldp("Receive frame failed: %s", strerror(-r));
	120
	121	return 0;
	122	}
	123
	124	/* 10.3.2 LLDPDU validation: rxProcessFrame() */
	125	int lldp_handle_packet(tlv_packet *tlv, uint16_t length) {
859c37b1 SS	126	bool system_description = false, system_name = false, chassis_id = false;
859c37b1 SS	127	bool malformed = false, port_id = false, ttl = false, end = false;
ad1ad5c8	128	uint16_t type, len, i, l, t;
ad1ad5c8 SS	129	lldp_port *port;
	130	uint8_t p, q;
	131	sd_lldp *lldp;
	132	int r;
	133
	134	assert(tlv);
	135	assert(length > 0);
	136
	137	port = (lldp_port *) tlv->userdata;
	138	lldp = (sd_lldp *) port->userdata;
	139
	140	if (lldp->port->status == LLDP_PORT_STATUS_DISABLED) {
e5c42b7f	141	log_lldp("Port: %s is disabled. Dropping.", lldp->port->ifname);
ad1ad5c8 SS	142	goto out;
	143	}
	144
	145	p = tlv->pdu;
	146	p += sizeof(struct ether_header);
	147
	148	for (i = 1, l = 0; l <= length; i++) {
	149
	150	memcpy(&t, p, sizeof(uint16_t));
	151
	152	type = ntohs(t) >> 9;
	153	len = ntohs(t) & 0x01ff;
	154
	155	if (type == LLDP_TYPE_END) {
	156	if (len != 0) {
e5c42b7f	157	log_lldp("TLV type end must be length 0 (not %d). Dropping.", len);
ad1ad5c8 SS	158
	159	malformed = true;
	160	goto out;
	161	}
	162
	163	end = true;
	164
	165	break;
	166	} else if (type >=_LLDP_TYPE_MAX) {
e5c42b7f	167	log_lldp("TLV type: %d not recognized. Dropping.", type);
ad1ad5c8 SS	168
	169	malformed = true;
	170	goto out;
	171	}
	172
d8c89d61	173	/* skip type and length encoding */
ad1ad5c8 SS	174	p += 2;
	175	q = p;
	176
	177	p += len;
	178	l += (len + 2);
	179
	180	if (i <= 3) {
	181	if (i != type) {
e5c42b7f	182	log_lldp("TLV missing or out of order. Dropping.");
ad1ad5c8 SS	183
	184	malformed = true;
	185	goto out;
	186	}
	187	}
	188
	189	switch(type) {
	190	case LLDP_TYPE_CHASSIS_ID:
	191
	192	if (len < 2) {
e5c42b7f	193	log_lldp("Received malformed Chassis ID TLV length: %d. Dropping.", len);
ad1ad5c8 SS	194
	195	malformed = true;
	196	goto out;
	197	}
	198
	199	if (chassis_id) {
e5c42b7f	200	log_lldp("Duplicate Chassis ID TLV found. Dropping.");
ad1ad5c8 SS	201
	202	malformed = true;
	203	goto out;
	204	}
	205
	206	/* Look what subtype it has */
859c37b1	207	if (q == LLDP_CHASSIS_SUBTYPE_RESERVED \|\| q > LLDP_CHASSIS_SUBTYPE_LOCALLY_ASSIGNED) {
e5c42b7f	208	log_lldp("Unknown subtype: %d found in Chassis ID TLV. Dropping.", *q);
ad1ad5c8 SS	209
	210	malformed = true;
	211	goto out;
	212
	213	}
	214
	215	chassis_id = true;
	216
	217	break;
	218	case LLDP_TYPE_PORT_ID:
	219
	220	if (len < 2) {
e5c42b7f	221	log_lldp("Received malformed Port ID TLV length: %d. Dropping.", len);
ad1ad5c8 SS	222
	223	malformed = true;
	224	goto out;
	225	}
	226
	227	if (port_id) {
e5c42b7f	228	log_lldp("Duplicate Port ID TLV found. Dropping.");
ad1ad5c8 SS	229
	230	malformed = true;
	231	goto out;
	232	}
	233
	234	/* Look what subtype it has */
859c37b1	235	if (q == LLDP_PORT_SUBTYPE_RESERVED \|\| q > LLDP_PORT_SUBTYPE_LOCALLY_ASSIGNED) {
e5c42b7f	236	log_lldp("Unknown subtype: %d found in Port ID TLV. Dropping.", *q);
ad1ad5c8 SS	237
	238	malformed = true;
	239	goto out;
	240
	241	}
	242
	243	port_id = true;
	244
	245	break;
	246	case LLDP_TYPE_TTL:
	247
	248	if(len != 2) {
e5c42b7f	249	log_lldp("Received invalid TTL TLV lenth: %d. Dropping.", len);
ad1ad5c8 SS	250
	251	malformed = true;
	252	goto out;
	253	}
	254
	255	if (ttl) {
e5c42b7f	256	log_lldp("Duplicate TTL TLV found. Dropping.");
ad1ad5c8 SS	257
	258	malformed = true;
	259	goto out;
	260	}
	261
	262	ttl = true;
	263
859c37b1 SS	264	break;
	265	case LLDP_TYPE_SYSTEM_NAME:
	266
	267	/* According to RFC 1035 the length of a FQDN is limited to 255 characters */
	268	if (len > 255) {
d790d8c1	269	log_lldp("Received invalid system name length: %d. Dropping.", len);
859c37b1 SS	270	malformed = true;
	271	goto out;
	272	}
	273
	274	if (system_name) {
	275	log_lldp("Duplicate system name found. Dropping.");
	276	malformed = true;
	277	goto out;
	278	}
	279
	280	system_name = true;
	281
	282	break;
	283	case LLDP_TYPE_SYSTEM_DESCRIPTION:
	284
	285	/* 0 <= n <= 255 octets */
	286	if (len > 255) {
	287	log_lldp("Received invalid system description length: %d. Dropping.", len);
	288	malformed = true;
	289	goto out;
	290	}
	291
	292	if (system_description) {
	293	log_lldp("Duplicate system description found. Dropping.");
	294	malformed = true;
	295	goto out;
	296	}
	297
	298	system_description = true;
ad1ad5c8 SS	299	break;
	300	default:
	301
	302	if (len == 0) {
e5c42b7f	303	log_lldp("TLV type: %d length 0 received. Dropping.", type);
ad1ad5c8 SS	304
	305	malformed = true;
	306	goto out;
	307	}
	308	break;
	309	}
	310	}
	311
	312	if(!chassis_id \|\| !port_id \|\| !ttl \|\| !end) {
e5c42b7f	313	log_lldp("One or more mandatory TLV missing. Dropping.");
ad1ad5c8 SS	314
	315	malformed = true;
	316	goto out;
	317
	318	}
	319
	320	r = tlv_packet_parse_pdu(tlv, length);
	321	if (r < 0) {
e5c42b7f	322	log_lldp("Failed to parse the TLV. Dropping.");
ad1ad5c8 SS	323
	324	malformed = true;
	325	goto out;
	326	}
	327
	328	return lldp_receive_frame(lldp, tlv);
	329
	330	out:
	331	if (malformed) {
6b3fd9a1 TH	332	lldp->statistics.stats_frames_discarded_total ++;
6b3fd9a1 TH	333	lldp->statistics.stats_frames_in_errors_total ++;
ad1ad5c8 SS	334	}
ad1ad5c8 SS	335
176c355b	336	sd_lldp_packet_unref(tlv);
ad1ad5c8 SS	337
	338	return 0;
	339	}
	340
	341	static int ttl_expiry_item_prioq_compare_func(const void a, const void b) {
	342	const lldp_neighbour_port p = a, q = b;
	343
	344	if (p->until < q->until)
	345	return -1;
	346
	347	if (p->until > q->until)
	348	return 1;
	349
	350	return 0;
	351	}
	352
	353	/* 10.5.5.2.1 mibDeleteObjects ()
	354	* The mibDeleteObjects () procedure deletes all information in the LLDP remote
	355	* systems MIB associated with the MSAP identifier if an LLDPDU is received with
	356	* an rxTTL value of zero (see 10.3.2) or the timing counter rxInfoTTL expires. */
	357
	358	static void lldp_mib_delete_objects(sd_lldp *lldp) {
	359	lldp_neighbour_port *p;
	360	usec_t t = 0;
	361
	362	/* Remove all entries that are past their TTL */
	363	for (;;) {
	364
	365	if (prioq_size(lldp->by_expiry) <= 0)
	366	break;
	367
	368	p = prioq_peek(lldp->by_expiry);
	369	if (!p)
	370	break;
	371
	372	if (t <= 0)
27ec691b	373	t = now(clock_boottime_or_monotonic());
ad1ad5c8 SS	374
	375	if (p->until > t)
	376	break;
	377
	378	lldp_neighbour_port_remove_and_free(p);
	379
6b3fd9a1	380	lldp->statistics.stats_ageouts_total ++;
ad1ad5c8 SS	381	}
	382	}
	383
	384	static void lldp_mib_objects_flush(sd_lldp *lldp) {
	385	lldp_neighbour_port p, q;
	386	lldp_chassis *c;
	387
	388	assert(lldp);
	389	assert(lldp->neighbour_mib);
	390	assert(lldp->by_expiry);
	391
	392	/* Drop all packets */
	393	while ((c = hashmap_steal_first(lldp->neighbour_mib))) {
	394
	395	LIST_FOREACH_SAFE(port, p, q, c->ports) {
	396	lldp_neighbour_port_remove_and_free(p);
	397	}
	398	}
	399
	400	assert(hashmap_size(lldp->neighbour_mib) == 0);
	401	assert(prioq_size(lldp->by_expiry) == 0);
	402	}
	403
49699bac	404	int sd_lldp_save(sd_lldp lldp, const char lldp_file) {
49699bac SS	405	_cleanup_free_ char *temp_path = NULL;
	406	_cleanup_fclose_ FILE *f = NULL;
	407	uint8_t mac, port_id, type;
	408	lldp_neighbour_port *p;
	409	uint16_t data = 0, length = 0;
	410	char buf[LINE_MAX];
	411	lldp_chassis *c;
	412	usec_t time;
	413	Iterator i;
	414	int r;
	415
	416	assert(lldp);
	417	assert(lldp_file);
	418
	419	r = fopen_temporary(lldp_file, &f, &temp_path);
	420	if (r < 0)
dacd6cee	421	goto fail;
49699bac SS	422
	423	fchmod(fileno(f), 0644);
	424
	425	HASHMAP_FOREACH(c, lldp->neighbour_mib, i) {
	426	LIST_FOREACH(port, p, c->ports) {
ee14ebf2 DH	427	_cleanup_free_ char *s = NULL;
ee14ebf2 DH	428	char k, t;
49699bac	429
176c355b	430	r = sd_lldp_packet_read_chassis_id(p->packet, &type, &mac, &length);
49699bac SS	431	if (r < 0)
	432	continue;
	433
49699bac SS	434	sprintf(buf, "'_Chassis=%02x:%02x:%02x:%02x:%02x:%02x' '_CType=%d' ",
	435	mac[0], mac[1], mac[2], mac[3], mac[4], mac[5], type);
	436
	437	s = strdup(buf);
dacd6cee LP	438	if (!s) {
	439	r = -ENOMEM;
	440	goto fail;
	441	}
49699bac	442
176c355b	443	r = sd_lldp_packet_read_port_id(p->packet, &type, &port_id, &length);
ee14ebf2	444	if (r < 0)
49699bac	445	continue;
49699bac	446
49699bac	447	if (type != LLDP_PORT_SUBTYPE_MAC_ADDRESS) {
49699bac	448	k = strndup((char *) port_id, length -1);
dacd6cee LP	449	if (!k) {
	450	r = -ENOMEM;
	451	goto fail;
	452	}
49699bac SS	453
49699bac SS	454	sprintf(buf, "'_Port=%s' '_PType=%d' ", k , type);
49699bac SS	455	free(k);
49699bac SS	456	} else {
49699bac	457	mac = port_id;
49699bac SS	458	sprintf(buf, "'_Port=%02x:%02x:%02x:%02x:%02x:%02x' '_PType=%d' ",
49699bac SS	459	mac[0], mac[1], mac[2], mac[3], mac[4], mac[5], type);
49699bac SS	460	}
49699bac SS	461
ee14ebf2	462	k = strappend(s, buf);
dacd6cee LP	463	if (!k) {
	464	r = -ENOMEM;
	465	goto fail;
	466	}
49699bac SS	467
49699bac SS	468	free(s);
ee14ebf2	469	s = k;
49699bac	470
27ec691b	471	time = now(clock_boottime_or_monotonic());
49699bac SS	472
49699bac SS	473	/* Don't write expired packets */
ee14ebf2	474	if (time - p->until <= 0)
49699bac	475	continue;
49699bac	476
ef753253	477	sprintf(buf, "'_TTL="USEC_FMT"' ", p->until);
49699bac	478
ee14ebf2	479	k = strappend(s, buf);
dacd6cee LP	480	if (!k) {
	481	r = -ENOMEM;
	482	goto fail;
	483	}
49699bac SS	484
49699bac SS	485	free(s);
ee14ebf2	486	s = k;
49699bac	487
176c355b	488	r = sd_lldp_packet_read_system_name(p->packet, &k, &length);
49699bac SS	489	if (r < 0)
	490	k = strappend(s, "'_NAME=N/A' ");
	491	else {
	492	t = strndup(k, length);
dacd6cee LP	493	if (!t) {
	494	r = -ENOMEM;
	495	goto fail;
	496	}
49699bac SS	497
49699bac SS	498	k = strjoin(s, "'_NAME=", t, "' ", NULL);
ee14ebf2	499	free(t);
49699bac SS	500	}
49699bac SS	501
dacd6cee LP	502	if (!k) {
	503	r = -ENOMEM;
	504	goto fail;
	505	}
49699bac SS	506
	507	free(s);
	508	s = k;
	509
176c355b	510	(void) sd_lldp_packet_read_system_capability(p->packet, &data);
49699bac SS	511
	512	sprintf(buf, "'_CAP=%x'", data);
	513
ee14ebf2	514	k = strappend(s, buf);
dacd6cee LP	515	if (!k) {
	516	r = -ENOMEM;
	517	goto fail;
	518	}
49699bac	519
49699bac	520	free(s);
ee14ebf2 DH	521	s = k;
	522
	523	fprintf(f, "%s\n", s);
49699bac SS	524	}
49699bac SS	525	}
49699bac	526
dacd6cee LP	527	r = fflush_and_check(f);
	528	if (r < 0)
	529	goto fail;
49699bac	530
dacd6cee	531	if (rename(temp_path, lldp_file) < 0) {
49699bac	532	r = -errno;
dacd6cee	533	goto fail;
49699bac SS	534	}
49699bac SS	535
dacd6cee LP	536	return 0;
	537
	538	fail:
	539	if (temp_path)
	540	(void) unlink(temp_path);
49699bac	541
dacd6cee	542	return log_error_errno(r, "Failed to save lldp data %s: %m", lldp_file);
49699bac SS	543	}
49699bac SS	544
ad1ad5c8 SS	545	int sd_lldp_start(sd_lldp *lldp) {
	546	int r;
	547
	548	assert_return(lldp, -EINVAL);
	549	assert_return(lldp->port, -EINVAL);
	550
	551	lldp->port->status = LLDP_PORT_STATUS_ENABLED;
	552
	553	r = lldp_port_start(lldp->port);
	554	if (r < 0) {
	555	log_lldp("Failed to start Port : %s , %s",
	556	lldp->port->ifname,
	557	strerror(-r));
49699bac	558
ad1ad5c8 SS	559	return r;
	560	}
	561
	562	return 0;
	563	}
	564
	565	int sd_lldp_stop(sd_lldp *lldp) {
	566	int r;
	567
	568	assert_return(lldp, -EINVAL);
	569	assert_return(lldp->port, -EINVAL);
	570
	571	lldp->port->status = LLDP_PORT_STATUS_DISABLED;
	572
	573	r = lldp_port_stop(lldp->port);
	574	if (r < 0)
	575	return r;
	576
	577	lldp_mib_objects_flush(lldp);
	578
	579	return 0;
	580	}
	581
	582	int sd_lldp_attach_event(sd_lldp lldp, sd_event event, int priority) {
	583	int r;
	584
	585	assert_return(lldp, -EINVAL);
	586	assert_return(!lldp->port->event, -EBUSY);
	587
	588	if (event)
	589	lldp->port->event = sd_event_ref(event);
	590	else {
	591	r = sd_event_default(&lldp->port->event);
	592	if (r < 0)
	593	return r;
	594	}
	595
	596	lldp->port->event_priority = priority;
	597
	598	return 0;
	599	}
	600
	601	int sd_lldp_detach_event(sd_lldp *lldp) {
	602
	603	assert_return(lldp, -EINVAL);
	604
	605	lldp->port->event = sd_event_unref(lldp->port->event);
	606
	607	return 0;
	608	}
	609
49699bac SS	610	int sd_lldp_set_callback(sd_lldp lldp, sd_lldp_cb_t cb, void userdata) {
	611	assert_return(lldp, -EINVAL);
	612
	613	lldp->cb = cb;
	614	lldp->userdata = userdata;
	615
	616	return 0;
	617	}
	618
4afd3348	619	sd_lldp* sd_lldp_unref(sd_lldp *lldp) {
ad1ad5c8 SS	620
ad1ad5c8 SS	621	if (!lldp)
4afd3348	622	return NULL;
ad1ad5c8 SS	623
	624	/* Drop all packets */
	625	lldp_mib_objects_flush(lldp);
	626
	627	lldp_port_free(lldp->port);
	628
	629	hashmap_free(lldp->neighbour_mib);
	630	prioq_free(lldp->by_expiry);
	631
	632	free(lldp);
4afd3348	633	return NULL;
ad1ad5c8 SS	634	}
	635
	636	int sd_lldp_new(int ifindex,
7a6f1457 TG	637	const char *ifname,
7a6f1457 TG	638	const struct ether_addr *mac,
ad1ad5c8	639	sd_lldp **ret) {
4afd3348	640	_cleanup_(sd_lldp_unrefp) sd_lldp *lldp = NULL;
ad1ad5c8 SS	641	int r;
	642
	643	assert_return(ret, -EINVAL);
	644	assert_return(ifindex > 0, -EINVAL);
	645	assert_return(ifname, -EINVAL);
	646	assert_return(mac, -EINVAL);
	647
	648	lldp = new0(sd_lldp, 1);
	649	if (!lldp)
	650	return -ENOMEM;
	651
	652	r = lldp_port_new(ifindex, ifname, mac, lldp, &lldp->port);
	653	if (r < 0)
	654	return r;
	655
	656	lldp->neighbour_mib = hashmap_new(&chassis_id_hash_ops);
	657	if (!lldp->neighbour_mib)
	658	return -ENOMEM;
	659
	660	r = prioq_ensure_allocated(&lldp->by_expiry,
	661	ttl_expiry_item_prioq_compare_func);
	662	if (r < 0)
	663	return r;
	664
	665	*ret = lldp;
	666	lldp = NULL;
	667
	668	return 0;
	669	}
7434883c BG	670
	671	int sd_lldp_get_packets(sd_lldp lldp, sd_lldp_packet **tlvs) {
	672	lldp_neighbour_port *p;
	673	lldp_chassis *c;
	674	Iterator iter;
	675	unsigned count = 0, i;
	676
	677	assert_return(lldp, -EINVAL);
	678	assert_return(tlvs, -EINVAL);
	679
	680	HASHMAP_FOREACH(c, lldp->neighbour_mib, iter) {
	681	LIST_FOREACH(port, p, c->ports)
	682	count++;
	683	}
	684
	685	if (!count) {
	686	*tlvs = NULL;
	687	return 0;
	688	}
	689
	690	tlvs = new(sd_lldp_packet , count);
	691	if (!*tlvs)
	692	return -ENOMEM;
	693
	694	i = 0;
	695	HASHMAP_FOREACH(c, lldp->neighbour_mib, iter) {
	696	LIST_FOREACH(port, p, c->ports)
	697	(*tlvs)[i++] = sd_lldp_packet_ref(p->packet);
	698	}
	699
	700	return count;
	701	}