Convert to using AF_PACKET for send and receive

Use AF_PACKET to send and receive spanning tree bpdu's
UNTESTED at this point.

Signed-off-by: Stephen Hemminger <shemminger@linux-foundation.org>

6 files changed, 195 insertions, 169 deletions

diff --git a/Makefile b/Makefile
index cace491..994e9df 100644
--- a/Makefile
+++ b/Makefile
@@ -1,5 +1,6 @@
 
-DSOURCES = brmon.c brstate.c libnetlink.c epoll_loop.c bridge_track.c ctl_socket.c netif_utils.c main.c
+DSOURCES = brmon.c brstate.c libnetlink.c epoll_loop.c bridge_track.c \
+	   packet.c ctl_socket.c netif_utils.c main.c
 
 DOBJECTS = $(DSOURCES:.c=.o)
 
diff --git a/bpdu_sock.c b/bpdu_sock.c
deleted file mode 100644
index 6ebfbb3..0000000
--- a/bpdu_sock.c
+++ /dev/null
@@ -1,151 +0,0 @@
-/*****************************************************************************
-  Copyright (c) 2006 EMC Corporation.
-
-  This program is free software; you can redistribute it and/or modify it 
-  under the terms of the GNU General Public License as published by the Free 
-  Software Foundation; either version 2 of the License, or (at your option) 
-  any later version.
-  
-  This program is distributed in the hope that it will be useful, but WITHOUT 
-  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
-  FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for 
-  more details.
-  
-  You should have received a copy of the GNU General Public License along with
-  this program; if not, write to the Free Software Foundation, Inc., 59 
-  Temple Place - Suite 330, Boston, MA  02111-1307, USA.
-  
-  The full GNU General Public License is included in this distribution in the
-  file called LICENSE.
-
-  Authors: Srinivas Aji <Aji_Srinivas@emc.com>
-
-******************************************************************************/
-
-#include "bpdu_sock.h"
-#include "epoll_loop.h"
-#include "netif_utils.h"
-
-#include <stdio.h>
-#include <stdlib.h>
-#include <errno.h>
-#include <string.h>
-#include <unistd.h>
-#include <stdint.h>
-#include <sys/socket.h>
-#include <sys/ioctl.h>
-#include <fcntl.h>
-
-#include <linux/if.h>
-#include <linux/if_arp.h>
-#include <linux/llc.h>
-
-#include "log.h"
-
-#ifndef AF_LLC
-#define AF_LLC 26
-#endif
-
-static const uint8_t stp_mc[ETH_ALEN] = { 0x01, 0x80, 0xc2, 0x00, 0x00, 0x00 };
-
-void bpdu_send(struct epoll_event_handler *h, unsigned char *data, int len)
-{
-	struct sockaddr_llc to;
-	memset(&to, 0, sizeof(to));
-	to.sllc_family = AF_LLC;
-	to.sllc_arphrd = ARPHRD_ETHER;
-	to.sllc_sap = LLC_SAP_BSPAN;
-	memcpy(to.sllc_mac, stp_mc, ETH_ALEN);
-
-	if (fcntl(h->fd, F_SETFL, 0) < 0)
-		ERROR("Error unsetting O_NONBLOCK: %m");
-
-	int l = sendto(h->fd, data, len, 0, (struct sockaddr *)&to, sizeof(to));
-	if (l < 0)
-		ERROR("sendto failed: %m");
-	else if (l != len)
-		ERROR("short write in sendto: %d instead of %d", l, len);
-
-	if (fcntl(h->fd, F_SETFL, O_NONBLOCK) < 0)
-		ERROR("Error setting O_NONBLOCK: %m");
-}
-
-void bpdu_rcv_handler(uint32_t events, struct epoll_event_handler *h)
-{
-	struct sockaddr_llc from;
-	socklen_t fromlen = sizeof(from);
-	int cc;
-	unsigned char buf[2048];
-
-	cc = recvfrom(h->fd, &buf, sizeof(buf), 0,
-		      (struct sockaddr *)&from, &fromlen);
-	if (cc <= 0) {
-		ERROR("recvfrom failed: %m");
-		return;
-	}
-#if 0
-	printf("Src %02x:%02x:%02x:%02x:%02x:%02x\n",
-	       from.sllc_mac[0], from.sllc_mac[1],
-	       from.sllc_mac[2], from.sllc_mac[3],
-	       from.sllc_mac[4], from.sllc_mac[5]);
-	int i, j;
-	for (i = 0; i < cc; i += 16) {
-		for (j = 0; j < 16 && i + j < cc; j++)
-			printf(" %02x", buf[i + j]);
-		printf("\n");
-	}
-	printf("\n");
-	fflush(stdout);
-#endif
-
-	bpdu_rcv(h->arg, buf, cc);
-}
-
-/* We added name as an arg here because we can't do if_indextoname here,
-   That needs <net/if.h> which conflicts with <linux/if.h> */
-/* Needs fixing. Socket should be closed in case of errors */
-int bpdu_sock_create(struct epoll_event_handler *h,
-		     int if_index, char *name, struct ifdata *arg)
-{
-	struct sockaddr_llc llc_addr;
-	memset(&llc_addr, 0, sizeof(llc_addr));
-	llc_addr.sllc_family = AF_LLC;
-	llc_addr.sllc_arphrd = ARPHRD_ETHER;
-	llc_addr.sllc_sap = LLC_SAP_BSPAN;
-
-	int s;
-
-	TSTM((s = socket(AF_LLC, SOCK_DGRAM, 0)) >= 0, -1, "%m");
-
-	TST(get_hwaddr(name, llc_addr.sllc_mac) == 0, -1);
-
-	TSTM(bind(s, (struct sockaddr *)&llc_addr, sizeof(llc_addr)) == 0, -1,
-	     "Can't bind to LLC SAP %#x: %m", llc_addr.sllc_sap);
-	{
-		struct ifreq ifr;
-		memset(&ifr, 0, sizeof(ifr));
-		strncpy(ifr.ifr_name, name, IFNAMSIZ);
-		ifr.ifr_hwaddr.sa_family = AF_UNSPEC;
-		memcpy(ifr.ifr_hwaddr.sa_data, stp_mc, ETH_ALEN);
-
-		TSTM(ioctl(s, SIOCADDMULTI, &ifr) == 0, -1,
-		     "can't set multicast address for %s: %m", ifr.ifr_name);
-	}
-
-	TSTM(fcntl(s, F_SETFL, O_NONBLOCK) == 0, -1, "%m");
-
-	h->fd = s;
-	h->arg = arg;
-	h->handler = bpdu_rcv_handler;
-
-	if (add_epoll(h) < 0)
-		return -1;
-
-	return 0;
-}
-
-void bpdu_sock_delete(struct epoll_event_handler *h)
-{
-	remove_epoll(h);
-	close(h->fd);
-}
diff --git a/bridge_ctl.h b/bridge_ctl.h
index fdb1970..bcb9754 100644
--- a/bridge_ctl.h
+++ b/bridge_ctl.h
@@ -25,6 +25,8 @@
 #ifndef BRIDGE_CTL_H
 #define BRIDGE_CTL_H
 
