ipoe: fixed race during receiving relay reply

6 files changed, 526 insertions, 25 deletions

diff --git a/accel-pppd/ctrl/ipoe/ipoe.c b/accel-pppd/ctrl/ipoe/ipoe.c
index 73854c0c..bdcbb15f 100644
--- a/accel-pppd/ctrl/ipoe/ipoe.c
+++ b/accel-pppd/ctrl/ipoe/ipoe.c
@@ -1358,11 +1358,16 @@ static int parse_dhcpv4_mask(uint32_t mask)
 	return 32 - (i + 1);
 }
 
-static void ipoe_ses_recv_dhcpv4_relay(struct ipoe_session *ses)
+static void ipoe_ses_recv_dhcpv4_relay(struct dhcpv4_packet *pack)
 {
-	struct dhcpv4_packet *pack = ses->dhcpv4_relay_reply;
+	struct ipoe_session *ses = container_of(triton_context_self(), typeof(*ses), ctx);
 	struct dhcpv4_option *opt;
 
+	if (ses->dhcpv4_relay_reply)
+		dhcpv4_packet_free(ses->dhcpv4_relay_reply);
+	
+	ses->dhcpv4_relay_reply = pack;
+
 	if (conf_verbose) {
 		log_ppp_info2("recv ");
 		dhcpv4_print_packet(pack, 1, log_ppp_info2);
@@ -1433,9 +1438,8 @@ static void ipoe_recv_dhcpv4_relay(struct dhcpv4_packet *pack)
 		break;
 	}
 	
-	if (found && !ses->dhcpv4_relay_reply) {
-		ses->dhcpv4_relay_reply = pack;
-		triton_context_call(&ses->ctx, (triton_event_func)ipoe_ses_recv_dhcpv4_relay, ses);
+	if (found) {
+		triton_context_call(&ses->ctx, (triton_event_func)ipoe_ses_recv_dhcpv4_relay, pack);
 	} else
 		dhcpv4_packet_free(pack);
 
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);
diff --git a/accel-pppd/libnetlink/iputils.c b/accel-pppd/libnetlink/iputils.c
index 029d09a6..71c14e28 100644
--- a/accel-pppd/libnetlink/iputils.c
+++ b/accel-pppd/libnetlink/iputils.c
@@ -119,7 +119,7 @@ int __export iplink_get_stats(int ifindex, struct rtnl_link_stats *stats)
 	struct iplink_req {
 		struct nlmsghdr n;
 		struct ifinfomsg i;
-		char buf[4096];
+		char buf[1024];
 	} req;
 	struct ifinfomsg *ifi;
 	int len;
@@ -161,6 +161,87 @@ int __export iplink_get_stats(int ifindex, struct rtnl_link_stats *stats)
 	
 	return 0;
 }
