packet.c

   1 /*****************************************************************************
   2   Copyright (c) 2006 EMC Corporation.
   3
   4   This program is free software; you can redistribute it and/or modify it
   5   under the terms of the GNU General Public License as published by the Free
   6   Software Foundation; either version 2 of the License, or (at your option)
   7   any later version.
   8
   9   This program is distributed in the hope that it will be useful, but WITHOUT
  10   ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
  11   FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  12   more details.
  13
  14   You should have received a copy of the GNU General Public License along with
  15   this program; if not, write to the Free Software Foundation, Inc., 59
  16   Temple Place - Suite 330, Boston, MA  02111-1307, USA.
  17
  18   The full GNU General Public License is included in this distribution in the
  19   file called LICENSE.
  20
  21   Authors: Srinivas Aji <Aji_Srinivas@emc.com>
  22   Authors: Stephen Hemminger <shemminger@linux-foundation.org>
  23
  24 ******************************************************************************/
  25
  26 /* #define PACKET_DEBUG */
  27
  28 #include <string.h>
  29 #include <unistd.h>
  30 #include <stdbool.h>
  31 #include <fcntl.h>
  32 #include <netinet/in.h>
  33 #include <linux/if_packet.h>
  34 #include <linux/filter.h>
  35
  36 #include "epoll_loop.h"
  37 #include "netif_utils.h"
  38 #include "bridge_ctl.h"
  39 #include "packet.h"
  40 #include "log.h"
  41
  42 static struct epoll_event_handler packet_event;
  43
  44 #ifdef PACKET_DEBUG
  45 static void dump_packet(const unsigned char *buf, int cc)
  46 {
  47     int i, j;
  48     for(i = 0; i < cc; i += 16)
  49     {
  50         for(j = 0; j < 16 && i + j < cc; ++j)
  51             printf(" %02x", buf[i + j]);
  52         printf("\n");
  53     }
  54     printf("\n");
  55     fflush(stdout);
  56 }
  57 #endif
  58
  59 /*
  60  * To send/receive Spanning Tree packets we use PF_PACKET because
  61  * it allows the filtering we want but gives raw data
  62  */
  63 void packet_send(int ifindex, const struct iovec *iov, int iov_count, int len)
  64 {
  65     int l;
  66     struct sockaddr_ll sl =
  67     {
  68         .sll_family = AF_PACKET,
  69         .sll_protocol = htons(ETH_P_802_2),
  70         .sll_ifindex = ifindex,
  71         .sll_halen = ETH_ALEN,
  72     };
  73
  74     if(iov_count > 0 && iov[0].iov_len > ETH_ALEN)
  75         memcpy(&sl.sll_addr, iov[0].iov_base, ETH_ALEN);
  76
  77     struct msghdr msg =
  78     {
  79         .msg_name = &sl,
  80         .msg_namelen = sizeof(sl),
  81         .msg_iov = (struct iovec *)iov,
  82         .msg_iovlen = iov_count,
  83         .msg_control = NULL,
  84         .msg_controllen = 0,
  85         .msg_flags = 0,
  86     };
  87
  88 #ifdef PACKET_DEBUG
  89     printf("Transmit Dst index %d %02x:%02x:%02x:%02x:%02x:%02x\n",
  90            sl.sll_ifindex,
  91            sl.sll_addr[0], sl.sll_addr[1], sl.sll_addr[2],
  92            sl.sll_addr[3], sl.sll_addr[4], sl.sll_addr[5]);
  93     {
  94         int i;
  95         for(i = 0; i < iov_count; ++i)
  96             dump_packet(iov[i].iov_base, iov[i].iov_len);
  97     }
  98 #endif
  99
 100     l = sendmsg(packet_event.fd, &msg, 0);
 101
 102     if(l < 0)
 103     {
 104         if(errno != EWOULDBLOCK)
 105             ERROR("send failed: %m");
 106     }
 107     else if(l != len)
 108         ERROR("short write in sendto: %d instead of %d", l, len);
 109 }
 110
 111 static void packet_rcv(uint32_t events, struct epoll_event_handler *h)
 112 {
 113     int cc;
 114     unsigned char buf[2048];
 115     struct sockaddr_ll sl;
 116     socklen_t salen = sizeof sl;
 117
 118     cc = recvfrom(h->fd, &buf, sizeof(buf), 0, (struct sockaddr *) &sl, &salen);
 119     if(cc <= 0)
 120     {
 121         ERROR("recvfrom failed: %m");
 122         return;
 123     }
 124
 125 #ifdef PACKET_DEBUG
 126     printf("Receive Src ifindex %d %02x:%02x:%02x:%02x:%02x:%02x\n",
 127            sl.sll_ifindex,
 128            sl.sll_addr[0], sl.sll_addr[1], sl.sll_addr[2],
 129            sl.sll_addr[3], sl.sll_addr[4], sl.sll_addr[5]);
 130
 131     dump_packet(buf, cc);
 132 #endif
 133
 134     bridge_bpdu_rcv(sl.sll_ifindex, buf, cc);
 135 }
 136
 137 /* Berkeley Packet filter code to filter out spanning tree packets.
 138    from tcpdump -s 1152 -dd stp
 139  */
 140 static struct sock_filter stp_filter[] = {
 141     { 0x28, 0, 0, 0x0000000c },
 142     { 0x25, 3, 0, 0x000005dc },
 143     { 0x30, 0, 0, 0x0000000e },
 144     { 0x15, 0, 1, 0x00000042 },
 145     { 0x6, 0, 0, 0x00000480 },
 146     { 0x6, 0, 0, 0x00000000 },
 147 };
 148
 149 /*
 150  * Open up a raw packet socket to catch all 802.2 packets.
 151  * and install a packet filter to only see STP (SAP 42)
 152  *
 153  * Since any bridged devices are already in promiscious mode
 154  * no need to add multicast address.
 155  */
 156 int packet_sock_init(void)
 157 {
 158     int s;
 159     struct sock_fprog prog =
 160     {
 161         .len = sizeof(stp_filter) / sizeof(stp_filter[0]),
 162         .filter = stp_filter,
 163     };
 164
 165     s = socket(PF_PACKET, SOCK_RAW, htons(ETH_P_802_2));
 166     if(s < 0)
 167     {
 168         ERROR("socket failed: %m");
 169         return -1;
 170     }
 171
 172     if(setsockopt(s, SOL_SOCKET, SO_ATTACH_FILTER, &prog, sizeof(prog)) < 0)
 173         ERROR("setsockopt packet filter failed: %m");
 174     else if(fcntl(s, F_SETFL, O_NONBLOCK) < 0)
 175         ERROR("fcntl set nonblock failed: %m");
 176     else
 177     {
 178         packet_event.fd = s;
 179         packet_event.handler = packet_rcv;
 180
 181         if(0 == add_epoll(&packet_event))
 182             return 0;
 183     }
 184
 185     close(s);
 186     return -1;
 187 }