vlan_mon: make possible to work other module after message "vlan not started"

6 files changed, 106 insertions, 3 deletions

diff --git a/accel-pppd/ctrl/ipoe/ipoe.c b/accel-pppd/ctrl/ipoe/ipoe.c
index bb1f9629..3edf7f00 100644
--- a/accel-pppd/ctrl/ipoe/ipoe.c
+++ b/accel-pppd/ctrl/ipoe/ipoe.c
@@ -1913,8 +1913,6 @@ void ipoe_recv_up(int ifindex, struct ethhdr *eth, struct iphdr *iph, struct _ar
 	struct ipoe_session *ses;
 	in_addr_t saddr = arph ? arph->ar_spa : iph->saddr;
 
-	log_debug("ipoe: recv up %08x\n", saddr);
-
 	pthread_mutex_lock(&serv_lock);
 	list_for_each_entry(serv, &serv_list, entry) {
 		if (serv->ifindex != ifindex)
@@ -2423,6 +2421,7 @@ void ipoe_vlan_mon_notify(int ifindex, int vid, int vlan_ifindex)
 
 	log_warn("ipoe: vlan %s not started\n", ifname);
 	iplink_vlan_del(ifr.ifr_ifindex);
+	vlan_mon_del_vid(ifindex, ETH_P_IP, vid);
 }
 
 static void ipoe_serv_timeout(struct triton_timer_t *t)
diff --git a/accel-pppd/ctrl/pppoe/pppoe.c b/accel-pppd/ctrl/pppoe/pppoe.c
index 70978cf3..4e4b3811 100644
--- a/accel-pppd/ctrl/pppoe/pppoe.c
+++ b/accel-pppd/ctrl/pppoe/pppoe.c
@@ -1721,6 +1721,7 @@ void pppoe_vlan_mon_notify(int ifindex, int vid, int vlan_ifindex)
 
 	log_warn("pppoe: vlan %s not started\n", ifname);
 	iplink_vlan_del(ifr.ifr_ifindex);
+	vlan_mon_del_vid(ifindex, ETH_P_PPP_DISC, vid);
 }
 
 static void add_vlan_mon(const char *opt, long *mask)
diff --git a/accel-pppd/include/vlan_mon.h b/accel-pppd/include/vlan_mon.h
index ebcfdc6c..23d5e60d 100644
--- a/accel-pppd/include/vlan_mon.h
+++ b/accel-pppd/include/vlan_mon.h
@@ -7,6 +7,7 @@ void vlan_mon_register_proto(uint16_t proto, vlan_mon_notify cb);
 
 int vlan_mon_add(int ifindex, uint16_t proto, long *mask, int len);
 int vlan_mon_add_vid(int ifindex, uint16_t proto, uint16_t vid);
+int vlan_mon_del_vid(int ifindex, uint16_t proto, uint16_t vid);
 int vlan_mon_del(int ifindex, uint16_t proto);
 int vlan_mon_check_busy(int ifindex, uint16_t vid);
 
diff --git a/accel-pppd/vlan-mon/vlan_mon.c b/accel-pppd/vlan-mon/vlan_mon.c
index 0f1b12a9..102f6f41 100644
--- a/accel-pppd/vlan-mon/vlan_mon.c
+++ b/accel-pppd/vlan-mon/vlan_mon.c
@@ -129,6 +129,47 @@ int __export vlan_mon_add_vid(int ifindex, uint16_t proto, uint16_t vid)
 	return r;
 }
 
