Revised packet SOCK_RAW code

Still not complete.

7 files changed, 171 insertions, 206 deletions

diff --git a/Makefile b/Makefile
index 994e9df..eb8df0c 100644
--- a/Makefile
+++ b/Makefile
@@ -1,5 +1,5 @@
 
-DSOURCES = brmon.c brstate.c libnetlink.c epoll_loop.c bridge_track.c \
+DSOURCES = 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/bridge_ctl.h b/bridge_ctl.h
index bcb9754..2838330 100644
--- a/bridge_ctl.h
+++ b/bridge_ctl.h
@@ -33,11 +33,9 @@ void bridge_get_configuration(void);
 
 int bridge_set_state(int ifindex, int state);
 
-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(struct ifdata *, const unsigned char *data, int len);
+void bridge_bpdu_rcv(int ifindex, const unsigned char *data, int len);
 
 void bridge_one_second(void);
 
diff --git a/bridge_track.c b/bridge_track.c
index fbb8288..18e59b7 100644
--- a/bridge_track.c
+++ b/bridge_track.c
@@ -340,11 +340,6 @@ struct ifdata *create_if(int if_index, struct ifdata *br)
 		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 */
 
@@ -379,7 +374,6 @@ 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 */
@@ -548,12 +542,17 @@ int bridge_notify(int br_index, int if_index, int newlink, int up)
 	return 0;
 }
 
