1 files changed, 162 insertions, 17 deletions

diff --git a/accel-pppd/ctrl/ipoe/ipoe_netlink.c b/accel-pppd/ctrl/ipoe/ipoe_netlink.c
index dde83a52..1930214a 100644
--- a/accel-pppd/ctrl/ipoe/ipoe_netlink.c
+++ b/accel-pppd/ctrl/ipoe/ipoe_netlink.c
@@ -26,7 +26,7 @@
 #define PKT_ATTR_MAX 256
 
 static struct rtnl_handle rth;
-static struct triton_md_handler_t up_hnd;
+static struct triton_md_handler_t mc_hnd;
 static int ipoe_genl_id;
 
 void ipoe_nl_delete_nets(void)
@@ -378,6 +378,106 @@ void ipoe_nl_delete(int ifindex)
 	rtnl_close(&rth);
 }
 
+int ipoe_nl_add_vlan_mon(int ifindex, long *mask, int len)
+{
+	struct nlmsghdr *nlh;
+	struct genlmsghdr *ghdr;
+	struct {
+		struct nlmsghdr n;
+		char buf[1024];
+	} req;
+	
+	if (rth.fd == -1)
+		return -1;
+
+	nlh = &req.n;
+	nlh->nlmsg_len = NLMSG_LENGTH(GENL_HDRLEN);
+	nlh->nlmsg_flags = NLM_F_REQUEST | NLM_F_ACK;
+	nlh->nlmsg_type = ipoe_genl_id;
+
+	ghdr = NLMSG_DATA(&req.n);
+	ghdr->cmd = IPOE_CMD_ADD_VLAN_MON;
+
+	addattr32(nlh, 1024, IPOE_ATTR_IFINDEX, ifindex);
+	addattr_l(nlh, 1024, IPOE_ATTR_VLAN_MASK, mask, len);
+
+	if (rtnl_talk(&rth, nlh, 0, 0, nlh, NULL, NULL, 0) < 0 ) {
+		log_error("ipoe: nl_delete: error talking to kernel\n");
+		return -1;
+	}
+
+	return 0;
+}
+
+int ipoe_nl_add_vlan_mon_vid(int ifindex, int vid)
+{
+	struct nlmsghdr *nlh;
+	struct genlmsghdr *ghdr;
+	struct {
+		struct nlmsghdr n;
+		char buf[1024];
+	} req;
+	int r = 0;
+	
+	if (rtnl_open_byproto(&rth, 0, NETLINK_GENERIC)) {
+		log_error("ipoe: cannot open generic netlink socket\n");
+		return -1;
+	}
+	
+	nlh = &req.n;
+	nlh->nlmsg_len = NLMSG_LENGTH(GENL_HDRLEN);
+	nlh->nlmsg_flags = NLM_F_REQUEST | NLM_F_ACK;
+	nlh->nlmsg_type = ipoe_genl_id;
+
+	ghdr = NLMSG_DATA(&req.n);
+	ghdr->cmd = IPOE_CMD_ADD_VLAN_MON_VID;
+
+	addattr32(nlh, 1024, IPOE_ATTR_IFINDEX, ifindex);
+	addattr32(nlh, 1024, IPOE_ATTR_ADDR, vid);
+
+	if (rtnl_talk(&rth, nlh, 0, 0, nlh, NULL, NULL, 0) < 0 ) {
+		log_error("ipoe: nl_delete: error talking to kernel\n");
+		r = -1;
+	}
+	
+	rtnl_close(&rth);
+
+	return r;
+}
+
+
+int ipoe_nl_del_vlan_mon(int ifindex)
+{
+	struct rtnl_handle rth;
+	struct nlmsghdr *nlh;
+	struct genlmsghdr *ghdr;
+	struct {
+		struct nlmsghdr n;
+		char buf[1024];
+	} req;
+	
+	if (rth.fd == -1)
+		return -1;
+
+	nlh = &req.n;
+	nlh->nlmsg_len = NLMSG_LENGTH(GENL_HDRLEN);
+	nlh->nlmsg_flags = NLM_F_REQUEST | NLM_F_ACK;
+	nlh->nlmsg_type = ipoe_genl_id;
+
+	ghdr = NLMSG_DATA(&req.n);
+	ghdr->cmd = IPOE_CMD_DEL_VLAN_MON;
+
+	addattr32(nlh, 1024, IPOE_ATTR_IFINDEX, ifindex);
+
+	if (rtnl_talk(&rth, nlh, 0, 0, nlh, NULL, NULL, 0) < 0 ) {
+		log_error("ipoe: nl_delete: error talking to kernel\n");
+		return -1;
+	}
+
+	return 0;
+}
+
+
 static void ipoe_up_handler(const struct sockaddr_nl *addr, struct nlmsghdr *h)
 {
 	struct rtattr *tb[PKT_ATTR_MAX + 1];
@@ -390,9 +490,6 @@ static void ipoe_up_handler(const struct sockaddr_nl *addr, struct nlmsghdr *h)
 	struct iphdr *iph;
 	struct ethhdr *eth;
 
-	if (ghdr->cmd != IPOE_REP_PKT)
-		return;
-
 	len -= NLMSG_LENGTH(GENL_HDRLEN);
 
 	if (len < 0) {
@@ -420,10 +517,48 @@ static void ipoe_up_handler(const struct sockaddr_nl *addr, struct nlmsghdr *h)
 	}
 }
 
