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

        return 0;

 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->rx_state == LLDP_AGENT_RX_UPDATE_INFO)
                if (lldp->cb)
                        lldp->cb(lldp, LLDP_AGENT_RX_UPDATE_INFO, lldp->userdata);
}

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

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

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

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

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

                        free(s);
                        s = k;

                        time = now(CLOCK_BOOTTIME);

                        /* Don't write expired packets */
                        if (time - p->until <= 0)
                                continue;

                        sprintf(buf, "'_TTL="USEC_FMT"' ", p->until);

                        k = strappend(s, buf);
                        if (!k)
                                return -ENOMEM;

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

                                k = strjoin(s, "'_NAME=", t, "' ", NULL);
                                free(t);
                        }

                        if (!k)
                                return -ENOMEM;

                        free(s);
                        s = k;

                        (void) lldp_read_system_capability(p->packet, &data);

                        sprintf(buf, "'_CAP=%x'", data);

                        k = strappend(s, buf);
                        if (!k)
                                return -ENOMEM;

                        free(s);
                        s = k;

                        fprintf(f, "%s\n", s);
                }
        }
        r = 0;

        fflush(f);

        if (ferror(f) || rename(temp_path, lldp_file) < 0) {
                r = -errno;
                unlink(lldp_file);
                unlink(temp_path);
        }

 finish:
        if (r < 0)
                log_error("Failed to save lldp data %s: %s", lldp_file, strerror(-r));

        return r;
}

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
e735f4d4 MP	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"
e735f4d4 MP	27
	28	#include "lldp-tlv.h"
	29	#include "lldp-port.h"
	30	#include "sd-lldp.h"
	31	#include "prioq.h"
e735f4d4 MP	32	#include "lldp-internal.h"
e735f4d4 MP	33	#include "lldp-util.h"
e735f4d4 MP	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;
	46
	47	/* Section 10.5.2.2 Reception counters */
	48	struct lldp_agent_statistics {
	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
	63	sd_lldp_cb_t cb;
	64
	65	void *userdata;
	66
	67	LLDPAgentRXState rx_state;
	68	lldp_agent_statistics statistics;
	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);
