[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 "lldp-util.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;
}

void sd_lldp_free(sd_lldp *lldp) {

        if (!lldp)
                return;

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

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