-static int ipoe_up_read(struct triton_md_handler_t *h)
+static void ipoe_vlan_mon_handler(const struct sockaddr_nl *addr, struct nlmsghdr *h)
+{
+	struct rtattr *tb[PKT_ATTR_MAX + 1];
+	struct rtattr *tb2[IPOE_ATTR_MAX + 1];
+	struct genlmsghdr *ghdr = NLMSG_DATA(h);
+	int len = h->nlmsg_len;
+	struct rtattr *attrs;
+	int i;
+	int ifindex, vid;
+
+	len -= NLMSG_LENGTH(GENL_HDRLEN);
+
+	if (len < 0) {
+		log_warn("ipoe: wrong controller message length %d\n", len);
+		return;
+	}
+
+	attrs = (struct rtattr *)((char *)ghdr + GENL_HDRLEN);
+	parse_rtattr(tb, PKT_ATTR_MAX, attrs, len);
+
+	for (i = 1; i < PKT_ATTR_MAX; i++) {
+		if (!tb[i])
+			break;
+
+		parse_rtattr_nested(tb2, IPOE_ATTR_MAX, tb[i]);
+		
+		if (!tb2[IPOE_ATTR_IFINDEX] || !tb2[IPOE_ATTR_ADDR])
+			continue;
+
+		ifindex = *(uint32_t *)(RTA_DATA(tb2[IPOE_ATTR_IFINDEX]));
+		vid = *(uint32_t *)(RTA_DATA(tb2[IPOE_ATTR_ADDR]));
+
+		ipoe_vlan_notify(ifindex, vid);
+	}
+}
+
+
+static int ipoe_mc_read(struct triton_md_handler_t *h)
 {
 	int status;
 	struct nlmsghdr *hdr;
+	struct genlmsghdr *ghdr;
 	struct sockaddr_nl nladdr;
 	struct iovec iov;
 	struct msghdr msg = {
@@ -452,14 +587,17 @@ static int ipoe_up_read(struct triton_md_handler_t *h)
 				continue;
 			return 0;
 		}
+		
 		if (status == 0) {
 			log_error("ipoe: EOF on netlink\n");
 			return 0;
 		}
+
 		if (msg.msg_namelen != sizeof(nladdr)) {
 			log_error("ipoe: netlink sender address length == %d\n", msg.msg_namelen);
 			return 0;
 		}
+
 		for (hdr = (struct nlmsghdr*)buf; status >= sizeof(*hdr); ) {
 			int len = hdr->nlmsg_len;
 			int l = len - sizeof(*h);
@@ -473,15 +611,22 @@ static int ipoe_up_read(struct triton_md_handler_t *h)
 				continue;
 			}
 
-			ipoe_up_handler(&nladdr, hdr);
+			ghdr = NLMSG_DATA(hdr);
+
+			if (ghdr->cmd == IPOE_REP_PKT)
+				ipoe_up_handler(&nladdr, hdr);
+			else if (ghdr->cmd == IPOE_VLAN_NOTIFY)
+				ipoe_vlan_mon_handler(&nladdr, hdr);
 
 			status -= NLMSG_ALIGN(len);
 			hdr = (struct nlmsghdr*)((char*)hdr + NLMSG_ALIGN(len));
 		}
+
 		if (msg.msg_flags & MSG_TRUNC) {
 			log_warn("ipoe: netlink message truncated\n");
 			continue;
 		}
+
 		if (status) {
 			log_error("ipoe: netlink remnant of size %d\n", status);
 			return 0;
@@ -491,18 +636,18 @@ static int ipoe_up_read(struct triton_md_handler_t *h)
 	return 0;
 }
 
-static void ipoe_up_close(struct triton_context_t *ctx)
+static void ipoe_mc_close(struct triton_context_t *ctx)
 {
-	triton_md_unregister_handler(&up_hnd);
+	triton_md_unregister_handler(&mc_hnd);
 	triton_context_unregister(ctx);
 }
 
-static struct triton_context_t up_ctx = {
-	.close = ipoe_up_close,
+static struct triton_context_t mc_ctx = {
+	.close = ipoe_mc_close,
 };
 
-static struct triton_md_handler_t up_hnd = {
-	.read = ipoe_up_read,
+static struct triton_md_handler_t mc_hnd = {
+	.read = ipoe_mc_read,
 };
 
 static void init(void)
@@ -523,11 +668,11 @@ static void init(void)
 	fcntl(rth.fd, F_SETFL, O_NONBLOCK);
 	fcntl(rth.fd, F_SETFD, fcntl(rth.fd, F_GETFD) | FD_CLOEXEC);
 
-	triton_context_register(&up_ctx, NULL);
-	up_hnd.fd = rth.fd;
-	triton_md_register_handler(&up_ctx, &up_hnd);
-	triton_md_enable_handler(&up_hnd, MD_MODE_READ);
-	triton_context_wakeup(&up_ctx);
+	triton_context_register(&mc_ctx, NULL);
+	mc_hnd.fd = rth.fd;
+	triton_md_register_handler(&mc_ctx, &mc_hnd);
+	triton_md_enable_handler(&mc_hnd, MD_MODE_READ);
+	triton_context_wakeup(&mc_ctx);
 }
 
 DEFINE_INIT(19, init);