vlan_mon,ipoe,pppoe: implemented detection of vlan existance

6 files changed, 151 insertions, 36 deletions

diff --git a/accel-pppd/ctrl/ipoe/ipoe.c b/accel-pppd/ctrl/ipoe/ipoe.c
index 1e3c04c..68841fe 100644
--- a/accel-pppd/ctrl/ipoe/ipoe.c
+++ b/accel-pppd/ctrl/ipoe/ipoe.c
@@ -2181,7 +2181,7 @@ static int get_offer_delay()
 	return 0;
 }
 
-void ipoe_vlan_mon_notify(int ifindex, int vid)
+void ipoe_vlan_mon_notify(int ifindex, int vid, int vlan_ifindex)
 {
 	struct conf_sect_t *sect = conf_get_section("ipoe");
 	struct conf_option_t *opt;
@@ -2217,24 +2217,59 @@ void ipoe_vlan_mon_notify(int ifindex, int vid)
 		return;
 	}
 
-	if (iplink_vlan_add(ifname, ifindex, vid))
-		return;
+	if (vlan_ifindex) {
+		struct ipoe_serv *serv;
+
+		pthread_mutex_lock(&serv_lock);
+		list_for_each_entry(serv, &serv_list, entry) {
+			if (serv->ifindex == vlan_ifindex) {
+				pthread_mutex_unlock(&serv_lock);
+				return;
+			}
+		}
+		pthread_mutex_unlock(&serv_lock);
+
+		log_info2("ipoe: create vlan %s parent %s\n", ifname, ifr.ifr_name);
+
+		ifr.ifr_ifindex = vlan_ifindex;
+		if (ioctl(sock_fd, SIOCGIFNAME, &ifr, sizeof(ifr))) {
+			log_error("ipoe: vlan-mon: failed to get interface name, ifindex=%i\n", ifindex);
+			return;
+		}
+
+		if (ioctl(sock_fd, SIOCGIFFLAGS, &ifr, sizeof(ifr)))
+			return;
+
+		if (ifr.ifr_flags & IFF_UP) {
+			ifr.ifr_flags &= ~IFF_UP;
+
+			if (ioctl(sock_fd, SIOCSIFFLAGS, &ifr, sizeof(ifr)))
+				return;
+		}
+
+		if (strcmp(ifr.ifr_name, ifname)) {
+			strcpy(ifr.ifr_newname, ifname);
+			if (ioctl(sock_fd, SIOCSIFNAME, &ifr, sizeof(ifr))) {
+				log_error("ipoe: vlan-mon: failed to rename interface %s to %s\n", ifr.ifr_name, ifr.ifr_newname);
+				return;
+			}
+			strcpy(ifr.ifr_name, ifname);
+		}
+	} else {
+		log_info2("ipoe: create vlan %s parent %s\n", ifname, ifr.ifr_name);
 
-	log_info2("ipoe: create vlan %s parent %s\n", ifname, ifr.ifr_name);
+		if (iplink_vlan_add(ifname, ifindex, vid))
+			return;
+	}
 
 	len = strlen(ifname);
 	memcpy(ifr.ifr_name, ifname, len + 1);
 
-	ioctl(sock_fd, SIOCGIFFLAGS, &ifr, sizeof(ifr));
-	ifr.ifr_flags |= IFF_UP;
-	ioctl(sock_fd, SIOCSIFFLAGS, &ifr, sizeof(ifr));
-
 	if (ioctl(sock_fd, SIOCGIFINDEX, &ifr, sizeof(ifr))) {
 		log_error("ipoe: vlan-mon: %s: failed to get interface index\n", ifr.ifr_name);
 		return;
 	}
 
