diff options
-rw-r--r-- | CMakeLists.txt | 4 | ||||
-rw-r--r-- | accel-pppd/CMakeLists.txt | 1 | ||||
-rw-r--r-- | accel-pppd/accel-ppp.conf | 1 | ||||
-rw-r--r-- | accel-pppd/ctrl/ipoe/ipoe.c | 66 | ||||
-rw-r--r-- | accel-pppd/ctrl/ipoe/ipoe.h | 4 | ||||
-rw-r--r-- | accel-pppd/ctrl/ipoe/ipoe_netlink.c | 139 | ||||
-rw-r--r-- | accel-pppd/include/vlan_mon.h | 15 | ||||
-rw-r--r-- | accel-pppd/vlan-mon/CMakeLists.txt | 8 | ||||
l--------- | accel-pppd/vlan-mon/if_vlan_mon.h | 1 | ||||
-rw-r--r-- | accel-pppd/vlan-mon/vlan_mon.c | 348 | ||||
-rw-r--r-- | drivers/ipoe/ipoe.c | 410 | ||||
-rw-r--r-- | drivers/ipoe/ipoe.h | 5 | ||||
-rw-r--r-- | drivers/vlan_mon/CMakeLists.txt | 17 | ||||
-rw-r--r-- | drivers/vlan_mon/Makefile | 4 | ||||
-rw-r--r-- | drivers/vlan_mon/vlan_mon.c | 664 | ||||
-rw-r--r-- | drivers/vlan_mon/vlan_mon.h | 36 |
16 files changed, 1119 insertions, 604 deletions
diff --git a/CMakeLists.txt b/CMakeLists.txt index 53cdc393..c9872890 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -76,4 +76,8 @@ if (BUILD_IPOE_DRIVER) add_subdirectory(drivers/ipoe) endif (BUILD_IPOE_DRIVER) +if (BUILD_VLAN_MON_DRIVER) + add_subdirectory(drivers/vlan_mon) +endif () + CONFIGURE_FILE(${CMAKE_CURRENT_SOURCE_DIR}/config.h.in ${CMAKE_CURRENT_BINARY_DIR}/config.h) diff --git a/accel-pppd/CMakeLists.txt b/accel-pppd/CMakeLists.txt index aa13e550..ab96bc01 100644 --- a/accel-pppd/CMakeLists.txt +++ b/accel-pppd/CMakeLists.txt @@ -32,6 +32,7 @@ ENDIF (RADIUS) ADD_SUBDIRECTORY(triton) +ADD_SUBDIRECTORY(vlan-mon) ADD_SUBDIRECTORY(ctrl) ADD_SUBDIRECTORY(auth) ADD_SUBDIRECTORY(logs) diff --git a/accel-pppd/accel-ppp.conf b/accel-pppd/accel-ppp.conf index 81d176d6..70dbef8d 100644 --- a/accel-pppd/accel-ppp.conf +++ b/accel-pppd/accel-ppp.conf @@ -25,6 +25,7 @@ pppd_compat #net-snmp #logwtmp #connlimit +#vlan-mon #ipv6_nd #ipv6_dhcp diff --git a/accel-pppd/ctrl/ipoe/ipoe.c b/accel-pppd/ctrl/ipoe/ipoe.c index 594df29e..d77da31f 100644 --- a/accel-pppd/ctrl/ipoe/ipoe.c +++ b/accel-pppd/ctrl/ipoe/ipoe.c @@ -33,6 +33,7 @@ #include "ipset.h" #include "connlimit.h" +#include "vlan_mon.h" #include "ipoe.h" @@ -2082,7 +2083,7 @@ static void ipoe_serv_release(struct ipoe_serv *serv) if (serv->vid) { log_info2("ipoe: remove vlan %s\n", serv->ifname); iplink_vlan_del(serv->ifindex); - ipoe_nl_add_vlan_mon_vid(serv->parent_ifindex, serv->vid); + vlan_mon_add_vid(serv->parent_ifindex, ETH_P_IP, serv->vid); } triton_context_unregister(&serv->ctx); @@ -2205,43 +2206,7 @@ static int get_offer_delay() return 0; } -static int make_vlan_name(const char *parent, int svid, int cvid, char *name) -{ - char *ptr1 = name, *endptr = name + IFNAMSIZ; - const char *ptr2 = conf_vlan_name; - char svid_str[5], cvid_str[5], *ptr3; - - sprintf(svid_str, "%i", svid); - sprintf(cvid_str, "%i", cvid); - - while (ptr1 < endptr && *ptr2) { - if (ptr2[0] == '%' && ptr2[1] == 'I') { - while (ptr1 < endptr && *parent) - *ptr1++ = *parent++; - ptr2 += 2; - } else if (ptr2[0] == '%' && ptr2[1] == 'N') { - ptr3 = cvid_str; - while (ptr1 < endptr && *ptr3) - *ptr1++ = *ptr3++; - ptr2 += 2; - } else if (ptr2[0] == '%' && ptr2[1] == 'P') { - ptr3 = svid_str; - while (ptr1 < endptr && *ptr3) - *ptr1++ = *ptr3++; - ptr2 += 2; - } else - *ptr1++ = *ptr2++; - } - - if (ptr1 == endptr) - return 1; - - *ptr1 = 0; - - return 0; -} - -void ipoe_vlan_notify(int ifindex, int vid) +void ipoe_vlan_mon_notify(int ifindex, int vid) { struct conf_sect_t *sect = conf_get_section("ipoe"); struct conf_option_t *opt; @@ -2266,20 +2231,18 @@ void ipoe_vlan_notify(int ifindex, int vid) svid = iplink_vlan_get_vid(ifindex); - if (make_vlan_name(ifr.ifr_name, svid, vid, ifname)) { + if (make_vlan_name(conf_vlan_name, ifr.ifr_name, svid, vid, ifname)) { log_error("ipoe: vlan-mon: %s.%i: interface name is too long\n", ifr.ifr_name, vid); return; } - log_info2("ipoe: create vlan %s parent %s\n", ifname, ifr.ifr_name); - strcpy(ifr.ifr_name, ifname); len = strlen(ifr.ifr_name); - if (iplink_vlan_add(ifr.ifr_name, ifindex, vid)) { - log_warn("ipoe: vlan-mon: %s: failed to add vlan\n", ifr.ifr_name); + if (iplink_vlan_add(ifr.ifr_name, ifindex, vid)) return; - } + + log_info2("ipoe: create vlan %s parent %s\n", ifname, ifr.ifr_name); ioctl(sock_fd, SIOCGIFFLAGS, &ifr, sizeof(ifr)); ifr.ifr_flags |= IFF_UP; @@ -3012,7 +2975,7 @@ static void add_vlan_mon(const char *opt, long *mask) mask1[serv->vid / (8*sizeof(long))] |= 1lu << (serv->vid % (8*sizeof(long))); } - ipoe_nl_add_vlan_mon(ifindex, mask1, sizeof(mask1)); + vlan_mon_add(ifindex, ETH_P_IP, mask1, sizeof(mask1)); } static int __load_vlan_mon_re(int index, int flags, const char *name, struct iplink_arg *arg) @@ -3041,7 +3004,7 @@ static int __load_vlan_mon_re(int index, int flags, const char *name, struct ipl mask1[serv->vid / (8*sizeof(long))] |= 1lu << (serv->vid % (8*sizeof(long))); } - ipoe_nl_add_vlan_mon(index, mask1, sizeof(mask1)); + vlan_mon_add(index, ETH_P_IP, mask1, sizeof(mask1)); return 0; } @@ -3083,8 +3046,17 @@ static void load_vlan_mon(struct conf_sect_t *sect) { struct conf_option_t *opt; long mask[4096/8/sizeof(long)]; + static int registered = 0; + + if (!triton_module_loaded("vlan-mon")) + return; + + if (!registered) { + vlan_mon_register_proto(ETH_P_IP, ipoe_vlan_mon_notify); + registered = 1; + } - ipoe_nl_del_vlan_mon(-1); + vlan_mon_del(-1, ETH_P_IP); list_for_each_entry(opt, §->items, entry) { if (strcmp(opt->name, "vlan-mon")) diff --git a/accel-pppd/ctrl/ipoe/ipoe.h b/accel-pppd/ctrl/ipoe/ipoe.h index 1405092e..224b9e91 100644 --- a/accel-pppd/ctrl/ipoe/ipoe.h +++ b/accel-pppd/ctrl/ipoe/ipoe.h @@ -114,7 +114,6 @@ struct iphdr; struct ethhdr; void ipoe_recv_up(int ifindex, struct ethhdr *eth, struct iphdr *iph); -void ipoe_vlan_notify(int ifindex, int vid); struct ipoe_session *ipoe_session_alloc(void); @@ -128,9 +127,6 @@ int ipoe_nl_create(uint32_t peer_addr, uint32_t addr, const char *ifname, uint8_ void ipoe_nl_delete(int ifindex); int ipoe_nl_modify(int ifindex, uint32_t peer_addr, uint32_t addr, const char *ifname, uint8_t *hwaddr); void ipoe_nl_get_sessions(struct list_head *list); -int ipoe_nl_add_vlan_mon(int ifindex, long *mask, int len); -int ipoe_nl_add_vlan_mon_vid(int ifindex, int vid); -int ipoe_nl_del_vlan_mon(int ifindex); int ipoe_nl_add_exclude(uint32_t addr, int mask); void ipoe_nl_del_exclude(uint32_t addr); diff --git a/accel-pppd/ctrl/ipoe/ipoe_netlink.c b/accel-pppd/ctrl/ipoe/ipoe_netlink.c index c0610367..18542cde 100644 --- a/accel-pppd/ctrl/ipoe/ipoe_netlink.c +++ b/accel-pppd/ctrl/ipoe/ipoe_netlink.c @@ -448,106 +448,6 @@ 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_add_vlan_mon: error talking to kernel\n"); - return -1; - } - - return 0; -} - -int ipoe_nl_add_vlan_mon_vid(int ifindex, int vid) -{ - struct rtnl_handle rth; - 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_add_vlan_mon_vid: error talking to kernel\n"); - r = -1; - } - - rtnl_close(&rth); - - return r; -} - - -int ipoe_nl_del_vlan_mon(int ifindex) -{ - 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_del_vlan_mon: 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]; @@ -587,43 +487,6 @@ static void ipoe_up_handler(const struct sockaddr_nl *addr, struct nlmsghdr *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; @@ -685,8 +548,6 @@ static int ipoe_mc_read(struct triton_md_handler_t *h) 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)); diff --git a/accel-pppd/include/vlan_mon.h b/accel-pppd/include/vlan_mon.h new file mode 100644 index 00000000..928c732e --- /dev/null +++ b/accel-pppd/include/vlan_mon.h @@ -0,0 +1,15 @@ +#ifndef __VLAN_MON_H +#define __VLAN_MON_H + +typedef void (*vlan_mon_notify)(int ifindex, int vid); + +void vlan_mon_register_proto(int proto, vlan_mon_notify cb); + +int vlan_mon_add(int ifindex, int proto, long *mask, int len); +int vlan_mon_add_vid(int ifindex, int proto, int vid); +int vlan_mon_del(int ifindex, int proto); + +int make_vlan_name(const char *pattern, const char *parent, int svid, int cvid, char *name); + + +#endif diff --git a/accel-pppd/vlan-mon/CMakeLists.txt b/accel-pppd/vlan-mon/CMakeLists.txt new file mode 100644 index 00000000..1305825a --- /dev/null +++ b/accel-pppd/vlan-mon/CMakeLists.txt @@ -0,0 +1,8 @@ +#INCLUDE_DIRECTORIES(${CMAKE_CURRENT_SOURCE_DIR}) + +ADD_LIBRARY(vlan-mon SHARED vlan_mon.c) + +INSTALL(TARGETS vlan-mon + LIBRARY DESTINATION lib${LIB_SUFFIX}/accel-ppp +) + diff --git a/accel-pppd/vlan-mon/if_vlan_mon.h b/accel-pppd/vlan-mon/if_vlan_mon.h new file mode 120000 index 00000000..a1ad472b --- /dev/null +++ b/accel-pppd/vlan-mon/if_vlan_mon.h @@ -0,0 +1 @@ +../../drivers/vlan_mon/vlan_mon.h
\ No newline at end of file diff --git a/accel-pppd/vlan-mon/vlan_mon.c b/accel-pppd/vlan-mon/vlan_mon.c new file mode 100644 index 00000000..898d7dcd --- /dev/null +++ b/accel-pppd/vlan-mon/vlan_mon.c @@ -0,0 +1,348 @@ +#include <unistd.h> +#include <stdlib.h> +#include <stdio.h> +#include <errno.h> +#include <string.h> +#include <pthread.h> +#include <fcntl.h> +#include <sys/socket.h> +#include <sys/ioctl.h> +#include <net/ethernet.h> +#include <netinet/ip.h> +#include <arpa/inet.h> +#include <linux/if.h> +#include <linux/genetlink.h> + +#include "triton.h" +#include "log.h" +#include "genl.h" +#include "libnetlink.h" +#include "iputils.h" + +#include "vlan_mon.h" +#include "if_vlan_mon.h" + +#include "memdebug.h" + +#define PKT_ATTR_MAX 256 + +static struct rtnl_handle rth; +static struct triton_md_handler_t mc_hnd; +static int vlan_mon_genl_id; + +static vlan_mon_notify cb[2]; + +static void init(void); + +void __export vlan_mon_register_proto(int proto, vlan_mon_notify func) +{ + if (proto == ETH_P_PPP_DISC) + proto = 1; + else + proto = 0; + + cb[proto] = func; + + if (!vlan_mon_genl_id) + init(); +} + +int __export vlan_mon_add(int ifindex, int proto, 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 = vlan_mon_genl_id; + + ghdr = NLMSG_DATA(&req.n); + ghdr->cmd = VLAN_MON_CMD_ADD; + + addattr32(nlh, 1024, VLAN_MON_ATTR_IFINDEX, ifindex); + addattr_l(nlh, 1024, VLAN_MON_ATTR_VLAN_MASK, mask, len); + addattr32(nlh, 1024, VLAN_MON_ATTR_PROTO, proto); + + if (rtnl_talk(&rth, nlh, 0, 0, nlh, NULL, NULL, 0) < 0 ) { + log_error("vlan_mon: nl_add_vlan_mon: error talking to kernel\n"); + return -1; + } + + return 0; +} + +int __export vlan_mon_add_vid(int ifindex, int proto, int vid) +{ + struct nlmsghdr *nlh; + struct genlmsghdr *ghdr; + struct { + struct nlmsghdr n; + char buf[1024]; + } req; + int r = 0; + + 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 = vlan_mon_genl_id; + + ghdr = NLMSG_DATA(&req.n); + ghdr->cmd = VLAN_MON_CMD_ADD_VID; + + addattr32(nlh, 1024, VLAN_MON_ATTR_IFINDEX, ifindex); + addattr32(nlh, 1024, VLAN_MON_ATTR_VID, vid); + addattr32(nlh, 1024, VLAN_MON_ATTR_PROTO, proto); + + 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; + } + + return r; +} + +int __export vlan_mon_del(int ifindex, int proto) +{ + 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 = vlan_mon_genl_id; + + ghdr = NLMSG_DATA(&req.n); + ghdr->cmd = VLAN_MON_CMD_DEL; + + addattr32(nlh, 1024, VLAN_MON_ATTR_IFINDEX, ifindex); + addattr32(nlh, 1024, VLAN_MON_ATTR_PROTO, proto); + + if (rtnl_talk(&rth, nlh, 0, 0, nlh, NULL, NULL, 0) < 0 ) { + log_error("vlan_mon: nl_del_vlan_mon: error talking to kernel\n"); + return -1; + } + + return 0; +} + +static void vlan_mon_handler(const struct sockaddr_nl *addr, struct nlmsghdr *h) +{ + struct rtattr *tb[PKT_ATTR_MAX + 1]; + struct rtattr *tb2[VLAN_MON_ATTR_MAX + 1]; + struct genlmsghdr *ghdr = NLMSG_DATA(h); + int len = h->nlmsg_len; + struct rtattr *attrs; + int i; + int ifindex, vid, proto; + + len -= NLMSG_LENGTH(GENL_HDRLEN); + + if (len < 0) { + log_warn("vlan_mon: 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, VLAN_MON_ATTR_MAX, tb[i]); + + //if (!tb2[VLAN_MON_ATTR_IFINDEX] || !tb2[VLAN_MON_ATTR_VID] || !t) + // continue; + + ifindex = *(uint32_t *)(RTA_DATA(tb2[VLAN_MON_ATTR_IFINDEX])); + 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 (proto == ETH_P_PPP_DISC) + proto = 1; + else + proto = 0; + + if (cb[proto]) + cb[proto](ifindex, vid); + } +} + + +static int vlan_mon_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 = { + .msg_name = &nladdr, + .msg_namelen = sizeof(nladdr), + .msg_iov = &iov, + .msg_iovlen = 1, + }; + char buf[8192]; + + memset(&nladdr, 0, sizeof(nladdr)); + nladdr.nl_family = AF_NETLINK; + nladdr.nl_pid = 0; + nladdr.nl_groups = 0; + + iov.iov_base = buf; + while (1) { + iov.iov_len = sizeof(buf); + status = recvmsg(h->fd, &msg, 0); + + if (status < 0) { + if (errno == EAGAIN) + break; + log_error("vlan_mon: netlink error: %s\n", strerror(errno)); + if (errno == ENOBUFS) + continue; + return 0; + } + + if (status == 0) { + log_error("vlan_mon: EOF on netlink\n"); + return 0; + } + + if (msg.msg_namelen != sizeof(nladdr)) { + log_error("vlan_mon: 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); + + if (l<0 || len>status) { + if (msg.msg_flags & MSG_TRUNC) { + log_warn("vlan_mon: truncated netlink message\n"); + continue; + } + log_error("vlan_mon: malformed netlink message\n"); + continue; + } + + ghdr = NLMSG_DATA(hdr); + + if (ghdr->cmd == VLAN_MON_NOTIFY) + 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("vlan_mon: netlink message truncated\n"); + continue; + } + + if (status) { + log_error("vlan_mon: netlink remnant of size %d\n", status); + return 0; + } + } + + return 0; +} + +int __export make_vlan_name(const char *pattern, const char *parent, int svid, int cvid, char *name) +{ + char *ptr1 = name, *endptr = name + IFNAMSIZ; + const char *ptr2 = pattern; + char svid_str[5], cvid_str[5], *ptr3; + + sprintf(svid_str, "%i", svid); + sprintf(cvid_str, "%i", cvid); + + while (ptr1 < endptr && *ptr2) { + if (ptr2[0] == '%' && ptr2[1] == 'I') { + while (ptr1 < endptr && *parent) + *ptr1++ = *parent++; + ptr2 += 2; + } else if (ptr2[0] == '%' && ptr2[1] == 'N') { + ptr3 = cvid_str; + while (ptr1 < endptr && *ptr3) + *ptr1++ = *ptr3++; + ptr2 += 2; + } else if (ptr2[0] == '%' && ptr2[1] == 'P') { + ptr3 = svid_str; + while (ptr1 < endptr && *ptr3) + *ptr1++ = *ptr3++; + ptr2 += 2; + } else + *ptr1++ = *ptr2++; + } + + if (ptr1 == endptr) + return 1; + + *ptr1 = 0; + + return 0; +} + + +static void vlan_mon_mc_close(struct triton_context_t *ctx) +{ + triton_md_unregister_handler(&mc_hnd, 0); + triton_context_unregister(ctx); +} + +static struct triton_context_t mc_ctx = { + .close = vlan_mon_mc_close, +}; + +static struct triton_md_handler_t mc_hnd = { + .read = vlan_mon_mc_read, +}; + +static void init(void) +{ + int mcg_id = genl_resolve_mcg(VLAN_MON_GENL_NAME, VLAN_MON_GENL_MCG, &vlan_mon_genl_id); + if (mcg_id == -1) { + log_warn("vlan_mon: kernel module is not loaded\n"); + rth.fd = -1; + vlan_mon_genl_id = -1; + return; + } + + if (rtnl_open_byproto(&rth, 1 << (mcg_id - 1), NETLINK_GENERIC)) { + log_error("vlan_mon: cannot open generic netlink socket\n"); + rth.fd = -1; + return; + } + + fcntl(rth.fd, F_SETFL, O_NONBLOCK); + fcntl(rth.fd, F_SETFD, fcntl(rth.fd, F_GETFD) | FD_CLOEXEC); + + 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); +} + diff --git a/drivers/ipoe/ipoe.c b/drivers/ipoe/ipoe.c index 3fc266e6..d7de5e62 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[] = { @@ -2061,24 +1709,6 @@ static struct genl_ops ipoe_nl_ops[] = { .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, .policy = ipoe_nl_policy, @@ -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 192fa2f2..e0cfc4e5 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 00000000..892fed16 --- /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 00000000..c70e47a7 --- /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 00000000..f61d9324 --- /dev/null +++ b/drivers/vlan_mon/vlan_mon.c @@ -0,0 +1,664 @@ +#include <linux/capability.h> +#include <linux/module.h> +#include <linux/types.h> +#include <linux/kernel.h> +#include <linux/slab.h> +#include <linux/skbuff.h> +#include <linux/netdevice.h> +#include <linux/in.h> +#include <linux/init.h> +#include <linux/if_ether.h> +#include <linux/if_vlan.h> +#include <linux/semaphore.h> +#include <linux/version.h> + +#include <net/genetlink.h> +#include <net/sock.h> +#include <net/net_namespace.h> +#include <net/netns/generic.h> + +#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 00000000..e65f65ee --- /dev/null +++ b/drivers/vlan_mon/vlan_mon.h @@ -0,0 +1,36 @@ +#ifndef __LINUX_VLAN_MON_H +#define __LINUX_VLAN_MON_H + +#include <linux/types.h> + +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 + |