Line data    Source code

       1             : /* SPDX-License-Identifier: LGPL-2.1+ */
       2             : 
       3             : #include <linux/filter.h>
       4             : #include <netinet/if_ether.h>
       5             : 
       6             : #include "fd-util.h"
       7             : #include "lldp-network.h"
       8             : #include "missing.h"
       9             : #include "socket-util.h"
      10             : 
      11           0 : int lldp_network_bind_raw_socket(int ifindex) {
      12             : 
      13             :         static const struct sock_filter filter[] = {
      14             :                 BPF_STMT(BPF_LD + BPF_W + BPF_ABS, offsetof(struct ethhdr, h_dest)),      /* A <- 4 bytes of destination MAC */
      15             :                 BPF_JUMP(BPF_JMP + BPF_JEQ + BPF_K, 0x0180c200, 1, 0),                    /* A != 01:80:c2:00 */
      16             :                 BPF_STMT(BPF_RET + BPF_K, 0),                                             /* drop packet */
      17             :                 BPF_STMT(BPF_LD + BPF_H + BPF_ABS, offsetof(struct ethhdr, h_dest) + 4),  /* A <- remaining 2 bytes of destination MAC */
      18             :                 BPF_JUMP(BPF_JMP + BPF_JEQ + BPF_K, 0x0000, 3, 0),                        /* A != 00:00 */
      19             :                 BPF_JUMP(BPF_JMP + BPF_JEQ + BPF_K, 0x0003, 2, 0),                        /* A != 00:03 */
      20             :                 BPF_JUMP(BPF_JMP + BPF_JEQ + BPF_K, 0x000e, 1, 0),                        /* A != 00:0e */
      21             :                 BPF_STMT(BPF_RET + BPF_K, 0),                                             /* drop packet */
      22             :                 BPF_STMT(BPF_LD + BPF_H + BPF_ABS, offsetof(struct ethhdr, h_proto)),     /* A <- protocol */
      23             :                 BPF_JUMP(BPF_JMP + BPF_JEQ + BPF_K, ETHERTYPE_LLDP, 1, 0),                /* A != ETHERTYPE_LLDP */
      24             :                 BPF_STMT(BPF_RET + BPF_K, 0),                                             /* drop packet */
      25             :                 BPF_STMT(BPF_RET + BPF_K, (uint32_t) -1),                                 /* accept packet */
      26             :         };
      27             : 
      28             :         static const struct sock_fprog fprog = {
      29             :                 .len = ELEMENTSOF(filter),
      30             :                 .filter = (struct sock_filter*) filter,
      31             :         };
      32             : 
      33           0 :         struct packet_mreq mreq = {
      34             :                 .mr_ifindex = ifindex,
      35             :                 .mr_type = PACKET_MR_MULTICAST,
      36             :                 .mr_alen = ETH_ALEN,
      37             :                 .mr_address = { 0x01, 0x80, 0xC2, 0x00, 0x00, 0x00 }
      38             :         };
      39             : 
      40           0 :         union sockaddr_union saddrll = {
      41             :                 .ll.sll_family = AF_PACKET,
      42             :                 .ll.sll_ifindex = ifindex,
      43             :         };
      44             : 
      45           0 :         _cleanup_close_ int fd = -1;
      46             :         int r;
      47             : 
      48           0 :         assert(ifindex > 0);
      49             : 
      50           0 :         fd = socket(PF_PACKET, SOCK_RAW|SOCK_CLOEXEC|SOCK_NONBLOCK,
      51           0 :                     htobe16(ETHERTYPE_LLDP));
      52           0 :         if (fd < 0)
      53           0 :                 return -errno;
      54             : 
      55           0 :         r = setsockopt(fd, SOL_SOCKET, SO_ATTACH_FILTER, &fprog, sizeof(fprog));
      56           0 :         if (r < 0)
      57           0 :                 return -errno;
      58             : 
      59           0 :         r = setsockopt(fd, SOL_PACKET, PACKET_ADD_MEMBERSHIP, &mreq, sizeof(mreq));
      60           0 :         if (r < 0)
      61           0 :                 return -errno;
      62             : 
      63           0 :         mreq.mr_address[ETH_ALEN - 1] = 0x03;
      64           0 :         r = setsockopt(fd, SOL_PACKET, PACKET_ADD_MEMBERSHIP, &mreq, sizeof(mreq));
      65           0 :         if (r < 0)
      66           0 :                 return -errno;
      67             : 
      68           0 :         mreq.mr_address[ETH_ALEN - 1] = 0x0E;
      69           0 :         r = setsockopt(fd, SOL_PACKET, PACKET_ADD_MEMBERSHIP, &mreq, sizeof(mreq));
      70           0 :         if (r < 0)
      71           0 :                 return -errno;
      72             : 
      73           0 :         r = bind(fd, &saddrll.sa, sizeof(saddrll.ll));
      74           0 :         if (r < 0)
      75           0 :                 return -errno;
      76             : 
      77           0 :         return TAKE_FD(fd);
      78             : }