107	static void lldp_set_state(sd_lldp *lldp, LLDPAgentRXState state);
108	static void lldp_run_state_machine(sd_lldp *ll);
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 */
117	if (prioq_size(lldp->by_expiry) > 0) {
118
119	lldp_set_state(lldp, LLDP_AGENT_RX_DELETE_INFO);
120
121	lldp_mib_delete_objects(lldp);
122	}
123
124	r = lldp_mib_add_objects(lldp->by_expiry, lldp->neighbour_mib, tlv);
125	if (r < 0)
126	goto out;
127
128	lldp_set_state(lldp, LLDP_AGENT_RX_UPDATE_INFO);
129
130	log_lldp("Packet added. MIB size: %d , PQ size: %d",
131	hashmap_size(lldp->neighbour_mib),
132	prioq_size(lldp->by_expiry));
133
134	lldp->statistics.stats_frames_in_total ++;
135
136	return 0;
137
138	out:
139	if (r < 0)
140	log_lldp("Receive frame failed: %s", strerror(-r));
141
142	lldp_set_state(lldp, LLDP_AGENT_RX_WAIT_FOR_FRAME);
143
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
172	lldp_set_state(lldp, LLDP_AGENT_RX_RX_FRAME);
173
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
204	/* skip type and lengh encoding */
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:
336	lldp_set_state(lldp, LLDP_AGENT_RX_WAIT_FOR_FRAME);
337
338	if (malformed) {
339	lldp->statistics.stats_frames_discarded_total ++;
340	lldp->statistics.stats_frames_in_errors_total ++;
341	}
342
343	tlv_packet_free(tlv);
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
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) {
371
372	if (lldp->rx_state == LLDP_AGENT_RX_UPDATE_INFO)
373	if (lldp->cb)
374	lldp->cb(lldp, LLDP_AGENT_RX_UPDATE_INFO, lldp->userdata);
375	}
376
377	/* 10.5.5.2.1 mibDeleteObjects ()
378	* The mibDeleteObjects () procedure deletes all information in the LLDP remote
379	* systems MIB associated with the MSAP identifier if an LLDPDU is received with
380	* an rxTTL value of zero (see 10.3.2) or the timing counter rxInfoTTL expires. */
381
382	static void lldp_mib_delete_objects(sd_lldp *lldp) {
383	lldp_neighbour_port *p;
384	usec_t t = 0;
385
386	/* Remove all entries that are past their TTL */
387	for (;;) {
388
389	if (prioq_size(lldp->by_expiry) <= 0)
390	break;
391
392	p = prioq_peek(lldp->by_expiry);
393	if (!p)
394	break;
395
396	if (t <= 0)
397	t = now(CLOCK_BOOTTIME);
398
399	if (p->until > t)
400	break;
401
402	lldp_neighbour_port_remove_and_free(p);
403
404	lldp->statistics.stats_ageouts_total ++;
405	}
406	}
407
408	static void lldp_mib_objects_flush(sd_lldp *lldp) {
409	lldp_neighbour_port p, q;
410	lldp_chassis *c;
411
412	assert(lldp);
413	assert(lldp->neighbour_mib);
414	assert(lldp->by_expiry);
415
416	/* Drop all packets */
417	while ((c = hashmap_steal_first(lldp->neighbour_mib))) {
418
419	LIST_FOREACH_SAFE(port, p, q, c->ports) {
420	lldp_neighbour_port_remove_and_free(p);
421	}
422	}
423
424	assert(hashmap_size(lldp->neighbour_mib) == 0);
425	assert(prioq_size(lldp->by_expiry) == 0);
426	}
427
428	int sd_lldp_save(sd_lldp lldp, const char lldp_file) {
429	_cleanup_free_ char *temp_path = NULL;
430	_cleanup_fclose_ FILE *f = NULL;
431	uint8_t mac, port_id, type;
432	lldp_neighbour_port *p;
433	uint16_t data = 0, length = 0;
434	char buf[LINE_MAX];
435	lldp_chassis *c;
436	usec_t time;
437	Iterator i;
438	int r;
439
440	assert(lldp);
441	assert(lldp_file);
442
443	r = fopen_temporary(lldp_file, &f, &temp_path);
444	if (r < 0)
445	goto finish;
446
447	fchmod(fileno(f), 0644);
448
449	HASHMAP_FOREACH(c, lldp->neighbour_mib, i) {
450	LIST_FOREACH(port, p, c->ports) {
451	_cleanup_free_ char *s = NULL;
452	char k, t;
453
454	r = lldp_read_chassis_id(p->packet, &type, &length, &mac);
455	if (r < 0)
456	continue;
457
458	sprintf(buf, "'_Chassis=%02x:%02x:%02x:%02x:%02x:%02x' '_CType=%d' ",
459	mac[0], mac[1], mac[2], mac[3], mac[4], mac[5], type);
460
461	s = strdup(buf);
462	if (!s)
463	return -ENOMEM;
464
465	r = lldp_read_port_id(p->packet, &type, &length, &port_id);
466	if (r < 0)
467	continue;
468
469	if (type != LLDP_PORT_SUBTYPE_MAC_ADDRESS) {
470	k = strndup((char *) port_id, length -1);
471	if (!k)
472	return -ENOMEM;
473
474	sprintf(buf, "'_Port=%s' '_PType=%d' ", k , type);
475	free(k);
476	} else {
477	mac = port_id;
478	sprintf(buf, "'_Port=%02x:%02x:%02x:%02x:%02x:%02x' '_PType=%d' ",
479	mac[0], mac[1], mac[2], mac[3], mac[4], mac[5], type);
480	}
481
482	k = strappend(s, buf);
483	if (!k)
484	return -ENOMEM;
485
486	free(s);
487	s = k;
488
489	time = now(CLOCK_BOOTTIME);
490
491	/* Don't write expired packets */
492	if (time - p->until <= 0)
493	continue;
494
495	sprintf(buf, "'_TTL="USEC_FMT"' ", p->until);
496
497	k = strappend(s, buf);
498	if (!k)
499	return -ENOMEM;
500
501	free(s);
502	s = k;
503
504	r = lldp_read_system_name(p->packet, &length, &k);
505	if (r < 0)
506	k = strappend(s, "'_NAME=N/A' ");
507	else {
508	t = strndup(k, length);
509	if (!t)
510	return -ENOMEM;
511
512	k = strjoin(s, "'_NAME=", t, "' ", NULL);
513	free(t);
514	}
515
516	if (!k)
517	return -ENOMEM;
518
519	free(s);
520	s = k;
521
e3bff60a	522	(void) lldp_read_system_capability(p->packet, &data);
e735f4d4 MP	523
	524	sprintf(buf, "'_CAP=%x'", data);
	525
	526	k = strappend(s, buf);
	527	if (!k)
	528	return -ENOMEM;
	529
	530	free(s);
	531	s = k;
	532
	533	fprintf(f, "%s\n", s);
	534	}
	535	}
	536	r = 0;
	537
	538	fflush(f);
	539
	540	if (ferror(f) \|\| rename(temp_path, lldp_file) < 0) {
	541	r = -errno;
	542	unlink(lldp_file);
	543	unlink(temp_path);
	544	}
	545
	546	finish:
	547	if (r < 0)
	548	log_error("Failed to save lldp data %s: %s", lldp_file, strerror(-r));
	549
	550	return r;
	551	}
	552
	553	int sd_lldp_start(sd_lldp *lldp) {
	554	int r;
	555
	556	assert_return(lldp, -EINVAL);
	557	assert_return(lldp->port, -EINVAL);
	558
	559	lldp->port->status = LLDP_PORT_STATUS_ENABLED;
	560
	561	lldp_set_state(lldp, LLDP_AGENT_RX_LLDP_INITIALIZE);
	562
	563	r = lldp_port_start(lldp->port);
	564	if (r < 0) {
	565	log_lldp("Failed to start Port : %s , %s",
	566	lldp->port->ifname,
	567	strerror(-r));
	568
	569	lldp_set_state(lldp, LLDP_AGENT_RX_WAIT_PORT_OPERATIONAL);
	570
	571	return r;
	572	}
	573
	574	lldp_set_state(lldp, LLDP_AGENT_RX_WAIT_FOR_FRAME);
	575
	576	return 0;
	577	}
	578
	579	int sd_lldp_stop(sd_lldp *lldp) {
	580	int r;
	581
	582	assert_return(lldp, -EINVAL);
	583	assert_return(lldp->port, -EINVAL);
	584
	585	lldp->port->status = LLDP_PORT_STATUS_DISABLED;
	586
587	r = lldp_port_stop(lldp->port);
588	if (r < 0)
589	return r;
590
591	lldp_mib_objects_flush(lldp);
592
593	return 0;
594	}
595
596	int sd_lldp_attach_event(sd_lldp lldp, sd_event event, int priority) {
597	int r;
598
599	assert_return(lldp, -EINVAL);
600	assert_return(!lldp->port->event, -EBUSY);
601
602	if (event)
603	lldp->port->event = sd_event_ref(event);
604	else {
605	r = sd_event_default(&lldp->port->event);
606	if (r < 0)
607	return r;
608	}
609
610	lldp->port->event_priority = priority;
611
612	return 0;
613	}
614
615	int sd_lldp_detach_event(sd_lldp *lldp) {
616
617	assert_return(lldp, -EINVAL);
618
619	lldp->port->event = sd_event_unref(lldp->port->event);
620
621	return 0;
622	}
623
624	int sd_lldp_set_callback(sd_lldp lldp, sd_lldp_cb_t cb, void userdata) {
625	assert_return(lldp, -EINVAL);
626
627	lldp->cb = cb;
628	lldp->userdata = userdata;
629
630	return 0;
631	}
632
633	void sd_lldp_free(sd_lldp *lldp) {
634
635	if (!lldp)
636	return;
637
638	/* Drop all packets */
639	lldp_mib_objects_flush(lldp);
640
641	lldp_port_free(lldp->port);
642
643	hashmap_free(lldp->neighbour_mib);
644	prioq_free(lldp->by_expiry);
645
646	free(lldp);
647	}
648
649	int sd_lldp_new(int ifindex,
650	const char *ifname,
651	const struct ether_addr *mac,
652	sd_lldp **ret) {
653	_cleanup_lldp_free_ sd_lldp *lldp = NULL;
654	int r;
655
656	assert_return(ret, -EINVAL);
657	assert_return(ifindex > 0, -EINVAL);
658	assert_return(ifname, -EINVAL);
659	assert_return(mac, -EINVAL);
660
661	lldp = new0(sd_lldp, 1);
662	if (!lldp)
663	return -ENOMEM;
664
665	r = lldp_port_new(ifindex, ifname, mac, lldp, &lldp->port);
666	if (r < 0)
667	return r;
668
669	lldp->neighbour_mib = hashmap_new(&chassis_id_hash_ops);
670	if (!lldp->neighbour_mib)
671	return -ENOMEM;
672
673	r = prioq_ensure_allocated(&lldp->by_expiry,
674	ttl_expiry_item_prioq_compare_func);
675	if (r < 0)
676	return r;
677
678	lldp->rx_state = LLDP_AGENT_RX_WAIT_PORT_OPERATIONAL;
679
680	*ret = lldp;
681	lldp = NULL;
682
683	return 0;
684	}