From 9190fe47e0ce57e8f0b3360f2fe0404169b44263 Mon Sep 17 00:00:00 2001 From: Dmitry Kozlov Date: Sat, 21 Nov 2015 12:34:02 +0300 Subject: split ipoe driver to ipoe and vlan_mon introduced new module "vlan-mon" --- drivers/ipoe/ipoe.c | 410 +------------------------ drivers/ipoe/ipoe.h | 5 - drivers/vlan_mon/CMakeLists.txt | 17 + drivers/vlan_mon/Makefile | 4 + drivers/vlan_mon/vlan_mon.c | 664 ++++++++++++++++++++++++++++++++++++++++ drivers/vlan_mon/vlan_mon.h | 36 +++ 6 files changed, 722 insertions(+), 414 deletions(-) create mode 100644 drivers/vlan_mon/CMakeLists.txt create mode 100644 drivers/vlan_mon/Makefile create mode 100644 drivers/vlan_mon/vlan_mon.c create mode 100644 drivers/vlan_mon/vlan_mon.h (limited to 'drivers') diff --git a/drivers/ipoe/ipoe.c b/drivers/ipoe/ipoe.c index 3fc266e..d7de5e6 100644 --- a/drivers/ipoe/ipoe.c +++ b/drivers/ipoe/ipoe.c @@ -103,22 +103,6 @@ struct ipoe_entry_u { unsigned long tstamp; }; -struct vlan_dev { - unsigned int magic; - int ifindex; - struct rcu_head rcu_head; - struct list_head entry; - - spinlock_t lock; - 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 struct list_head ipoe_excl_list[HASH_BITS + 1]; @@ -130,11 +114,6 @@ 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); @@ -194,10 +173,6 @@ static void ipoe_kfree_rcu(struct rcu_head *head) } #endif -#if LINUX_VERSION_CODE >= KERNEL_VERSION(4,0,0) -#define vlan_tx_tag_present(skb) skb_vlan_tag_present(skb) -#endif - static int ipoe_check_network(__be32 addr) { struct ipoe_network *n; @@ -937,143 +912,6 @@ static unsigned int ipt_out_hook(const struct nf_hook_ops *ops, struct sk_buff * 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 (!dev->ml_priv) - goto out; - - if (!vlan_tx_tag_present(skb)) - goto out; - - rcu_read_lock(); - - d = rcu_dereference(dev->ml_priv); - if (!d || d->magic != IPOE_MAGIC2 || d->ifindex != dev->ifindex) { - rcu_read_unlock(); - goto out; - } - - vid = skb->vlan_tci & VLAN_VID_MASK; - //pr_info("vid %i\n", vid); - - if (d->vid[vid / (8*sizeof(long))] & (1lu << (vid % (8*sizeof(long))))) - vid = -1; - else { - spin_lock(&d->lock); - d->vid[vid / (8*sizeof(long))] |= 1lu << (vid % (8*sizeof(long))); - spin_unlock(&d->lock); - } - rcu_read_unlock(); - - if (vid == -1) - goto out; - - //pr_info("queue %i %i\n", dev->ifindex, vid); - - 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; - - //pr_info("vlan_do_notify\n"); - - 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); -#if LINUX_VERSION_CODE < KERNEL_VERSION(3,13,0) - header = genlmsg_put(report_skb, 0, ipoe_nl_mcg.id, &ipoe_nl_family, 0, IPOE_VLAN_NOTIFY); -#else - header = genlmsg_put(report_skb, 0, ipoe_nl_family.mcgrp_offset, &ipoe_nl_family, 0, IPOE_VLAN_NOTIFY); -#endif - } - - //pr_info("notify %i vlan %i\n", id, n->vid); - - 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); -#if LINUX_VERSION_CODE < KERNEL_VERSION(3,13,0) - genlmsg_multicast(report_skb, 0, ipoe_nl_mcg.id, GFP_KERNEL); -#else - genlmsg_multicast(&ipoe_nl_family, report_skb, 0, 0, GFP_KERNEL); -#endif - 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); -#if LINUX_VERSION_CODE < KERNEL_VERSION(3,13,0) - genlmsg_multicast(report_skb, 0, ipoe_nl_mcg.id, GFP_KERNEL); -#else - genlmsg_multicast(&ipoe_nl_family, report_skb, 0, 0, GFP_KERNEL); -#endif - } -} - #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) @@ -1806,195 +1644,6 @@ 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; - struct net_device *dev; - int ifindex, i; - - if (!info->attrs[IPOE_ATTR_IFINDEX]) - return -EINVAL; - - ifindex = nla_get_u32(info->attrs[IPOE_ATTR_IFINDEX]); - -#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,34) - rtnl_lock(); - dev = __dev_get_by_index(&init_net, ifindex); - rtnl_unlock(); -#else - dev = dev_get_by_index(&init_net, ifindex); -#endif - - if (!dev) - return -ENODEV; - - down(&ipoe_wlock); - if (dev->ml_priv) { - up(&ipoe_wlock); - dev_put(dev); - return -EBUSY; - } - - d = kzalloc(sizeof(*d), GFP_KERNEL); - if (!d) { - up(&ipoe_wlock); - dev_put(dev); - return -ENOMEM; - } - - d->magic = IPOE_MAGIC2; - d->ifindex = ifindex; - spin_lock_init(&d->lock); - - 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))); - -#if LINUX_VERSION_CODE < KERNEL_VERSION(3,10,0) - if (dev->features & NETIF_F_HW_VLAN_FILTER) { - rtnl_lock(); - for (i = 1; i < 4096; i++) { - if (!(d->vid[i / (8*sizeof(long))] & (1lu << (i % (8*sizeof(long)))))) - dev->netdev_ops->ndo_vlan_rx_add_vid(dev, i); - } - rtnl_unlock(); - } -#else - if (dev->features & NETIF_F_HW_VLAN_CTAG_FILTER) { - rtnl_lock(); - for (i = 1; i < 4096; i++) { - if (!(d->vid[i / (8*sizeof(long))] & (1lu << (i % (8*sizeof(long)))))) - dev->netdev_ops->ndo_vlan_rx_add_vid(dev, htons(ETH_P_8021Q), i); - } - rtnl_unlock(); - } -#endif - } - - rcu_assign_pointer(dev->ml_priv, d); - - list_add_tail_rcu(&d->entry, &vlan_devices); - up(&ipoe_wlock); - - dev_put(dev); - - 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; - struct net_device *dev; - unsigned long flags; - - 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]); - -#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,34) - rtnl_lock(); - dev = __dev_get_by_index(&init_net, ifindex); - rtnl_unlock(); -#else - dev = dev_get_by_index(&init_net, ifindex); -#endif - - if (!dev) - return -ENODEV; - - down(&ipoe_wlock); - - if (!dev->ml_priv) { - up(&ipoe_wlock); - dev_put(dev); - return -EINVAL; - } - - d = dev->ml_priv; - - spin_lock_irqsave(&d->lock, flags); - d->vid[vid / (8*sizeof(long))] &= ~(1lu << (vid % (8*sizeof(long)))); - spin_unlock_irqrestore(&d->lock, flags); -#if LINUX_VERSION_CODE < KERNEL_VERSION(3,10,0) - if (dev->features & NETIF_F_HW_VLAN_FILTER) { - rtnl_lock(); - dev->netdev_ops->ndo_vlan_rx_add_vid(dev, vid); - rtnl_unlock(); - } -#else - if (dev->features & NETIF_F_HW_VLAN_CTAG_FILTER) { - rtnl_lock(); - dev->netdev_ops->ndo_vlan_rx_add_vid(dev, htons(ETH_P_8021Q), vid); - rtnl_unlock(); - } -#endif - - up(&ipoe_wlock); - - dev_put(dev); - - 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; - struct net_device *dev; - - if (info->attrs[IPOE_ATTR_IFINDEX]) - ifindex = nla_get_u32(info->attrs[IPOE_ATTR_IFINDEX]); - else - ifindex = -1; - - down(&ipoe_wlock); - list_for_each_entry(d, &vlan_devices, entry) { - if (ifindex == -1 || d->ifindex == ifindex) { - //pr_info("del net %08x/%08x\n", n->addr, n->mask); -#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,34) - rtnl_lock(); - dev = __dev_get_by_index(&init_net, d->ifindex); - rtnl_unlock(); -#else - dev = dev_get_by_index(&init_net, d->ifindex); -#endif - - if (dev) { - if (dev->ml_priv == d) - rcu_assign_pointer(dev->ml_priv, NULL); - dev_put(dev); - } - - 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, ipoe_kfree_rcu); -#endif - } - } - up(&ipoe_wlock); - - 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] = { [IPOE_ATTR_NONE] = { .type = NLA_UNSPEC, }, [IPOE_ATTR_ADDR] = { .type = NLA_U32, }, @@ -2003,7 +1652,6 @@ 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 }, }; static struct genl_ops ipoe_nl_ops[] = { @@ -2060,24 +1708,6 @@ 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, - }, { .cmd = IPOE_CMD_ADD_EXCLUDE, .doit = ipoe_nl_cmd_add_exclude, @@ -2134,11 +1764,6 @@ 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, -}; - #if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,32) static const struct net_device_ops ipoe_netdev_ops = { .ndo_start_xmit = ipoe_xmit, @@ -2159,7 +1784,7 @@ static int __init ipoe_init(void) { int err, i; - printk("IPoE session driver v1.9.0\n"); + printk("IPoE session driver v1.10-rc1\n"); /*err = register_pernet_device(&ipoe_net_ops); if (err < 0) @@ -2173,8 +1798,6 @@ 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); if (err < 0) { @@ -2218,8 +1841,6 @@ static int __init ipoe_init(void) goto out_unreg; } - dev_add_pack(&vlan_pt); - return 0; out_unreg: @@ -2233,12 +1854,8 @@ 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; - struct net_device *dev; int i; - dev_remove_pack(&vlan_pt); nf_unregister_hooks(ipt_ops, ARRAY_SIZE(ipt_ops)); #if LINUX_VERSION_CODE < KERNEL_VERSION(3,13,0) @@ -2281,31 +1898,6 @@ static void __exit ipoe_fini(void) kfree(e); } - while (!list_empty(&vlan_devices)) { - d = list_first_entry(&vlan_devices, typeof(*d), entry); -#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,34) - rtnl_lock(); - dev = __dev_get_by_index(&init_net, d->ifindex); - rtnl_unlock(); -#else - dev = dev_get_by_index(&init_net, d->ifindex); -#endif - if (dev) - rcu_assign_pointer(dev->ml_priv, NULL); - list_del(&d->entry); -#if LINUX_VERSION_CODE >= KERNEL_VERSION(3,0,0) - kfree_rcu(d, rcu_head); -#else - call_rcu(&d->rcu_head, ipoe_kfree_rcu); -#endif - } - - while (!list_empty(&vlan_notifies)) { - vn = list_first_entry(&vlan_notifies, typeof(*vn), entry); - list_del(&vn->entry); - kfree(vn); - } - clean_excl_list(); synchronize_rcu(); diff --git a/drivers/ipoe/ipoe.h b/drivers/ipoe/ipoe.h index 192fa2f..e0cfc4e 100644 --- a/drivers/ipoe/ipoe.h +++ b/drivers/ipoe/ipoe.h @@ -13,11 +13,7 @@ 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_ADD_EXCLUDE, IPOE_CMD_DEL_EXCLUDE, __IPOE_CMD_MAX, @@ -35,7 +31,6 @@ enum { IPOE_ATTR_IFINDEX, /* u32 */ IPOE_ATTR_ETH_HDR, /* u32 */ IPOE_ATTR_IP_HDR, /* u32 */ - IPOE_ATTR_VLAN_MASK, /* u32 */ __IPOE_ATTR_MAX, }; diff --git a/drivers/vlan_mon/CMakeLists.txt b/drivers/vlan_mon/CMakeLists.txt new file mode 100644 index 0000000..892fed1 --- /dev/null +++ b/drivers/vlan_mon/CMakeLists.txt @@ -0,0 +1,17 @@ +if (NOT DEFINED KDIR) + set(KDIR "/usr/src/linux") +endif (NOT DEFINED KDIR) + +ADD_CUSTOM_COMMAND(OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/driver/vlan_mon.ko + COMMAND rm -rf ${CMAKE_CURRENT_BINARY_DIR}/driver + COMMAND mkdir ${CMAKE_CURRENT_BINARY_DIR}/driver + COMMAND ln -sf ${CMAKE_CURRENT_SOURCE_DIR}/* ${CMAKE_CURRENT_BINARY_DIR}/driver + COMMAND make -C ${KDIR} M=${CMAKE_CURRENT_BINARY_DIR}/driver modules + DEPENDS vlan_mon.c vlan_mon.h +) + +ADD_CUSTOM_TARGET(vlan_mon_drv ALL + DEPENDS ${CMAKE_CURRENT_BINARY_DIR}/driver/vlan_mon.ko +) + +INSTALL(CODE "EXECUTE_PROCESS(COMMAND make -C ${KDIR} M=${CMAKE_CURRENT_BINARY_DIR}/drivers/vlan_mon modules_install)") diff --git a/drivers/vlan_mon/Makefile b/drivers/vlan_mon/Makefile new file mode 100644 index 0000000..c70e47a --- /dev/null +++ b/drivers/vlan_mon/Makefile @@ -0,0 +1,4 @@ +obj-m += vlan_mon.o + +default: + make -C $(KDIR) M=$(PWD) modules diff --git a/drivers/vlan_mon/vlan_mon.c b/drivers/vlan_mon/vlan_mon.c new file mode 100644 index 0000000..f61d932 --- /dev/null +++ b/drivers/vlan_mon/vlan_mon.c @@ -0,0 +1,664 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "vlan_mon.h" + +#define VLAN_MON_MAGIC 0x639fa78c + +#define VLAN_MON_PROTO_IP 0 +#define VLAN_MON_PROTO_PPPOE 1 +//#define VLAN_MON_PROTO_IP6 2 + +#define VLAN_MON_NLMSG_SIZE (NLMSG_DEFAULT_SIZE - GENL_HDRLEN - 128) + +#ifndef DEFINE_SEMAPHORE +#define DEFINE_SEMAPHORE(name) struct semaphore name = __SEMAPHORE_INITIALIZER(name, 1) +#endif + +#ifndef NETIF_F_HW_VLAN_FILTER +#define NETIF_F_HW_VLAN_FILTER NETIF_F_HW_VLAN_CTAG_FILTER +#endif + +#if LINUX_VERSION_CODE >= KERNEL_VERSION(4,0,0) +#define vlan_tx_tag_present(skb) skb_vlan_tag_present(skb) +#endif + +struct vlan_dev { + unsigned int magic; + int ifindex; + struct rcu_head rcu_head; + struct list_head entry; + + spinlock_t lock; + unsigned long vid[2][4096/8/sizeof(long)]; + int proto; +}; + +struct vlan_notify { + struct list_head entry; + int ifindex; + int vid; + int proto; +}; + +static LIST_HEAD(vlan_devices); +static LIST_HEAD(vlan_notifies); +static DEFINE_SPINLOCK(vlan_lock); +static struct work_struct vlan_notify_work; + +static struct genl_family vlan_mon_nl_family; +#if LINUX_VERSION_CODE < KERNEL_VERSION(3,13,0) +static struct genl_multicast_group vlan_mon_nl_mcg; +#endif + +#if LINUX_VERSION_CODE < KERNEL_VERSION(3,0,0) +static void vlan_mon_kfree_rcu(struct rcu_head *head) +{ + kfree(head); +} +#endif + +static DEFINE_SEMAPHORE(vlan_mon_lock); + +static inline int vlan_mon_proto(int proto) +{ + if (proto == ETH_P_PPP_DISC) + return VLAN_MON_PROTO_PPPOE; + + if (proto == ETH_P_IP) + return VLAN_MON_PROTO_IP; + + return -ENOSYS; +} + +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; + int proto; + + if (!dev->ml_priv) + goto out; + + if (!vlan_tx_tag_present(skb)) + goto out; + + if (skb->protocol == htons(ETH_P_IP) || skb->protocol == htons(ETH_P_ARP)) + proto = VLAN_MON_PROTO_IP; + //else if (skb->protocol == htons(ETH_P_IPV6)) + // proto = VLAN_MON_PROTO_IP6; + else if (skb->protocol == htons(ETH_P_PPP_DISC)) + proto = VLAN_MON_PROTO_PPPOE; + else + goto out; + + rcu_read_lock(); + + d = rcu_dereference(dev->ml_priv); + if (!d || d->magic != VLAN_MON_MAGIC || d->ifindex != dev->ifindex || (d->proto & (1 << proto)) == 0) { + rcu_read_unlock(); + goto out; + } + + vid = skb->vlan_tci & VLAN_VID_MASK; + //pr_info("vid %i\n", vid); + + if (d->vid[proto][vid / (8*sizeof(long))] & (1lu << (vid % (8*sizeof(long))))) + vid = -1; + else { + spin_lock(&d->lock); + d->vid[proto][vid / (8*sizeof(long))] |= 1lu << (vid % (8*sizeof(long))); + spin_unlock(&d->lock); + } + rcu_read_unlock(); + + if (vid == -1) + goto out; + + //pr_info("queue %i %i %04x\n", dev->ifindex, vid, skb->protocol); + + n = kmalloc(sizeof(*n), GFP_ATOMIC); + if (!n) + goto out; + + n->ifindex = dev->ifindex; + n->vid = vid; + n->proto = ntohs(skb->protocol); + + 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; + + //pr_info("vlan_do_notify\n"); + + 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); +#if LINUX_VERSION_CODE < KERNEL_VERSION(3,13,0) + header = genlmsg_put(report_skb, 0, vlan_mon_nl_mcg.id, &vlan_mon_nl_family, 0, VLAN_MON_NOTIFY); +#else + header = genlmsg_put(report_skb, 0, vlan_mon_nl_family.mcgrp_offset, &vlan_mon_nl_family, 0, VLAN_MON_NOTIFY); +#endif + } + + //pr_info("notify %i vlan %i\n", id, n->vid); + + ns = nla_nest_start(report_skb, id++); + if (!ns) + goto nl_err; + + if (nla_put_u32(report_skb, VLAN_MON_ATTR_IFINDEX, n->ifindex)) + goto nl_err; + + if (nla_put_u16(report_skb, VLAN_MON_ATTR_VID, n->vid)) + goto nl_err; + + if (nla_put_u16(report_skb, VLAN_MON_ATTR_PROTO, n->proto)) + goto nl_err; + + if (nla_nest_end(report_skb, ns) >= VLAN_MON_NLMSG_SIZE || id == 255) { + genlmsg_end(report_skb, header); +#if LINUX_VERSION_CODE < KERNEL_VERSION(3,13,0) + genlmsg_multicast(report_skb, 0, vlan_mon_nl_mcg.id, GFP_KERNEL); +#else + genlmsg_multicast(&vlan_mon_nl_family, report_skb, 0, 0, GFP_KERNEL); +#endif + 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); +#if LINUX_VERSION_CODE < KERNEL_VERSION(3,13,0) + genlmsg_multicast(report_skb, 0, vlan_mon_nl_mcg.id, GFP_KERNEL); +#else + genlmsg_multicast(&vlan_mon_nl_family, report_skb, 0, 0, GFP_KERNEL); +#endif + } +} + +static int vlan_mon_nl_cmd_noop(struct sk_buff *skb, struct genl_info *info) +{ + struct sk_buff *msg; + void *hdr; + int ret = -ENOBUFS; + + msg = nlmsg_new(NLMSG_GOODSIZE, GFP_KERNEL); + if (!msg) { + ret = -ENOMEM; + goto out; + } + +#if LINUX_VERSION_CODE < KERNEL_VERSION(3,7,0) + hdr = genlmsg_put(msg, info->snd_pid, info->snd_seq, &vlan_mon_nl_family, 0, VLAN_MON_CMD_NOOP); +#else + hdr = genlmsg_put(msg, info->snd_portid, info->snd_seq, &vlan_mon_nl_family, 0, VLAN_MON_CMD_NOOP); +#endif + + if (IS_ERR(hdr)) { + ret = PTR_ERR(hdr); + goto err_out; + } + + genlmsg_end(msg, hdr); + +#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,32) + return genlmsg_unicast(msg, info->snd_pid); +#elif LINUX_VERSION_CODE < KERNEL_VERSION(3,7,0) + return genlmsg_unicast(genl_info_net(info), msg, info->snd_pid); +#else + return genlmsg_unicast(genl_info_net(info), msg, info->snd_portid); +#endif + +err_out: + nlmsg_free(msg); + +out: + return ret; +} + +static int vlan_mon_nl_cmd_add_vlan_mon(struct sk_buff *skb, struct genl_info *info) +{ + struct vlan_dev *d; + struct net_device *dev; + int ifindex, i, proto; + + if (!info->attrs[VLAN_MON_ATTR_IFINDEX]) + return -EINVAL; + + if (!info->attrs[VLAN_MON_ATTR_PROTO]) + return -EINVAL; + + proto = nla_get_u16(info->attrs[VLAN_MON_ATTR_PROTO]); + + proto = vlan_mon_proto(proto); + if (proto < 0) + return proto; + + ifindex = nla_get_u32(info->attrs[VLAN_MON_ATTR_IFINDEX]); + +#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,34) + rtnl_lock(); + dev = __dev_get_by_index(&init_net, ifindex); + rtnl_unlock(); +#else + dev = dev_get_by_index(&init_net, ifindex); +#endif + + if (!dev) + return -ENODEV; + + down(&vlan_mon_lock); + if (dev->ml_priv) { + d = (struct vlan_dev *)dev->ml_priv; + if (d->magic != VLAN_MON_MAGIC || (d->proto & (1 << proto))) { + up(&vlan_mon_lock); + dev_put(dev); + return -EBUSY; + } + } else { + d = kzalloc(sizeof(*d), GFP_KERNEL); + if (!d) { + up(&vlan_mon_lock); + dev_put(dev); + return -ENOMEM; + } + + spin_lock_init(&d->lock); + d->magic = VLAN_MON_MAGIC; + d->ifindex = ifindex; + d->proto = 0; + + rcu_assign_pointer(dev->ml_priv, d); + } + + d->proto |= 1 << proto; + + if (info->attrs[VLAN_MON_ATTR_VLAN_MASK]) { + memcpy(d->vid[proto], nla_data(info->attrs[VLAN_MON_ATTR_VLAN_MASK]), min((int)nla_len(info->attrs[VLAN_MON_ATTR_VLAN_MASK]), (int)sizeof(d->vid))); + +#if LINUX_VERSION_CODE < KERNEL_VERSION(3,10,0) + if (dev->features & NETIF_F_HW_VLAN_FILTER) { + rtnl_lock(); + for (i = 1; i < 4096; i++) { + if (!(d->vid[proto][i / (8*sizeof(long))] & (1lu << (i % (8*sizeof(long)))))) + dev->netdev_ops->ndo_vlan_rx_add_vid(dev, i); + } + rtnl_unlock(); + } +#else + if (dev->features & NETIF_F_HW_VLAN_CTAG_FILTER) { + rtnl_lock(); + for (i = 1; i < 4096; i++) { + if (!(d->vid[proto][i / (8*sizeof(long))] & (1lu << (i % (8*sizeof(long)))))) + dev->netdev_ops->ndo_vlan_rx_add_vid(dev, htons(ETH_P_8021Q), i); + } + rtnl_unlock(); + } +#endif + } + + list_add_tail_rcu(&d->entry, &vlan_devices); + up(&vlan_mon_lock); + + dev_put(dev); + + return 0; +} + +static int vlan_mon_nl_cmd_add_vlan_mon_vid(struct sk_buff *skb, struct genl_info *info) +{ + 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; + + 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 = nla_get_u16(info->attrs[VLAN_MON_ATTR_PROTO]); + + proto = vlan_mon_proto(proto); + if (proto < 0) + return proto; + +#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,34) + rtnl_lock(); + dev = __dev_get_by_index(&init_net, ifindex); + rtnl_unlock(); +#else + dev = dev_get_by_index(&init_net, ifindex); +#endif + + if (!dev) + return -ENODEV; + + down(&vlan_mon_lock); + + if (!dev->ml_priv) { + up(&vlan_mon_lock); + dev_put(dev); + return -EINVAL; + } + + d = dev->ml_priv; + + spin_lock_irqsave(&d->lock, flags); + d->vid[proto][vid / (8*sizeof(long))] &= ~(1lu << (vid % (8*sizeof(long)))); + spin_unlock_irqrestore(&d->lock, flags); +#if LINUX_VERSION_CODE < KERNEL_VERSION(3,10,0) + if (dev->features & NETIF_F_HW_VLAN_FILTER) { + rtnl_lock(); + dev->netdev_ops->ndo_vlan_rx_add_vid(dev, vid); + rtnl_unlock(); + } +#else + if (dev->features & NETIF_F_HW_VLAN_CTAG_FILTER) { + rtnl_lock(); + dev->netdev_ops->ndo_vlan_rx_add_vid(dev, htons(ETH_P_8021Q), vid); + rtnl_unlock(); + } +#endif + + up(&vlan_mon_lock); + + dev_put(dev); + + return 0; +} + +static int vlan_mon_nl_cmd_del_vlan_mon(struct sk_buff *skb, struct genl_info *info) +{ + struct vlan_dev *d; + struct vlan_notify *vn; + int ifindex, proto = 0xffff; + unsigned long flags; + struct list_head *pos, *n; + struct net_device *dev; + + if (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; + + proto = 1 << proto; + } + + if (info->attrs[VLAN_MON_ATTR_IFINDEX]) + ifindex = nla_get_u32(info->attrs[VLAN_MON_ATTR_IFINDEX]); + else + ifindex = -1; + + down(&vlan_mon_lock); + list_for_each_entry(d, &vlan_devices, entry) { + if ((ifindex == -1 || d->ifindex == ifindex) && (d->proto & proto)) { + //pr_info("del net %08x/%08x\n", n->addr, n->mask); +#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,34) + rtnl_lock(); + dev = __dev_get_by_index(&init_net, d->ifindex); + rtnl_unlock(); +#else + dev = dev_get_by_index(&init_net, d->ifindex); +#endif + + if (dev) { + if (dev->ml_priv == d) { + d->proto &= ~proto; + if (!d->proto) + rcu_assign_pointer(dev->ml_priv, NULL); + } + dev_put(dev); + } + + if (!d->proto) { + //pr_info("vlan_mon del %i\n", ifindex); + 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, vlan_mon_kfree_rcu); +#endif + } + } + } + up(&vlan_mon_lock); + + 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) && (proto & (1 << vlan_mon_proto(vn->proto)))) { + list_del(&vn->entry); + kfree(vn); + } + } + spin_unlock_irqrestore(&vlan_lock, flags); + + return 0; +} + +static struct nla_policy vlan_mon_nl_policy[VLAN_MON_ATTR_MAX + 1] = { + [VLAN_MON_ATTR_NONE] = { .type = NLA_UNSPEC, }, + [VLAN_MON_ATTR_VLAN_MASK] = { .type = NLA_BINARY, .len = 4096/8 }, + [VLAN_MON_ATTR_PROTO] = { .type = NLA_U16, }, + [VLAN_MON_ATTR_IFINDEX] = { .type = NLA_U32, }, + [VLAN_MON_ATTR_VID] = { .type = NLA_U16, }, +}; + +static struct genl_ops vlan_mon_nl_ops[] = { + { + .cmd = VLAN_MON_CMD_NOOP, + .doit = vlan_mon_nl_cmd_noop, + .policy = vlan_mon_nl_policy, + /* can be retrieved by unprivileged users */ + }, + { + .cmd = VLAN_MON_CMD_ADD, + .doit = vlan_mon_nl_cmd_add_vlan_mon, + .policy = vlan_mon_nl_policy, + .flags = GENL_ADMIN_PERM, + }, + { + .cmd = VLAN_MON_CMD_ADD_VID, + .doit = vlan_mon_nl_cmd_add_vlan_mon_vid, + .policy = vlan_mon_nl_policy, + .flags = GENL_ADMIN_PERM, + }, + { + .cmd = VLAN_MON_CMD_DEL, + .doit = vlan_mon_nl_cmd_del_vlan_mon, + .policy = vlan_mon_nl_policy, + .flags = GENL_ADMIN_PERM, + }, +}; + +static struct genl_family vlan_mon_nl_family = { + .id = GENL_ID_GENERATE, + .name = VLAN_MON_GENL_NAME, + .version = VLAN_MON_GENL_VERSION, + .hdrsize = 0, + .maxattr = VLAN_MON_ATTR_MAX, +}; + +#if LINUX_VERSION_CODE < KERNEL_VERSION(3,13,0) +static struct genl_multicast_group vlan_mon_nl_mcg = { + .name = VLAN_MON_GENL_MCG, +}; +#else +static struct genl_multicast_group vlan_mon_nl_mcgs[] = { + { .name = VLAN_MON_GENL_MCG, } +}; +#endif + +static struct packet_type vlan_pt __read_mostly = { + .type = __constant_htons(ETH_P_ALL), + .func = vlan_pt_recv, +}; + +static int __init vlan_mon_init(void) +{ + int err; +#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,35) + int i; +#endif + + + printk("vlan-mon driver v1.10-rc1\n"); + + INIT_WORK(&vlan_notify_work, vlan_do_notify); + +#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,35) + err = genl_register_family(&vlan_mon_nl_family); + if (err < 0) { + printk(KERN_INFO "vlan_mon: can't register netlink interface\n"); + goto out; + } + + for (i = 0; i < ARRAY_SIZE(vlan_mon_nl_ops); i++) { + err = genl_register_ops(&vlan_mon_nl_family, &vlan_mon_nl_ops[i]); + if (err) + break; + } + + if (err < 0) { + printk(KERN_INFO "vlan_mon: can't register netlink interface\n"); + goto out_unreg; + } +#else +#if LINUX_VERSION_CODE < KERNEL_VERSION(3,13,0) + err = genl_register_family_with_ops(&vlan_mon_nl_family, vlan_mon_nl_ops, ARRAY_SIZE(vlan_mon_nl_ops)); +#else + err = genl_register_family_with_ops_groups(&vlan_mon_nl_family, vlan_mon_nl_ops, vlan_mon_nl_mcgs); +#endif + if (err < 0) { + printk(KERN_INFO "vlan_mon: can't register netlink interface\n"); + goto out; + } +#endif + +#if LINUX_VERSION_CODE < KERNEL_VERSION(3,13,0) + err = genl_register_mc_group(&vlan_mon_nl_family, &vlan_mon_nl_mcg); + if (err < 0) { + printk(KERN_INFO "vlan_mon: can't register netlink multicast group\n"); + goto out_unreg; + } +#endif + + dev_add_pack(&vlan_pt); + + return 0; + +#if LINUX_VERSION_CODE < KERNEL_VERSION(3,13,0) +out_unreg: +#endif + genl_unregister_family(&vlan_mon_nl_family); +out: + return err; +} + +static void __exit vlan_mon_fini(void) +{ + struct vlan_dev *d; + struct vlan_notify *vn; + struct net_device *dev; + + dev_remove_pack(&vlan_pt); + +#if LINUX_VERSION_CODE < KERNEL_VERSION(3,13,0) + genl_unregister_mc_group(&vlan_mon_nl_family, &vlan_mon_nl_mcg); +#endif + genl_unregister_family(&vlan_mon_nl_family); + + down(&vlan_mon_lock); + up(&vlan_mon_lock); + + while (!list_empty(&vlan_devices)) { + d = list_first_entry(&vlan_devices, typeof(*d), entry); +#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,34) + rtnl_lock(); + dev = __dev_get_by_index(&init_net, d->ifindex); + rtnl_unlock(); +#else + dev = dev_get_by_index(&init_net, d->ifindex); +#endif + if (dev) + rcu_assign_pointer(dev->ml_priv, NULL); + list_del(&d->entry); +#if LINUX_VERSION_CODE >= KERNEL_VERSION(3,0,0) + kfree_rcu(d, rcu_head); +#else + call_rcu(&d->rcu_head, vlan_mon_kfree_rcu); +#endif + } + + while (!list_empty(&vlan_notifies)) { + vn = list_first_entry(&vlan_notifies, typeof(*vn), entry); + list_del(&vn->entry); + kfree(vn); + } + + synchronize_rcu(); +} + +module_init(vlan_mon_init); +module_exit(vlan_mon_fini); +MODULE_LICENSE("GPL"); + diff --git a/drivers/vlan_mon/vlan_mon.h b/drivers/vlan_mon/vlan_mon.h new file mode 100644 index 0000000..e65f65e --- /dev/null +++ b/drivers/vlan_mon/vlan_mon.h @@ -0,0 +1,36 @@ +#ifndef __LINUX_VLAN_MON_H +#define __LINUX_VLAN_MON_H + +#include + +enum { + VLAN_MON_CMD_NOOP, + VLAN_MON_CMD_ADD, + VLAN_MON_CMD_ADD_VID, + VLAN_MON_CMD_DEL, + VLAN_MON_NOTIFY, + __VLAN_MON_CMD_MAX, +}; + +#define VLAN_MON_CMD_MAX (__VLAN_MON_CMD_MAX - 1) + +enum { + VLAN_MON_ATTR_NONE, /* u32 */ + VLAN_MON_ATTR_VLAN_MASK, /* u32 */ + VLAN_MON_ATTR_PROTO, /* u32 */ + VLAN_MON_ATTR_IFINDEX, /* u32 */ + VLAN_MON_ATTR_VID, /* u32 */ + __VLAN_MON_ATTR_MAX, +}; + +#define VLAN_MON_ATTR_MAX (__VLAN_MON_ATTR_MAX - 1) + +/* + * NETLINK_GENERIC related info + */ +#define VLAN_MON_GENL_NAME "vlan-mon" +#define VLAN_MON_GENL_MCG "notify" +#define VLAN_MON_GENL_VERSION 0x01 + +#endif + -- cgit v1.2.3