-void bridge_bpdu_rcv(struct ifdata *ifc, const unsigned char *data, int len)
+void bridge_bpdu_rcv(int if_index, const unsigned char *data, int len)
 {
-	TST(ifc && !ifc->is_bridge,);
-	TST(ifc->up && ifc->master->stp_up,);
+	struct ifdata *ifc = find_if(if_index);
 	BPDU_T bpdu;
 
+	LOG("ifindex %d, len %d", if_index, len);
+	if (!ifc)
+		return;
+
+	TST(ifc->up && ifc->master->stp_up,);
+
 	memset(&bpdu.eth, 0, sizeof(bpdu.eth));
 	if (len > sizeof(bpdu) - sizeof(bpdu.eth))
 		len = sizeof(bpdu) - sizeof(bpdu.eth);
@@ -740,7 +739,9 @@ STP_OUT_tx_bpdu(IN int port_index, IN int vlan_id,
 	TST(vlan_id == 0, 0);
 	//  dump_hex(bpdu + sizeof(MAC_HEADER_T) + sizeof(ETH_HEADER_T),
 	//           bpdu_len - (sizeof(MAC_HEADER_T) + sizeof(ETH_HEADER_T)));
-	bridge_send_bpdu(port->if_index, bpdu + sizeof(MAC_HEADER_T) + sizeof(ETH_HEADER_T), bpdu_len);	// The length we get excludes headers!
+	packet_send(port->if_index, 
+		    bpdu + sizeof(MAC_HEADER_T) + sizeof(ETH_HEADER_T),
+		    bpdu_len);	// The length we get excludes headers!
 	return 0;
 }
 
diff --git a/brmon.c b/brmon.c
index 644532f..4afe5e1 100644
--- a/brmon.c
+++ b/brmon.c
@@ -30,6 +30,7 @@
 
 static const char SNAPSHOT[] = "v0.1";
 
+
 /* RFC 2863 operational status */
 enum {
 	IF_OPER_UNKNOWN,
@@ -55,50 +56,53 @@ static const char *port_states[] = {
 	[BR_STATE_BLOCKING] = "blocking",
 };
 
+
 static int dump_msg(const struct sockaddr_nl *who, struct nlmsghdr *n,
 		    void *arg)
 {
 	FILE *fp = arg;
 	struct ifinfomsg *ifi = NLMSG_DATA(n);
-	struct rtattr *tb[IFLA_MAX + 1];
+	struct rtattr * tb[IFLA_MAX+1];
 	int len = n->nlmsg_len;
 	char b1[IFNAMSIZ];
 	int af_family = ifi->ifi_family;
 
-	if (n->nlmsg_type == NLMSG_DONE)
-		return 0;
-
+        if (n->nlmsg_type == NLMSG_DONE)
+          return 0;
+        
 	len -= NLMSG_LENGTH(sizeof(*ifi));
 	if (len < 0) {
-		return -1;
-	}
+          return -1;
+        }
+        
 #if 0
 
 	if (filter.ifindex && ifi->ifi_index != filter.ifindex)
 		return 0;
 
-	if (filter.up && !(ifi->ifi_flags & IFF_UP))
+	if (filter.up && !(ifi->ifi_flags&IFF_UP))
 		return 0;
 #endif
-	if (ifi->ifi_family != AF_BRIDGE && ifi->ifi_family != AF_UNSPEC)
-		return 0;
+        if (ifi->ifi_family != AF_BRIDGE && ifi->ifi_family != AF_UNSPEC)
+          return 0;
 
-	if (n->nlmsg_type != RTM_NEWLINK && n->nlmsg_type != RTM_DELLINK)
-		return 0;
+        if (n->nlmsg_type != RTM_NEWLINK &&
+            n->nlmsg_type != RTM_DELLINK)
+          return 0;
 
 	parse_rtattr(tb, IFLA_MAX, IFLA_RTA(ifi), len);
 
-	/* Check if we got this from bonding */
-	if (tb[IFLA_MASTER] && af_family != AF_BRIDGE)
-		return 0;
+        /* Check if we got this from bonding */
+        if (tb[IFLA_MASTER] && af_family != AF_BRIDGE)
+          return 0;
 
-	/* Check for BPDU */
-	if (tb[IFLA_PRIORITY] && af_family == AF_BRIDGE) {
-		bridge_bpdu_rcv(ifi->ifi_index,
-				RTA_DATA(tb[IFLA_PRIORITY]),
-				RTA_PAYLOAD(tb[IFLA_PRIORITY]));
-		return 0;
-	}
+        /* Check for BPDU */
+        if (tb[IFLA_PRIORITY] && af_family == AF_BRIDGE) {
+          bridge_bpdu_rcv(ifi->ifi_index,
+                   RTA_DATA(tb[IFLA_PRIORITY]),
+                   RTA_PAYLOAD(tb[IFLA_PRIORITY]));
+          return 0;
+        }
 
 	if (tb[IFLA_IFNAME] == NULL) {
 		fprintf(stderr, "BUG: nil ifname\n");
@@ -109,68 +113,61 @@ static int dump_msg(const struct sockaddr_nl *who, struct nlmsghdr *n,
 		fprintf(fp, "Deleted ");
 
 	fprintf(fp, "%d: %s ", ifi->ifi_index,
-		tb[IFLA_IFNAME] ? (char *)RTA_DATA(tb[IFLA_IFNAME]) : "<nil>");
+		tb[IFLA_IFNAME] ? (char*)RTA_DATA(tb[IFLA_IFNAME]) : "<nil>");
+
 
 	if (tb[IFLA_OPERSTATE]) {
-		int state = *(int *)RTA_DATA(tb[IFLA_OPERSTATE]);
+		int state = *(int*)RTA_DATA(tb[IFLA_OPERSTATE]);
 		switch (state) {
-		case IF_OPER_UNKNOWN:
-			fprintf(fp, "Unknown ");
-			break;
+		case IF_OPER_UNKNOWN: 
+			fprintf(fp, "Unknown "); break;
 		case IF_OPER_NOTPRESENT:
-			fprintf(fp, "Not Present ");
-			break;
+			fprintf(fp, "Not Present "); break;
 		case IF_OPER_DOWN:
-			fprintf(fp, "Down ");
-			break;
+			fprintf(fp, "Down "); break;
 		case IF_OPER_LOWERLAYERDOWN:
-			fprintf(fp, "Lowerlayerdown ");
-			break;
+			fprintf(fp, "Lowerlayerdown "); break;
 		case IF_OPER_TESTING:
-			fprintf(fp, "Testing ");
-			break;
+			fprintf(fp, "Testing "); break;
 		case IF_OPER_DORMANT:
-			fprintf(fp, "Dormant ");
-			break;
+			fprintf(fp, "Dormant "); break;
 		case IF_OPER_UP:
-			fprintf(fp, "Up ");
-			break;
+			fprintf(fp, "Up "); break;
 		default:
 			fprintf(fp, "State(%d) ", state);
 		}
 	}
-
+	
 	if (tb[IFLA_MTU])
-		fprintf(fp, "mtu %u ", *(int *)RTA_DATA(tb[IFLA_MTU]));
+		fprintf(fp, "mtu %u ", *(int*)RTA_DATA(tb[IFLA_MTU]));
 
 	if (tb[IFLA_MASTER]) {
-		fprintf(fp, "master %s ",
-			if_indextoname(*(int *)RTA_DATA(tb[IFLA_MASTER]), b1));
+		fprintf(fp, "master %s ", 
+			if_indextoname(*(int*)RTA_DATA(tb[IFLA_MASTER]), b1));
 	}
 
 	if (tb[IFLA_PROTINFO]) {
-		uint8_t state = *(uint8_t *) RTA_DATA(tb[IFLA_PROTINFO]);
+		uint8_t state = *(uint8_t *)RTA_DATA(tb[IFLA_PROTINFO]);
 		if (state <= BR_STATE_BLOCKING)
 			fprintf(fp, "state %s", port_states[state]);
 		else
 			fprintf(fp, "state (%d)", state);
 	}
 
+
 	fprintf(fp, "\n");
 	fflush(fp);
-	{
-		int newlink = (n->nlmsg_type == RTM_NEWLINK);
-		int up = 0;
-		if (newlink && tb[IFLA_OPERSTATE]) {
-			int state = *(int *)RTA_DATA(tb[IFLA_OPERSTATE]);
-			up = (state == IF_OPER_UP)
-			    || (state == IF_OPER_UNKNOWN);
-		}
-
-		bridge_notify((tb[IFLA_MASTER] ? *(int *)
-			       RTA_DATA(tb[IFLA_MASTER]) : -1), ifi->ifi_index,
-			      newlink, up);
-	}
+        {
+          int newlink = (n->nlmsg_type == RTM_NEWLINK);
+          int up = 0;
+          if (newlink && tb[IFLA_OPERSTATE]) {
+            int state = *(int*)RTA_DATA(tb[IFLA_OPERSTATE]);
+            up = (state == IF_OPER_UP) || (state == IF_OPER_UNKNOWN);
+          }
+
+          bridge_notify((tb[IFLA_MASTER]?*(int*)RTA_DATA(tb[IFLA_MASTER]):-1), 
+                        ifi->ifi_index, newlink, up);
+        }
 	return 0;
 }
 
@@ -189,7 +186,8 @@ static int matches(const char *cmd, const char *pattern)
 	return memcmp(pattern, cmd, len);
 }
 
-int main(int argc, char **argv)
+int
+main(int argc, char **argv)
 {
 	struct rtnl_handle rth;
 	unsigned groups = ~RTMGRP_TC;
@@ -201,23 +199,20 @@ int main(int argc, char **argv)
 			printf("brmon %s\n", SNAPSHOT);
 			exit(0);
 		} else if (matches(argv[1], "link") == 0) {
-			llink = 1;
+			llink=1;
 			groups = 0;
 		} else if (matches(argv[1], "bridge") == 0) {
-			laddr = 1;
+			laddr=1;
 			groups = 0;
 		} else if (strcmp(argv[1], "all") == 0) {
 			groups = ~RTMGRP_TC;
 		} else if (matches(argv[1], "help") == 0) {
 			usage();
 		} else {
-			fprintf(stderr,
-				"Argument \"%s\" is unknown, try \"rtmon help\".\n",
-				argv[1]);
+			fprintf(stderr, "Argument \"%s\" is unknown, try \"rtmon help\".\n", argv[1]);
 			exit(-1);
 		}
-		argc--;
-		argv++;
+		argc--;	argv++;
 	}
 
 	if (llink)
