]> git.ipfire.org Git - thirdparty/lldpd.git/blobdiff - src/lldpd.c
Move LLDP-MED location/policy to port instead of chassis.
[thirdparty/lldpd.git] / src / lldpd.c
index 02b46095903f096d9bfce1569edb38dfb756ac29..1d2364fbb43b06af1aab1db0a050436ab686f6dc 100644 (file)
@@ -24,6 +24,7 @@
 #include <fcntl.h>
 #include <fnmatch.h>
 #include <time.h>
+#include <libgen.h>
 #include <sys/utsname.h>
 #include <sys/types.h>
 #include <sys/socket.h>
 #include <sys/time.h>
 #include <sys/ioctl.h>
 #include <arpa/inet.h>
-#include <netpacket/packet.h>
 #include <ifaddrs.h>
 #include <net/if_arp.h>
 #include <linux/filter.h>
 #include <linux/if_vlan.h>
+#include <linux/if_packet.h>
 #include <linux/sockios.h>
-#include <linux/ethtool.h>
 
 #ifdef USE_SNMP
 #include <net-snmp/net-snmp-config.h>
@@ -51,7 +51,6 @@ void           usage(void);
 int                     lldpd_iface_init(struct lldpd *, struct lldpd_hardware *);
 int                     lldpd_iface_init_vlan(struct lldpd *, struct lldpd_vif *);
 void                    lldpd_iface_init_mtu(struct lldpd *, struct lldpd_hardware *);
-int                     lldpd_iface_init_socket(struct lldpd *, struct lldpd_hardware *);
 int                     lldpd_iface_close(struct lldpd *, struct lldpd_hardware *);
 void                    lldpd_iface_multicast(struct lldpd *, const char *, int);
 
@@ -66,6 +65,18 @@ void                  lldpd_iface_multicast(struct lldpd *, const char *, int);
         { 0x6, 0, 0, 0x0000ffff },                                     \
         { 0x6, 0, 0, 0x00000000 },
 struct sock_filter lldpd_filter_lldp_f[] = { LLDPD_FILTER_LLDP_F };
+#ifdef ENABLE_FDP
+/* "ether dst 01:e0:52:cc:cc:cc" */
+#define LLDPD_FILTER_FDP_F                     \
+        { 0x20, 0, 0, 0x00000002 },            \
+        { 0x15, 0, 3, 0x52cccccc },            \
+        { 0x28, 0, 0, 0x00000000 },            \
+        { 0x15, 0, 1, 0x000001e0 },            \
+        { 0x6, 0, 0, 0x0000ffff },             \
+        { 0x6, 0, 0, 0x00000000 },
+struct sock_filter lldpd_filter_fdp_f[] = { LLDPD_FILTER_FDP_F };
+#endif /* ENABLE_FDP */
+#ifdef ENABLE_CDP
 /* "ether dst 01:00:0c:cc:cc:cc" */
 #define LLDPD_FILTER_CDP_F                     \
         { 0x20, 0, 0, 0x00000002 },            \
@@ -75,6 +86,8 @@ struct sock_filter lldpd_filter_lldp_f[] = { LLDPD_FILTER_LLDP_F };
         { 0x6, 0, 0, 0x0000ffff },             \
         { 0x6, 0, 0, 0x00000000 },
 struct sock_filter lldpd_filter_cdp_f[] = { LLDPD_FILTER_CDP_F };
+#endif /* ENABLE_CDP */
+#ifdef ENABLE_SONMP
 /* "ether dst 01:00:81:00:01:00" */
 #define LLDPD_FILTER_SONMP_F                   \
         { 0x20, 0, 0, 0x00000002 },            \
@@ -84,6 +97,8 @@ struct sock_filter lldpd_filter_cdp_f[] = { LLDPD_FILTER_CDP_F };
         { 0x6, 0, 0, 0x0000ffff },             \
         { 0x6, 0, 0, 0x00000000 },
 struct sock_filter lldpd_filter_sonmp_f[] = { LLDPD_FILTER_SONMP_F };
