Line data    Source code

       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             : 
      31           0 : int lldp_network_bind_raw_socket(int ifindex) {
      32             :         typedef struct LLDPFrame {
      33             :                 struct ethhdr hdr;
      34             :                 uint8_t tlvs[0];
      35             :         } LLDPFrame;
      36             : 
      37           0 :         struct sock_filter filter[] = {
      38             :                 BPF_STMT(BPF_LD + BPF_W + BPF_ABS, offsetof(LLDPFrame, hdr.h_dest)),      /* A <- 4 bytes of destination MAC */
      39             :                 BPF_JUMP(BPF_JMP + BPF_JEQ + BPF_K, 0x0180c200, 1, 0),                    /* A != 01:80:c2:00 */
      40             :                 BPF_STMT(BPF_RET + BPF_K, 0),                                             /* drop packet */
      41             :                 BPF_STMT(BPF_LD + BPF_H + BPF_ABS, offsetof(LLDPFrame, hdr.h_dest) + 4),  /* A <- remaining 2 bytes of destination MAC */
      42             :                 BPF_JUMP(BPF_JMP + BPF_JEQ + BPF_K, 0x0000, 3, 0),                        /* A != 00:00 */
      43             :                 BPF_JUMP(BPF_JMP + BPF_JEQ + BPF_K, 0x0003, 2, 0),                        /* A != 00:03 */
      44             :                 BPF_JUMP(BPF_JMP + BPF_JEQ + BPF_K, 0x000e, 1, 0),                        /* A != 00:0e */
      45             :                 BPF_STMT(BPF_RET + BPF_K, 0),                                             /* drop packet */
      46             :                 BPF_STMT(BPF_LD + BPF_H + BPF_ABS, offsetof(LLDPFrame, hdr.h_proto)),     /* A <- protocol */
      47             :                 BPF_JUMP(BPF_JMP + BPF_JEQ + BPF_K, ETHERTYPE_LLDP, 1, 0),                /* A != ETHERTYPE_LLDP */
      48             :                 BPF_STMT(BPF_RET + BPF_K, 0),                                             /* drop packet */
      49             :                 BPF_STMT(BPF_RET + BPF_K, (uint32_t) -1),                                 /* accept packet */
      50             :         };
      51             : 
      52           0 :         struct sock_fprog fprog = {
      53             :                 .len = ELEMENTSOF(filter),
      54             :                 .filter = filter
      55             :         };
      56             : 
      57           0 :         _cleanup_close_ int s = -1;
      58             : 
      59           0 :         union sockaddr_union saddrll = {
      60             :                 .ll.sll_family = AF_PACKET,
      61             :                 .ll.sll_ifindex = ifindex,
      62             :         };
      63             : 
      64             :         int r;
      65             : 
      66           0 :         assert(ifindex > 0);
      67             : 
      68           0 :         s = socket(PF_PACKET, SOCK_RAW, htons(ETH_P_ALL));
      69           0 :         if (s < 0)
      70           0 :                 return -errno;
      71             : 
      72           0 :         r = setsockopt(s, SOL_SOCKET, SO_ATTACH_FILTER, &fprog, sizeof(fprog));
      73           0 :         if (r < 0)
      74           0 :                 return -errno;
      75             : 
      76           0 :         r = bind(s, &saddrll.sa, sizeof(saddrll.ll));
      77           0 :         if (r < 0)
      78           0 :                 return -errno;
      79             : 
      80           0 :         r = s;
      81           0 :         s = -1;
      82             : 
      83           0 :         return r;
      84             : }
      85             : 
      86           0 : int lldp_receive_packet(sd_event_source *s, int fd, uint32_t revents, void *userdata) {
      87           0 :         _cleanup_tlv_packet_free_ tlv_packet *packet = NULL;
      88             :         tlv_packet *p;
      89             :         uint16_t length;
      90             :         int r;
      91             : 
      92           0 :         assert(fd);
      93           0 :         assert(userdata);
      94             : 
      95           0 :         r = tlv_packet_new(&packet);
      96           0 :         if (r < 0)
      97           0 :                 return r;
      98             : 
      99           0 :         length = read(fd, &packet->pdu, sizeof(packet->pdu));
     100             : 
     101             :         /* Silently drop the packet */
     102           0 :         if ((size_t) length > ETHER_MAX_LEN)
     103           0 :                 return 0;
     104             : 
     105           0 :         packet->userdata = userdata;
     106             : 
     107           0 :         p = packet;
     108           0 :         packet = NULL;
     109             : 
     110           0 :         return lldp_handle_packet(p, (uint16_t) length);
     111             : }