@@ -253,54 +248,53 @@ struct rtnl_handle rth_state;
 
 void br_ev_handler(uint32_t events, struct epoll_event_handler *h)
 {
-	if (rtnl_listen(&rth, dump_msg, stdout) < 0) {
-		fprintf(stderr, "Error on bridge monitoring socket\n");
-		exit(-1);
-	}
+  if (rtnl_listen(&rth, dump_msg, stdout) < 0) {
+    fprintf(stderr, "Error on bridge monitoring socket\n");
+    exit(-1);
+  }
 }
 
 int init_bridge_ops(void)
 {
-	if (rtnl_open(&rth, ~RTMGRP_TC) < 0) {
-		fprintf(stderr, "Couldn't open rtnl socket for monitoring\n");
-		return -1;
-	}
-
-	if (rtnl_open(&rth_state, 0) < 0) {
-		fprintf(stderr,
-			"Couldn't open rtnl socket for setting state\n");
-		return -1;
-	}
-
-	if (rtnl_wilddump_request(&rth, PF_BRIDGE, RTM_GETLINK) < 0) {
-		fprintf(stderr, "Cannot send dump request: %m\n");
-		return -1;
-	}
-
-	if (rtnl_dump_filter(&rth, dump_msg, stdout, NULL, NULL) < 0) {
-		fprintf(stderr, "Dump terminated\n");
-		return -1;
-	}
-
-	if (fcntl(rth.fd, F_SETFL, O_NONBLOCK) < 0) {
-		fprintf(stderr, "Error setting O_NONBLOCK: %m\n");
-		return -1;
-	}
-
-	br_handler.fd = rth.fd;
-	br_handler.arg = NULL;
-	br_handler.handler = br_ev_handler;
-
-	if (add_epoll(&br_handler) < 0)
-		return -1;
-
-	return 0;
+  if (rtnl_open(&rth, ~RTMGRP_TC) < 0) {
+    fprintf(stderr, "Couldn't open rtnl socket for monitoring\n");
+    return -1;
+  }
+  
+  if (rtnl_open(&rth_state, 0) < 0) {
+    fprintf(stderr, "Couldn't open rtnl socket for setting state\n");
+    return -1;
+  }
+
+  if (rtnl_wilddump_request(&rth, PF_BRIDGE, RTM_GETLINK) < 0) {
+    fprintf(stderr, "Cannot send dump request: %m\n");
+    return -1;
+  }
+  
+  if (rtnl_dump_filter(&rth, dump_msg, stdout, NULL, NULL) < 0) {
+    fprintf(stderr, "Dump terminated\n");
+    return -1;
+  }
+
+  if (fcntl(rth.fd, F_SETFL, O_NONBLOCK) < 0) {
+    fprintf(stderr, "Error setting O_NONBLOCK: %m\n");
+    return -1;
+  }
+
+  br_handler.fd = rth.fd;
+  br_handler.arg = NULL;
+  br_handler.handler = br_ev_handler;
+  
+  if (add_epoll(&br_handler) < 0)
+    return -1;
+  
+  return 0;
 }
 
 /* Send message. Response is through bridge_notify */
 void bridge_get_configuration(void)
 {
-	if (rtnl_wilddump_request(&rth, PF_BRIDGE, RTM_GETLINK) < 0) {
-		fprintf(stderr, "Cannot send dump request: %m\n");
-	}
+  if (rtnl_wilddump_request(&rth, PF_BRIDGE, RTM_GETLINK) < 0) {
+    fprintf(stderr, "Cannot send dump request: %m\n");
+  }
 }