+#endif /* ENABLE_SONMP */
+#ifdef ENABLE_EDP
 /* "ether dst 00:e0:2b:00:00:00" */
 #define LLDPD_FILTER_EDP_F              \
        { 0x20, 0, 0, 0x00000002 },     \
@@ -93,21 +108,25 @@ struct sock_filter lldpd_filter_sonmp_f[] = { LLDPD_FILTER_SONMP_F };
        { 0x6, 0, 0, 0x0000ffff },      \
        { 0x6, 0, 0, 0x00000000 },
 struct sock_filter lldpd_filter_edp_f[] = { LLDPD_FILTER_EDP_F };
+#endif /* ENABLE_EDP */
 #define LLDPD_FILTER_ANY_F             \
        { 0x28, 0, 0, 0x0000000c },     \
        { 0x15, 0, 4, 0x000088cc },     \
        { 0x20, 0, 0, 0x00000002 },     \
        { 0x15, 0, 2, 0xc200000e },     \
        { 0x28, 0, 0, 0x00000000 },     \
-       { 0x15, 8, 9, 0x00000180 },     \
+       { 0x15, 11, 12, 0x00000180 },   \
        { 0x20, 0, 0, 0x00000002 },     \
        { 0x15, 0, 2, 0x2b000000 },     \
        { 0x28, 0, 0, 0x00000000 },     \
-       { 0x15, 4, 5, 0x000000e0 },     \
+       { 0x15, 7, 8, 0x000000e0 },     \
        { 0x15, 1, 0, 0x0ccccccc },     \
-       { 0x15, 0, 3, 0x81000100 },     \
+       { 0x15, 0, 2, 0x81000100 },     \
+       { 0x28, 0, 0, 0x00000000 },     \
+       { 0x15, 3, 4, 0x00000100 },     \
+       { 0x15, 0, 3, 0x52cccccc },     \
        { 0x28, 0, 0, 0x00000000 },     \
-       { 0x15, 0, 1, 0x00000100 },     \
+       { 0x15, 0, 1, 0x000001e0 },     \
        { 0x6, 0, 0, 0x0000ffff },      \
        { 0x6, 0, 0, 0x00000000 },
 struct sock_filter lldpd_filter_any_f[] = { LLDPD_FILTER_ANY_F };
@@ -116,14 +135,24 @@ struct protocol protos[] =
 {
        { LLDPD_MODE_LLDP, 1, "LLDP", ' ', lldp_send, lldp_decode, NULL,
          LLDP_MULTICAST_ADDR, lldpd_filter_lldp_f, sizeof(lldpd_filter_lldp_f) },
+#ifdef ENABLE_CDP
        { LLDPD_MODE_CDPV1, 0, "CDPv1", 'c', cdpv1_send, cdp_decode, cdpv1_guess,
          CDP_MULTICAST_ADDR, lldpd_filter_cdp_f, sizeof(lldpd_filter_cdp_f) },
        { LLDPD_MODE_CDPV2, 0, "CDPv2", 'c', cdpv2_send, cdp_decode, cdpv2_guess,
          CDP_MULTICAST_ADDR, lldpd_filter_cdp_f, sizeof(lldpd_filter_cdp_f) },
+#endif
+#ifdef ENABLE_SONMP
        { LLDPD_MODE_SONMP, 0, "SONMP", 's', sonmp_send, sonmp_decode, NULL,
          SONMP_MULTICAST_ADDR, lldpd_filter_sonmp_f, sizeof(lldpd_filter_sonmp_f) },
+#endif
+#ifdef ENABLE_EDP
        { LLDPD_MODE_EDP, 0, "EDP", 'e', edp_send, edp_decode, NULL,
          EDP_MULTICAST_ADDR, lldpd_filter_edp_f, sizeof(lldpd_filter_edp_f) },
+#endif
+#ifdef ENABLE_FDP
+       { LLDPD_MODE_FDP, 0, "FDP", 'f', fdp_send, cdp_decode, NULL,
+         FDP_MULTICAST_ADDR, lldpd_filter_fdp_f, sizeof(lldpd_filter_fdp_f) },
+#endif
        { 0, 0, "any", ' ', NULL, NULL, NULL,
          {0,0,0,0,0,0}, lldpd_filter_any_f, sizeof(lldpd_filter_any_f) }
 };