-
 	list_for_each_entry(opt, &sect->items, entry) {
 		if (strcmp(opt->name, "interface"))
 			continue;
@@ -2482,6 +2517,10 @@ static void add_interface(const char *ifname, int ifindex, const char *opt, int
 	memset(&ifr, 0, sizeof(ifr));
 	strcpy(ifr.ifr_name, ifname);
 
+	((struct sockaddr_in *)&ifr.ifr_addr)->sin_family = AF_INET;
+
+	ioctl(sock_fd, SIOCSIFADDR, &ifr, sizeof(ifr));
+
 	if (ioctl(sock_fd, SIOCGIFHWADDR, &ifr)) {
 		log_error("ipoe: '%s': ioctl(SIOCGIFHWADDR): %s\n", ifname, strerror(errno));
 		return;
@@ -2489,12 +2528,14 @@ static void add_interface(const char *ifname, int ifindex, const char *opt, int
 
 	ioctl(sock_fd, SIOCGIFFLAGS, &ifr);
 
-	if (!(ifr.ifr_flags & IFF_UP)) {
-		ifr.ifr_flags |= IFF_UP;
-
+	if (ifr.ifr_flags & IFF_UP) {
+		ifr.ifr_flags &= ~IFF_UP;
 		ioctl(sock_fd, SIOCSIFFLAGS, &ifr);
 	}
 
+	ifr.ifr_flags |= IFF_UP;
+	ioctl(sock_fd, SIOCSIFFLAGS, &ifr);
+
 	serv = _malloc(sizeof(*serv));
 	memset(serv, 0, sizeof(*serv));
 	serv->ctx.close = ipoe_serv_close;
diff --git a/accel-pppd/ctrl/pppoe/pppoe.c b/accel-pppd/ctrl/pppoe/pppoe.c
index f219152..b649b0b 100644
--- a/accel-pppd/ctrl/pppoe/pppoe.c
+++ b/accel-pppd/ctrl/pppoe/pppoe.c
@@ -1351,6 +1351,19 @@ static void __pppoe_server_start(const char *ifname, const char *opt, void *cli,
 	}
 
 	strncpy(ifr.ifr_name, ifname, sizeof(ifr.ifr_name));
+
+	if (net->sock_ioctl(SIOCGIFFLAGS, &ifr)) {
+		if (cli)
+			cli_sendv(cli, "%s: %s\r\n", ifname, strerror(errno));
+		log_error("pppoe: %s: %s\n", ifname, strerror(errno));
+		goto out_err;
+	}
+
+	if (!ifr.ifr_flags & IFF_UP) {
+		ifr.ifr_flags |= IFF_UP;
+		net->sock_ioctl(SIOCSIFFLAGS, &ifr);
+	}
+
 	if (net->sock_ioctl(SIOCGIFHWADDR, &ifr)) {
 		if (cli)
 			cli_sendv(cli, "ioctl(SIOCGIFHWADDR): %s\r\n", strerror(errno));
@@ -1526,7 +1539,7 @@ static int init_secret(struct pppoe_serv_t *serv)
 	return 0;
 }
 
-void pppoe_vlan_mon_notify(int ifindex, int vid)
+void pppoe_vlan_mon_notify(int ifindex, int vid, int vlan_ifindex)
 {
 	struct conf_sect_t *sect = conf_get_section("pppoe");
 	struct conf_option_t *opt;
@@ -1556,17 +1569,53 @@ void pppoe_vlan_mon_notify(int ifindex, int vid)
 		return;
 	}
 
-	strcpy(ifr.ifr_name, ifname);
-	len = strlen(ifr.ifr_name);
+	if (vlan_ifindex) {
+		struct pppoe_serv_t *serv;
 
-	if (iplink_vlan_add(ifr.ifr_name, ifindex, vid))
-		return;
+		pthread_rwlock_rdlock(&serv_lock);
+		list_for_each_entry(serv, &serv_list, entry) {
+			if (serv->ifindex == vlan_ifindex) {
+				pthread_rwlock_unlock(&serv_lock);
+				return;
+			}
+		}
+		pthread_rwlock_unlock(&serv_lock);
+
+		log_info2("pppoe: create vlan %s parent %s\n", ifname, ifr.ifr_name);
+
+		ifr.ifr_ifindex = vlan_ifindex;
+		if (ioctl(sock_fd, SIOCGIFNAME, &ifr, sizeof(ifr))) {
+			log_error("pppoe: vlan-mon: failed to get interface name, ifindex=%i\n", ifindex);
+			return;
+		}
 
-	log_info2("pppoe: create vlan %s parent %s\n", ifname, ifr.ifr_name);
+		if (ioctl(sock_fd, SIOCGIFFLAGS, &ifr, sizeof(ifr)))
+			return;
+
+		if (ifr.ifr_flags & IFF_UP) {
+			ifr.ifr_flags &= ~IFF_UP;
+
+			if (ioctl(sock_fd, SIOCSIFFLAGS, &ifr, sizeof(ifr)))
+				return;
+		}
+
+		if (strcmp(ifr.ifr_name, ifname)) {
+			strcpy(ifr.ifr_newname, ifname);
+			if (ioctl(sock_fd, SIOCSIFNAME, &ifr, sizeof(ifr))) {
+				log_error("pppoe: vlan-mon: failed to rename interface %s to %s\n", ifr.ifr_name, ifr.ifr_newname);
+				return;
+			}
+			strcpy(ifr.ifr_name, ifname);
+		}
+	} else {
+		log_info2("pppoe: create vlan %s parent %s\n", ifname, ifr.ifr_name);
 
-	ioctl(sock_fd, SIOCGIFFLAGS, &ifr, sizeof(ifr));
-	ifr.ifr_flags |= IFF_UP;
-	ioctl(sock_fd, SIOCSIFFLAGS, &ifr, sizeof(ifr));
+		if (iplink_vlan_add(ifr.ifr_name, ifindex, vid))
+			return;
+	}
+
+	strcpy(ifr.ifr_name, ifname);
+	len = strlen(ifname);
 
 	if (ioctl(sock_fd, SIOCGIFINDEX, &ifr, sizeof(ifr))) {
 		log_error("pppoe: vlan-mon: %s: failed to get interface index\n", ifr.ifr_name);
@@ -1602,9 +1651,14 @@ void pppoe_vlan_mon_notify(int ifindex, int vid)
 				continue;
 
 			__pppoe_server_start(ifr.ifr_name, opt->val, NULL, ifindex, vid);
-		} else if (ptr - opt->val == len && memcmp(opt->val, ifr.ifr_name, len) == 0)
+			return;
+		} else if (ptr - opt->val == len && memcmp(opt->val, ifr.ifr_name, len) == 0) {
 			__pppoe_server_start(ifr.ifr_name, opt->val, NULL, ifindex, vid);
+			return;
+		}
 	}
+
+	log_warn("pppoe: vlan %s not started\n", ifname);
 }
 
 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 c6b2db7..328e230 100644
--- a/accel-pppd/include/vlan_mon.h
+++ b/accel-pppd/include/vlan_mon.h
@@ -1,7 +1,7 @@
 #ifndef __VLAN_MON_H
 #define __VLAN_MON_H
 
-typedef void (*vlan_mon_notify)(int ifindex, int vid);
+typedef void (*vlan_mon_notify)(int ifindex, int vid, int vlan_ifindex);
 
 void vlan_mon_register_proto(int proto, vlan_mon_notify cb);
 
diff --git a/accel-pppd/vlan-mon/vlan_mon.c b/accel-pppd/vlan-mon/vlan_mon.c
index 7f15aa0..9b77fbc 100644
--- a/accel-pppd/vlan-mon/vlan_mon.c
+++ b/accel-pppd/vlan-mon/vlan_mon.c
@@ -151,7 +151,7 @@ static void vlan_mon_handler(const struct sockaddr_nl *addr, struct nlmsghdr *h)
 	int len = h->nlmsg_len;
 	struct rtattr *attrs;
 	int i;
-	int ifindex, vid, proto;
+	int ifindex, vid, proto, vlan_ifindex;
 
 	len -= NLMSG_LENGTH(GENL_HDRLEN);
 
@@ -176,7 +176,12 @@ static void vlan_mon_handler(const struct sockaddr_nl *addr, struct nlmsghdr *h)
 		vid = *(uint32_t *)(RTA_DATA(tb2[VLAN_MON_ATTR_VID]));
 		proto = *(uint32_t *)(RTA_DATA(tb2[VLAN_MON_ATTR_PROTO]));
 
-		log_debug("vlan-mon: notify %i %i %04x\n", ifindex, vid, proto);
+		if (tb2[VLAN_MON_ATTR_VLAN_IFINDEX])
+			vlan_ifindex = *(uint32_t *)(RTA_DATA(tb2[VLAN_MON_ATTR_VLAN_IFINDEX]));
+		else
+			vlan_ifindex = 0;
+
+		log_debug("vlan-mon: notify %i %i %04x %i\n", ifindex, vid, proto, vlan_ifindex);
 
 		if (proto == ETH_P_PPP_DISC)
 			proto = 1;
@@ -184,7 +189,7 @@ static void vlan_mon_handler(const struct sockaddr_nl *addr, struct nlmsghdr *h)
 			proto = 0;
 
 		if (cb[proto])
-			cb[proto](ifindex, vid);
+			cb[proto](ifindex, vid, vlan_ifindex);
 	}
 }
 