+struct ifdata;
+
 int init_bridge_ops(void);
 
 void bridge_get_configuration(void);
@@ -35,7 +37,7 @@ int bridge_send_bpdu(int ifindex, const unsigned char *data, int len);
 
 int bridge_notify(int br_index, int if_index, int newlink, int up);
 
-void bridge_bpdu_rcv(int if_index, const unsigned char *data, int len);
+void bridge_bpdu_rcv(struct ifdata *, const unsigned char *data, int len);
 
 void bridge_one_second(void);
 
diff --git a/bridge_track.c b/bridge_track.c
index bf4a2da..fbb8288 100644
--- a/bridge_track.c
+++ b/bridge_track.c
@@ -24,9 +24,11 @@
 
 #include "bridge_ctl.h"
 #include "netif_utils.h"
+#include "packet.h"
 
 #include <unistd.h>
 #include <net/if.h>
+#include <stdlib.h>
 #include <linux/if_bridge.h>
 #include <arpa/inet.h>
 #include <sys/types.h>
@@ -80,6 +82,8 @@ struct ifdata {
 	ADMIN_P2P_T admin_point2point;
 	unsigned char admin_edge;
 	unsigned char admin_non_stp;	/* 1- doesn't participate in STP, 1 - regular */
+
+	struct epoll_event_handler event;
 };
 
 /* Instances */
