mirror of
https://github.com/systemd/systemd-stable.git
synced 2024-12-22 13:33:56 +03:00
networkd: Introduce Link Layer Discovery Protocol (LLDP)
This patch introduces LLDP support to networkd. it implements the receiver side of the protocol. The Link Layer Discovery Protocol (LLDP) is an industry-standard, vendor-neutral method to allow networked devices to advertise capabilities, identity, and other information onto a LAN. The Layer 2 protocol, detailed in IEEE 802.1AB-2005.LLDP allows network devices that operate at the lower layers of a protocol stack (such as Layer 2 bridges and switches) to learn some of the capabilities and characteristics of LAN devices available to higher layer protocols.
This commit is contained in:
parent
266b538958
commit
ad1ad5c8e3
27
Makefile.am
27
Makefile.am
@ -3079,7 +3079,18 @@ libsystemd_network_la_SOURCES = \
|
||||
src/libsystemd-network/dhcp6-network.c \
|
||||
src/libsystemd-network/dhcp6-option.c \
|
||||
src/libsystemd-network/dhcp6-lease-internal.h \
|
||||
src/libsystemd-network/sd-dhcp6-lease.c
|
||||
src/libsystemd-network/sd-dhcp6-lease.c \
|
||||
src/libsystemd-network/lldp.h \
|
||||
src/libsystemd-network/lldp-tlv.h \
|
||||
src/libsystemd-network/lldp-tlv.c \
|
||||
src/libsystemd-network/lldp-network.h \
|
||||
src/libsystemd-network/lldp-network.c \
|
||||
src/libsystemd-network/lldp-port.h \
|
||||
src/libsystemd-network/lldp-port.c \
|
||||
src/libsystemd-network/lldp-internal.h \
|
||||
src/libsystemd-network/lldp-internal.c \
|
||||
src/libsystemd-network/sd-lldp.h \
|
||||
src/libsystemd-network/sd-lldp.c
|
||||
|
||||
libsystemd_network_la_LIBADD = \
|
||||
libudev-internal.la \
|
||||
@ -3158,13 +3169,25 @@ test_dhcp6_client_LDADD = \
|
||||
libsystemd-internal.la \
|
||||
libsystemd-shared.la
|
||||
|
||||
test_lldp_SOURCES = \
|
||||
src/libsystemd-network/lldp.h \
|
||||
src/libsystemd-network/lldp-tlv.h \
|
||||
src/libsystemd-network/lldp-tlv.c \
|
||||
src/libsystemd-network/test-lldp.c
|
||||
|
||||
test_lldp_LDADD = \
|
||||
libsystemd-network.la \
|
||||
libsystemd-internal.la \
|
||||
libsystemd-shared.la
|
||||
|
||||
tests += \
|
||||
test-dhcp-option \
|
||||
test-dhcp-client \
|
||||
test-dhcp-server \
|
||||
test-ipv4ll \
|
||||
test-icmp6-rs \
|
||||
test-dhcp6-client
|
||||
test-dhcp6-client \
|
||||
test-lldp
|
||||
|
||||
manual_tests += \
|
||||
test-pppoe
|
||||
|
437
src/libsystemd-network/lldp-internal.c
Normal file
437
src/libsystemd-network/lldp-internal.c
Normal file
@ -0,0 +1,437 @@
|
||||
/*-*- 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 "lldp-internal.h"
|
||||
|
||||
/* We store maximum 1K chassis entries */
|
||||
#define LLDP_MIB_MAX_CHASSIS 1024
|
||||
|
||||
/* Maximum Ports can be attached to any chassis */
|
||||
#define LLDP_MIB_MAX_PORT_PER_CHASSIS 32
|
||||
|
||||
int lldp_read_chassis_id(tlv_packet *tlv,
|
||||
uint8_t *type,
|
||||
uint16_t *length,
|
||||
uint8_t **data) {
|
||||
uint8_t subtype;
|
||||
int r;
|
||||
|
||||
assert_return(tlv, -EINVAL);
|
||||
|
||||
r = lldp_tlv_packet_enter_container(tlv, LLDP_TYPE_CHASSIS_ID);
|
||||
if (r < 0)
|
||||
goto out2;
|
||||
|
||||
r = tlv_packet_read_u8(tlv, &subtype);
|
||||
if (r < 0)
|
||||
goto out1;
|
||||
|
||||
switch (subtype) {
|
||||
case LLDP_CHASSIS_SUBTYPE_MAC_ADDRESS:
|
||||
|
||||
r = tlv_packet_read_bytes(tlv, data, length);
|
||||
if (r < 0)
|
||||
goto out1;
|
||||
|
||||
break;
|
||||
default:
|
||||
r = -ENOTSUP;
|
||||
break;
|
||||
}
|
||||
|
||||
*type = subtype;
|
||||
|
||||
out1:
|
||||
(void)lldp_tlv_packet_exit_container(tlv);
|
||||
|
||||
out2:
|
||||
return r;
|
||||
}
|
||||
|
||||
int lldp_read_port_id(tlv_packet *tlv,
|
||||
uint8_t *type,
|
||||
uint16_t *length,
|
||||
uint8_t **data) {
|
||||
uint8_t subtype;
|
||||
char *s;
|
||||
int r;
|
||||
|
||||
assert_return(tlv, -EINVAL);
|
||||
|
||||
r = lldp_tlv_packet_enter_container(tlv, LLDP_TYPE_PORT_ID);
|
||||
if (r < 0)
|
||||
goto out2;
|
||||
|
||||
r = tlv_packet_read_u8(tlv, &subtype);
|
||||
if (r < 0)
|
||||
goto out1;
|
||||
|
||||
switch (subtype) {
|
||||
case LLDP_PORT_SUBTYPE_INTERFACE_NAME:
|
||||
|
||||
r = tlv_packet_read_string(tlv, &s, length);
|
||||
if (r < 0)
|
||||
goto out1;
|
||||
|
||||
*data = (uint8_t *) s;
|
||||
|
||||
break;
|
||||
case LLDP_PORT_SUBTYPE_MAC_ADDRESS:
|
||||
|
||||
r = tlv_packet_read_bytes(tlv, data, length);
|
||||
if (r < 0)
|
||||
goto out1;
|
||||
|
||||
break;
|
||||
default:
|
||||
r = -ENOTSUP;
|
||||
break;
|
||||
}
|
||||
|
||||
*type = subtype;
|
||||
|
||||
out1:
|
||||
(void)lldp_tlv_packet_exit_container(tlv);
|
||||
|
||||
out2:
|
||||
return r;
|
||||
}
|
||||
|
||||
int lldp_read_ttl(tlv_packet *tlv, uint16_t *ttl) {
|
||||
int r;
|
||||
|
||||
assert_return(tlv, -EINVAL);
|
||||
|
||||
r = lldp_tlv_packet_enter_container(tlv, LLDP_TYPE_TTL);
|
||||
if (r < 0)
|
||||
goto out;
|
||||
|
||||
r = tlv_packet_read_u16(tlv, ttl);
|
||||
|
||||
(void) lldp_tlv_packet_exit_container(tlv);
|
||||
|
||||
out:
|
||||
return r;
|
||||
}
|
||||
|
||||
/* 10.5.5.2.2 mibUpdateObjects ()
|
||||
* The mibUpdateObjects () procedure updates the MIB objects corresponding to
|
||||
* the TLVs contained in the received LLDPDU for the LLDP remote system
|
||||
* indicated by the LLDP remote systems update process defined in 10.3.5 */
|
||||
|
||||
int lldp_mib_update_objects(lldp_chassis *c, tlv_packet *tlv) {
|
||||
lldp_neighbour_port *p;
|
||||
uint16_t length, ttl;
|
||||
uint8_t *data;
|
||||
uint8_t type;
|
||||
int r;
|
||||
|
||||
assert_return(c, -EINVAL);
|
||||
assert_return(tlv, -EINVAL);
|
||||
|
||||
r = lldp_read_port_id(tlv, &type, &length, &data);
|
||||
if (r < 0)
|
||||
return r;
|
||||
|
||||
/* Update the packet if we already have */
|
||||
LIST_FOREACH(port, p, c->ports) {
|
||||
|
||||
if ((p->type == type && p->length == length && !memcmp(p->data, data, p->length))) {
|
||||
|
||||
r = lldp_read_ttl(tlv, &ttl);
|
||||
if (r < 0)
|
||||
return r;
|
||||
|
||||
p->until = ttl * USEC_PER_SEC + now(clock_boottime_or_monotonic());
|
||||
|
||||
tlv_packet_free(p->packet);
|
||||
p->packet = tlv;
|
||||
|
||||
prioq_reshuffle(p->c->by_expiry, p, &p->prioq_idx);
|
||||
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
return -1;
|
||||
}
|
||||
|
||||
int lldp_mib_remove_objects(lldp_chassis *c, tlv_packet *tlv) {
|
||||
lldp_neighbour_port *p, *q;
|
||||
uint8_t *data;
|
||||
uint16_t length;
|
||||
uint8_t type;
|
||||
int r;
|
||||
|
||||
assert_return(c, -EINVAL);
|
||||
assert_return(tlv, -EINVAL);
|
||||
|
||||
r = lldp_read_port_id(tlv, &type, &length, &data);
|
||||
if (r < 0)
|
||||
return r;
|
||||
|
||||
LIST_FOREACH_SAFE(port, p, q, c->ports) {
|
||||
|
||||
/* Find the port */
|
||||
if (p->type == type && p->length == length && !memcmp(p->data, data, p->length)) {
|
||||
lldp_neighbour_port_remove_and_free(p);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int lldp_mib_add_objects(Prioq *by_expiry,
|
||||
Hashmap *neighbour_mib,
|
||||
tlv_packet *tlv) {
|
||||
_cleanup_lldp_neighbour_port_free_ lldp_neighbour_port *p = NULL;
|
||||
_cleanup_lldp_chassis_free_ lldp_chassis *c = NULL;
|
||||
lldp_chassis_id chassis_id;
|
||||
bool new_chassis = false;
|
||||
uint8_t subtype, *data;
|
||||
uint16_t ttl, length;
|
||||
int r;
|
||||
|
||||
assert_return(by_expiry, -EINVAL);
|
||||
assert_return(neighbour_mib, -EINVAL);
|
||||
assert_return(tlv, -EINVAL);
|
||||
|
||||
r = lldp_read_chassis_id(tlv, &subtype, &length, &data);
|
||||
if (r < 0)
|
||||
goto drop;
|
||||
|
||||
r = lldp_read_ttl(tlv, &ttl);
|
||||
if (r < 0)
|
||||
goto drop;
|
||||
|
||||
/* Make hash key */
|
||||
chassis_id.type = subtype;
|
||||
chassis_id.length = length;
|
||||
chassis_id.data = data;
|
||||
|
||||
/* Try to find the Chassis */
|
||||
c = hashmap_get(neighbour_mib, &chassis_id);
|
||||
if (!c) {
|
||||
|
||||
/* Don't create chassis if ttl 0 is received . Silently drop it */
|
||||
if (ttl == 0) {
|
||||
log_lldp("TTL value 0 received. Skiping Chassis creation.");
|
||||
goto drop;
|
||||
}
|
||||
|
||||
/* Admission Control: Can we store this packet ? */
|
||||
if (hashmap_size(neighbour_mib) >= LLDP_MIB_MAX_CHASSIS) {
|
||||
|
||||
log_lldp("Exceeding number of chassie: %d. Dropping ...",
|
||||
hashmap_size(neighbour_mib));
|
||||
goto drop;
|
||||
}
|
||||
|
||||
r = lldp_chassis_new(tlv, by_expiry, neighbour_mib, &c);
|
||||
if (r < 0)
|
||||
goto drop;
|
||||
|
||||
new_chassis = true;
|
||||
|
||||
r = hashmap_put(neighbour_mib, &c->chassis_id, c);
|
||||
if (r < 0)
|
||||
goto drop;
|
||||
|
||||
} else {
|
||||
|
||||
/* When the TTL field is set to zero, the receiving LLDP agent is notified all
|
||||
* system information associated with the LLDP agent/port is to be deleted */
|
||||
if (ttl == 0) {
|
||||
log_lldp("TTL value 0 received . Deleting associated Port ...");
|
||||
|
||||
lldp_mib_remove_objects(c, tlv);
|
||||
|
||||
c = NULL;
|
||||
goto drop;
|
||||
}
|
||||
|
||||
/* if we already have this port just update it */
|
||||
r = lldp_mib_update_objects(c, tlv);
|
||||
if (r >= 0) {
|
||||
c = NULL;
|
||||
return r;
|
||||
}
|
||||
|
||||
/* Admission Control: Can this port attached to the existing chassis ? */
|
||||
if (REFCNT_GET(c->n_ref) >= LLDP_MIB_MAX_PORT_PER_CHASSIS) {
|
||||
log_lldp("Port limit reached. Chassis has: %d ports. Dropping ...",
|
||||
REFCNT_GET(c->n_ref));
|
||||
|
||||
c = NULL;
|
||||
goto drop;
|
||||
}
|
||||
}
|
||||
|
||||
/* This is a new port */
|
||||
r = lldp_neighbour_port_new(c, tlv, &p);
|
||||
if (r < 0)
|
||||
goto drop;
|
||||
|
||||
r = prioq_put(c->by_expiry, p, &p->prioq_idx);
|
||||
if (r < 0)
|
||||
goto drop;
|
||||
|
||||
/* Attach new port to chassis */
|
||||
LIST_PREPEND(port, c->ports, p);
|
||||
REFCNT_INC(c->n_ref);
|
||||
|
||||
p = NULL;
|
||||
c = NULL;
|
||||
|
||||
return 0;
|
||||
|
||||
drop:
|
||||
tlv_packet_free(tlv);
|
||||
|
||||
if (new_chassis)
|
||||
hashmap_remove(neighbour_mib, &c->chassis_id);
|
||||
|
||||
return r;
|
||||
}
|
||||
|
||||
void lldp_neighbour_port_remove_and_free(lldp_neighbour_port *p) {
|
||||
lldp_chassis *c;
|
||||
|
||||
assert(p);
|
||||
assert(p->c);
|
||||
|
||||
c = p->c;
|
||||
|
||||
prioq_remove(c->by_expiry, p, &p->prioq_idx);
|
||||
|
||||
LIST_REMOVE(port, c->ports, p);
|
||||
lldp_neighbour_port_free(p);
|
||||
|
||||
/* Drop the Chassis if no port is attached */
|
||||
if (REFCNT_DEC(c->n_ref) <= 1) {
|
||||
hashmap_remove(c->neighbour_mib, &c->chassis_id);
|
||||
lldp_chassis_free(c);
|
||||
}
|
||||
}
|
||||
|
||||
void lldp_neighbour_port_free(lldp_neighbour_port *p) {
|
||||
|
||||
if(!p)
|
||||
return;
|
||||
|
||||
tlv_packet_free(p->packet);
|
||||
|
||||
free(p->data);
|
||||
free(p);
|
||||
}
|
||||
|
||||
int lldp_neighbour_port_new(lldp_chassis *c,
|
||||
tlv_packet *tlv,
|
||||
lldp_neighbour_port **ret) {
|
||||
_cleanup_lldp_neighbour_port_free_ lldp_neighbour_port *p;
|
||||
uint16_t length, ttl;
|
||||
uint8_t *data;
|
||||
uint8_t type;
|
||||
int r;
|
||||
|
||||
assert(tlv);
|
||||
|
||||
r = lldp_read_port_id(tlv, &type, &length, &data);
|
||||
if (r < 0)
|
||||
return r;
|
||||
|
||||
r = lldp_read_ttl(tlv, &ttl);
|
||||
if (r < 0)
|
||||
return r;
|
||||
|
||||
p = new0(lldp_neighbour_port, 1);
|
||||
if (!p)
|
||||
return -ENOMEM;
|
||||
|
||||
p->c = c;
|
||||
p->type = type;
|
||||
p->length = length;
|
||||
p->packet = tlv;
|
||||
p->prioq_idx = PRIOQ_IDX_NULL;
|
||||
p->until = ttl * USEC_PER_SEC + now(clock_boottime_or_monotonic());
|
||||
|
||||
p->data = memdup(data, length);
|
||||
if (!p->data)
|
||||
return -ENOMEM;
|
||||
|
||||
*ret = p;
|
||||
p = NULL;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
void lldp_chassis_free(lldp_chassis *c) {
|
||||
|
||||
if (!c)
|
||||
return;
|
||||
|
||||
if (REFCNT_GET(c->n_ref) > 1)
|
||||
return;
|
||||
|
||||
free(c->chassis_id.data);
|
||||
free(c);
|
||||
}
|
||||
|
||||
int lldp_chassis_new(tlv_packet *tlv,
|
||||
Prioq *by_expiry,
|
||||
Hashmap *neighbour_mib,
|
||||
lldp_chassis **ret) {
|
||||
_cleanup_lldp_chassis_free_ lldp_chassis *c;
|
||||
uint16_t length;
|
||||
uint8_t *data;
|
||||
uint8_t type;
|
||||
int r;
|
||||
|
||||
assert(tlv);
|
||||
|
||||
r = lldp_read_chassis_id(tlv, &type, &length, &data);
|
||||
if (r < 0)
|
||||
return r;
|
||||
|
||||
c = new0(lldp_chassis, 1);
|
||||
if (!c)
|
||||
return -ENOMEM;
|
||||
|
||||
c->n_ref = REFCNT_INIT;
|
||||
c->chassis_id.type = type;
|
||||
c->chassis_id.length = length;
|
||||
|
||||
c->chassis_id.data = memdup(data, length);
|
||||
if (!c->chassis_id.data)
|
||||
return -ENOMEM;
|
||||
|
||||
LIST_HEAD_INIT(c->ports);
|
||||
|
||||
c->by_expiry = by_expiry;
|
||||
c->neighbour_mib = neighbour_mib;
|
||||
|
||||
*ret = c;
|
||||
c = NULL;
|
||||
|
||||
return 0;
|
||||
}
|
93
src/libsystemd-network/lldp-internal.h
Normal file
93
src/libsystemd-network/lldp-internal.h
Normal file
@ -0,0 +1,93 @@
|
||||
/*-*- 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/>.
|
||||
***/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "log.h"
|
||||
#include "list.h"
|
||||
#include "refcnt.h"
|
||||
#include "lldp-tlv.h"
|
||||
#include "prioq.h"
|
||||
|
||||
typedef struct lldp_neighbour_port lldp_neighbour_port;
|
||||
typedef struct lldp_chassis lldp_chassis;
|
||||
typedef struct lldp_chassis_id lldp_chassis_id;
|
||||
|
||||
struct lldp_neighbour_port {
|
||||
uint8_t type;
|
||||
uint8_t *data;
|
||||
|
||||
uint16_t length;
|
||||
usec_t until;
|
||||
|
||||
unsigned prioq_idx;
|
||||
|
||||
lldp_chassis *c;
|
||||
tlv_packet *packet;
|
||||
|
||||
LIST_FIELDS(lldp_neighbour_port, port);
|
||||
};
|
||||
|
||||
int lldp_neighbour_port_new(lldp_chassis *c, tlv_packet *tlv, lldp_neighbour_port **ret);
|
||||
void lldp_neighbour_port_free(lldp_neighbour_port *p);
|
||||
void lldp_neighbour_port_remove_and_free(lldp_neighbour_port *p);
|
||||
|
||||
DEFINE_TRIVIAL_CLEANUP_FUNC(lldp_neighbour_port *, lldp_neighbour_port_free);
|
||||
#define _cleanup_lldp_neighbour_port_free_ _cleanup_(lldp_neighbour_port_freep)
|
||||
|
||||
struct lldp_chassis_id {
|
||||
uint8_t type;
|
||||
uint16_t length;
|
||||
|
||||
uint8_t *data;
|
||||
};
|
||||
|
||||
struct lldp_chassis {
|
||||
RefCount n_ref;
|
||||
|
||||
lldp_chassis_id chassis_id;
|
||||
|
||||
Prioq *by_expiry;
|
||||
Hashmap *neighbour_mib;
|
||||
|
||||
LIST_HEAD(lldp_neighbour_port, ports);
|
||||
};
|
||||
|
||||
int lldp_chassis_new(tlv_packet *tlv,
|
||||
Prioq *by_expiry,
|
||||
Hashmap *neighbour_mib,
|
||||
lldp_chassis **ret);
|
||||
|
||||
void lldp_chassis_free(lldp_chassis *c);
|
||||
|
||||
DEFINE_TRIVIAL_CLEANUP_FUNC(lldp_chassis *, lldp_chassis_free);
|
||||
#define _cleanup_lldp_chassis_free_ _cleanup_(lldp_chassis_freep)
|
||||
|
||||
int lldp_mib_update_objects(lldp_chassis *c, tlv_packet *tlv);
|
||||
int lldp_mib_add_objects(Prioq *by_expiry, Hashmap *neighbour_mib, tlv_packet *tlv);
|
||||
int lldp_mib_remove_objects(lldp_chassis *c, tlv_packet *tlv);
|
||||
|
||||
int lldp_read_chassis_id(tlv_packet *tlv, uint8_t *type, uint16_t *length, uint8_t **data);
|
||||
int lldp_read_port_id(tlv_packet *tlv, uint8_t *type, uint16_t *length, uint8_t **data);
|
||||
int lldp_read_ttl(tlv_packet *tlv, uint16_t *ttl);
|
||||
|
||||
#define log_lldp(fmt, ...) log_internal(LOG_DEBUG, 0, __FILE__, __LINE__, __func__, "LLDP: " fmt, ##__VA_ARGS__)
|
111
src/libsystemd-network/lldp-network.c
Normal file
111
src/libsystemd-network/lldp-network.c
Normal file
@ -0,0 +1,111 @@
|
||||
/*-*- 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 <linux/filter.h>
|
||||
#include <linux/if_ether.h>
|
||||
|
||||
#include "socket-util.h"
|
||||
#include "lldp-tlv.h"
|
||||
#include "lldp-network.h"
|
||||
#include "sd-lldp.h"
|
||||
|
||||
int lldp_network_bind_raw_socket(int ifindex) {
|
||||
typedef struct LLDPFrame {
|
||||
struct ethhdr hdr;
|
||||
uint8_t tlvs[0];
|
||||
} LLDPFrame;
|
||||
|
||||
struct sock_filter filter[] = {
|
||||
BPF_STMT(BPF_LD + BPF_W + BPF_ABS, offsetof(LLDPFrame, hdr.h_dest)), /* A <- 4 bytes of destination MAC */
|
||||
BPF_JUMP(BPF_JMP + BPF_JEQ + BPF_K, 0x0180c200, 1, 0), /* A != 01:80:c2:00 */
|
||||
BPF_STMT(BPF_RET + BPF_K, 0), /* drop packet */
|
||||
BPF_STMT(BPF_LD + BPF_H + BPF_ABS, offsetof(LLDPFrame, hdr.h_dest) + 4), /* A <- remaining 2 bytes of destination MAC */
|
||||
BPF_JUMP(BPF_JMP + BPF_JEQ + BPF_K, 0x0000, 3, 0), /* A != 00:00 */
|
||||
BPF_JUMP(BPF_JMP + BPF_JEQ + BPF_K, 0x0003, 2, 0), /* A != 00:03 */
|
||||
BPF_JUMP(BPF_JMP + BPF_JEQ + BPF_K, 0x000e, 1, 0), /* A != 00:0e */
|
||||
BPF_STMT(BPF_RET + BPF_K, 0), /* drop packet */
|
||||
BPF_STMT(BPF_LD + BPF_H + BPF_ABS, offsetof(LLDPFrame, hdr.h_proto)), /* A <- protocol */
|
||||
BPF_JUMP(BPF_JMP + BPF_JEQ + BPF_K, ETHERTYPE_LLDP, 1, 0), /* A != ETHERTYPE_LLDP */
|
||||
BPF_STMT(BPF_RET + BPF_K, 0), /* drop packet */
|
||||
BPF_STMT(BPF_RET + BPF_K, (uint32_t) -1), /* accept packet */
|
||||
};
|
||||
|
||||
struct sock_fprog fprog = {
|
||||
.len = ELEMENTSOF(filter),
|
||||
.filter = filter
|
||||
};
|
||||
|
||||
_cleanup_close_ int s = -1;
|
||||
|
||||
union sockaddr_union saddrll = {
|
||||
.ll.sll_family = AF_PACKET,
|
||||
.ll.sll_ifindex = ifindex,
|
||||
};
|
||||
|
||||
int r;
|
||||
|
||||
assert(ifindex > 0);
|
||||
|
||||
s = socket(PF_PACKET, SOCK_RAW, htons(ETH_P_ALL));
|
||||
if (s < 0)
|
||||
return -errno;
|
||||
|
||||
r = setsockopt(s, SOL_SOCKET, SO_ATTACH_FILTER, &fprog, sizeof(fprog));
|
||||
if (r < 0)
|
||||
return -errno;
|
||||
|
||||
r = bind(s, &saddrll.sa, sizeof(saddrll.ll));
|
||||
if (r < 0)
|
||||
return -errno;
|
||||
|
||||
r = s;
|
||||
s = -1;
|
||||
|
||||
return r;
|
||||
}
|
||||
|
||||
int lldp_receive_packet(sd_event_source *s, int fd, uint32_t revents, void *userdata) {
|
||||
_cleanup_tlv_packet_free_ tlv_packet *packet = NULL;
|
||||
tlv_packet *p;
|
||||
uint16_t length;
|
||||
int r;
|
||||
|
||||
assert(fd);
|
||||
assert(userdata);
|
||||
|
||||
r = tlv_packet_new(&packet);
|
||||
if (r < 0)
|
||||
return r;
|
||||
|
||||
length = read(fd, &packet->pdu, sizeof(packet->pdu));
|
||||
|
||||
/* Silently drop the packet */
|
||||
if ((size_t) length > ETHER_MAX_LEN)
|
||||
return 0;
|
||||
|
||||
packet->userdata = userdata;
|
||||
|
||||
p = packet;
|
||||
packet = NULL;
|
||||
|
||||
return lldp_handle_packet(p, (uint16_t) length);
|
||||
}
|
28
src/libsystemd-network/lldp-network.h
Normal file
28
src/libsystemd-network/lldp-network.h
Normal file
@ -0,0 +1,28 @@
|
||||
/*-*- 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/>.
|
||||
***/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "sd-event.h"
|
||||
|
||||
int lldp_network_bind_raw_socket(int ifindex);
|
||||
int lldp_receive_packet(sd_event_source *s, int fd, uint32_t revents, void *userdata);
|
116
src/libsystemd-network/lldp-port.c
Normal file
116
src/libsystemd-network/lldp-port.c
Normal file
@ -0,0 +1,116 @@
|
||||
/*-*- 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 "async.h"
|
||||
#include "lldp-port.h"
|
||||
#include "lldp-network.h"
|
||||
|
||||
int lldp_port_start(lldp_port *p) {
|
||||
int r;
|
||||
|
||||
assert_return(p, -EINVAL);
|
||||
|
||||
r = lldp_network_bind_raw_socket(p->ifindex);
|
||||
if (r < 0)
|
||||
return r;
|
||||
|
||||
p->rawfd = r;
|
||||
|
||||
r = sd_event_add_io(p->event, &p->lldp_port_rx,
|
||||
p->rawfd, EPOLLIN, lldp_receive_packet, p);
|
||||
if (r < 0) {
|
||||
log_debug("Failed to allocate event source: %s", strerror(-r));
|
||||
return r;
|
||||
}
|
||||
|
||||
r = sd_event_source_set_priority(p->lldp_port_rx, p->event_priority);
|
||||
if (r < 0) {
|
||||
log_debug("Failed to set event priority: %s", strerror(-r));
|
||||
goto fail;
|
||||
}
|
||||
|
||||
r = sd_event_source_set_description(p->lldp_port_rx, "lldp-port-rx");
|
||||
if (r < 0) {
|
||||
log_debug("Failed to set event name: %s", strerror(-r));
|
||||
goto fail;
|
||||
}
|
||||
|
||||
return 0;
|
||||
|
||||
fail:
|
||||
lldp_port_stop(p);
|
||||
|
||||
return r;
|
||||
}
|
||||
|
||||
int lldp_port_stop(lldp_port *p) {
|
||||
|
||||
assert_return(p, -EINVAL);
|
||||
|
||||
p->rawfd = asynchronous_close(p->rawfd);
|
||||
p->lldp_port_rx = sd_event_source_unref(p->lldp_port_rx);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
void lldp_port_free(lldp_port *p) {
|
||||
if (!p)
|
||||
return;
|
||||
|
||||
lldp_port_stop(p);
|
||||
|
||||
free(p->ifname);
|
||||
free(p);
|
||||
}
|
||||
|
||||
int lldp_port_new(int ifindex,
|
||||
char *ifname,
|
||||
const struct ether_addr *addr,
|
||||
void *userdata,
|
||||
lldp_port **ret) {
|
||||
_cleanup_free_ lldp_port *p = NULL;
|
||||
|
||||
assert_return(ifindex, -EINVAL);
|
||||
assert_return(ifname, -EINVAL);
|
||||
assert_return(addr, -EINVAL);
|
||||
|
||||
p = new0(lldp_port, 1);
|
||||
if (!p)
|
||||
return -ENOMEM;
|
||||
|
||||
p->rawfd = -1;
|
||||
p->ifindex = ifindex;
|
||||
|
||||
p->ifname = strdup(ifname);
|
||||
if (!p->ifname)
|
||||
return -ENOMEM;
|
||||
|
||||
memcpy(&p->mac, addr, ETH_ALEN);
|
||||
|
||||
p->userdata = userdata;
|
||||
|
||||
*ret = p;
|
||||
|
||||
p = NULL;
|
||||
|
||||
return 0;
|
||||
}
|
61
src/libsystemd-network/lldp-port.h
Normal file
61
src/libsystemd-network/lldp-port.h
Normal file
@ -0,0 +1,61 @@
|
||||
/*-*- 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/>.
|
||||
***/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <net/ethernet.h>
|
||||
|
||||
#include "sd-event.h"
|
||||
#include "sd-lldp.h"
|
||||
|
||||
typedef struct lldp_port lldp_port;
|
||||
|
||||
struct lldp_port {
|
||||
LLDPPortStatus status;
|
||||
|
||||
int ifindex;
|
||||
char *ifname;
|
||||
|
||||
struct ether_addr mac;
|
||||
|
||||
int rawfd;
|
||||
|
||||
sd_event *event;
|
||||
sd_event_source *lldp_port_rx;
|
||||
|
||||
int event_priority;
|
||||
|
||||
void *userdata;
|
||||
};
|
||||
|
||||
int lldp_port_new(int ifindex,
|
||||
char *ifname,
|
||||
const struct ether_addr *addr,
|
||||
void *userdata,
|
||||
lldp_port **ret);
|
||||
void lldp_port_free(lldp_port *p);
|
||||
|
||||
DEFINE_TRIVIAL_CLEANUP_FUNC(lldp_port*, lldp_port_free);
|
||||
#define _cleanup_lldp_port_free_ _cleanup_(lldp_port_freep)
|
||||
|
||||
int lldp_port_start(lldp_port *p);
|
||||
int lldp_port_stop(lldp_port *p);
|
321
src/libsystemd-network/lldp-tlv.c
Normal file
321
src/libsystemd-network/lldp-tlv.c
Normal file
@ -0,0 +1,321 @@
|
||||
/*-*- 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 <net/ethernet.h>
|
||||
#include <arpa/inet.h>
|
||||
|
||||
#include "macro.h"
|
||||
#include "lldp-tlv.h"
|
||||
|
||||
int tlv_section_new(tlv_section **ret) {
|
||||
tlv_section *s;
|
||||
|
||||
s = new0(tlv_section, 1);
|
||||
if (!s)
|
||||
return -ENOMEM;
|
||||
|
||||
*ret = s;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
void tlv_section_free(tlv_section *m) {
|
||||
|
||||
if (!m)
|
||||
return;
|
||||
|
||||
free(m);
|
||||
}
|
||||
|
||||
int tlv_packet_new(tlv_packet **ret) {
|
||||
tlv_packet *m;
|
||||
|
||||
m = new0(tlv_packet, 1);
|
||||
if (!m)
|
||||
return -ENOMEM;
|
||||
|
||||
LIST_HEAD_INIT(m->sections);
|
||||
|
||||
*ret = m;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
void tlv_packet_free(tlv_packet *m) {
|
||||
tlv_section *s, *n;
|
||||
|
||||
if (!m)
|
||||
return;
|
||||
|
||||
LIST_FOREACH_SAFE(section, s, n, m->sections)
|
||||
tlv_section_free(s);
|
||||
|
||||
free(m);
|
||||
}
|
||||
|
||||
int tlv_packet_append_bytes(tlv_packet *m, const void *data, size_t data_length) {
|
||||
uint8_t *p;
|
||||
|
||||
assert_return(m, -EINVAL);
|
||||
assert_return(data, -EINVAL);
|
||||
assert_return(data_length, -EINVAL);
|
||||
|
||||
if (m->length + data_length > ETHER_MAX_LEN)
|
||||
return -ENOMEM;
|
||||
|
||||
p = m->pdu + m->length;
|
||||
memcpy(p, data, data_length);
|
||||
m->length += data_length;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int tlv_packet_append_u8(tlv_packet *m, uint8_t data) {
|
||||
|
||||
assert_return(m, -EINVAL);
|
||||
|
||||
return tlv_packet_append_bytes(m, &data, sizeof(uint8_t));
|
||||
}
|
||||
|
||||
int tlv_packet_append_u16(tlv_packet *m, uint16_t data) {
|
||||
uint16_t type;
|
||||
|
||||
assert_return(m, -EINVAL);
|
||||
|
||||
type = htons(data);
|
||||
|
||||
return tlv_packet_append_bytes(m, &type, sizeof(uint16_t));
|
||||
}
|
||||
|
||||
int tlv_packet_append_u32(tlv_packet *m, uint32_t data) {
|
||||
uint32_t type;
|
||||
|
||||
assert_return(m, -EINVAL);
|
||||
|
||||
type = htonl(data);
|
||||
|
||||
return tlv_packet_append_bytes(m, &type, sizeof(uint32_t));
|
||||
}
|
||||
|
||||
int tlv_packet_append_string(tlv_packet *m, char *data, uint16_t size) {
|
||||
|
||||
assert_return(m, -EINVAL);
|
||||
|
||||
return tlv_packet_append_bytes(m, data, size);
|
||||
}
|
||||
|
||||
int lldp_tlv_packet_open_container(tlv_packet *m, uint16_t type) {
|
||||
|
||||
assert_return(m, -EINVAL);
|
||||
|
||||
m->container_pos = m->pdu + m->length;
|
||||
|
||||
return tlv_packet_append_u16(m, type << 9);
|
||||
}
|
||||
|
||||
int lldp_tlv_packet_close_container(tlv_packet *m) {
|
||||
uint16_t type;
|
||||
|
||||
assert_return(m, -EINVAL);
|
||||
assert_return(m->container_pos, -EINVAL);
|
||||
|
||||
memcpy(&type, m->container_pos, sizeof(uint16_t));
|
||||
|
||||
type |= htons(((m->pdu + m->length) - (m->container_pos + 2)) & 0x01ff);
|
||||
memcpy(m->container_pos, &type, sizeof(uint16_t));
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static inline int tlv_packet_read_internal(tlv_section *m, void **data) {
|
||||
|
||||
assert_return(m->read_pos, -EINVAL);
|
||||
|
||||
*data = m->read_pos;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int tlv_packet_read_u8(tlv_packet *m, uint8_t *data) {
|
||||
void *val;
|
||||
int r;
|
||||
|
||||
assert_return(m, -EINVAL);
|
||||
|
||||
r = tlv_packet_read_internal(m->container, &val);
|
||||
if (r < 0)
|
||||
return r;
|
||||
|
||||
memcpy(data, val, sizeof(uint8_t));
|
||||
|
||||
m->container->read_pos ++;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int tlv_packet_read_u16(tlv_packet *m, uint16_t *data) {
|
||||
uint16_t t;
|
||||
void *val;
|
||||
int r;
|
||||
|
||||
assert_return(m, -EINVAL);
|
||||
|
||||
r = tlv_packet_read_internal(m->container, &val);
|
||||
if (r < 0)
|
||||
return r;
|
||||
|
||||
memcpy(&t, val, sizeof(uint16_t));
|
||||
*data = ntohs(t);
|
||||
|
||||
m->container->read_pos += 2;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int tlv_packet_read_u32(tlv_packet *m, uint32_t *data) {
|
||||
uint32_t t;
|
||||
void *val;
|
||||
int r;
|
||||
|
||||
assert_return(m, -EINVAL);
|
||||
|
||||
r = tlv_packet_read_internal(m->container, &val);
|
||||
if (r < 0)
|
||||
return r;
|
||||
|
||||
memcpy(&t, val, sizeof(uint32_t));
|
||||
*data = ntohl(t);
|
||||
|
||||
m->container->read_pos += 4;
|
||||
|
||||
return r;
|
||||
}
|
||||
|
||||
int tlv_packet_read_string(tlv_packet *m, char **data, uint16_t *data_length) {
|
||||
void *val;
|
||||
int r;
|
||||
|
||||
assert_return(m, -EINVAL);
|
||||
|
||||
r = tlv_packet_read_internal(m->container, &val);
|
||||
if (r < 0)
|
||||
return r;
|
||||
|
||||
*data = (char *) val;
|
||||
*data_length = m->container->length;
|
||||
|
||||
m->container->read_pos += m->container->length;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int tlv_packet_read_bytes(tlv_packet *m, uint8_t **data, uint16_t *data_length) {
|
||||
void *val;
|
||||
int r;
|
||||
|
||||
assert_return(m, -EINVAL);
|
||||
|
||||
r = tlv_packet_read_internal(m->container, &val);
|
||||
if (r < 0)
|
||||
return r;
|
||||
|
||||
*data = (uint8_t *) val;
|
||||
*data_length = m->container->length;
|
||||
|
||||
m->container->read_pos += m->container->length;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* parse raw TLV packet */
|
||||
int tlv_packet_parse_pdu(tlv_packet *m, uint16_t size) {
|
||||
tlv_section *section, *tail;
|
||||
uint16_t t, l;
|
||||
uint8_t *p;
|
||||
int r;
|
||||
|
||||
assert_return(m, -EINVAL);
|
||||
assert_return(size, -EINVAL);
|
||||
|
||||
p = m->pdu;
|
||||
|
||||
/* extract ethernet herader */
|
||||
memcpy(&m->mac, p, ETH_ALEN);
|
||||
p += sizeof(struct ether_header);
|
||||
|
||||
for (l = 0; l <= size; ) {
|
||||
r = tlv_section_new(§ion);
|
||||
if (r < 0)
|
||||
return r;
|
||||
|
||||
memcpy(&t, p, sizeof(uint16_t));
|
||||
|
||||
section->type = ntohs(t) >> 9;
|
||||
section->length = ntohs(t) & 0x01ff;
|
||||
|
||||
if (section->type == LLDP_TYPE_END || section->type >=_LLDP_TYPE_MAX) {
|
||||
tlv_section_free(section);
|
||||
break;
|
||||
}
|
||||
|
||||
p += 2;
|
||||
section->data = p;
|
||||
|
||||
LIST_FIND_TAIL(section, m->sections, tail);
|
||||
LIST_INSERT_AFTER(section, m->sections, tail, section);
|
||||
|
||||
p += section->length;
|
||||
l += (section->length + 2);
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int lldp_tlv_packet_enter_container(tlv_packet *m, uint16_t type) {
|
||||
tlv_section *s;
|
||||
|
||||
assert_return(m, -EINVAL);
|
||||
|
||||
LIST_FOREACH(section, s, m->sections)
|
||||
if (s->type == type)
|
||||
break;
|
||||
if (!s)
|
||||
return -1;
|
||||
|
||||
m->container = s;
|
||||
|
||||
m->container->read_pos = s->data;
|
||||
if (!m->container->read_pos) {
|
||||
m->container = 0;
|
||||
return -1;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int lldp_tlv_packet_exit_container(tlv_packet *m) {
|
||||
assert_return(m, -EINVAL);
|
||||
|
||||
m->container = 0;
|
||||
|
||||
return 0;
|
||||
}
|
87
src/libsystemd-network/lldp-tlv.h
Normal file
87
src/libsystemd-network/lldp-tlv.h
Normal file
@ -0,0 +1,87 @@
|
||||
/*-*- 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/>.
|
||||
***/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <net/ethernet.h>
|
||||
|
||||
#include "util.h"
|
||||
#include "lldp.h"
|
||||
#include "list.h"
|
||||
|
||||
typedef struct tlv_packet tlv_packet;
|
||||
typedef struct tlv_section tlv_section;
|
||||
|
||||
struct tlv_section {
|
||||
uint16_t type;
|
||||
uint16_t length;
|
||||
|
||||
uint8_t *read_pos;
|
||||
uint8_t *data;
|
||||
|
||||
LIST_FIELDS(tlv_section, section);
|
||||
};
|
||||
|
||||
int tlv_section_new(tlv_section **ret);
|
||||
void tlv_section_free(tlv_section *ret);
|
||||
|
||||
struct tlv_packet {
|
||||
uint16_t type;
|
||||
uint16_t length;
|
||||
usec_t ts;
|
||||
|
||||
uint8_t *container_pos;
|
||||
uint8_t pdu[ETHER_MAX_LEN];
|
||||
|
||||
void *userdata;
|
||||
|
||||
struct ether_addr mac;
|
||||
tlv_section *container;
|
||||
|
||||
LIST_HEAD(tlv_section, sections);
|
||||
};
|
||||
|
||||
int tlv_packet_new(tlv_packet **ret);
|
||||
void tlv_packet_free(tlv_packet *m);
|
||||
|
||||
DEFINE_TRIVIAL_CLEANUP_FUNC(tlv_packet*, tlv_packet_free);
|
||||
#define _cleanup_tlv_packet_free_ _cleanup_(tlv_packet_freep)
|
||||
|
||||
int lldp_tlv_packet_open_container(tlv_packet *m, uint16_t type);
|
||||
int lldp_tlv_packet_close_container(tlv_packet *m);
|
||||
|
||||
int tlv_packet_append_bytes(tlv_packet *m, const void *data, size_t data_length);
|
||||
int tlv_packet_append_u8(tlv_packet *m, uint8_t data);
|
||||
int tlv_packet_append_u16(tlv_packet *m, uint16_t data);
|
||||
int tlv_packet_append_u32(tlv_packet *m, uint32_t data);
|
||||
int tlv_packet_append_string(tlv_packet *m, char *data, uint16_t size);
|
||||
|
||||
int lldp_tlv_packet_enter_container(tlv_packet *m, uint16_t type);
|
||||
int lldp_tlv_packet_exit_container(tlv_packet *m);
|
||||
|
||||
int tlv_packet_read_bytes(tlv_packet *m, uint8_t **data, uint16_t *data_length);
|
||||
int tlv_packet_read_string(tlv_packet *m, char **data, uint16_t *data_length);
|
||||
int tlv_packet_read_u8(tlv_packet *m, uint8_t *data);
|
||||
int tlv_packet_read_u16(tlv_packet *m, uint16_t *data);
|
||||
int tlv_packet_read_u32(tlv_packet *m, uint32_t *data);
|
||||
|
||||
int tlv_packet_parse_pdu(tlv_packet *t, uint16_t size);
|
115
src/libsystemd-network/lldp.h
Normal file
115
src/libsystemd-network/lldp.h
Normal file
@ -0,0 +1,115 @@
|
||||
/*-*- 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/>.
|
||||
***/
|
||||
|
||||
#pragma once
|
||||
|
||||
#define LLDP_MULTICAST_ADDR { 0x01, 0x80, 0xc2, 0x00, 0x00, 0x0e }
|
||||
|
||||
#define ETHERTYPE_LLDP 0x88cc
|
||||
|
||||
/* IEEE 802.3AB Clause 9: TLV Types */
|
||||
typedef enum LLDPTypes {
|
||||
LLDP_TYPE_END = 0,
|
||||
LLDP_TYPE_CHASSIS_ID = 1,
|
||||
LLDP_TYPE_PORT_ID = 2,
|
||||
LLDP_TYPE_TTL = 3,
|
||||
LLDP_TYPE_PORT_DESCRIPTION = 4,
|
||||
LLDP_TYPE_SYSTEM_NAME = 5,
|
||||
LLDP_TYPE_SYSTEM_DESCRIPTION = 6,
|
||||
LLDP_TYPE_SYSTEM_CAPABILITIES = 7,
|
||||
LLDP_TYPE_MGMT_ADDRESS = 8,
|
||||
LLDP_TYPE_PRIVATE = 127,
|
||||
_LLDP_TYPE_MAX,
|
||||
_LLDP_TYPE_INVALID = -1,
|
||||
} LLDPTypes;
|
||||
|
||||
/* IEEE 802.3AB Clause 9.5.2: Chassis subtypes */
|
||||
typedef enum LLDPChassisSubtypes {
|
||||
LLDP_CHASSIS_SUBTYPE_RESERVED = 0,
|
||||
LLDP_CHASSIS_SUBTYPE_CHASSIS_COMPONENT = 1,
|
||||
LLDP_CHASSIS_SUBTYPE_INTERFACE_ALIAS = 2,
|
||||
LLDP_CHASSIS_SUBTYPE_PORT_COMPONENT = 3,
|
||||
LLDP_CHASSIS_SUBTYPE_MAC_ADDRESS = 4,
|
||||
LLDP_CHASSIS_SUBTYPE_NETWORK_ADDRESS = 5,
|
||||
LLDP_CHASSIS_SUBTYPE_INTERFACE_NAME = 6,
|
||||
LLDP_CHASSIS_SUBTYPE_LOCALLY_ASSIGNED = 7,
|
||||
_LLDP_CHASSIS_SUBTYPE_MAX,
|
||||
_LLDP_CHASSIS_SUBTYPE_INVALID = -1,
|
||||
} LLDPChassisSubtypes;
|
||||
|
||||
/* IEEE 802.3AB Clause 9.5.3: Port subtype */
|
||||
typedef enum LLDPPortSubtypes {
|
||||
LLDP_PORT_SUBTYPE_RESERVED = 0,
|
||||
LLDP_PORT_SUBTYPE_INTERFACE_ALIAS = 1,
|
||||
LLDP_PORT_SUBTYPE_PORT_COMPONENT = 2,
|
||||
LLDP_PORT_SUBTYPE_MAC_ADDRESS = 3,
|
||||
LLDP_PORT_SUBTYPE_NETWORK = 4,
|
||||
LLDP_PORT_SUBTYPE_INTERFACE_NAME = 5,
|
||||
LLDP_PORT_SUBTYPE_AGENT_CIRCUIT_ID = 6,
|
||||
LLDP_PORT_SUBTYPE_LOCALLY_ASSIGNED = 7,
|
||||
_LLDP_PORT_SUBTYPE_MAX,
|
||||
_LLDP_PORT_SUBTYPE_INVALID = -1
|
||||
} LLDPPortSubtypes;
|
||||
|
||||
typedef enum LLDPSystemCapabilities {
|
||||
LLDP_SYSTEM_CAPABILITIES_OTHER = 1 << 0,
|
||||
LLDP_SYSTEM_CAPABILITIES_REPEATER = 1 << 1,
|
||||
LLDP_SYSTEM_CAPABILITIES_BRIDGE = 1 << 2,
|
||||
LLDP_SYSTEM_CAPABILITIES_WLAN_AP = 1 << 3,
|
||||
LLDP_SYSTEM_CAPABILITIES_ROUTER = 1 << 4,
|
||||
LLDP_SYSTEM_CAPABILITIES_PHONE = 1 << 5,
|
||||
LLDP_SYSTEM_CAPABILITIES_DOCSIS = 1 << 6,
|
||||
LLDP_SYSTEM_CAPABILITIES_STATION = 1 << 7,
|
||||
LLDP_SYSTEM_CAPABILITIES_CVLAN = 1 << 8,
|
||||
LLDP_SYSTEM_CAPABILITIES_SVLAN = 1 << 9,
|
||||
LLDP_SYSTEM_CAPABILITIES_TPMR = 1 << 10,
|
||||
_LLDP_SYSTEM_CAPABILITIES_MAX,
|
||||
_LLDP_SYSTEM_CAPABILITIES_INVALID = -1,
|
||||
} LLDPSystemCapabilities;
|
||||
|
||||
typedef enum LLDPMedSubtype {
|
||||
LLDP_MED_SUBTYPE_RESERVED = 0,
|
||||
LLDP_MED_SUBTYPE_CAPABILITIES = 1,
|
||||
LLDP_MED_SUBTYPE_NETWORK_POLICY = 2,
|
||||
LLDP_MED_SUBTYPE_LOCATION_ID = 3,
|
||||
LLDP_MED_SUBTYPE_EXTENDED_PVMDI = 4,
|
||||
LLDP_MED_SUBTYPE_INV_HWREV = 5,
|
||||
LLDP_MED_SUBTYPE_INV_FWREV = 6,
|
||||
LLDP_MED_SUBTYPE_INV_SWREV = 7,
|
||||
LLDP_MED_SUBTYPE_INV_SERIAL = 8,
|
||||
LLDP_MED_SUBTYPE_INV_MANUFACTURER = 9,
|
||||
LLDP_MED_SUBTYPE_INV_MODELNAME = 10,
|
||||
LLDP_MED_SUBTYPE_INV_ASSETID = 11,
|
||||
_LLDP_MED_SUBTYPE_MAX,
|
||||
_LLDP_MED_SUBTYPE_INVALID = -1,
|
||||
} LLDPMedSubtype;
|
||||
|
||||
typedef enum LLDPMedCapability {
|
||||
LLDP_MED_CAPABILITY_CAPAPILITIES = 1 << 0,
|
||||
LLDP_MED_CAPABILITY_NETWORK_POLICY = 1 << 1,
|
||||
LLDP_MED_CAPABILITY_LOCATION_ID = 1 << 2,
|
||||
LLDP_MED_CAPABILITY_EXTENDED_PSE = 1 << 3,
|
||||
LLDP_MED_CAPABILITY_EXTENDED_PD = 1 << 4,
|
||||
LLDP_MED_CAPABILITY_INVENTORY = 1 << 5,
|
||||
LLDP_MED_CAPABILITY_MAX,
|
||||
LLDP_MED_CAPABILITY_INVALID = -1,
|
||||
} LLDPMedCapability;
|
493
src/libsystemd-network/sd-lldp.c
Normal file
493
src/libsystemd-network/sd-lldp.c
Normal file
@ -0,0 +1,493 @@
|
||||
/*-*- 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 "event-util.h"
|
||||
|
||||
#include "lldp-tlv.h"
|
||||
#include "lldp-port.h"
|
||||
#include "sd-lldp.h"
|
||||
#include "prioq.h"
|
||||
#include "lldp-internal.h"
|
||||
|
||||
/* Section 10.5.2.2 Reception counters */
|
||||
struct lldp_agent_statitics {
|
||||
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;
|
||||
|
||||
lldp_agent_statitics statitics;
|
||||
};
|
||||
|
||||
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 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_mib_delete_objects(lldp);
|
||||
|
||||
r = lldp_mib_add_objects(lldp->by_expiry, lldp->neighbour_mib, tlv);
|
||||
if (r < 0)
|
||||
goto out;
|
||||
|
||||
log_lldp("Packet added. MIB size: %d , PQ size: %d",
|
||||
hashmap_size(lldp->neighbour_mib),
|
||||
prioq_size(lldp->by_expiry));
|
||||
|
||||
lldp->statitics.stats_frames_in_total ++;
|
||||
|
||||
return 0;
|
||||
|
||||
out:
|
||||
if (r < 0)
|
||||
log_lldp("Receive frame failed: %s", strerror(-r));
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
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:
|
||||
if (malformed) {
|
||||
lldp->statitics.stats_frames_discarded_total ++;
|
||||
lldp->statitics.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;
|
||||
}
|
||||
|
||||
/* 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->statitics.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_start(sd_lldp *lldp) {
|
||||
int r;
|
||||
|
||||
assert_return(lldp, -EINVAL);
|
||||
assert_return(lldp->port, -EINVAL);
|
||||
|
||||
lldp->port->status = LLDP_PORT_STATUS_ENABLED;
|
||||
|
||||
r = lldp_port_start(lldp->port);
|
||||
if (r < 0) {
|
||||
log_lldp("Failed to start Port : %s , %s",
|
||||
lldp->port->ifname,
|
||||
strerror(-r));
|
||||
return r;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
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,
|
||||
char *ifname,
|
||||
struct ether_addr *mac,
|
||||
sd_lldp **ret) {
|
||||
_cleanup_sd_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;
|
||||
|
||||
*ret = lldp;
|
||||
lldp = NULL;
|
||||
|
||||
return 0;
|
||||
}
|
51
src/libsystemd-network/sd-lldp.h
Normal file
51
src/libsystemd-network/sd-lldp.h
Normal file
@ -0,0 +1,51 @@
|
||||
/*-*- 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/>.
|
||||
***/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "lldp-tlv.h"
|
||||
#include "sd-event.h"
|
||||
|
||||
typedef struct sd_lldp sd_lldp;
|
||||
typedef struct lldp_agent_statitics lldp_agent_statitics;
|
||||
|
||||
typedef enum LLDPPortStatus {
|
||||
LLDP_PORT_STATUS_NONE,
|
||||
LLDP_PORT_STATUS_ENABLED,
|
||||
LLDP_PORT_STATUS_DISABLED,
|
||||
_LLDP_PORT_STATUS_MAX,
|
||||
_LLDP_PORT_STATUS_INVALID = -1,
|
||||
} LLDPPortStatus;
|
||||
|
||||
int sd_lldp_new(int ifindex, char *ifname, struct ether_addr *mac, sd_lldp **ret);
|
||||
void sd_lldp_free(sd_lldp *lldp);
|
||||
|
||||
DEFINE_TRIVIAL_CLEANUP_FUNC(sd_lldp *, sd_lldp_free);
|
||||
#define _cleanup_sd_lldp_free_ _cleanup_(sd_lldp_freep)
|
||||
|
||||
int sd_lldp_start(sd_lldp *lldp);
|
||||
int sd_lldp_stop(sd_lldp *lldp);
|
||||
|
||||
int sd_lldp_attach_event(sd_lldp *lldp, sd_event *event, int priority);
|
||||
int sd_lldp_detach_event(sd_lldp *lldp);
|
||||
|
||||
int lldp_handle_packet(tlv_packet *m, uint16_t length);
|
233
src/libsystemd-network/test-lldp.c
Normal file
233
src/libsystemd-network/test-lldp.c
Normal file
@ -0,0 +1,233 @@
|
||||
/*-*- 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 <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <sys/socket.h>
|
||||
#include <linux/if.h>
|
||||
#include <linux/if_ether.h>
|
||||
#include <net/ethernet.h>
|
||||
#include <sys/types.h>
|
||||
#include <arpa/inet.h>
|
||||
|
||||
#include "macro.h"
|
||||
#include "lldp.h"
|
||||
#include "lldp-tlv.h"
|
||||
|
||||
#define TEST_LLDP_PORT "em1"
|
||||
#define TEST_LLDP_TYPE_SYSTEM_NAME "systemd-lldp"
|
||||
#define TEST_LLDP_TYPE_SYSTEM_DESC "systemd-lldp-desc"
|
||||
|
||||
static struct ether_addr mac_addr = {
|
||||
.ether_addr_octet = {'A', 'B', 'C', '1', '2', '3'}
|
||||
};
|
||||
|
||||
static int lldp_build_tlv_packet(tlv_packet **ret) {
|
||||
_cleanup_tlv_packet_free_ tlv_packet *m = NULL;
|
||||
const uint8_t lldp_dst[] = LLDP_MULTICAST_ADDR;
|
||||
struct ether_header ether;
|
||||
|
||||
/* Append ethernet header */
|
||||
memset(ðer, 0, sizeof(ether));
|
||||
memcpy(ðer.ether_dhost, lldp_dst, ETHER_ADDR_LEN);
|
||||
memcpy(ðer.ether_shost, &mac_addr, ETHER_ADDR_LEN);
|
||||
ether.ether_type = htons(ETHERTYPE_LLDP);
|
||||
|
||||
assert_se(tlv_packet_new(&m) >= 0);
|
||||
|
||||
assert_se(tlv_packet_append_bytes(m, ðer, sizeof(struct ether_header)) >= 0);
|
||||
|
||||
assert_se(lldp_tlv_packet_open_container(m, LLDP_TYPE_CHASSIS_ID) >= 0);
|
||||
|
||||
assert_se(tlv_packet_append_u8(m, LLDP_CHASSIS_SUBTYPE_MAC_ADDRESS) >= 0);
|
||||
assert_se(tlv_packet_append_bytes(m, &mac_addr, ETHER_ADDR_LEN) >= 0);
|
||||
|
||||
assert_se(lldp_tlv_packet_close_container(m) >= 0);
|
||||
|
||||
/* port name */
|
||||
assert_se(lldp_tlv_packet_open_container(m, LLDP_TYPE_PORT_ID) >= 0);
|
||||
|
||||
assert_se(tlv_packet_append_u8(m, LLDP_PORT_SUBTYPE_INTERFACE_NAME) >= 0);
|
||||
assert_se(tlv_packet_append_bytes(m, TEST_LLDP_PORT, strlen(TEST_LLDP_PORT) + 1) >= 0);
|
||||
|
||||
assert_se(lldp_tlv_packet_close_container(m) >= 0);
|
||||
|
||||
/* ttl */
|
||||
assert_se(lldp_tlv_packet_open_container(m, LLDP_TYPE_TTL) >= 0);
|
||||
|
||||
assert_se(tlv_packet_append_u16(m, 170) >= 0);
|
||||
|
||||
assert_se(lldp_tlv_packet_close_container(m) >= 0);
|
||||
|
||||
/* system name */
|
||||
assert_se(lldp_tlv_packet_open_container(m, LLDP_TYPE_SYSTEM_NAME) >= 0);
|
||||
|
||||
assert_se(tlv_packet_append_bytes(m, TEST_LLDP_TYPE_SYSTEM_NAME,
|
||||
strlen(TEST_LLDP_TYPE_SYSTEM_NAME)) >= 0);
|
||||
assert_se(lldp_tlv_packet_close_container(m) >= 0);
|
||||
|
||||
/* system descrition */
|
||||
assert_se(lldp_tlv_packet_open_container(m, LLDP_TYPE_SYSTEM_DESCRIPTION) >= 0);
|
||||
|
||||
assert_se(tlv_packet_append_bytes(m, TEST_LLDP_TYPE_SYSTEM_DESC,
|
||||
strlen(TEST_LLDP_TYPE_SYSTEM_DESC)) >= 0);
|
||||
|
||||
assert_se(lldp_tlv_packet_close_container(m) >= 0);
|
||||
|
||||
/* Mark end of packet */
|
||||
assert_se(lldp_tlv_packet_open_container(m, LLDP_TYPE_END) >= 0);
|
||||
assert_se(lldp_tlv_packet_close_container(m) >= 0);
|
||||
|
||||
*ret = m;
|
||||
|
||||
m = NULL;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int lldp_parse_chassis_tlv(tlv_packet *m, uint8_t *type) {
|
||||
uint8_t *p, subtype;
|
||||
uint16_t length;
|
||||
|
||||
assert_se(lldp_tlv_packet_enter_container(m, LLDP_TYPE_CHASSIS_ID) >= 0);
|
||||
assert_se(tlv_packet_read_u8(m, &subtype) >= 0);
|
||||
|
||||
switch (subtype) {
|
||||
case LLDP_CHASSIS_SUBTYPE_MAC_ADDRESS:
|
||||
|
||||
*type = LLDP_CHASSIS_SUBTYPE_MAC_ADDRESS;
|
||||
assert_se(tlv_packet_read_bytes(m, &p, &length) >= 0);
|
||||
|
||||
assert_se(memcmp(p, &mac_addr.ether_addr_octet, ETHER_ADDR_LEN) == 0);
|
||||
|
||||
break;
|
||||
default:
|
||||
assert_not_reached("Unhandled option");
|
||||
}
|
||||
|
||||
assert_se(lldp_tlv_packet_exit_container(m) >= 0);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int lldp_parse_port_id_tlv(tlv_packet *m) {
|
||||
char *str = NULL, *p;
|
||||
uint16_t length;
|
||||
uint8_t subtype;
|
||||
|
||||
assert_se(lldp_tlv_packet_enter_container(m, LLDP_TYPE_PORT_ID) >= 0);
|
||||
|
||||
assert_se(tlv_packet_read_u8(m, &subtype) >= 0);
|
||||
|
||||
switch (subtype) {
|
||||
case LLDP_PORT_SUBTYPE_INTERFACE_NAME:
|
||||
assert_se(tlv_packet_read_string(m, &str, &length) >= 0);
|
||||
|
||||
p = malloc0(length + 1);
|
||||
strncpy(p, str, length-1);
|
||||
|
||||
assert_se(streq(p, TEST_LLDP_PORT) == 1);
|
||||
break;
|
||||
default:
|
||||
assert_not_reached("Unhandled option");
|
||||
}
|
||||
|
||||
assert_se(lldp_tlv_packet_exit_container(m) >= 0);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int lldp_parse_system_name_tlv(tlv_packet *m) {
|
||||
char *str = NULL, *p;
|
||||
uint16_t length;
|
||||
|
||||
assert_se(lldp_tlv_packet_enter_container(m, LLDP_TYPE_SYSTEM_NAME) >= 0);
|
||||
assert_se(tlv_packet_read_string(m, &str, &length) >= 0);
|
||||
|
||||
p = malloc0(length + 1);
|
||||
strncpy(p, str, length);
|
||||
|
||||
assert_se(streq(p, TEST_LLDP_TYPE_SYSTEM_NAME) == 1);
|
||||
|
||||
assert_se(lldp_tlv_packet_exit_container(m) >= 0);
|
||||
|
||||
return 1;
|
||||
}
|
||||
|
||||
static int lldp_parse_system_desc_tlv(tlv_packet *m) {
|
||||
char *str = NULL, *p;
|
||||
uint16_t length;
|
||||
|
||||
assert_se(lldp_tlv_packet_enter_container(m, LLDP_TYPE_SYSTEM_DESCRIPTION) >= 0);
|
||||
assert_se(tlv_packet_read_string(m, &str, &length) >= 0);
|
||||
|
||||
p = malloc0(length + 1);
|
||||
strncpy(p, str, length);
|
||||
|
||||
assert_se(streq(p, TEST_LLDP_TYPE_SYSTEM_DESC) == 1);
|
||||
|
||||
assert_se(lldp_tlv_packet_exit_container(m) >= 0);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int lldp_parse_ttl_tlv(tlv_packet *m) {
|
||||
uint16_t ttl;
|
||||
|
||||
assert_se(lldp_tlv_packet_enter_container(m, LLDP_TYPE_TTL) >= 0);
|
||||
assert_se(tlv_packet_read_u16(m, &ttl) >= 0);
|
||||
|
||||
assert_se(ttl == 170);
|
||||
|
||||
assert_se(lldp_tlv_packet_exit_container(m) >= 0);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int lldp_parse_tlv_packet(tlv_packet *m, int len) {
|
||||
uint8_t subtype;
|
||||
|
||||
assert_se(tlv_packet_parse_pdu(m, len) >= 0);
|
||||
assert_se(lldp_parse_chassis_tlv(m, &subtype) >= 0);
|
||||
assert_se(lldp_parse_port_id_tlv(m) >= 0);
|
||||
assert_se(lldp_parse_system_name_tlv(m) >= 0);
|
||||
assert_se(lldp_parse_ttl_tlv(m) >= 0);
|
||||
assert_se(lldp_parse_system_desc_tlv(m) >= 0);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int main(int argc, char *argv[]) {
|
||||
_cleanup_tlv_packet_free_ tlv_packet *tlv = NULL;
|
||||
|
||||
/* form a packet */
|
||||
lldp_build_tlv_packet(&tlv);
|
||||
|
||||
/* parse the packet */
|
||||
tlv_packet_parse_pdu(tlv, tlv->length);
|
||||
|
||||
/* verify */
|
||||
lldp_parse_tlv_packet(tlv, tlv->length);
|
||||
|
||||
return 0;
|
||||
}
|
Loading…
Reference in New Issue
Block a user