[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"

typedef enum LLDPAgentRXState {
        LLDP_AGENT_RX_WAIT_PORT_OPERATIONAL = 4,
        LLDP_AGENT_RX_DELETE_AGED_INFO,
        LLDP_AGENT_RX_LLDP_INITIALIZE,
        LLDP_AGENT_RX_WAIT_FOR_FRAME,
        LLDP_AGENT_RX_RX_FRAME,
        LLDP_AGENT_RX_DELETE_INFO,
        LLDP_AGENT_RX_UPDATE_INFO,
        _LLDP_AGENT_RX_STATE_MAX,
        _LLDP_AGENT_RX_INVALID = -1,
} LLDPAgentRXState;

/* 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;

        LLDPAgentRXState rx_state;
        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 void lldp_set_state(sd_lldp *lldp, LLDPAgentRXState state);
static void lldp_run_state_machine(sd_lldp *ll);

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_set_state(lldp, LLDP_AGENT_RX_DELETE_INFO);

                lldp_mib_delete_objects(lldp);
        }

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

        lldp_set_state(lldp, LLDP_AGENT_RX_UPDATE_INFO);

        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));

        lldp_set_state(lldp, LLDP_AGENT_RX_WAIT_FOR_FRAME);

        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;
        }

        lldp_set_state(lldp, LLDP_AGENT_RX_RX_FRAME);

        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:
        lldp_set_state(lldp, LLDP_AGENT_RX_WAIT_FOR_FRAME);

        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;
}

static void lldp_set_state(sd_lldp *lldp, LLDPAgentRXState state) {

        assert(lldp);
        assert(state < _LLDP_AGENT_RX_STATE_MAX);

        lldp->rx_state = state;

        lldp_run_state_machine(lldp);
}

static void lldp_run_state_machine(sd_lldp *lldp) {
        if (!lldp->cb)
                return;

        switch (lldp->rx_state) {
        case LLDP_AGENT_RX_UPDATE_INFO:
                lldp->cb(lldp, SD_LLDP_EVENT_UPDATE_INFO, lldp->userdata);
                break;
        default:
                break;
        }
}

/* 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;

        lldp_set_state(lldp, LLDP_AGENT_RX_LLDP_INITIALIZE);

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

                lldp_set_state(lldp, LLDP_AGENT_RX_WAIT_PORT_OPERATIONAL);

                return r;
        }

        lldp_set_state(lldp, LLDP_AGENT_RX_WAIT_FOR_FRAME);

        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;

        lldp->rx_state = LLDP_AGENT_RX_WAIT_PORT_OPERATIONAL;

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