diff --git a/drivers/vlan_mon/vlan_mon.c b/drivers/vlan_mon/vlan_mon.c
index 2037495..f88dda3 100644
--- a/drivers/vlan_mon/vlan_mon.c
+++ b/drivers/vlan_mon/vlan_mon.c
@@ -53,6 +53,7 @@ struct vlan_dev {
 struct vlan_notify {
 	struct list_head entry;
 	int ifindex;
+	int vlan_ifindex;
 	int vid;
 	int proto;
 };
@@ -85,6 +86,7 @@ static int vlan_pt_recv(struct sk_buff *skb, struct net_device *dev, struct pack
 	struct vlan_dev *d;
 	struct vlan_notify *n;
 	int vid;
+	int vlan_ifindex = 0;
 	int proto;
 
 	if (!dev->ml_priv)
@@ -120,6 +122,19 @@ static int vlan_pt_recv(struct sk_buff *skb, struct net_device *dev, struct pack
 		d->vid[proto][vid / (8*sizeof(long))] |= 1lu << (vid % (8*sizeof(long)));
 		spin_unlock(&d->lock);
 	}
+
+	if (vid > 0) {
+#if LINUX_VERSION_CODE < KERNEL_VERSION(3,10,0)
+		struct net_device *vd = __vlan_find_dev_deep(dev, vid);
+#elif LINUX_VERSION_CODE < KERNEL_VERSION(3,16,0)
+		struct net_device *vd = __vlan_find_dev_deep(dev, skb->vlan_proto, vid);
+#else
+		struct net_device *vd = __vlan_find_dev_deep_rcu(dev, skb->vlan_proto, vid);
+#endif
+		if (vd)
+			vlan_ifindex = vd->ifindex;
+	}
+
 	rcu_read_unlock();
 
 	if (vid == -1)
@@ -132,6 +147,7 @@ static int vlan_pt_recv(struct sk_buff *skb, struct net_device *dev, struct pack
 		goto out;
 
 	n->ifindex = dev->ifindex;
+	n->vlan_ifindex = vlan_ifindex;
 	n->vid = vid;
 	n->proto = ntohs(skb->protocol);
 
@@ -153,19 +169,18 @@ static void vlan_do_notify(struct work_struct *w)
 	void *header = NULL;
 	struct nlattr *ns;
 	int id = 1;
-	unsigned long flags;
 
 	//pr_info("vlan_do_notify\n");
 
 	while (1) {
-		spin_lock_irqsave(&vlan_lock, flags);
+		spin_lock_bh(&vlan_lock);
 		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);
+		spin_unlock_bh(&vlan_lock);
 
 		if (!n)
 			break;
@@ -188,6 +203,9 @@ static void vlan_do_notify(struct work_struct *w)
 		if (nla_put_u32(report_skb, VLAN_MON_ATTR_IFINDEX, n->ifindex))
 			goto nl_err;
 
+		if (n->vlan_ifindex && nla_put_u32(report_skb, VLAN_MON_ATTR_VLAN_IFINDEX, n->vlan_ifindex))
+			goto nl_err;
+
 		if (nla_put_u16(report_skb, VLAN_MON_ATTR_VID, n->vid))
 			goto nl_err;
 
@@ -350,7 +368,6 @@ static int vlan_mon_nl_cmd_add_vlan_mon_vid(struct sk_buff *skb, struct genl_inf
 	struct vlan_dev *d;
 	int ifindex, vid, proto;
 	struct net_device *dev;
-	unsigned long flags;
 
 	if (!info->attrs[VLAN_MON_ATTR_IFINDEX] || !info->attrs[VLAN_MON_ATTR_VID] || !info->attrs[VLAN_MON_ATTR_PROTO])
 		return -EINVAL;
@@ -359,8 +376,6 @@ static int vlan_mon_nl_cmd_add_vlan_mon_vid(struct sk_buff *skb, struct genl_inf
 	vid = nla_get_u16(info->attrs[VLAN_MON_ATTR_VID]);
 	proto = nla_get_u16(info->attrs[VLAN_MON_ATTR_PROTO]);
 
-	proto = nla_get_u16(info->attrs[VLAN_MON_ATTR_PROTO]);
-
 	proto = vlan_mon_proto(proto);
 	if (proto < 0)
 		return proto;
@@ -380,9 +395,9 @@ static int vlan_mon_nl_cmd_add_vlan_mon_vid(struct sk_buff *skb, struct genl_inf
 
 	d = dev->ml_priv;
 
-	spin_lock_irqsave(&d->lock, flags);
+	spin_lock_bh(&d->lock);
 	d->vid[proto][vid / (8*sizeof(long))] &= ~(1lu << (vid % (8*sizeof(long))));
-	spin_unlock_irqrestore(&d->lock, flags);
+	spin_unlock_bh(&d->lock);
 #if LINUX_VERSION_CODE < KERNEL_VERSION(3,10,0)
 	if (dev->features & NETIF_F_HW_VLAN_FILTER) {
 		rtnl_lock();
@@ -536,8 +551,7 @@ static int __init vlan_mon_init(void)
 	int i;
 #endif
 
-
-	printk("vlan-mon driver v1.10-rc1\n");
+	printk("vlan-mon driver v1.11\n");
 
 	INIT_WORK(&vlan_notify_work, vlan_do_notify);
 
diff --git a/drivers/vlan_mon/vlan_mon.h b/drivers/vlan_mon/vlan_mon.h
index e65f65e..ff5c852 100644
--- a/drivers/vlan_mon/vlan_mon.h
+++ b/drivers/vlan_mon/vlan_mon.h
@@ -20,6 +20,7 @@ enum {
 	VLAN_MON_ATTR_PROTO,	   /* u32 */
 	VLAN_MON_ATTR_IFINDEX,	   /* u32 */
 	VLAN_MON_ATTR_VID,	   /* u32 */
+	VLAN_MON_ATTR_VLAN_IFINDEX,	   /* u32 */
 	__VLAN_MON_ATTR_MAX,
 };