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 static struct epoll_event_handler packet_event;
  50
  51 #ifdef PACKET_DEBUG
  52 static void dump_packet(const unsigned char *buf, int cc)
  53 {
  54         int i, j;
  55         for (i = 0; i < cc; i += 16) {
  56                 for (j = 0; j < 16 && i + j < cc; j++)
  57                         printf(" %02x", buf[i + j]);
  58                 printf("\n");
  59         }
  60         printf("\n");
  61         fflush(stdout);
  62 }
  63 #endif
  64
  65 /*
  66  * To send/receive Spanning Tree packets we use PF_PACKET because
  67  * it allows the filtering we want but gives raw data
  68  */
  69 void packet_send(int ifindex, const struct iovec *iov, int iov_count, int len)
  70 {
  71         int l;
  72         struct sockaddr_ll sl = {
  73                 .sll_family = AF_PACKET,
  74                 .sll_protocol = htons(ETH_P_802_2),
  75                 .sll_ifindex = ifindex,
  76                 .sll_halen = ETH_ALEN,
  77         };
  78
  79         if (iov_count > 0 && iov[0].iov_len > ETH_ALEN)
  80                 memcpy(&sl.sll_addr, iov[0].iov_base, ETH_ALEN);
  81
  82         struct msghdr msg = {
  83                 .msg_name = &sl,
  84                 .msg_namelen = sizeof(sl),
  85                 .msg_iov = (struct iovec *)iov,
  86                 .msg_iovlen = iov_count,
  87                 .msg_control = NULL,
  88                 .msg_controllen = 0,
  89                 .msg_flags = 0,
  90         };
  91
  92 #ifdef PACKET_DEBUG
  93         printf("Transmit Dst index %d %02x:%02x:%02x:%02x:%02x:%02x\n",
  94                sl.sll_ifindex,
  95                sl.sll_addr[0], sl.sll_addr[1], sl.sll_addr[2],
  96                sl.sll_addr[3], sl.sll_addr[4], sl.sll_addr[5]);
  97
  98         {
  99                 int i;
 100                 for (i = 0; i < iov_count; i++)
 101                         dump_packet(iov[i].iov_base, iov[i].iov_len);
 102         }
 103 #endif
 104
 105         l = sendmsg(packet_event.fd, &msg, 0);
 106
 107         if (l < 0) {
 108                 if (errno != EWOULDBLOCK)
 109                         ERROR("send failed: %m");
 110         } else if (l != len)
 111                 ERROR("short write in sendto: %d instead of %d", l, len);
 112 }
 113
 114 static void packet_rcv(uint32_t events, struct epoll_event_handler *h)
 115 {
 116         int cc;
 117         unsigned char buf[2048];
 118         struct sockaddr_ll sl;
 119         socklen_t salen = sizeof sl;
 120
 121         cc = recvfrom(h->fd, &buf, sizeof(buf), 0, (struct sockaddr *) &sl, &salen);
 122         if (cc <= 0) {
 123                 ERROR("recvfrom failed: %m");
 124                 return;
 125         }
 126
 127 #ifdef PACKET_DEBUG
 128         printf("Receive Src ifindex %d %02x:%02x:%02x:%02x:%02x:%02x\n",
 129                sl.sll_ifindex,
 130                sl.sll_addr[0], sl.sll_addr[1], sl.sll_addr[2],
 131                sl.sll_addr[3], sl.sll_addr[4], sl.sll_addr[5]);
 132
 133         dump_packet(buf, cc);
 134 #endif
 135
 136         bridge_bpdu_rcv(sl.sll_ifindex, buf, cc);
 137 }
 138
 139 /* Berkeley Packet filter code to filter out spanning tree packets.
 140    from tcpdump -dd stp
 141  */
 142 static struct sock_filter stp_filter[] = {
 143         { 0x28, 0, 0, 0x0000000c },
 144         { 0x25, 3, 0, 0x000005dc },
 145         { 0x30, 0, 0, 0x0000000e },
 146         { 0x15, 0, 1, 0x00000042 },
 147         { 0x6, 0, 0, 0x00000060 },
 148         { 0x6, 0, 0, 0x00000000 },
 149 };
 150
 151 /*
 152  * Open up a raw packet socket to catch all 802.2 packets.
 153  * and install a packet filter to only see STP (SAP 42)
 154  *
 155  * Since any bridged devices are already in promiscious mode
 156  * no need to add multicast address.
 157  */
 158 int packet_sock_init(void)
 159 {
 160         int s;
 161         struct sock_fprog prog = {
 162                 .len = sizeof(stp_filter) / sizeof(stp_filter[0]),
 163                 .filter = stp_filter,
 164         };
 165
 166         s = socket(PF_PACKET, SOCK_RAW, htons(ETH_P_802_2));
 167         if (s < 0) {
 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
 175         else if (fcntl(s, F_SETFL, O_NONBLOCK) < 0)
 176                 ERROR("fcntl set nonblock failed: %m");
 177
 178         else {
 179                 packet_event.fd = s;
 180                 packet_event.handler = packet_rcv;
 181
 182                 if (add_epoll(&packet_event) == 0)
 183                         return 0;
 184         }
 185
 186         close(s);
 187         return -1;
 188 }