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

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