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 <errno.h>
  33 #include <netinet/in.h>
  34 #include <linux/if_packet.h>
  35 #include <linux/filter.h>
  36 #include <asm/byteorder.h>
  37 #include <libubox/uloop.h>
  38
  39 #include "netif_utils.h"
  40 #include "bridge_ctl.h"
  41 #include "packet.h"
  42 #include "log.h"
  43 #include "worker.h"
  44
  45 static struct uloop_fd ufd;
  46
  47 #ifdef PACKET_DEBUG
  48 static void dump_packet(const unsigned char *buf, int cc)
  49 {
  50     int i, j;
  51     for(i = 0; i < cc; i += 16)
  52     {
  53         for(j = 0; j < 16 && i + j < cc; ++j)
  54             printf(" %02x", buf[i + j]);
  55         printf("\n");
  56     }
  57     printf("\n");
  58     fflush(stdout);
  59 }
  60 #endif
  61
  62 /*
  63  * To send/receive Spanning Tree packets we use PF_PACKET because
  64  * it allows the filtering we want but gives raw data
  65  */
  66 void packet_send(int ifindex, const struct iovec *iov, int iov_count, int len)
  67 {
  68     int l;
  69     struct sockaddr_ll sl =
  70     {
  71         .sll_family = AF_PACKET,
  72         .sll_protocol = __constant_cpu_to_be16(ETH_P_802_2),
  73         .sll_ifindex = ifindex,
  74         .sll_halen = ETH_ALEN,
  75     };
  76
  77     if(iov_count > 0 && iov[0].iov_len > ETH_ALEN)
  78         memcpy(&sl.sll_addr, iov[0].iov_base, ETH_ALEN);
  79
  80     struct msghdr msg =
  81     {
  82         .msg_name = &sl,
  83         .msg_namelen = sizeof(sl),
  84         .msg_iov = (struct iovec *)iov,
  85         .msg_iovlen = iov_count,
  86         .msg_control = NULL,
  87         .msg_controllen = 0,
  88         .msg_flags = 0,
  89     };
  90
  91 #ifdef PACKET_DEBUG
  92     printf("Transmit Dst index %d %02x:%02x:%02x:%02x:%02x:%02x\n",
  93            sl.sll_ifindex,
  94            sl.sll_addr[0], sl.sll_addr[1], sl.sll_addr[2],
  95            sl.sll_addr[3], sl.sll_addr[4], sl.sll_addr[5]);
  96     {
  97         int i;
  98         for(i = 0; i < iov_count; ++i)
  99             dump_packet(iov[i].iov_base, iov[i].iov_len);
 100     }
 101 #endif
 102
 103     l = sendmsg(ufd.fd, &msg, 0);
 104
 105     if(l < 0)
 106     {
 107         if(errno != EWOULDBLOCK)
 108             ERROR("send failed: %m");
 109     }
 110     else if(l != len)
 111         ERROR("short write in sendto: %d instead of %d", l, len);
 112 }
 113
 114 void packet_rcv(void)
 115 {
 116     unsigned char buf[2048];
 117     struct sockaddr_ll sl;
 118     socklen_t salen = sizeof sl;
 119     int cc;
 120
 121         while (1) {
 122                 cc = recvfrom(ufd.fd, &buf, sizeof(buf), 0, (struct sockaddr *) &sl, &salen);
 123                 if (cc < 0) {
 124                         switch (errno) {
 125                         case EINTR:
 126                                 continue;
 127                         case EAGAIN:
 128                                 return;
 129                         default:
 130                                 cc = 0;
 131                         }
 132                 }
 133
 134                 if (cc == 0) {
 135                         ERROR("recvfrom failed: %m");
 136                         uloop_fd_delete(&ufd);
 137                         return;
 138                 }
 139
 140 #ifdef PACKET_DEBUG
 141                 printf("Receive Src ifindex %d %02x:%02x:%02x:%02x:%02x:%02x\n",
 142                            sl.sll_ifindex,
 143                            sl.sll_addr[0], sl.sll_addr[1], sl.sll_addr[2],
 144                            sl.sll_addr[3], sl.sll_addr[4], sl.sll_addr[5]);
 145
 146                 dump_packet(buf, cc);
 147 #endif
 148
 149                 bridge_bpdu_rcv(sl.sll_ifindex, buf, cc);
 150         }
 151 }
 152
 153 /* Berkeley Packet filter code to filter out spanning tree packets.
 154    from tcpdump -s 1152 -dd stp
 155  */
 156 static struct sock_filter stp_filter[] = {
 157     { 0x28, 0, 0, 0x0000000c },
 158     { 0x25, 3, 0, 0x000005dc },
 159     { 0x30, 0, 0, 0x0000000e },
 160     { 0x15, 0, 1, 0x00000042 },
 161     { 0x6, 0, 0, 0x00000480 },
 162     { 0x6, 0, 0, 0x00000000 },
 163 };
 164
 165 static void
 166 packet_event(struct uloop_fd *fd, unsigned int events)
 167 {
 168         struct worker_event ev = {
 169                 .type = WORKER_EV_RECV_PACKET,
 170         };
 171
 172         worker_queue_event(&ev);
 173 }
 174
 175 /*
 176  * Open up a raw packet socket to catch all 802.2 packets.
 177  * and install a packet filter to only see STP (SAP 42)
 178  *
 179  * Since any bridged devices are already in promiscious mode
 180  * no need to add multicast address.
 181  */
 182 int packet_sock_init(void)
 183 {
 184     struct sock_fprog prog =
 185     {
 186         .len = sizeof(stp_filter) / sizeof(stp_filter[0]),
 187         .filter = stp_filter,
 188     };
 189     int s;
 190
 191     s = socket(PF_PACKET, SOCK_RAW, htons(ETH_P_802_2));
 192     if(s < 0)
 193     {
 194         ERROR("socket failed: %m");
 195         return -1;
 196     }
 197
 198     if (setsockopt(s, SOL_SOCKET, SO_ATTACH_FILTER, &prog, sizeof(prog)) < 0) {
 199         ERROR("setsockopt packet filter failed: %m");
 200                 goto out;
 201     }
 202
 203         if (fcntl(s, F_SETFL, O_NONBLOCK) < 0) {
 204         ERROR("fcntl set nonblock failed: %m");
 205                 goto out;
 206         }
 207
 208     ufd.fd = s;
 209         ufd.cb = packet_event;
 210         uloop_fd_add(&ufd, ULOOP_READ | ULOOP_EDGE_TRIGGER);
 211
 212         return 0;
 213
 214 out:
 215     close(s);
 216     return -1;
 217 }