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 } 218
This page was automatically generated by LXR 0.3.1. • OpenWrt