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

/*-*- 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 <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) {
        uint16_t type, len, i, l, t;
        bool chassis_id = false;
        bool malformed = false;
        bool port_id = false;
        bool ttl = false;
        bool end = false;
        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 is disabled : %s . 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 is not length 0. Length:%d received . Dropping ...",
                                         len);

                                malformed = true;
                                goto out;
                        }

                        end = true;

                        break;
                } else if (type >=_LLDP_TYPE_MAX) {
                        log_lldp("TLV type not recognized %d . 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 len = %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 len = %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 lenth: %d TTL TLV. Dropping ...",
                                         len);

                                malformed = true;
                                goto out;
                        }

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

                                malformed = true;
                                goto out;
                        }

                        ttl = true;

                        break;
                default:

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

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

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