@@ -162,8 +166,6 @@ void update_port_stp_config(struct ifdata *ifc, UID_STP_PORT_CFG_T * cfg)
 
 int add_port_stp(struct ifdata *ifc)
 {				/* Bridge is ifc->master */
-	char name[IFNAMSIZ];
-	TST(if_indextoname(ifc->if_index, name) != 0, -1);
 	TST((ifc->port_index = get_bridge_portno(ifc->name)) >= 0, -1);
 
 	/* Add port to STP */
@@ -311,11 +313,15 @@ struct ifdata *create_if(int if_index, struct ifdata *br)
 	struct ifdata *p;
 	TST((p = malloc(sizeof(*p))) != NULL, NULL);
 
+	memset(p, 0, sizeof(*p));
+
 	/* Init fields */
 	p->if_index = if_index;
 	p->is_bridge = (br == NULL);
-	memset(p->name, 0, sizeof(p->name));
+
+	/* TODO: purge use of name, due to issue with renameing */
 	if_indextoname(if_index, p->name);
+
 	if (p->is_bridge) {
 		INFO("Add bridge %s", p->name);
 		/* Init slave list */
@@ -333,12 +339,20 @@ struct ifdata *create_if(int if_index, struct ifdata *br)
 		p->speed = 0;
 		p->duplex = 0;
 		p->master = br;
+
+		if (packet_sock_create(&p->event, p->if_index, p)) {
+			free(p);
+			return NULL;
+		}
+
 		update_port_stp_config(p, &default_port_stp_cfg);
 		ADD_TO_LIST(br->port_list, port_next, p);	/* Add to bridge port list */
+
 		if (br->stp_up) {
 			add_port_stp(p);
 		}
 	}
+
 	/* Add to interface list */
 	ADD_TO_LIST(if_head, next, p);
 
@@ -365,11 +379,14 @@ void delete_if(struct ifdata *ifc)
 		REMOVE_FROM_LIST(ifc->master->port_list, port_next, ifc,
 				 "Can't find interface ifindex %d on br %d's port list",
 				 ifc->if_index, ifc->master->if_index);
+		packet_sock_delete(&ifc->event);
 	}
+
 	/* Remove from bridge interface list */
 	REMOVE_FROM_LIST(if_head, next, ifc,
 			 "Can't find interface ifindex %d on iflist",
 			 ifc->if_index);
+	free(ifc);
 }
 
 void set_br_up(struct ifdata *br, int up)
@@ -531,17 +548,17 @@ int bridge_notify(int br_index, int if_index, int newlink, int up)
 	return 0;
 }
 
