src/libsystemd-network/lldp-network.c

   1 /*-*- Mode: C; c-basic-offset: 8; indent-tabs-mode: nil -*-*/
   2
   3 /***
   4   This file is part of systemd.
   5
   6   Copyright (C) 2014 Tom Gundersen
   7   Copyright (C) 2014 Susant Sahani
   8
   9   systemd is free software; you can redistribute it and/or modify it
  10   under the terms of the GNU Lesser General Public License as published by
  11   the Free Software Foundation; either version 2.1 of the License, or
  12   (at your option) any later version.
  13
  14   systemd is distributed in the hope that it will be useful, but
  15   WITHOUT ANY WARRANTY; without even the implied warranty of
  16   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
  17   Lesser General Public License for more details.
  18
  19   You should have received a copy of the GNU Lesser General Public License
  20   along with systemd; If not, see <http://www.gnu.org/licenses/>.
  21 ***/
  22
  23 #include <linux/filter.h>
  24 #include <linux/if_ether.h>
  25
  26 #include "socket-util.h"
  27 #include "lldp-tlv.h"
  28 #include "lldp-network.h"
  29 #include "lldp-internal.h"
  30 #include "sd-lldp.h"
  31
  32 int lldp_network_bind_raw_socket(int ifindex) {
  33         typedef struct LLDPFrame {
  34                 struct ethhdr hdr;
  35                 uint8_t tlvs[0];
  36         } LLDPFrame;
  37
  38         struct sock_filter filter[] = {
  39                 BPF_STMT(BPF_LD + BPF_W + BPF_ABS, offsetof(LLDPFrame, hdr.h_dest)),      /* A <- 4 bytes of destination MAC */
  40                 BPF_JUMP(BPF_JMP + BPF_JEQ + BPF_K, 0x0180c200, 1, 0),                    /* A != 01:80:c2:00 */
  41                 BPF_STMT(BPF_RET + BPF_K, 0),                                             /* drop packet */
  42                 BPF_STMT(BPF_LD + BPF_H + BPF_ABS, offsetof(LLDPFrame, hdr.h_dest) + 4),  /* A <- remaining 2 bytes of destination MAC */
  43                 BPF_JUMP(BPF_JMP + BPF_JEQ + BPF_K, 0x0000, 3, 0),                        /* A != 00:00 */
  44                 BPF_JUMP(BPF_JMP + BPF_JEQ + BPF_K, 0x0003, 2, 0),                        /* A != 00:03 */
  45                 BPF_JUMP(BPF_JMP + BPF_JEQ + BPF_K, 0x000e, 1, 0),                        /* A != 00:0e */
  46                 BPF_STMT(BPF_RET + BPF_K, 0),                                             /* drop packet */
  47                 BPF_STMT(BPF_LD + BPF_H + BPF_ABS, offsetof(LLDPFrame, hdr.h_proto)),     /* A <- protocol */
  48                 BPF_JUMP(BPF_JMP + BPF_JEQ + BPF_K, ETHERTYPE_LLDP, 1, 0),                /* A != ETHERTYPE_LLDP */
  49                 BPF_STMT(BPF_RET + BPF_K, 0),                                             /* drop packet */
  50                 BPF_STMT(BPF_RET + BPF_K, (uint32_t) -1),                                 /* accept packet */
  51         };
  52
  53         struct sock_fprog fprog = {
  54                 .len = ELEMENTSOF(filter),
  55                 .filter = filter
  56         };
  57
  58         _cleanup_close_ int s = -1;
  59
  60         union sockaddr_union saddrll = {
  61                 .ll.sll_family = AF_PACKET,
  62                 .ll.sll_ifindex = ifindex,
  63         };
  64
  65         int r;
  66
  67         assert(ifindex > 0);
  68
  69         s = socket(PF_PACKET, SOCK_RAW, htons(ETH_P_ALL));
  70         if (s < 0)
  71                 return -errno;
  72
  73         r = setsockopt(s, SOL_SOCKET, SO_ATTACH_FILTER, &fprog, sizeof(fprog));
  74         if (r < 0)
  75                 return -errno;
  76
  77         r = bind(s, &saddrll.sa, sizeof(saddrll.ll));
  78         if (r < 0)
  79                 return -errno;
  80
  81         r = s;
  82         s = -1;
  83
  84         return r;
  85 }
  86
  87 int lldp_receive_packet(sd_event_source *s, int fd, uint32_t revents, void *userdata) {
  88         _cleanup_tlv_packet_free_ tlv_packet *packet = NULL;
  89         tlv_packet *p;
  90         uint16_t length;
  91         int r;
  92
  93         assert(fd);
  94         assert(userdata);
  95
  96         r = tlv_packet_new(&packet);
  97         if (r < 0)
  98                 return r;
  99
 100         length = read(fd, &packet->pdu, sizeof(packet->pdu));
 101
 102         /* Silently drop the packet */
 103         if ((size_t) length > ETHER_MAX_LEN)
 104                 return 0;
 105
 106         packet->userdata = userdata;
 107
 108         p = packet;
 109         packet = NULL;
 110
 111         return lldp_handle_packet(p, (uint16_t) length);
 112 }