summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--CMakeLists.txt4
-rw-r--r--accel-pppd/CMakeLists.txt1
-rw-r--r--accel-pppd/accel-ppp.conf6
-rw-r--r--accel-pppd/ctrl/ipoe/ipoe.c130
-rw-r--r--accel-pppd/ctrl/ipoe/ipoe.h4
-rw-r--r--accel-pppd/ctrl/ipoe/ipoe_netlink.c139
-rw-r--r--accel-pppd/ctrl/pppoe/pppoe.c327
-rw-r--r--accel-pppd/ctrl/pppoe/pppoe.h4
-rw-r--r--accel-pppd/include/vlan_mon.h15
-rw-r--r--accel-pppd/vlan-mon/CMakeLists.txt8
l---------accel-pppd/vlan-mon/if_vlan_mon.h1
-rw-r--r--accel-pppd/vlan-mon/vlan_mon.c401
-rw-r--r--drivers/ipoe/ipoe.c410
-rw-r--r--drivers/ipoe/ipoe.h5
-rw-r--r--drivers/vlan_mon/CMakeLists.txt17
-rw-r--r--drivers/vlan_mon/Makefile4
-rw-r--r--drivers/vlan_mon/vlan_mon.c671
-rw-r--r--drivers/vlan_mon/vlan_mon.h36
18 files changed, 1509 insertions, 674 deletions
diff --git a/CMakeLists.txt b/CMakeLists.txt
index a813b95..93889cd 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 aa13e55..ab96bc0 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 81d176d..fc0fc76 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
@@ -77,8 +78,11 @@ called-sid=mac
#tr101=1
#padi-limit=0
#ip-pool=pppoe
-#interface=eth1,padi-limit=1000
#sid-uppercase=0
+#vlan-mon=eth0,10-200
+#vlan-timeout=60
+#vlan-name=%I.%N
+#interface=eth1,padi-limit=1000
interface=eth0
[l2tp]
diff --git a/accel-pppd/ctrl/ipoe/ipoe.c b/accel-pppd/ctrl/ipoe/ipoe.c
index 95b7533..05a9ff5 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"
@@ -2119,7 +2120,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);
@@ -2255,48 +2256,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;
-
-#ifdef USE_LUA
- if (!memcmp(conf_vlan_name, "lua:", 4))
- return ipoe_lua_make_vlan_name(conf_vlan_name + 4, parent, svid, cvid, name);
-#endif
-
- 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;
@@ -2321,20 +2281,24 @@ void ipoe_vlan_notify(int ifindex, int vid)
svid = iplink_vlan_get_vid(ifindex);
- if (make_vlan_name(ifr.ifr_name, svid, vid, ifname)) {
+#ifdef USE_LUA
+ if (!memcmp(conf_vlan_name, "lua:", 4))
+ r = ipoe_lua_make_vlan_name(conf_vlan_name + 4, parent, svid, cvid, name);
+ else
+#endif
+ r = make_vlan_name(conf_vlan_name, ifr.ifr_name, svid, vid, ifname);
+ if (r) {
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;
@@ -2993,59 +2957,6 @@ out_err:
return -1;
}
-static int parse_vlan_mon(const char *opt, long *mask)
-{
- char *ptr, *ptr2;
- int vid, vid2;
-
- ptr = strchr(opt, ',');
- if (!ptr)
- ptr = strchr(opt, 0);
-
- if (*ptr == ',')
- memset(mask, 0xff, 4096/8);
- else if (*ptr == 0) {
- memset(mask, 0, 4096/8);
- return 0;
- } else
- goto out_err;
-
- while (1) {
- vid = strtol(ptr + 1, &ptr2, 10);
- if (vid <= 0 || vid >= 4096) {
- log_error("ipoe: vlan-mon=%s: invalid vlan %i\n", opt, vid);
- return -1;
- }
-
- if (*ptr2 == '-') {
- vid2 = strtol(ptr2 + 1, &ptr2, 10);
- if (vid2 <= 0 || vid2 >= 4096) {
- log_error("ipoe: vlan-mon=%s: invalid vlan %i\n", opt, vid2);
- return -1;
- }
-
- for (; vid < vid2; vid++)
- mask[vid / (8*sizeof(long))] &= ~(1lu << (vid % (8*sizeof(long))));
- }
-
- mask[vid / (8*sizeof(long))] &= ~(1lu << (vid % (8*sizeof(long))));
-
- if (*ptr2 == 0)
- break;
-
- if (*ptr2 != ',')
- goto out_err;
-
- ptr = ptr2;
- }
-
- return 0;
-
-out_err:
- log_error("ipoe: vlan-mon=%s: failed to parse\n", opt);
- return -1;
-}
-
static void add_vlan_mon(const char *opt, long *mask)
{
const char *ptr;
@@ -3087,7 +2998,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)
@@ -3116,7 +3027,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;
}
@@ -3158,8 +3069,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, &sect->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 036eda8..cd1993d 100644
--- a/accel-pppd/ctrl/ipoe/ipoe.h
+++ b/accel-pppd/ctrl/ipoe/ipoe.h
@@ -115,7 +115,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);
@@ -129,9 +128,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 c061036..18542cd 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/ctrl/pppoe/pppoe.c b/accel-pppd/ctrl/pppoe/pppoe.c
index 995a60d..fc8a032 100644
--- a/accel-pppd/ctrl/pppoe/pppoe.c
+++ b/accel-pppd/ctrl/pppoe/pppoe.c
@@ -28,6 +28,7 @@
#include "iputils.h"
#include "connlimit.h"
+#include "vlan_mon.h"
#include "pppoe.h"
@@ -74,11 +75,11 @@ struct padi_t
uint8_t addr[ETH_ALEN];
};
-struct iplink_arg
-{
+struct iplink_arg {
pcre *re;
const char *opt;
void *cli;
+ long *arg1;
};
int conf_verbose;
@@ -94,6 +95,8 @@ static const char *conf_ip_pool;
enum {CSID_MAC, CSID_IFNAME, CSID_IFNAME_MAC};
static int conf_called_sid;
static int conf_cookie_timeout;
+static const char *conf_vlan_name;
+static int conf_vlan_timeout;
static mempool_t conn_pool;
static mempool_t pado_pool;
@@ -125,7 +128,30 @@ static uint8_t bc_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
static void pppoe_send_PADT(struct pppoe_conn_t *conn);
void pppoe_server_free(struct pppoe_serv_t *serv);
static int init_secret(struct pppoe_serv_t *serv);
-static void __pppoe_server_start(const char *ifname, const char *opt, void *cli);
+static void __pppoe_server_start(const char *ifname, const char *opt, void *cli, int parent_ifindex, int vid);
+static void pppoe_serv_timeout(struct triton_timer_t *t);
+
+static void pppoe_serv_start_timer(struct pppoe_serv_t *serv)
+{
+ pthread_mutex_lock(&serv->lock);
+ if (serv->conn_cnt) {
+ pthread_mutex_unlock(&serv->lock);
+ return;
+ }
+
+ if (conf_vlan_timeout) {
+ serv->timer.expire = pppoe_serv_timeout;
+ serv->timer.expire_tv.tv_sec = conf_vlan_timeout;
+ if (serv->timer.tpd)
+ triton_timer_mod(&serv->timer, 0);
+ else
+ triton_timer_add(&serv->ctx, &serv->timer, 0);
+ pthread_mutex_unlock(&serv->lock);
+ } else {
+ pthread_mutex_unlock(&serv->lock);
+ pppoe_server_free(serv);
+ }
+}
static void disconnect(struct pppoe_conn_t *conn)
{
@@ -144,9 +170,15 @@ static void disconnect(struct pppoe_conn_t *conn)
pthread_mutex_lock(&conn->serv->lock);
list_del(&conn->entry);
conn->serv->conn_cnt--;
- if (conn->serv->stopping && conn->serv->conn_cnt == 0) {
- pthread_mutex_unlock(&conn->serv->lock);
- pppoe_server_free(conn->serv);
+ if (conn->serv->conn_cnt == 0) {
+ if (conn->serv->stopping) {
+ pthread_mutex_unlock(&conn->serv->lock);
+ pppoe_server_free(conn->serv);
+ } else if (conn->serv->vid) {
+ triton_context_call(&conn->serv->ctx, (triton_event_func)pppoe_serv_start_timer, conn->serv);
+ pthread_mutex_unlock(&conn->serv->lock);
+ } else
+ pthread_mutex_unlock(&conn->serv->lock);
} else
pthread_mutex_unlock(&conn->serv->lock);
@@ -1176,6 +1208,21 @@ static void pppoe_serv_close(struct triton_context_t *ctx)
pthread_mutex_unlock(&serv->lock);
}
+static void pppoe_serv_timeout(struct triton_timer_t *t)
+{
+ struct pppoe_serv_t *serv = container_of(t, typeof(*serv), timer);
+
+ pthread_mutex_lock(&serv->lock);
+ if (serv->conn_cnt) {
+ triton_timer_del(&serv->timer);
+ pthread_mutex_unlock(&serv->lock);
+ return;
+ }
+
+ pthread_mutex_unlock(&serv->lock);
+ pppoe_server_free(serv);
+}
+
static int parse_server(const char *opt, int *padi_limit)
{
char *ptr, *endptr;
@@ -1198,7 +1245,7 @@ static int __pppoe_add_interface_re(int index, int flags, const char *name, stru
if (pcre_exec(arg->re, NULL, name, strlen(name), 0, 0, NULL, 0) < 0)
return 0;
- __pppoe_server_start(name, arg->opt, arg->cli);
+ __pppoe_server_start(name, arg->opt, arg->cli, 0, 0);
return 0;
}
@@ -1249,12 +1296,12 @@ void pppoe_server_start(const char *opt, void *cli)
if (ptr) {
memcpy(name, opt, ptr - opt);
name[ptr - opt] = 0;
- __pppoe_server_start(name, ptr, cli);
+ __pppoe_server_start(name, ptr, cli, 0, 0);
} else
- __pppoe_server_start(opt, opt, cli);
+ __pppoe_server_start(opt, opt, cli, 0, 0);
}
-static void __pppoe_server_start(const char *ifname, const char *opt, void *cli)
+static void __pppoe_server_start(const char *ifname, const char *opt, void *cli, int parent_ifindex, int vid)
{
struct pppoe_serv_t *serv;
struct ifreq ifr;
@@ -1344,12 +1391,23 @@ static void __pppoe_server_start(const char *ifname, const char *opt, void *cli)
serv->padi_limit = padi_limit;
triton_context_register(&serv->ctx, NULL);
- triton_context_wakeup(&serv->ctx);
+
+ if (vid) {
+ serv->parent_ifindex = parent_ifindex;
+ serv->vid = vid;
+ if (conf_vlan_timeout) {
+ serv->timer.expire = pppoe_serv_timeout;
+ serv->timer.expire_tv.tv_sec = conf_vlan_timeout;
+ triton_timer_add(&serv->ctx, &serv->timer, 0);
+ }
+ }
pthread_rwlock_wrlock(&serv_lock);
list_add_tail(&serv->entry, &serv_list);
pthread_rwlock_unlock(&serv_lock);
+ triton_context_wakeup(&serv->ctx);
+
pppoe_disc_start(serv);
return;
@@ -1397,6 +1455,15 @@ void pppoe_server_free(struct pppoe_serv_t *serv)
free_delayed_pado(pado);
}
+ if (serv->timer.tpd)
+ triton_timer_del(&serv->timer);
+
+ if (serv->vid) {
+ log_info2("ipoe: remove vlan %s\n", serv->ifname);
+ iplink_vlan_del(serv->ifindex);
+ vlan_mon_add_vid(serv->parent_ifindex, ETH_P_PPP_DISC, serv->vid);
+ }
+
triton_context_unregister(&serv->ctx);
_free(serv->ifname);
_free(serv);
@@ -1438,9 +1505,233 @@ static int init_secret(struct pppoe_serv_t *serv)
return 0;
}
+void pppoe_vlan_mon_notify(int ifindex, int vid)
+{
+ struct conf_sect_t *sect = conf_get_section("pppoe");
+ struct conf_option_t *opt;
+ struct ifreq ifr;
+ char *ptr;
+ int len, r, svid;
+ pcre *re = NULL;
+ const char *pcre_err;
+ char *pattern;
+ int pcre_offset;
+ char ifname[IFNAMSIZ];
+
+ if (!sect)
+ return;
+
+ memset(&ifr, 0, sizeof(ifr));
+ ifr.ifr_ifindex = ifindex;
+ if (ioctl(sock_fd, SIOCGIFNAME, &ifr, sizeof(ifr))) {
+ log_error("pppoe: vlan-mon: failed to get interface name, ifindex=%i\n", ifindex);
+ return;
+ }
+
+ svid = iplink_vlan_get_vid(ifindex);
+
+ if (make_vlan_name(conf_vlan_name, ifr.ifr_name, svid, vid, ifname)) {
+ log_error("pppoe: vlan-mon: %s.%i: interface name is too long\n", ifr.ifr_name, vid);
+ return;
+ }
+
+ strcpy(ifr.ifr_name, ifname);
+ len = strlen(ifr.ifr_name);
+
+ if (iplink_vlan_add(ifr.ifr_name, ifindex, vid))
+ return;
+
+ 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 (ioctl(sock_fd, SIOCGIFINDEX, &ifr, sizeof(ifr))) {
+ log_error("pppoe: 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;
+ if (!opt->val)
+ continue;
+
+ ptr = strchr(opt->val, ',');
+ if (!ptr)
+ ptr = strchr(opt->val, 0);
+
+ if (ptr - opt->val > 3 && memcmp(opt->val, "re:", 3) == 0) {
+ pattern = _malloc(ptr - (opt->val + 3) + 1);
+ memcpy(pattern, opt->val + 3, ptr - (opt->val + 3));
+ pattern[ptr - (opt->val + 3)] = 0;
+
+ re = pcre_compile2(pattern, 0, NULL, &pcre_err, &pcre_offset, NULL);
+
+ _free(pattern);
+
+ if (!re)
+ continue;
+
+ r = pcre_exec(re, NULL, ifr.ifr_name, len, 0, 0, NULL, 0);
+ pcre_free(re);
+
+ if (r < 0)
+ 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)
+ __pppoe_server_start(ifr.ifr_name, opt->val, NULL, ifindex, vid);
+ }
+}
+
+static void add_vlan_mon(const char *opt, long *mask)
+{
+ const char *ptr;
+ struct ifreq ifr;
+ int ifindex;
+ long mask1[4096/8/sizeof(long)];
+ struct pppoe_serv_t *serv;
+
+ for (ptr = opt; *ptr && *ptr != ','; ptr++);
+
+ if (ptr - opt >= IFNAMSIZ) {
+ log_error("pppoe: vlan-mon=%s: interface name is too long\n", opt);
+ return;
+ }
+
+ memset(&ifr, 0, sizeof(ifr));
+
+ memcpy(ifr.ifr_name, opt, ptr - opt);
+ ifr.ifr_name[ptr - opt] = 0;
+
+ if (ioctl(sock_fd, SIOCGIFINDEX, &ifr)) {
+ log_error("pppoe: '%s': ioctl(SIOCGIFINDEX): %s\n", ifr.ifr_name, strerror(errno));
+ return;
+ }
+
+ ifindex = ifr.ifr_ifindex;
+
+ ioctl(sock_fd, SIOCGIFFLAGS, &ifr);
+
+ if (!(ifr.ifr_flags & IFF_UP)) {
+ ifr.ifr_flags |= IFF_UP;
+
+ ioctl(sock_fd, SIOCSIFFLAGS, &ifr);
+ }
+
+ memcpy(mask1, mask, sizeof(mask1));
+ list_for_each_entry(serv, &serv_list, entry) {
+ if (serv->vid && serv->parent_ifindex == ifindex)
+ mask1[serv->vid / (8*sizeof(long))] |= 1lu << (serv->vid % (8*sizeof(long)));
+ }
+
+ vlan_mon_add(ifindex, ETH_P_PPP_DISC, mask1, sizeof(mask1));
+}
+
+static int __load_vlan_mon_re(int index, int flags, const char *name, struct iplink_arg *arg)
+{
+ struct ifreq ifr;
+ long mask1[4096/8/sizeof(long)];
+ struct pppoe_serv_t *serv;
+
+ if (pcre_exec(arg->re, NULL, name, strlen(name), 0, 0, NULL, 0) < 0)
+ return 0;
+
+ memset(&ifr, 0, sizeof(ifr));
+ strcpy(ifr.ifr_name, name);
+
+ ioctl(sock_fd, SIOCGIFFLAGS, &ifr);
+
+ if (!(ifr.ifr_flags & IFF_UP)) {
+ ifr.ifr_flags |= IFF_UP;
+
+ ioctl(sock_fd, SIOCSIFFLAGS, &ifr);
+ }
+
+ memcpy(mask1, arg->arg1, sizeof(mask1));
+ list_for_each_entry(serv, &serv_list, entry) {
+ if (serv->vid && serv->parent_ifindex == index)
+ mask1[serv->vid / (8*sizeof(long))] |= 1lu << (serv->vid % (8*sizeof(long)));
+ }
+
+ vlan_mon_add(index, ETH_P_PPP_DISC, mask1, sizeof(mask1));
+
+ return 0;
+}
+
+static void load_vlan_mon_re(const char *opt, long *mask, int len)
+{
+ pcre *re = NULL;
+ const char *pcre_err;
+ char *pattern;
+ const char *ptr;
+ int pcre_offset;
+ struct iplink_arg arg;
+
+ for (ptr = opt; *ptr && *ptr != ','; ptr++);
+
+ pattern = _malloc(ptr - (opt + 3) + 1);
+ memcpy(pattern, opt + 3, ptr - (opt + 3));
+ pattern[ptr - (opt + 3)] = 0;
+
+ re = pcre_compile2(pattern, 0, NULL, &pcre_err, &pcre_offset, NULL);
+
+ if (!re) {
+ log_error("ipoe: '%s': %s at %i\r\n", pattern, pcre_err, pcre_offset);
+ return;
+ }
+
+ arg.re = re;
+ arg.opt = opt;
+ arg.arg1 = mask;
+
+ iplink_list((iplink_list_func)__load_vlan_mon_re, &arg);
+
+ pcre_free(re);
+ _free(pattern);
+
+}
+
+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_PPP_DISC, pppoe_vlan_mon_notify);
+ registered = 1;
+ }
+
+ vlan_mon_del(-1, ETH_P_PPP_DISC);
+
+ list_for_each_entry(opt, &sect->items, entry) {
+ if (strcmp(opt->name, "vlan-mon"))
+ continue;
+
+ if (!opt->val)
+ continue;
+
+ if (parse_vlan_mon(opt->val, mask))
+ continue;
+
+ if (strlen(opt->val) > 3 && !memcmp(opt->val, "re:", 3))
+ load_vlan_mon_re(opt->val, mask, sizeof(mask));
+ else
+ add_vlan_mon(opt->val, mask);
+ }
+}
+
+
static void load_config(void)
{
char *opt;
+ struct conf_sect_t *s = conf_get_section("pppoe");
opt = conf_get_opt("pppoe", "verbose");
if (opt)
@@ -1529,6 +1820,20 @@ static void load_config(void)
else
log_error("pppoe: unknown called-sid type\n");
}
+
+ opt = conf_get_opt("pppoe", "vlan-name");
+ if (opt)
+ conf_vlan_name = opt;
+ else
+ conf_vlan_name = "%I.%N";
+
+ opt = conf_get_opt("pppoe", "vlan-timeout");
+ if (opt && atoi(opt) > 0)
+ conf_vlan_timeout = atoi(opt);
+ else
+ conf_vlan_timeout = 60;
+
+ load_vlan_mon(s);
}
static void load_interfaces()
diff --git a/accel-pppd/ctrl/pppoe/pppoe.h b/accel-pppd/ctrl/pppoe/pppoe.h
index 0acc561..d044e4c 100644
--- a/accel-pppd/ctrl/pppoe/pppoe.h
+++ b/accel-pppd/ctrl/pppoe/pppoe.h
@@ -73,6 +73,10 @@ struct pppoe_serv_t
char *ifname;
int ifindex;
+ int parent_ifindex;
+ int vid;
+ struct triton_timer_t timer;
+
uint8_t secret[SECRET_LENGTH];
DES_key_schedule des_ks;
diff --git a/accel-pppd/include/vlan_mon.h b/accel-pppd/include/vlan_mon.h
new file mode 100644
index 0000000..c6b2db7
--- /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);
+int parse_vlan_mon(const char *opt, long *mask);
+
+#endif
diff --git a/accel-pppd/vlan-mon/CMakeLists.txt b/accel-pppd/vlan-mon/CMakeLists.txt
new file mode 100644
index 0000000..1305825
--- /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 0000000..a1ad472
--- /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 0000000..7f15aa0
--- /dev/null
+++ b/accel-pppd/vlan-mon/vlan_mon.c
@@ -0,0 +1,401 @@
+#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;
+}
+
+int __export parse_vlan_mon(const char *opt, long *mask)
+{
+ char *ptr, *ptr2;
+ int vid, vid2;
+
+ ptr = strchr(opt, ',');
+ if (!ptr)
+ ptr = strchr(opt, 0);
+
+ if (*ptr == ',')
+ memset(mask, 0xff, 4096/8);
+ else if (*ptr == 0) {
+ memset(mask, 0, 4096/8);
+ return 0;
+ } else
+ goto out_err;
+
+ while (1) {
+ vid = strtol(ptr + 1, &ptr2, 10);
+ if (vid <= 0 || vid >= 4096) {
+ log_error("vlan-mon=%s: invalid vlan %i\n", opt, vid);
+ return -1;
+ }
+
+ if (*ptr2 == '-') {
+ vid2 = strtol(ptr2 + 1, &ptr2, 10);
+ if (vid2 <= 0 || vid2 >= 4096) {
+ log_error("vlan-mon=%s: invalid vlan %i\n", opt, vid2);
+ return -1;
+ }
+
+ for (; vid < vid2; vid++)
+ mask[vid / (8*sizeof(long))] &= ~(1lu << (vid % (8*sizeof(long))));
+ }
+
+ mask[vid / (8*sizeof(long))] &= ~(1lu << (vid % (8*sizeof(long))));
+
+ if (*ptr2 == 0)
+ break;
+
+ if (*ptr2 != ',')
+ goto out_err;
+
+ ptr = ptr2;
+ }
+
+ return 0;
+
+out_err:
+ log_error("vlan-mon=%s: failed to parse\n", opt);
+ return -1;
+}
+
+
+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 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[] = {
@@ -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 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..f2915b1
--- /dev/null
+++ b/drivers/vlan_mon/vlan_mon.c
@@ -0,0 +1,671 @@
+#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);
+
+ list_add_tail(&d->entry, &vlan_devices);
+ }
+
+ 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
+ }
+
+ up(&vlan_mon_lock);
+
+ dev_put(dev);
+
+ synchronize_rcu();
+
+ 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_safe(pos, n, &vlan_devices) {
+ d = list_entry(pos, typeof(*d), 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
+
+ d->proto &= ~proto;
+
+ if (dev) {
+ if (dev->ml_priv == d) {
+ 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(&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);
+
+ synchronize_rcu();
+
+ 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 <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
+
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-20 05:40:49 +0000