-void bridge_bpdu_rcv(int if_index, const unsigned char *data, int len)
+void bridge_bpdu_rcv(struct ifdata *ifc, const unsigned char *data, int len)
 {
-	LOG("ifindex %d, len %d", if_index, len);
-	struct ifdata *ifc = find_if(if_index);
 	TST(ifc && !ifc->is_bridge,);
 	TST(ifc->up && ifc->master->stp_up,);
 	BPDU_T bpdu;
+
 	memset(&bpdu.eth, 0, sizeof(bpdu.eth));
 	if (len > sizeof(bpdu) - sizeof(bpdu.eth))
 		len = sizeof(bpdu) - sizeof(bpdu.eth);
 	memcpy(&bpdu.hdr, data, len);
+
 	/* Do some validation */
 	TST(len >= 4,);
 	TST(bpdu.hdr.protocol[0] == 0 && bpdu.hdr.protocol[1] == 0,);
diff --git a/packet.c b/packet.c
new file mode 100644
index 0000000..286fb0a
--- /dev/null
+++ b/packet.c
@@ -0,0 +1,161 @@
+/*****************************************************************************
+  Copyright (c) 2006 EMC Corporation.
+
+  This program is free software; you can redistribute it and/or modify it 
+  under the terms of the GNU General Public License as published by the Free 
+  Software Foundation; either version 2 of the License, or (at your option) 
+  any later version.
+  
+  This program is distributed in the hope that it will be useful, but WITHOUT 
+  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
+  FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for 
+  more details.
+  
+  You should have received a copy of the GNU General Public License along with
+  this program; if not, write to the Free Software Foundation, Inc., 59 
+  Temple Place - Suite 330, Boston, MA  02111-1307, USA.
+  
+  The full GNU General Public License is included in this distribution in the
+  file called LICENSE.
+
+  Authors: Srinivas Aji <Aji_Srinivas@emc.com>
+	   Stephen Hemminger <shemminger@linux-foundation.org>
+
+******************************************************************************/
+
+#include "packet.h"
+#include "epoll_loop.h"
+#include "netif_utils.h"
+#include "bridge_ctl.h"
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <errno.h>
+#include <string.h>
+#include <unistd.h>
+#include <stdint.h>
+#include <sys/socket.h>
+#include <sys/ioctl.h>
+#include <fcntl.h>
+#include <netinet/in.h>
+
+#include <linux/if.h>
+#include <linux/if_ether.h>
+#include <linux/if_packet.h>
+#include <linux/filter.h>
+
+#include "log.h"
+
+#define DEBUG 1
+
+/*
+ * To send/receive Spanning Tree packets we use PF_PACKET because
+ * it allows the filtering we want but gives raw data
+ */
+void packet_send(struct epoll_event_handler *h, unsigned char *data, int len)
+{
+	int l;
+
+	if (fcntl(h->fd, F_SETFL, 0) < 0)
+		ERROR("Error unsetting O_NONBLOCK: %m");
+
+	l = send(h->fd, data, len, 0);
+	if (l < 0)
+		ERROR("send failed: %m");
+	else if (l != len)
+		ERROR("short write in sendto: %d instead of %d", l, len);
+
+	if (fcntl(h->fd, F_SETFL, O_NONBLOCK) < 0)
+		ERROR("Error setting O_NONBLOCK: %m");
+}
+
+void packet_rcv_handler(uint32_t events, struct epoll_event_handler *h)
+{
+	int cc;
+	unsigned char buf[2048];
+
+	cc = recv(h->fd, &buf, sizeof(buf), 0);
+	if (cc <= 0) {
+		ERROR("read failed: %m");
+		return;
+	}
+
+#ifdef DEBUG
+	printf("Src %02x:%02x:%02x:%02x:%02x:%02x\n",
+	       buf[0], buf[1], buf[2], buf[3], buf[4], buf[5]);
+
+	int i, j;
+	for (i = 0; i < cc; i += 16) {
+		for (j = 0; j < 16 && i + j < cc; j++)
+			printf(" %02x", buf[i + j]);
+		printf("\n");
+	}
+	printf("\n");
+	fflush(stdout);
+#endif
+
+	bridge_bpdu_rcv(h->arg, buf, cc);
+}
+
+/* Berkeley Packet filter code to filter out spanning tree packets.
+   from tcpdump -dd stp
+ */
+static struct sock_filter stp_filter[] = {
+	{ 0x28, 0, 0, 0x0000000c },
+	{ 0x25, 3, 0, 0x000005dc },
+	{ 0x30, 0, 0, 0x0000000e },
+	{ 0x15, 0, 1, 0x00000042 },
+	{ 0x6, 0, 0, 0x00000060 },
+	{ 0x6, 0, 0, 0x00000000 },
+};
+
+/*
+ * Open up a raw packet socket to catch all 802.2 packets on a device
+ * and install a packet filter to only see STP (SAP 42)
+ */
+int packet_sock_create(struct epoll_event_handler *h, int if_index,
+		       struct ifdata *arg)
+{
+	int s;
+	struct sockaddr_ll sll = { 
+		.sll_family = AF_PACKET,
+		.sll_ifindex = if_index,
+	};
+	struct sock_fprog prog = {
+		.len = sizeof(stp_filter) / sizeof(stp_filter[0]),
+		.filter = stp_filter,
+	};
+
+	s = socket(PF_PACKET, SOCK_PACKET, htons(ETH_P_802_2));
+	if (s < 0) {
+		ERROR("socket failed: %m");
+		return -1;
+	}
+
+	if (bind(s, (struct sockaddr *) &sll, sizeof(sll)) < 0)
+		ERROR("bind failed: %m");
+	
+	else if (setsockopt(s, SOL_SOCKET, SO_ATTACH_FILTER, &prog, sizeof(prog)) < 0) 
+		ERROR("setsockopt packet filter failed: %m");
+
+	else if (fcntl(s, F_SETFL, O_NONBLOCK) < 0)
+		ERROR("fcntl set nonblock failed: %m");
+
+	else {
+		h->fd = s;
+		h->arg = arg;
+		h->handler = packet_rcv_handler;
+
+		if (add_epoll(h) == 0)
+			return 0;
+	}
+
+	close(s);
+	return -1;
+}
+
+void packet_sock_delete(struct epoll_event_handler *h)
+{
+	remove_epoll(h);
+	close(h->fd);
+}
diff --git a/bpdu_sock.h b/packet.h
index d402070..3a03cc4 100644
--- a/bpdu_sock.h
+++ b/packet.h
@@ -22,21 +22,17 @@
 
 ******************************************************************************/
 
-#ifndef BPDU_SOCK_H
-#define BPDU_SOCK_H
+#ifndef PACKET_SOCK_H
+#define PACKET_SOCK_H
 
 #include "epoll_loop.h"
 
 struct ifdata;
 
-void bpdu_send(struct epoll_event_handler *h, unsigned char *data, int len);
+void packet_send(struct epoll_event_handler *h, unsigned char *data, int len);
 
-int bpdu_sock_create(struct epoll_event_handler *h,
-		     int if_index, char *name, struct ifdata *ifdata);
-
-void bpdu_sock_delete(struct epoll_event_handler *h);
-
-/* Externally provided, we call it */
-void bpdu_rcv(struct ifdata *ifdata, unsigned char *data, int len);
+int packet_sock_create(struct epoll_event_handler *h,
+		       int if_index, struct ifdata *ifdata);
 
+void packet_sock_delete(struct epoll_event_handler *h);
 #endif