+	
+int iplink_vlan_add(const char *ifname, int ifindex, int vid)
+{
+	struct iplink_req {
+		struct nlmsghdr n;
+		struct ifinfomsg i;
+		char buf[1024];
+	} req;
+	struct rtattr *linkinfo, *data;
+
+	if (!rth)
+		open_rth();
+	
+	if (!rth)
+		return -1;
+
+	memset(&req, 0, sizeof(req) - 1024);
+	
+	req.n.nlmsg_len = NLMSG_LENGTH(sizeof(struct ifinfomsg));
+	req.n.nlmsg_flags = NLM_F_REQUEST | NLM_F_ACK | NLM_F_CREATE | NLM_F_EXCL;
+	req.n.nlmsg_type = RTM_NEWLINK;
+	req.i.ifi_family = AF_UNSPEC;
+	
+	addattr_l(&req.n, 1024, IFLA_LINK, &ifindex, 4);
+	addattr_l(&req.n, 1024, IFLA_IFNAME, ifname, strlen(ifname));
+	
+	linkinfo = NLMSG_TAIL(&req.n);
+	addattr_l(&req.n, 1024, IFLA_LINKINFO, NULL, 0);
+	addattr_l(&req.n, 1024, IFLA_INFO_KIND, "vlan", 4);
+
+	data = NLMSG_TAIL(&req.n);
+	addattr_l(&req.n, 1024, IFLA_INFO_DATA, NULL, 0);
+	addattr_l(&req.n, 1024, IFLA_VLAN_ID, &vid, 2);
+	data->rta_len = (void *)NLMSG_TAIL(&req.n) - (void *)data;
+	
+	linkinfo->rta_len = (void *)NLMSG_TAIL(&req.n) - (void *)linkinfo;
+
+	if (rtnl_talk(rth, &req.n, 0, 0, NULL, NULL, NULL, 0) < 0)
+		return -1;
+	
+	return 0;
+}
+
+int iplink_vlan_del(int ifindex)
+{
+	struct iplink_req {
+		struct nlmsghdr n;
+		struct ifinfomsg i;
+		char buf[1024];
+	} req;
+	struct rtattr *linkinfo;
+
+	if (!rth)
+		open_rth();
+	
+	if (!rth)
+		return -1;
+
+	memset(&req, 0, sizeof(req) - 1024);
+	
+	req.n.nlmsg_len = NLMSG_LENGTH(sizeof(struct ifinfomsg));
+	req.n.nlmsg_flags = NLM_F_REQUEST | NLM_F_ACK;
+	req.n.nlmsg_type = RTM_DELLINK;
+	req.i.ifi_family = AF_UNSPEC;
+	req.i.ifi_index = ifindex;
+	
+	linkinfo = NLMSG_TAIL(&req.n);
+	addattr_l(&req.n, 1024, IFLA_LINKINFO, NULL, 0);
+	addattr_l(&req.n, 1024, IFLA_INFO_KIND, "vlan", 4);
+
+	/*data = NLMSG_TAIL(&req.n);
+	addattr_l(&req.n, 1024, IFLA_VLAN_ID, &vid, 2);
+	data->rta_len = (void *)NLMSG_TAIL(&req.n) - (void *)data;*/
+	
+	linkinfo->rta_len = (void *)NLMSG_TAIL(&req.n) - (void *)linkinfo;
+
+	if (rtnl_talk(rth, &req.n, 0, 0, NULL, NULL, NULL, 0) < 0)
+		return -1;
+	
+	return 0;
+}
 
 int __export ipaddr_add(int ifindex, in_addr_t addr, int mask)
 {
diff --git a/accel-pppd/libnetlink/iputils.h b/accel-pppd/libnetlink/iputils.h
index 5baf7426..2657a5c9 100644
--- a/accel-pppd/libnetlink/iputils.h
+++ b/accel-pppd/libnetlink/iputils.h
@@ -8,6 +8,9 @@ typedef int (*iplink_list_func)(int index, int flags, const char *name, void *ar
 int iplink_list(iplink_list_func func, void *arg);
 int iplink_get_stats(int ifindex, struct rtnl_link_stats *stats);
 
+int iplink_vlan_add(const char *ifname, int ifindex, int vid);
+int iplink_vlan_del(int ifindex);
+
 int ipaddr_add(int ifindex, in_addr_t addr, int mask);
 int ipaddr_del(int ifindex, in_addr_t addr);
 
diff --git a/drivers/ipoe/ipoe.c b/drivers/ipoe/ipoe.c
index 80d03f09..baa4710d 100644
--- a/drivers/ipoe/ipoe.c
+++ b/drivers/ipoe/ipoe.c
@@ -14,6 +14,7 @@
 #include <linux/mroute.h>
 #include <linux/init.h>
 #include <linux/if_ether.h>
+#include <linux/if_vlan.h>
 #include <linux/semaphore.h>
 #include <linux/netfilter_ipv4.h>
 #include <linux/version.h>
@@ -100,6 +101,20 @@ struct ipoe_entry_u {
 	unsigned long tstamp;
 };
 
+struct vlan_dev {
+	struct rcu_head rcu_head;
+	struct list_head entry;
+
+	int ifindex;
+	unsigned long vid[4096/8/sizeof(long)];
+};
+
+struct vlan_notify {
+	struct list_head entry;
+	int ifindex;
+	int vid;
+};
+
 static struct list_head ipoe_list[HASH_BITS + 1];
 static struct list_head ipoe_list1_u[HASH_BITS + 1];
 static LIST_HEAD(ipoe_list2);
@@ -110,6 +125,11 @@ static LIST_HEAD(ipoe_interfaces);
 static struct work_struct ipoe_queue_work;
 static struct sk_buff_head ipoe_queue;
 
+static LIST_HEAD(vlan_devices);
+static LIST_HEAD(vlan_notifies);
+static DEFINE_SPINLOCK(vlan_lock);
+static struct work_struct vlan_notify_work;
+
 static void ipoe_start_queue_work(unsigned long);
 static DEFINE_TIMER(ipoe_timer_u, ipoe_start_queue_work, 0, 0);
 
@@ -613,6 +633,7 @@ static void ipoe_process_queue(struct work_struct *w)
 					genlmsg_end(report_skb, header);
 					genlmsg_multicast(report_skb, 0, ipoe_nl_mcg.id, GFP_KERNEL);
 					report_skb = NULL;
+					id = 1;
 				}
 
 				kfree_skb(skb);
@@ -827,6 +848,119 @@ static unsigned int ipt_out_hook(unsigned int hook, struct sk_buff *skb, const s
 	return NF_ACCEPT;
 }
 
+static int vlan_pt_recv(struct sk_buff *skb, struct net_device *dev, struct packet_type *prev, struct net_device *orig_dev)
+{
+	struct vlan_dev *d;
+	struct vlan_notify *n;
+	int vid;
+
+	if (!vlan_tx_tag_present(skb))
+		goto out;
+
+	vid = skb->vlan_tci & VLAN_VID_MASK;
+
+	rcu_read_lock();
+	list_for_each_entry_rcu(d, &vlan_devices, entry) {
+		if (d->ifindex == dev->ifindex)
+			goto found;
+	}
+	rcu_read_lock();
+	goto out;
+
+found:
+	if (d->vid[vid / (8*sizeof(long))] & (1 << (vid % (8*sizeof(long)))))
+		vid = -1;
+	else
+		d->vid[vid / (8*sizeof(long))] |= 1 << (vid % (8*sizeof(long)));
+	rcu_read_lock();
+
+	if (vid == -1)
+		goto out;
+	
+	n = kmalloc(sizeof(*n), GFP_ATOMIC);
+	if (!n)
+		goto out;
+	
+	n->ifindex = dev->ifindex;
+	n->vid = vid;
+
+	spin_lock(&vlan_lock);
+	list_add_tail(&n->entry, &vlan_notifies);
+	spin_unlock(&vlan_lock);
+	
+	schedule_work(&vlan_notify_work);
+
+out:
+	kfree_skb(skb);
+	return 0;
+}
+
+static void vlan_do_notify(struct work_struct *w)
+{
+	struct vlan_notify *n;
+	struct sk_buff *report_skb = NULL;
+	void *header = NULL;
+	struct nlattr *ns;
+	int id = 1;
+	unsigned long flags;
+
+	while (1) {
+		spin_lock_irqsave(&vlan_lock, flags);
+		if (list_empty(&vlan_notifies))
+			n = NULL;
+		else {
+			n = list_first_entry(&vlan_notifies, typeof(*n), entry);
+			list_del(&n->entry);
+		}
+		spin_unlock_irqrestore(&vlan_lock, flags);
+
+		if (!n)
+			break;
+	
+		if (!report_skb) {
+			report_skb = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
+			header = genlmsg_put(report_skb, 0, ipoe_nl_mcg.id, &ipoe_nl_family, 0, IPOE_VLAN_NOTIFY);
+		}
+
+		ns = nla_nest_start(report_skb, id++);
+		if (!ns)
+			goto nl_err;
+	
+#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,32)
+		if (nla_put_u32(report_skb, IPOE_ATTR_IFINDEX, n->ifindex))
+#else
+		if (nla_put_u32(report_skb, IPOE_ATTR_IFINDEX, n->ifindex))
+#endif
+			goto nl_err;
+
+#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,32)
+		if (nla_put_u32(report_skb, IPOE_ATTR_ADDR, n->vid))
+#else
+		if (nla_put_u32(report_skb, IPOE_ATTR_ADDR, n->vid))
+#endif
+			goto nl_err;
+				
+		if (nla_nest_end(report_skb, ns) >= IPOE_NLMSG_SIZE) {
+			genlmsg_end(report_skb, header);
+			genlmsg_multicast(report_skb, 0, ipoe_nl_mcg.id, GFP_KERNEL);
+			report_skb = NULL;
+			id = 1;
+		}
+
+		kfree(n);
+		continue;
+
+nl_err:
+		nlmsg_free(report_skb);
+		report_skb = NULL;
+	}
+
+	if (report_skb) {
+		genlmsg_end(report_skb, header);
+		genlmsg_multicast(report_skb, 0, ipoe_nl_mcg.id, GFP_KERNEL);
+	}
+}
+
 #if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,35)
 static struct rtnl_link_stats64 *ipoe_stats64(struct net_device *dev,
 					     struct rtnl_link_stats64 *stats)
@@ -1461,6 +1595,92 @@ static int ipoe_nl_cmd_del_interface(struct sk_buff *skb, struct genl_info *info
 	return 0;
 }
 
+static int ipoe_nl_cmd_add_vlan_mon(struct sk_buff *skb, struct genl_info *info)
+{
+	struct vlan_dev *d;
+
+	if (!info->attrs[IPOE_ATTR_IFINDEX])
+		return -EINVAL;
+	
+	d = kzalloc(sizeof(*d), GFP_KERNEL);
+	if (!d)
+		return -ENOMEM;
+
+	d->ifindex = nla_get_u32(info->attrs[IPOE_ATTR_IFINDEX]);
+
+	if (info->attrs[IPOE_ATTR_VLAN_MASK])
+		memcpy(d->vid, nla_data(info->attrs[IPOE_ATTR_VLAN_MASK]), min((int)nla_len(info->attrs[IPOE_ATTR_VLAN_MASK]), (int)sizeof(d->vid)));
+
+	down(&ipoe_wlock);
+	list_add_tail_rcu(&d->entry, &vlan_devices);
+	up(&ipoe_wlock);
+
+	return 0;
+}
+
+static int ipoe_nl_cmd_add_vlan_mon_vid(struct sk_buff *skb, struct genl_info *info)
+{
+	struct vlan_dev *d;
+	int ifindex, vid;
+
+	if (!info->attrs[IPOE_ATTR_IFINDEX] || !info->attrs[IPOE_ATTR_ADDR])
+		return -EINVAL;
+	
+	ifindex = nla_get_u32(info->attrs[IPOE_ATTR_IFINDEX]);
+	vid = nla_get_u32(info->attrs[IPOE_ATTR_ADDR]);
+
+	down(&ipoe_wlock);
+	list_for_each_entry(d, &vlan_devices, entry) {
+		if (d->ifindex == ifindex) {
+			d->vid[vid / (8*sizeof(long))] &= ~(1 << (vid % (8*sizeof(long))));
+			break;
+		}
+	}
+	up(&ipoe_wlock);
+
+	return 0;
+}
+
+static int ipoe_nl_cmd_del_vlan_mon(struct sk_buff *skb, struct genl_info *info)
+{
+	struct vlan_dev *d;
+	struct vlan_notify *vn;
+	int ifindex;
+	unsigned long flags;
+	struct list_head *pos, *n;
+
+	if (info->attrs[IPOE_ATTR_IFINDEX])
+		ifindex = nla_get_u32(info->attrs[IPOE_ATTR_IFINDEX]);
+	else
+		ifindex = -1;
+
+	rcu_read_lock();
+	list_for_each_entry_rcu(d, &vlan_devices, entry) {
+		if (ifindex == -1 || d->ifindex == ifindex) {
+			//pr_info("del net %08x/%08x\n", n->addr, n->mask);
+			list_del_rcu(&d->entry);
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(3,0,0)
+			kfree_rcu(d, rcu_head);
+#else
+			call_rcu(&d->rcu_head, __kfree_rcu);
+#endif
+		}
+	}
+	rcu_read_unlock();
+
+	spin_lock_irqsave(&vlan_lock, flags);
+	list_for_each_safe(pos, n, &vlan_notifies) {
+		vn = list_entry(pos, typeof(*vn), entry);
+		if (ifindex == -1 || vn->ifindex == ifindex) {
+			list_del(&vn->entry);
+			kfree(vn);
+		}
+	}
+	spin_unlock_irqrestore(&vlan_lock, flags);
+
+	return 0;
+}
+
 
 
 static struct nla_policy ipoe_nl_policy[IPOE_ATTR_MAX + 1] = {
@@ -1471,6 +1691,7 @@ static struct nla_policy ipoe_nl_policy[IPOE_ATTR_MAX + 1] = {
 	[IPOE_ATTR_HWADDR]	    = { .type = NLA_U64                         },
 	[IPOE_ATTR_IFNAME]	    = { .type = NLA_STRING, .len = IFNAMSIZ - 1 },
 	[IPOE_ATTR_MASK]	      = { .type = NLA_U32,                        },
+	[IPOE_ATTR_VLAN_MASK]	  = { .type = NLA_BINARY, .len = 4096/8/sizeof(long) },
 };
 
 static struct genl_ops ipoe_nl_ops[] = {
@@ -1527,6 +1748,24 @@ static struct genl_ops ipoe_nl_ops[] = {
 		.policy = ipoe_nl_policy,
 		.flags = GENL_ADMIN_PERM,
 	},
+	{
+		.cmd = IPOE_CMD_ADD_VLAN_MON,
+		.doit = ipoe_nl_cmd_add_vlan_mon,
+		.policy = ipoe_nl_policy,
+		.flags = GENL_ADMIN_PERM,
+	},
+	{
+		.cmd = IPOE_CMD_ADD_VLAN_MON_VID,
+		.doit = ipoe_nl_cmd_add_vlan_mon_vid,
+		.policy = ipoe_nl_policy,
+		.flags = GENL_ADMIN_PERM,
+	},
+	{
+		.cmd = IPOE_CMD_DEL_VLAN_MON,
+		.doit = ipoe_nl_cmd_del_vlan_mon,
+		.policy = ipoe_nl_policy,
+		.flags = GENL_ADMIN_PERM,
+	},
 };
 
 static struct genl_family ipoe_nl_family = {
@@ -1574,6 +1813,11 @@ static struct nf_hook_ops ipt_ops[] __read_mostly = {
 	},
 };
 
+static struct packet_type vlan_pt __read_mostly = {
+	.type = __constant_htons(ETH_P_ALL),
+	.func = vlan_pt_recv,
+};
+
 /*static struct pernet_operations ipoe_net_ops = {
 	.init = ipoe_init_net,
 	.exit = ipoe_exit_net,
@@ -1597,6 +1841,8 @@ static int __init ipoe_init(void)
 	
 	skb_queue_head_init(&ipoe_queue);
 	INIT_WORK(&ipoe_queue_work, ipoe_process_queue);
+	
+	INIT_WORK(&vlan_notify_work, vlan_do_notify);
 
 #if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,35)
 	err = genl_register_family(&ipoe_nl_family);
@@ -1635,6 +1881,8 @@ static int __init ipoe_init(void)
 		printk(KERN_INFO "ipoe: can't register nf hooks\n");
 		goto out_unreg;
 	}
+	
+	dev_add_pack(&vlan_pt);
 
 	return 0;
 
@@ -1649,13 +1897,16 @@ static void __exit ipoe_fini(void)
 	struct ipoe_network *n;
 	struct ipoe_entry_u *e;
 	struct ipoe_session *ses;
+	struct vlan_dev *d;
+	struct vlan_notify *vn;
 	int i;
 	
+	dev_remove_pack(&vlan_pt);
+	nf_unregister_hooks(ipt_ops, ARRAY_SIZE(ipt_ops));
+
 	genl_unregister_mc_group(&ipoe_nl_family, &ipoe_nl_mcg);
 	genl_unregister_family(&ipoe_nl_family);
 
-	nf_unregister_hooks(ipt_ops, ARRAY_SIZE(ipt_ops));
-
 	flush_work(&ipoe_queue_work);
 	skb_queue_purge(&ipoe_queue);
 
@@ -1690,6 +1941,18 @@ static void __exit ipoe_fini(void)
 		list_del(&e->entry2);
 		kfree(e);
 	}
+
+	while (!list_empty(&vlan_devices)) {
+		d = list_first_entry(&vlan_devices, typeof(*d), entry);
+		list_del(&d->entry);
+		kfree(d);
+	}
+	
+	while (!list_empty(&vlan_notifies)) {
+		vn = list_first_entry(&vlan_notifies, typeof(*vn), entry);
+		list_del(&vn->entry);
+		kfree(vn);
+	}
 }
 
 module_init(ipoe_init);
diff --git a/drivers/ipoe/ipoe.h b/drivers/ipoe/ipoe.h
index d0ac1e41..7296505c 100644
--- a/drivers/ipoe/ipoe.h
+++ b/drivers/ipoe/ipoe.h
@@ -13,7 +13,11 @@ enum {
 	IPOE_CMD_DEL_NET,
 	IPOE_CMD_ADD_IF,
 	IPOE_CMD_DEL_IF,
+	IPOE_CMD_ADD_VLAN_MON,
+	IPOE_CMD_ADD_VLAN_MON_VID,
+	IPOE_CMD_DEL_VLAN_MON,
 	IPOE_REP_PKT,
+	IPOE_VLAN_NOTIFY,
 	__IPOE_CMD_MAX,
 };
 
@@ -29,6 +33,7 @@ enum {
 	IPOE_ATTR_IFINDEX,	   /* u32 */
 	IPOE_ATTR_ETH_HDR,	   /* u32 */
 	IPOE_ATTR_IP_HDR,	   /* u32 */
+	IPOE_ATTR_VLAN_MASK,	   /* u32 */
 	__IPOE_ATTR_MAX,
 };