[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 "siphash24.h"
#include "hashmap.h"

#include "lldp-tlv.h"
#include "lldp-port.h"
#include "sd-lldp.h"
#include "prioq.h"
#include "lldp-internal.h"
#include "lldp-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 unsigned long chassis_id_hash_func(const void *p,
                                          const uint8_t hash_key[HASH_KEY_SIZE]) {
        uint64_t u;
        const lldp_chassis_id *id = p;

        assert(id);

        siphash24((uint8_t *) &u, id->data, id->length, hash_key);

        return (unsigned long) u;
}

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