@@ -139,6 +168,9 @@ void                         lldpd_recv_all(struct lldpd *);
 int                     lldpd_guess_type(struct lldpd *, char *, int);
 void                    lldpd_decode(struct lldpd *, char *, int,
                            struct lldpd_hardware *, int);
+#ifdef ENABLE_LLDPMED
+void                    lldpd_med(struct lldpd_chassis *);
+#endif
 
 char   **saved_argv;
 
@@ -146,11 +178,8 @@ void
 usage(void)
 {
        extern const char       *__progname;
-#ifndef USE_SNMP
-       fprintf(stderr, "usage: %s [-d] [-v] [-c] [-s] [-e] [-p|-P] [-m ip]\n", __progname);
-#else /* USE_SNMP */
-       fprintf(stderr, "usage: %s [-d] [-v] [-c] [-s] [-e] [-p|-P] [-m ip] [-x]\n", __progname);
-#endif /* USE_SNMP */
+       fprintf(stderr, "usage: %s [options]\n", __progname);
+       fprintf(stderr, "see manual page lldpd(8) for more information\n");
        exit(1);
 }
 
@@ -166,26 +195,7 @@ lldpd_iface_init_mtu(struct lldpd *global, struct lldpd_hardware *hardware)
                LLOG_WARN("unable to get MTU of %s, using 1500", hardware->h_ifname);
                hardware->h_mtu = 1500;
        } else
-               hardware->h_mtu = ifr.ifr_mtu;
-}
-
-int
-lldpd_iface_init_socket(struct lldpd *global, struct lldpd_hardware *hardware)
-{
-       struct sockaddr_ll sa;
-
-       /* Open listening socket to receive/send frames */
-       if ((hardware->h_raw = socket(PF_PACKET, SOCK_RAW,
-                   htons(ETH_P_ALL))) < 0)
-               return errno;
-       memset(&sa, 0, sizeof(sa));
-       sa.sll_family = AF_PACKET;
-       sa.sll_protocol = 0;
-       sa.sll_ifindex = if_nametoindex(hardware->h_ifname);
-       if (bind(hardware->h_raw, (struct sockaddr*)&sa, sizeof(sa)) < 0)
-               return errno;
-
-       return 0;
+               hardware->h_mtu = hardware->h_lport.p_mfs = ifr.ifr_mtu;
 }
 
 int
@@ -195,7 +205,7 @@ lldpd_iface_init_vlan(struct lldpd *global, struct lldpd_vif *vif)
        short int filter;
 
        lldpd_iface_init_mtu(global, (struct lldpd_hardware*)vif);
-       status = lldpd_iface_init_socket(global, (struct lldpd_hardware*)vif);
+       status = priv_iface_init((struct lldpd_hardware*)vif, -1);
        if (status != 0)
                return status;
 
@@ -220,15 +230,13 @@ lldpd_iface_init_vlan(struct lldpd *global, struct lldpd_vif *vif)
 int
 lldpd_iface_init(struct lldpd *global, struct lldpd_hardware *hardware)
 {
-       struct sockaddr_ll sa;
        int master;             /* Bond device */
        char if_bond[IFNAMSIZ];
-       int un = 1;
        int status;
        short int filter;
 
        lldpd_iface_init_mtu(global, hardware);
-       status = lldpd_iface_init_socket(global, hardware);
+       status = priv_iface_init(hardware, -1);
        if (status != 0)
                return status;
 
@@ -242,25 +250,13 @@ lldpd_iface_init(struct lldpd *global, struct lldpd_hardware *hardware)
                hardware->h_raw_real = hardware->h_raw;
                hardware->h_master = master;
                hardware->h_raw = -1;
-               if ((hardware->h_raw = socket(PF_PACKET, SOCK_RAW,
-                           htons(ETH_P_ALL))) < 0)
-                       return errno;
-               memset(&sa, 0, sizeof(sa));
-               sa.sll_family = AF_PACKET;
-               sa.sll_protocol = 0;
-               sa.sll_ifindex = master;
-               if (bind(hardware->h_raw, (struct sockaddr*)&sa,
-                       sizeof(sa)) < 0)
-                       return errno;
-               /* With bonding, we need to listen to bond device. We use
-                * setsockopt() PACKET_ORIGDEV to get physical device instead of
-                * bond device */
-               if (setsockopt(hardware->h_raw, SOL_PACKET,
-                       PACKET_ORIGDEV, &un, sizeof(un)) == -1) {
-                       LLOG_WARN("unable to setsockopt for master bonding device of %s. "
-                            "You will get inaccurate results",
-                           hardware->h_ifname);
-                }
+               status = priv_iface_init(hardware, master);
+               if (status != 0) {
+                       close(hardware->h_raw_real);
+                       if (hardware->h_raw != -1)
+                               close(hardware->h_raw);
+                       return status;
+               }
        }
 
        if (global->g_multi)
