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            Stephen Hemminger <shemminger@linux-foundation.org>
  23
  24 ******************************************************************************/
  25
  26 #include "packet.h"
  27 #include "epoll_loop.h"
  28 #include "netif_utils.h"
  29 #include "bridge_ctl.h"
  30
  31 #include <stdio.h>
  32 #include <stdlib.h>
  33 #include <errno.h>
  34 #include <string.h>
  35 #include <unistd.h>
  36 #include <stdint.h>
  37 #include <sys/socket.h>
  38 #include <sys/ioctl.h>
  39 #include <fcntl.h>
  40 #include <netinet/in.h>
  41
  42 #include <linux/if.h>
  43 #include <linux/if_ether.h>
  44 #include <linux/if_packet.h>
  45 #include <linux/filter.h>
  46
  47 #include "log.h"
  48
  49 #define DEBUG 1
  50
  51 /*
  52  * To send/receive Spanning Tree packets we use PF_PACKET because
  53  * it allows the filtering we want but gives raw data
  54  */
  55 void packet_send(struct epoll_event_handler *h, unsigned char *data, int len)
  56 {
  57         int l;
  58
  59         if (fcntl(h->fd, F_SETFL, 0) < 0)
  60                 ERROR("Error unsetting O_NONBLOCK: %m");
  61
  62         l = send(h->fd, data, len, 0);
  63         if (l < 0)
  64                 ERROR("send failed: %m");
  65         else if (l != len)
  66                 ERROR("short write in sendto: %d instead of %d", l, len);
  67
  68         if (fcntl(h->fd, F_SETFL, O_NONBLOCK) < 0)
  69                 ERROR("Error setting O_NONBLOCK: %m");
  70 }
  71
  72 void packet_rcv_handler(uint32_t events, struct epoll_event_handler *h)
  73 {
  74         int cc;
  75         unsigned char buf[2048];
  76
  77         cc = recv(h->fd, &buf, sizeof(buf), 0);
  78         if (cc <= 0) {
  79                 ERROR("read failed: %m");
  80                 return;
  81         }
  82
  83 #ifdef DEBUG
  84         printf("Src %02x:%02x:%02x:%02x:%02x:%02x\n",
  85                buf[0], buf[1], buf[2], buf[3], buf[4], buf[5]);
  86
  87         int i, j;
  88         for (i = 0; i < cc; i += 16) {
  89                 for (j = 0; j < 16 && i + j < cc; j++)
  90                         printf(" %02x", buf[i + j]);
  91                 printf("\n");
  92         }
  93         printf("\n");
  94         fflush(stdout);
  95 #endif
  96
  97         bridge_bpdu_rcv(h->arg, buf, cc);
  98 }
  99
 100 /* Berkeley Packet filter code to filter out spanning tree packets.
 101    from tcpdump -dd stp
 102  */
 103 static struct sock_filter stp_filter[] = {
 104         { 0x28, 0, 0, 0x0000000c },
 105         { 0x25, 3, 0, 0x000005dc },
 106         { 0x30, 0, 0, 0x0000000e },
 107         { 0x15, 0, 1, 0x00000042 },
 108         { 0x6, 0, 0, 0x00000060 },
 109         { 0x6, 0, 0, 0x00000000 },
 110 };
 111
 112 /*
 113  * Open up a raw packet socket to catch all 802.2 packets on a device
 114  * and install a packet filter to only see STP (SAP 42)
 115  */
 116 int packet_sock_create(struct epoll_event_handler *h, int if_index,
 117                        struct ifdata *arg)
 118 {
 119         int s;
 120         struct sockaddr_ll sll = {
 121                 .sll_family = AF_PACKET,
 122                 .sll_ifindex = if_index,
 123         };
 124         struct sock_fprog prog = {
 125                 .len = sizeof(stp_filter) / sizeof(stp_filter[0]),
 126                 .filter = stp_filter,
 127         };
 128
 129         s = socket(PF_PACKET, SOCK_PACKET, htons(ETH_P_802_2));
 130         if (s < 0) {
 131                 ERROR("socket failed: %m");
 132                 return -1;
 133         }
 134
 135         if (bind(s, (struct sockaddr *) &sll, sizeof(sll)) < 0)
 136                 ERROR("bind failed: %m");
 137
 138         else if (setsockopt(s, SOL_SOCKET, SO_ATTACH_FILTER, &prog, sizeof(prog)) < 0)
 139                 ERROR("setsockopt packet filter failed: %m");
 140
 141         else if (fcntl(s, F_SETFL, O_NONBLOCK) < 0)
 142                 ERROR("fcntl set nonblock failed: %m");
 143
 144         else {
 145                 h->fd = s;
 146                 h->arg = arg;
 147                 h->handler = packet_rcv_handler;
 148
 149                 if (add_epoll(h) == 0)
 150                         return 0;
 151         }
 152
 153         close(s);
 154         return -1;
 155 }
 156
 157 void packet_sock_delete(struct epoll_event_handler *h)
 158 {
 159         remove_epoll(h);
 160         close(h->fd);
 161 }