+int __export vlan_mon_del_vid(int ifindex, uint16_t proto, uint16_t vid)
+{
+	struct rtnl_handle rth;
+	struct nlmsghdr *nlh;
+	struct genlmsghdr *ghdr;
+	struct {
+		struct nlmsghdr n;
+		char buf[1024];
+	} req;
+	int r = 0;
+
+	if (vlan_mon_genl_id < 0)
+		return -1;
+
+	if (rtnl_open_byproto(&rth, 0, NETLINK_GENERIC)) {
+		log_error("vlan_mon: 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 = vlan_mon_genl_id;
+
+	ghdr = NLMSG_DATA(&req.n);
+	ghdr->cmd = VLAN_MON_CMD_DEL_VID;
+
+	addattr32(nlh, 1024, VLAN_MON_ATTR_IFINDEX, ifindex);
+	addattr_l(nlh, 1024, VLAN_MON_ATTR_VID, &vid, 2);
+	addattr_l(nlh, 1024, VLAN_MON_ATTR_PROTO, &proto, 2);
+
+	if (rtnl_talk(&rth, nlh, 0, 0, nlh, NULL, NULL, 0) < 0 ) {
+		log_error("vlan_mon: nl_add_vlan_mon_vid: error talking to kernel\n");
+		r = -1;
+	}
+
+	rtnl_close(&rth);
+
+	return r;
+}
+
 int __export vlan_mon_del(int ifindex, uint16_t proto)
 {
 	struct rtnl_handle rth;
diff --git a/drivers/vlan_mon/vlan_mon.c b/drivers/vlan_mon/vlan_mon.c
index b968b6b3..f0f2f8b2 100644
--- a/drivers/vlan_mon/vlan_mon.c
+++ b/drivers/vlan_mon/vlan_mon.c
@@ -116,7 +116,6 @@ static int vlan_pt_recv(struct sk_buff *skb, struct net_device *dev, struct pack
 	}
 
 	vid = skb->vlan_tci & VLAN_VID_MASK;
-	//pr_info("vid %i\n", vid);
 
 	if (likely(d->busy[vid / (8*sizeof(long))] & (1lu << (vid % (8*sizeof(long))))))
 		vid = -1;
@@ -428,6 +427,61 @@ static int vlan_mon_nl_cmd_add_vlan_mon_vid(struct sk_buff *skb, struct genl_inf
 	return 0;
 }
 
+static int vlan_mon_nl_cmd_del_vlan_mon_vid(struct sk_buff *skb, struct genl_info *info)
+{
+	struct vlan_dev *d;
+	int ifindex, vid, proto;
+	struct net_device *dev;
+
+	if (!info->attrs[VLAN_MON_ATTR_IFINDEX] || !info->attrs[VLAN_MON_ATTR_VID] || !info->attrs[VLAN_MON_ATTR_PROTO])
+		return -EINVAL;
+
+	ifindex = nla_get_u32(info->attrs[VLAN_MON_ATTR_IFINDEX]);
+	vid = nla_get_u16(info->attrs[VLAN_MON_ATTR_VID]);
+	proto = nla_get_u16(info->attrs[VLAN_MON_ATTR_PROTO]);
+
+	proto = vlan_mon_proto(proto);
+	if (proto < 0)
+		return proto;
+
+	down(&vlan_mon_lock);
+
+	rtnl_lock();
+	dev = __dev_get_by_index(&init_net, ifindex);
+	if (!dev) {
+		rtnl_unlock();
+		up(&vlan_mon_lock);
+		return -ENODEV;
+	}
+
+	if (!dev->ml_priv) {
+		rtnl_unlock();
+		up(&vlan_mon_lock);
+		return -EINVAL;
+	}
+
+	d = dev->ml_priv;
+
+	spin_lock_bh(&d->lock);
+	d->vid[proto][vid / (8*sizeof(long))] |= 1lu << (vid % (8*sizeof(long)));
+	d->busy[vid / (8*sizeof(long))] &= ~(1lu << (vid % (8*sizeof(long))));
+	spin_unlock_bh(&d->lock);
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(3,10,0)
+	if (dev->features & NETIF_F_HW_VLAN_FILTER)
+		dev->netdev_ops->ndo_vlan_rx_add_vid(dev, vid);
+#else
+	if (dev->features & NETIF_F_HW_VLAN_CTAG_FILTER)
+		dev->netdev_ops->ndo_vlan_rx_add_vid(dev, htons(ETH_P_8021Q), vid);
+#endif
+
+	rtnl_unlock();
+
+	up(&vlan_mon_lock);
+
+	return 0;
+}
+
 static void vlan_dev_clean(struct vlan_dev *d, struct net_device *dev, struct list_head *list)
 {
 	int i;
@@ -599,6 +653,12 @@ static struct genl_ops vlan_mon_nl_ops[] = {
 		.policy = vlan_mon_nl_policy,
 		.flags = GENL_ADMIN_PERM,
 	},
+	{
+		.cmd = VLAN_MON_CMD_DEL_VID,
+		.doit = vlan_mon_nl_cmd_del_vlan_mon_vid,
+		.policy = vlan_mon_nl_policy,
+		.flags = GENL_ADMIN_PERM,
+	},
 };
 
 static struct genl_family vlan_mon_nl_family = {
diff --git a/drivers/vlan_mon/vlan_mon.h b/drivers/vlan_mon/vlan_mon.h
index d5f9f360..bb94164e 100644
--- a/drivers/vlan_mon/vlan_mon.h
+++ b/drivers/vlan_mon/vlan_mon.h
@@ -10,6 +10,7 @@ enum {
 	VLAN_MON_CMD_DEL,
 	VLAN_MON_NOTIFY,
 	VLAN_MON_CMD_CHECK_BUSY,
+	VLAN_MON_CMD_DEL_VID,
 	__VLAN_MON_CMD_MAX,
 };