@@ -284,23 +280,18 @@ lldpd_iface_init(struct lldpd *global, struct lldpd_hardware *hardware)
 void
 lldpd_iface_multicast(struct lldpd *global, const char *name, int remove)
 {
-       struct ifreq ifr;
-       int i;
+       int i, rc;
 
        for (i=0; global->g_protocols[i].mode != 0; i++) {
                if (!global->g_protocols[i].enabled) continue;
-               memset(&ifr, 0, sizeof(ifr));
-               strlcpy(ifr.ifr_name, name, sizeof(ifr.ifr_name));
-               memcpy(ifr.ifr_hwaddr.sa_data,
-                   global->g_protocols[i].mac, ETH_ALEN);
-               if (ioctl(global->g_sock, (remove)?SIOCDELMULTI:SIOCADDMULTI,
-                       &ifr) < 0) {
-                       if (errno == ENOENT)
-                               return;
-                       LLOG_INFO("unable to %s %s address to multicast filter for %s",
-                           (remove)?"delete":"add",
-                           global->g_protocols[i].name,
-                           name);
+               if ((rc = priv_iface_multicast(name,
+                           global->g_protocols[i].mac, !remove)) != 0) {
+                       errno = rc;
+                       if (errno != ENOENT)
+                               LLOG_INFO("unable to %s %s address to multicast filter for %s",
+                                   (remove)?"delete":"add",
+                                   global->g_protocols[i].name,
+                                   name);
                }
        }
 }
@@ -357,7 +348,7 @@ lldpd_iface_switchto(struct lldpd *cfg, short int filter, struct lldpd_hardware
        return 0;
 }
 
-
+#ifdef ENABLE_DOT1
 void
 lldpd_vlan_cleanup(struct lldpd_port *port)
 {
@@ -371,11 +362,19 @@ lldpd_vlan_cleanup(struct lldpd_port *port)
                free(vlan);
        }
 }