diff --git a/brstate.c b/brstate.c
index 171c59d..1fe792e 100644
--- a/brstate.c
+++ b/brstate.c
@@ -47,28 +47,6 @@ static int br_set_state(struct rtnl_handle *rth, unsigned ifindex, __u8 state)
 	return rtnl_talk(rth, &req.n, 0, 0, NULL, NULL, NULL);
 }
 
-static int br_send_bpdu(struct rtnl_handle *rth, unsigned ifindex,
-			const unsigned char *data, int len)
-{
-	struct {
-		struct nlmsghdr n;
-		struct ifinfomsg ifi;
-		char buf[256];
-	} req;
-
-	memset(&req, 0, sizeof(req));
-
-	req.n.nlmsg_len = NLMSG_LENGTH(sizeof(struct ifinfomsg));
-	req.n.nlmsg_flags = NLM_F_REQUEST | NLM_F_REPLACE;
-	req.n.nlmsg_type = RTM_SETLINK;
-	req.ifi.ifi_family = AF_BRIDGE;
-	req.ifi.ifi_index = ifindex;
-
-	addattr_l(&req.n, sizeof(req.buf), IFLA_PRIORITY, data, len);
-
-	return rtnl_talk(rth, &req.n, 0, 0, NULL, NULL, NULL);
-}
-
 #include "bridge_ctl.h"
 
 extern struct rtnl_handle rth_state;
@@ -84,13 +62,3 @@ int bridge_set_state(int ifindex, int brstate)
 	}
 	return 0;
 }
