src/libsystemd-network/lldp-network.c

   1 /* SPDX-License-Identifier: LGPL-2.1+ */
   2 /***
   3   This file is part of systemd.
   4
   5   Copyright (C) 2014 Tom Gundersen
   6   Copyright (C) 2014 Susant Sahani
   7
   8   systemd is free software; you can redistribute it and/or modify it
   9   under the terms of the GNU Lesser General Public License as published by
  10   the Free Software Foundation; either version 2.1 of the License, or
  11   (at your option) any later version.
  12
  13   systemd is distributed in the hope that it will be useful, but
  14   WITHOUT ANY WARRANTY; without even the implied warranty of
  15   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
  16   Lesser General Public License for more details.
  17
  18   You should have received a copy of the GNU Lesser General Public License
  19   along with systemd; If not, see <http://www.gnu.org/licenses/>.
  20 ***/
  21
  22 #include <linux/filter.h>
  23 #include <netinet/if_ether.h>
  24
  25 #include "fd-util.h"
  26 #include "lldp-network.h"
  27 #include "socket-util.h"
  28
  29 int lldp_network_bind_raw_socket(int ifindex) {
  30
  31         static const struct sock_filter filter[] = {
  32                 BPF_STMT(BPF_LD + BPF_W + BPF_ABS, offsetof(struct ethhdr, h_dest)),      /* A <- 4 bytes of destination MAC */
  33                 BPF_JUMP(BPF_JMP + BPF_JEQ + BPF_K, 0x0180c200, 1, 0),                    /* A != 01:80:c2:00 */
  34                 BPF_STMT(BPF_RET + BPF_K, 0),                                             /* drop packet */
  35                 BPF_STMT(BPF_LD + BPF_H + BPF_ABS, offsetof(struct ethhdr, h_dest) + 4),  /* A <- remaining 2 bytes of destination MAC */
  36                 BPF_JUMP(BPF_JMP + BPF_JEQ + BPF_K, 0x0000, 3, 0),                        /* A != 00:00 */
  37                 BPF_JUMP(BPF_JMP + BPF_JEQ + BPF_K, 0x0003, 2, 0),                        /* A != 00:03 */
  38                 BPF_JUMP(BPF_JMP + BPF_JEQ + BPF_K, 0x000e, 1, 0),                        /* A != 00:0e */
  39                 BPF_STMT(BPF_RET + BPF_K, 0),                                             /* drop packet */
  40                 BPF_STMT(BPF_LD + BPF_H + BPF_ABS, offsetof(struct ethhdr, h_proto)),     /* A <- protocol */
  41                 BPF_JUMP(BPF_JMP + BPF_JEQ + BPF_K, ETHERTYPE_LLDP, 1, 0),                /* A != ETHERTYPE_LLDP */
  42                 BPF_STMT(BPF_RET + BPF_K, 0),                                             /* drop packet */
  43                 BPF_STMT(BPF_RET + BPF_K, (uint32_t) -1),                                 /* accept packet */
  44         };
  45
  46         static const struct sock_fprog fprog = {
  47                 .len = ELEMENTSOF(filter),
  48                 .filter = (struct sock_filter*) filter,
  49         };
  50
  51         struct packet_mreq mreq = {
  52                 .mr_ifindex = ifindex,
  53                 .mr_type = PACKET_MR_MULTICAST,
  54                 .mr_alen = ETH_ALEN,
  55                 .mr_address = { 0x01, 0x80, 0xC2, 0x00, 0x00, 0x00 }
  56         };
  57
  58         union sockaddr_union saddrll = {
  59                 .ll.sll_family = AF_PACKET,
  60                 .ll.sll_ifindex = ifindex,
  61         };
  62
  63         _cleanup_close_ int fd = -1;
  64         int r;
  65
  66         assert(ifindex > 0);
  67
  68         fd = socket(PF_PACKET, SOCK_RAW|SOCK_CLOEXEC|SOCK_NONBLOCK,
  69                     htobe16(ETHERTYPE_LLDP));
  70         if (fd < 0)
  71                 return -errno;
  72
  73         r = setsockopt(fd, SOL_SOCKET, SO_ATTACH_FILTER, &fprog, sizeof(fprog));
  74         if (r < 0)
  75                 return -errno;
  76
  77         r = setsockopt(fd, SOL_PACKET, PACKET_ADD_MEMBERSHIP, &mreq, sizeof(mreq));
  78         if (r < 0)
  79                 return -errno;
  80
  81         mreq.mr_address[ETH_ALEN - 1] = 0x03;
  82         r = setsockopt(fd, SOL_PACKET, PACKET_ADD_MEMBERSHIP, &mreq, sizeof(mreq));
  83         if (r < 0)
  84                 return -errno;
  85
  86         mreq.mr_address[ETH_ALEN - 1] = 0x0E;
  87         r = setsockopt(fd, SOL_PACKET, PACKET_ADD_MEMBERSHIP, &mreq, sizeof(mreq));
  88         if (r < 0)
  89                 return -errno;
  90
  91         r = bind(fd, &saddrll.sa, sizeof(saddrll.ll));
  92         if (r < 0)
  93                 return -errno;
  94
  95         r = fd;
  96         fd = -1;
  97
  98         return r;
  99 }