+#endif
 
 void
 lldpd_port_cleanup(struct lldpd_port *port)
 {
+#ifdef ENABLE_LLDPMED
+       int i;
+       for (i=0; i < LLDPMED_LOCFORMAT_LAST; i++)
+               free(port->p_med_location[i].data);
+#endif
+#ifdef ENABLE_DOT1
        lldpd_vlan_cleanup(port);
+#endif
        free(port->p_id);
        free(port->p_descr);
        free(port);
@@ -384,6 +383,14 @@ lldpd_port_cleanup(struct lldpd_port *port)
 void
 lldpd_chassis_cleanup(struct lldpd_chassis *chassis)
 {
+#ifdef ENABLE_LLDPMED
+       free(chassis->c_med_hw);
+       free(chassis->c_med_fw);
+       free(chassis->c_med_sn);
+       free(chassis->c_med_manuf);
+       free(chassis->c_med_model);
+       free(chassis->c_med_asset);
+#endif
        free(chassis->c_id);
        free(chassis->c_name);
        free(chassis->c_descr);
@@ -412,6 +419,17 @@ lldpd_remote_cleanup(struct lldpd *cfg, struct lldpd_hardware *hardware, int res
        }
 }
 
+void
+lldpd_hardware_cleanup(struct lldpd_hardware *hardware)
+{
+#ifdef ENABLE_DOT1
+       lldpd_vlan_cleanup(&hardware->h_lport);
+#endif
+       free(hardware->h_proto_macs);
+       free(hardware->h_llastframe);
+       free(hardware);
+}
+
 void
 lldpd_cleanup(struct lldpd *cfg)
 {
@@ -424,11 +442,8 @@ lldpd_cleanup(struct lldpd *cfg)
                if (hardware->h_flags == 0) {
                        TAILQ_REMOVE(&cfg->g_hardware, hardware, h_entries);
                        lldpd_iface_close(cfg, hardware);
-                       lldpd_vlan_cleanup(&hardware->h_lport);
                        lldpd_remote_cleanup(cfg, hardware, 1);
-                       free(hardware->h_proto_macs);
-                       free(hardware->h_llastframe);
-                       free(hardware);
+                       lldpd_hardware_cleanup(hardware);
                } else if (hardware->h_rchassis != NULL) {
                        if (time(NULL) - hardware->h_rlastupdate >
                            hardware->h_rchassis->c_ttl) {
@@ -505,13 +520,18 @@ lldpd_port_add_vlan(struct lldpd *cfg, struct ifaddrs *ifa)
 struct lldpd_hardware *
 lldpd_port_add(struct lldpd *cfg, struct ifaddrs *ifa)
 {
+#if defined (ENABLE_DOT1) || defined (ENABLE_DOT3)
        struct ifaddrs *oifap, *oifa;
+#endif
        struct lldpd_hardware *hardware;
        struct lldpd_port *port;
+#ifdef ENABLE_DOT1
        struct lldpd_vlan *vlan;
-       struct ifreq ifr;
        struct vlan_ioctl_args ifv;
+#endif
+#ifdef ENABLE_DOT3
        struct ethtool_cmd ethc;
+#endif
        u_int8_t *lladdr;
 
        TAILQ_FOREACH(hardware, &cfg->g_hardware, h_entries) {
@@ -527,9 +547,16 @@ lldpd_port_add(struct lldpd *cfg, struct ifaddrs *ifa)
                hardware->h_raw_real = -1;
                hardware->h_start_probe = 0;
                hardware->h_proto_macs = (u_int8_t*)calloc(cfg->g_multi+1, ETH_ALEN);
+#ifdef ENABLE_LLDPMED
+               hardware->h_lport.p_med_cap_enabled = LLDPMED_CAP_CAP;
+               if (!cfg->g_noinventory)
+                       hardware->h_lport.p_med_cap_enabled |= LLDPMED_CAP_IV;
+#endif
+#ifdef ENABLE_DOT1
                TAILQ_INIT(&hardware->h_lport.p_vlans);
        } else {
                lldpd_vlan_cleanup(&hardware->h_lport);
+#endif
        }
 
        port = &hardware->h_lport;
@@ -538,8 +565,11 @@ lldpd_port_add(struct lldpd *cfg, struct ifaddrs *ifa)
        strlcpy(hardware->h_ifname, ifa->ifa_name, sizeof(hardware->h_ifname));
        lladdr = (u_int8_t*)(((struct sockaddr_ll *)ifa->ifa_addr)->sll_addr);
        memcpy(&hardware->h_lladdr, lladdr, sizeof(hardware->h_lladdr));
+       iface_get_permanent_mac(cfg, hardware);
        port->p_id_subtype = LLDP_PORTID_SUBTYPE_LLADDR;
-       port->p_id = (char*)hardware->h_lladdr;
+       if ((port->p_id = calloc(1, sizeof(hardware->h_lladdr))) == NULL)
+               fatal(NULL);
+       memcpy(port->p_id, hardware->h_lladdr, sizeof(hardware->h_lladdr));
        port->p_id_len = sizeof(hardware->h_lladdr);
        port->p_descr = hardware->h_ifname;
 
@@ -554,9 +584,11 @@ lldpd_port_add(struct lldpd *cfg, struct ifaddrs *ifa)
        }
 
        /* Get VLANS and aggregation status */
+#if defined (ENABLE_DOT3) || defined (ENABLE_DOT1)
        if (getifaddrs(&oifap) != 0)
                fatal("lldpd_port_add: failed to get interface list");
        for (oifa = oifap; oifa != NULL; oifa = oifa->ifa_next) {
+#ifdef ENABLE_DOT1
                /* Check if we already have checked this one */
                int skip = 0;
                TAILQ_FOREACH(vlan, &port->p_vlans, v_entries) {
@@ -564,22 +596,26 @@ lldpd_port_add(struct lldpd *cfg, struct ifaddrs *ifa)
                                skip = 1;
                }
                if (skip) continue;
+#endif
 
                /* Aggregation check */
-               if (iface_is_bond_slave(cfg, hardware->h_ifname, oifa->ifa_name))
+#ifdef ENABLE_DOT3
+               if (iface_is_bond_slave(cfg, hardware->h_ifname, oifa->ifa_name, NULL))
                        port->p_aggregid = if_nametoindex(oifa->ifa_name);
-               
+#endif
+
+#ifdef ENABLE_DOT1     
                /* VLAN check */
                memset(&ifv, 0, sizeof(ifv));
                ifv.cmd = GET_VLAN_REALDEV_NAME_CMD;
                strlcpy(ifv.device1, oifa->ifa_name, sizeof(ifv.device1));
                if ((ioctl(cfg->g_sock, SIOCGIFVLAN, &ifv) >= 0) &&
-                   ((iface_is_bond_slave(cfg, hardware->h_ifname, ifv.u.device2)) ||
+                   ((iface_is_bond_slave(cfg, hardware->h_ifname, ifv.u.device2, NULL)) ||
                     (strncmp(hardware->h_ifname, ifv.u.device2, sizeof(ifv.u.device2)) == 0))) {
                        if ((vlan = (struct lldpd_vlan *)
                             calloc(1, sizeof(struct lldpd_vlan))) == NULL)
                                continue;
-                       if (asprintf(&vlan->v_name, "%s", oifa->ifa_name) == -1) {
+                       if ((vlan->v_name = strdup(oifa->ifa_name)) == NULL) {
                                free(vlan);
                                continue;
                        }
@@ -595,16 +631,14 @@ lldpd_port_add(struct lldpd *cfg, struct ifaddrs *ifa)
                                TAILQ_INSERT_TAIL(&port->p_vlans, vlan, v_entries);
                        }
                }
+#endif
        }
        freeifaddrs(oifap);
+#endif
 
+#ifdef ENABLE_DOT3
        /* MAC/PHY */
-       memset(&ifr, 0, sizeof(ifr));
-       memset(&ethc, 0, sizeof(ethc));
-       strlcpy(ifr.ifr_name, hardware->h_ifname, sizeof(ifr.ifr_name));
-       ifr.ifr_data = (caddr_t)&ethc;
-       ethc.cmd = ETHTOOL_GSET;
-       if (ioctl(cfg->g_sock, SIOCETHTOOL, &ifr) == 0) {
+       if (priv_ethtool(hardware->h_ifname, &ethc) == 0) {
                int j;
                int advertised_ethtool_to_rfc3636[][2] = {
                        {ADVERTISED_10baseT_Half, LLDP_DOT3_LINK_AUTONEG_10BASE_T},
@@ -658,15 +692,14 @@ lldpd_port_add(struct lldpd *cfg, struct ifaddrs *ifa)
                }
                if (ethc.port == PORT_AUI) port->p_mau_type = LLDP_DOT3_MAU_AUI;
        } else
-               LLOG_INFO("unable to get eth info for %s", hardware->h_ifname);
+               LLOG_DEBUG("unable to get eth info for %s", hardware->h_ifname);
+#endif
 
        if (!INTERFACE_OPENED(hardware)) {
 
                if (lldpd_iface_init(cfg, hardware) != 0) {
                        LLOG_WARN("unable to initialize %s", hardware->h_ifname);
-                       lldpd_vlan_cleanup(&hardware->h_lport);
-                       free(hardware->h_proto_macs);
-                       free(hardware);
+                       lldpd_hardware_cleanup(hardware);
                        return (NULL);
                }
 
@@ -1157,13 +1190,25 @@ void
 lldpd_send_all(struct lldpd *cfg)
 {
        struct lldpd_hardware *hardware;
-       int i;
+       u_int8_t saved_lladdr[ETHER_ADDR_LEN];
+       int i, altermac;
+
        cfg->g_lastsent = time(NULL);
        TAILQ_FOREACH(hardware, &cfg->g_hardware, h_entries) {
                /* Ignore if interface is down */
                if ((hardware->h_flags & IFF_UP) == 0)
                        continue;
 
+               /* When sending on inactive slaves, just send using a 0:0:0:0:0:0 address */
+               altermac = 0;
+               if ((hardware->h_raw_real > 0) &&
+                   (!iface_is_slave_active(cfg, hardware->h_master,
+                       hardware->h_ifname))) {
+                       altermac = 1;
+                       memcpy(saved_lladdr, hardware->h_lladdr, ETHER_ADDR_LEN);
+                       memset(hardware->h_lladdr, 0, ETHER_ADDR_LEN);
+               }
+
                for (i=0; cfg->g_protocols[i].mode != 0; i++) {
                        if (!cfg->g_protocols[i].enabled)
                                continue;
@@ -1171,9 +1216,31 @@ lldpd_send_all(struct lldpd *cfg)
                            (cfg->g_protocols[i].mode == LLDPD_MODE_LLDP))
                                cfg->g_protocols[i].send(cfg, &cfg->g_lchassis, hardware);
                }
+               /* Restore MAC if needed */
+               if (altermac)
+                       memcpy(hardware->h_lladdr, saved_lladdr, ETHER_ADDR_LEN);
        }
 }
 
+#ifdef ENABLE_LLDPMED
+void
+lldpd_med(struct lldpd_chassis *chassis)
+{
+       free(chassis->c_med_hw);
+       free(chassis->c_med_fw);
+       free(chassis->c_med_sn);
+       free(chassis->c_med_manuf);
+       free(chassis->c_med_model);
+       free(chassis->c_med_asset);
+       chassis->c_med_hw = dmi_hw();
+       chassis->c_med_fw = dmi_fw();
+       chassis->c_med_sn = dmi_sn();
+       chassis->c_med_manuf = dmi_manuf();
+       chassis->c_med_model = dmi_model();
+       chassis->c_med_asset = dmi_asset();
+}
+#endif
+
 void
 lldpd_loop(struct lldpd *cfg)
 {
@@ -1195,21 +1262,28 @@ lldpd_loop(struct lldpd *cfg)
                fatal("failed to get system name");
        free(cfg->g_lchassis.c_name);
        free(cfg->g_lchassis.c_descr);
-       if (asprintf(&cfg->g_lchassis.c_name, "%s",
-               hp) == -1)
-               fatal("failed to set system name");
+       if ((cfg->g_lchassis.c_name = strdup(hp)) == NULL)
+               fatal(NULL);
        if (asprintf(&cfg->g_lchassis.c_descr, "%s %s %s %s",
                un->sysname, un->release, un->version, un->machine) == -1)
                fatal("failed to set system description");
-       free(un);
 
        /* Check forwarding */
        cfg->g_lchassis.c_cap_enabled = 0;
        if ((f = priv_open("/proc/sys/net/ipv4/ip_forward")) >= 0) {
-               if ((read(f, &status, 1) == 1) && (status == '1'))
+               if ((read(f, &status, 1) == 1) && (status == '1')) {
                        cfg->g_lchassis.c_cap_enabled = LLDP_CAP_ROUTER;
+               }
                close(f);
        }
+#ifdef ENABLE_LLDPMED
+       if (cfg->g_lchassis.c_cap_available & LLDP_CAP_TELEPHONE)
+               cfg->g_lchassis.c_cap_enabled |= LLDP_CAP_TELEPHONE;
+       lldpd_med(&cfg->g_lchassis);
+       free(cfg->g_lchassis.c_med_sw);
+       cfg->g_lchassis.c_med_sw = strdup(un->release);
+#endif
+       free(un);
 
        TAILQ_FOREACH(hardware, &cfg->g_hardware, h_entries)
            hardware->h_flags = 0;
@@ -1330,10 +1404,16 @@ int
 main(int argc, char *argv[])
 {
        struct lldpd *cfg;
-       int ch, snmp = 0, debug = 0;
+       int ch, debug = 0;
+#ifdef USE_SNMP
+       int snmp = 0;
+#endif
        char *mgmtp = NULL;
-       char *popt, opts[] = "vdxm:p:@                    ";
+       char *popt, opts[] = "vdxm:p:M:i@                    ";
        int probe = 0, i, found, vlan = 0;
+#ifdef ENABLE_LLDPMED
+       int lldpmed = 0, noinventory = 0;
+#endif
 
        saved_argv = argv;
 
@@ -1357,11 +1437,35 @@ main(int argc, char *argv[])
                case 'm':
                        mgmtp = optarg;
                        break;
+#ifdef ENABLE_LLDPMED
+               case 'M':
+                       lldpmed = atoi(optarg);
+                       if ((lldpmed < 1) || (lldpmed > 4)) {
+                               fprintf(stderr, "-M requires an argument between 1 and 4\n");
+                               usage();
+                       }
+                       break;
+               case 'i':
+                       noinventory = 1;
+                       break;
+#else
+               case 'M':
+               case 'i':
+               case 'P':
+                       fprintf(stderr, "LLDP-MED support is not built-in\n");
+                       usage();
+                       break;
+#endif
                case 'p':
                        probe = atoi(optarg);
                        break;
                case 'x':
+#ifdef USE_SNMP
                        snmp = 1;
+#else
+                       fprintf(stderr, "SNMP support is not built-in\n");
+                       usage();
+#endif
                        break;
                default:
                        found = 0;
@@ -1376,13 +1480,26 @@ main(int argc, char *argv[])
                                usage();
                }
        }
-
+       
        log_init(debug);
-       priv_init(
-#ifdef USE_SNMP
-               snmp
-#endif
-);
+
+       if (!debug) {
+               int pid;
+               char *spid;
+               if (daemon(0, 0) != 0)
+                       fatal("failed to detach daemon");
+               if ((pid = open(LLDPD_PID_FILE,
+                           O_TRUNC | O_CREAT | O_WRONLY, 0644)) == -1)
+                       fatal("unable to open pid file " LLDPD_PID_FILE);
+               if (asprintf(&spid, "%d\n", getpid()) == -1)
+                       fatal("unable to create pid file " LLDPD_PID_FILE);
+               if (write(pid, spid, strlen(spid)) == -1)
+                       fatal("unable to write pid file " LLDPD_PID_FILE);
+               free(spid);
+               close(pid);
+       }
+
+       priv_init(PRIVSEP_CHROOT);
 
        if (probe == 0) probe = LLDPD_TTL;
 
@@ -1401,6 +1518,16 @@ main(int argc, char *argv[])
        /* Set system capabilities */
        cfg->g_lchassis.c_cap_available = LLDP_CAP_BRIDGE | LLDP_CAP_WLAN |
            LLDP_CAP_ROUTER;
+#ifdef ENABLE_LLDPMED
+       if (lldpmed > 0) {
+               if (lldpmed == LLDPMED_CLASS_III)
+                       cfg->g_lchassis.c_cap_available |= LLDP_CAP_TELEPHONE;
+               cfg->g_lchassis.c_med_type = lldpmed;
+               cfg->g_lchassis.c_med_cap_available = LLDPMED_CAP_CAP |
+                   LLDPMED_CAP_IV | LLDPMED_CAP_LOCATION;
+               cfg->g_noinventory = noinventory;
+       }
+#endif
 
        /* Set TTL */
        cfg->g_lchassis.c_ttl = LLDPD_TTL;
@@ -1431,9 +1558,6 @@ main(int argc, char *argv[])
        TAILQ_INIT(&cfg->g_clients);
 
        gcfg = cfg;
-       if (!debug) {
-               priv_fork();
-       }
        if (atexit(lldpd_exit) != 0) {
                close(cfg->g_ctl);
                priv_ctl_cleanup();