-
-int bridge_send_bpdu(int ifindex, const unsigned char *data, int len)
-{
-	int err = br_send_bpdu(&rth_state, ifindex, data, len);
-	if (err < 0) {
-		fprintf(stderr, "Couldn't send bpdu, ifindex %d\n", ifindex);
-		return -1;
-	}
-	return 0;
-}
diff --git a/packet.c b/packet.c
index 286fb0a..aadf58c 100644
--- a/packet.c
+++ b/packet.c
@@ -46,55 +46,76 @@
 
 #include "log.h"
 
-#define DEBUG 1
+static struct epoll_event_handler packet_event;
+
+#ifdef STP_DBG
+static void dump_packet(const unsigned char *buf, int cc)
+{
+	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
 
 /*
  * 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)
+void packet_send(int ifindex, const unsigned char *data, int len)
 {
 	int l;
+	struct sockaddr_ll sl = {
+		.sll_family = AF_PACKET,
+		.sll_protocol = htons(ETH_P_802_2),
+		.sll_ifindex = ifindex,
+		.sll_halen = ETH_ALEN,
+	};
 
-	if (fcntl(h->fd, F_SETFL, 0) < 0)
-		ERROR("Error unsetting O_NONBLOCK: %m");
+	memcpy(sl.sll_addr, data, ETH_ALEN);
 
-	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);
+#ifdef STP_DBG
+	printf("Send to %02x:%02x:%02x:%02x:%02x:%02x\n",
+	       sl.sll_addr[0], sl.sll_addr[1], sl.sll_addr[2],
+	       sl.sll_addr[3], sl.sll_addr[4], sl.sll_addr[5]);
+	dump_packet(data, len);
+#endif
+	l = sendto(packet_event.fd, data, len, 0, 
+		   (struct sockaddr *) &sl, sizeof(sl));
 
-	if (fcntl(h->fd, F_SETFL, O_NONBLOCK) < 0)
-		ERROR("Error setting O_NONBLOCK: %m");
+	if (l < 0) {
+		if (errno != EWOULDBLOCK)
+			ERROR("send failed: %m");
+	} else if (l != len)
+		ERROR("short write in sendto: %d instead of %d", l, len);
 }
 
-void packet_rcv_handler(uint32_t events, struct epoll_event_handler *h)
+static void packet_rcv(uint32_t events, struct epoll_event_handler *h)
 {
 	int cc;
 	unsigned char buf[2048];
+	struct sockaddr_ll sl;
+	socklen_t salen = sizeof sl;
 
-	cc = recv(h->fd, &buf, sizeof(buf), 0);
+	cc = recvfrom(h->fd, &buf, sizeof(buf), 0,
+		      (struct sockaddr *) &sl, &salen);
 	if (cc <= 0) {
-		ERROR("read failed: %m");
+		ERROR("recvfrom 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]);
+#ifdef STP_DBG
+	printf("Receive Src %02x:%02x:%02x:%02x:%02x:%02x\n",
+	       buf[6], buf[7], buf[8], buf[9], buf[10], buf[11]);
 
-	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);
+	dump_packet(buf, cc);
 #endif
 
-	bridge_bpdu_rcv(h->arg, buf, cc);
+	bridge_bpdu_rcv(sl.sll_ifindex, buf, cc);
 }
 
 /* Berkeley Packet filter code to filter out spanning tree packets.
@@ -110,52 +131,40 @@ static struct sock_filter stp_filter[] = {
 };
 
 /*
- * Open up a raw packet socket to catch all 802.2 packets on a device
+ * Open up a raw packet socket to catch all 802.2 packets.
  * and install a packet filter to only see STP (SAP 42)
+ *
+ * Since any bridged devices are already in promiscious mode
+ * no need to add multicast address.
  */
-int packet_sock_create(struct epoll_event_handler *h, int if_index,
-		       struct ifdata *arg)
+int packet_sock_init(void)
 {
 	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));
+	s = socket(PF_PACKET, SOCK_RAW, 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) 
+	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;
+		packet_event.fd = s;
+		packet_event.handler = packet_rcv;
 
-		if (add_epoll(h) == 0)
+		if (add_epoll(&packet_event) == 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/packet.h b/packet.h
index 3a03cc4..c1a7879 100644
--- a/packet.h
+++ b/packet.h
@@ -27,12 +27,7 @@
 
 #include "epoll_loop.h"
 
-struct ifdata;
+void packet_send(int ifindex, const unsigned char *data, int len);
+int packet_sock_init(void);
 
-void packet_send(struct epoll_event_handler *h, 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