Merge branch 'master' of /home/dima/Projects/accel-ppp into vlanmon

4 files changed, 333 insertions, 151 deletions

diff --git a/accel-pppd/ctrl/pppoe/CMakeLists.txt b/accel-pppd/ctrl/pppoe/CMakeLists.txt
index dc65aa4..662042b 100644
--- a/accel-pppd/ctrl/pppoe/CMakeLists.txt
+++ b/accel-pppd/ctrl/pppoe/CMakeLists.txt
@@ -5,6 +5,7 @@ SET(sources
 	mac_filter.c
 	dpado.c
 	cli.c
+	disc.c
 )
 
 IF (RADIUS)
diff --git a/accel-pppd/ctrl/pppoe/disc.c b/accel-pppd/ctrl/pppoe/disc.c
new file mode 100644
index 0000000..151ef10
--- /dev/null
+++ b/accel-pppd/ctrl/pppoe/disc.c
@@ -0,0 +1,281 @@
+#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 <netpacket/packet.h>
+#include <arpa/inet.h>
+
+#include "triton.h"
+#include "log.h"
+#include "mempool.h"
+
+#include "pppoe.h"
+
+#include "memdebug.h"
+
+struct tree {
+	pthread_mutex_t lock;
+	struct rb_root root;
+};
+
+#define HASH_BITS 0xff
+static struct tree *tree;
+
+static uint8_t bc_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+
+static struct triton_md_handler_t disc_hnd;
+
+static mempool_t pkt_pool;
+
+int disc_sock;
+
+void pppoe_disc_start(struct pppoe_serv_t *serv)
+{
+	struct rb_node **p, *parent = NULL;
+	struct tree *t;
+	int ifindex = serv->ifindex, i;
+	struct pppoe_serv_t *n;
+
+	t = &tree[ifindex & HASH_BITS];
+
+	pthread_mutex_lock(&t->lock);
+
+	p = &t->root.rb_node;
+
+	while (*p) {
+		parent = *p;
+		n = rb_entry(parent, typeof(*n), node);
+		i = n->ifindex;
+
+		if (ifindex < i)
+			p = &(*p)->rb_left;
+		else if (ifindex > i)
+			p = &(*p)->rb_right;
+		else {
+			pthread_mutex_unlock(&t->lock);
+			log_error("pppoe: disc: attempt to add duplicate ifindex\n");
+			return;
+		}
+	}
+
+	rb_link_node(&serv->node, parent, p);
+	rb_insert_color(&serv->node, &t->root);
+
+	pthread_mutex_unlock(&t->lock);
+}
+
+void pppoe_disc_stop(struct pppoe_serv_t *serv)
+{
+	struct tree *t = &tree[serv->ifindex & HASH_BITS];
+
+	pthread_mutex_lock(&t->lock);
+	rb_erase(&serv->node, &t->root);
+	pthread_mutex_unlock(&t->lock);
+}
+
+static int forward(int ifindex, void *pkt, int len)
+{
+	struct pppoe_serv_t *n;
+	struct tree *t = &tree[ifindex & HASH_BITS];
+	struct rb_node **p = &t->root.rb_node, *parent = NULL;
+	int r = 0;
+	struct ethhdr *ethhdr = (struct ethhdr *)(pkt + 4);
+
+	pthread_mutex_lock(&t->lock);
+
+	while (*p) {
+		parent = *p;
+		n = rb_entry(parent, typeof(*n), node);
+
+		if (ifindex < n->ifindex)
+			p = &(*p)->rb_left;
+		else if (ifindex > n->ifindex)
+			p = &(*p)->rb_right;
+		else {
+			if (!memcmp(ethhdr->h_dest, bc_addr, ETH_ALEN) || !memcmp(ethhdr->h_dest, n->hwaddr, ETH_ALEN)) {
+				*(int *)pkt = len;
+				triton_context_call(&n->ctx, (triton_event_func)pppoe_serv_read, pkt);
+				r = 1;
+			}
+			break;
+		}
+	}
+
+	pthread_mutex_unlock(&t->lock);
+
+	return r;
+}
+
+static void notify_down(int ifindex)
+{
+	struct pppoe_serv_t *n;
+	struct tree *t = &tree[ifindex & HASH_BITS];
+	struct rb_node **p = &t->root.rb_node, *parent = NULL;
+
+	pthread_mutex_lock(&t->lock);
+
+	while (*p) {
+		parent = *p;
+		n = rb_entry(parent, typeof(*n), node);
+
+		if (ifindex < n->ifindex)
+			p = &(*p)->rb_left;
+		else if (ifindex > n->ifindex)
+			p = &(*p)->rb_right;
+		else {
+			triton_context_call(&n->ctx, (triton_event_func)_server_stop, n);
+			break;
+		}
+	}
+
+	pthread_mutex_unlock(&t->lock);
+}
+
+static int disc_read(struct triton_md_handler_t *h)
+{
+	uint8_t *pack = NULL;
+	struct ethhdr *ethhdr;
+	struct pppoe_hdr *hdr;
+	int n;
+	struct sockaddr_ll src;
+	socklen_t slen = sizeof(src);
+
+	while (1) {
+		if (!pack)
+			pack = mempool_alloc(pkt_pool);
+
+		n = recvfrom(disc_sock, pack + 4, ETHER_MAX_LEN, MSG_DONTWAIT, (struct sockaddr *)&src, &slen);
+
+		if (n < 0) {
+			if (errno == EAGAIN)
+				break;
+
+			if (errno == ENETDOWN) {
+				notify_down(src.sll_ifindex);
+				continue;
+			}
+
+			log_error("pppoe: disc: read: %s\n", strerror(errno));
+			continue;
+		}
+
+		ethhdr = (struct ethhdr *)(pack + 4);
+		hdr = (struct pppoe_hdr *)(pack + 4 + ETH_HLEN);
+
+		if (n < ETH_HLEN + sizeof(*hdr)) {
+			if (conf_verbose)
+				log_warn("pppoe: short packet received (%i)\n", n);
+			continue;
+		}
+
+		if (mac_filter_check(ethhdr->h_source)) {
+			__sync_add_and_fetch(&stat_filtered, 1);
+			continue;
+		}
+
+		//if (memcmp(ethhdr->h_dest, bc_addr, ETH_ALEN) && memcmp(ethhdr->h_dest, serv->hwaddr, ETH_ALEN))
+		//	continue;
+
+		if (!memcmp(ethhdr->h_source, bc_addr, ETH_ALEN)) {
+			if (conf_verbose)
+				log_warn("pppoe: discarding packet (host address is broadcast)\n");
+			continue;
+		}
+
+		if ((ethhdr->h_source[0] & 1) != 0) {
+			if (conf_verbose)
+				log_warn("pppoe: discarding packet (host address is not unicast)\n");
+			continue;
+		}
+
+		if (n < ETH_HLEN + sizeof(*hdr) + ntohs(hdr->length)) {
+			if (conf_verbose)
+				log_warn("pppoe: short packet received\n");
+			continue;
+		}
+
+		if (hdr->ver != 1) {
+			if (conf_verbose)
+				log_warn("pppoe: discarding packet (unsupported version %i)\n", hdr->ver);
+			continue;
+		}
+
+		if (hdr->type != 1) {
+			if (conf_verbose)
+				log_warn("pppoe: discarding packet (unsupported type %i)\n", hdr->type);
+		}
+
+		if (forward(src.sll_ifindex, pack, n))
+			pack = NULL;
+	}
+
+	mempool_free(pack);
+
+	return 0;
+}
+
+static void disc_close(struct triton_context_t *ctx);
+
+static struct triton_context_t disc_ctx = {
+	.close = disc_close,
+};
+
+static struct triton_md_handler_t disc_hnd = {
+	.read = disc_read,
+};
+
+static void disc_close(struct triton_context_t *ctx)
+{
+	triton_md_unregister_handler(&disc_hnd, 1);
+	triton_context_unregister(ctx);
+}
+
+static void init()
+{
+	struct sockaddr_ll addr;
+	int i, f = 1;
+
+	pkt_pool = mempool_create(ETHER_MAX_LEN + 4);
+
+	tree = malloc((HASH_BITS + 1) * sizeof(struct tree));
+	for (i = 0; i <= HASH_BITS; i++) {
+		pthread_mutex_init(&tree[i].lock, NULL);
+		tree[i].root = RB_ROOT;
+	}
+
+	disc_sock = socket(PF_PACKET, SOCK_RAW, htons(ETH_P_PPP_DISC));
+	if (disc_sock < 0)
+		return;
+
+	memset(&addr, 0, sizeof(addr));
+	addr.sll_family = AF_PACKET;
+	addr.sll_protocol = htons(ETH_P_PPP_DISC);
+
+	setsockopt(disc_sock, SOL_SOCKET, SO_BROADCAST, &f, sizeof(f));
+
+	if (bind(disc_sock, (struct sockaddr *)&addr, sizeof(addr))) {
+		log_error("pppoe: disc: bind: %s\n", strerror(errno));
+		close(disc_sock);
+		return;
+	}
+
+	fcntl(disc_sock, F_SETFL, O_NONBLOCK);
+	fcntl(disc_sock, F_SETFD, FD_CLOEXEC);
+
+	disc_hnd.fd = disc_sock;
+
+	triton_context_register(&disc_ctx, NULL);
+	triton_md_register_handler(&disc_ctx, &disc_hnd);
+	triton_md_enable_handler(&disc_hnd, MD_MODE_READ);
+	triton_context_wakeup(&disc_ctx);
+}
+
+DEFINE_INIT(1, init);
+
+
diff --git a/accel-pppd/ctrl/pppoe/pppoe.c b/accel-pppd/ctrl/pppoe/pppoe.c
index b51b4fe..fc8a032 100644
--- a/accel-pppd/ctrl/pppoe/pppoe.c
+++ b/accel-pppd/ctrl/pppoe/pppoe.c
@@ -40,7 +40,6 @@ struct pppoe_conn_t {
 	struct list_head entry;
 	struct triton_context_t ctx;
 	struct pppoe_serv_t *serv;
-	int disc_sock;
 	uint16_t sid;
 	uint8_t addr[ETH_ALEN];
 	int ppp_started:1;
@@ -127,7 +126,6 @@ static int sid_idx;
 static uint8_t bc_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
 
 static void pppoe_send_PADT(struct pppoe_conn_t *conn);
-static void _server_stop(struct pppoe_serv_t *serv);
 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, int parent_ifindex, int vid);
@@ -165,8 +163,6 @@ static void disconnect(struct pppoe_conn_t *conn)
 
 	pppoe_send_PADT(conn);
 
-	close(conn->disc_sock);
-
 	triton_event_fire(EV_CTRL_FINISHED, &conn->ppp.ses);
 
 	log_ppp_info1("disconnected\n");
@@ -390,8 +386,6 @@ static struct pppoe_conn_t *allocate_channel(struct pppoe_serv_t *serv, const ui
 	if (conf_ip_pool)
 		conn->ppp.ses.ipv4_pool_name = _strdup(conf_ip_pool);
 
-	conn->disc_sock = dup(serv->hnd.fd);
-
 	triton_context_register(&conn->ctx, &conn->ppp.ses);
 
 	pthread_mutex_lock(&serv->lock);
@@ -711,18 +705,17 @@ static void add_tag2(uint8_t *pack, const struct pppoe_tag *t)
 	hdr->length = htons(ntohs(hdr->length) + sizeof(*tag) + ntohs(t->tag_len));
 }
 
-static void pppoe_send(int fd, const uint8_t *pack)
+static void pppoe_send(int ifindex, const uint8_t *pack)
 {
-	struct pppoe_hdr *hdr = (struct pppoe_hdr *)(pack + ETH_HLEN);
-	int n, s;
+	struct sockaddr_ll addr = {
+		.sll_family = AF_PACKET,
+		.sll_protocol = htons(ETH_P_PPP_DISC),
+		.sll_ifindex = ifindex,
+	};
 
-	s = ETH_HLEN + sizeof(*hdr) + ntohs(hdr->length);
-	n = write(fd, pack, s);
-	if (n < 0 )
-		log_error("pppoe: write: %s\n", strerror(errno));
-	else if (n != s) {
-		log_warn("pppoe: short write %i/%i\n", n,s);
-	}
+	struct pppoe_hdr *hdr = (struct pppoe_hdr *)(pack + ETH_HLEN);
+	int len = ETH_HLEN + sizeof(*hdr) + ntohs(hdr->length);
+	sendto(disc_sock, pack, len, MSG_DONTWAIT, (struct sockaddr *)&addr, sizeof(addr));
 }
 
 static void pppoe_send_PADO(struct pppoe_serv_t *serv, const uint8_t *addr, const struct pppoe_tag *host_uniq, const struct pppoe_tag *relay_sid, const struct pppoe_tag *service_name)
@@ -755,7 +748,7 @@ static void pppoe_send_PADO(struct pppoe_serv_t *serv, const uint8_t *addr, cons
 	}
 
 	__sync_add_and_fetch(&stat_PADO_sent, 1);
-	pppoe_send(serv->hnd.fd, pack);
+	pppoe_send(serv->ifindex, pack);
 }
 
 static void pppoe_send_err(struct pppoe_serv_t *serv, const uint8_t *addr, const struct pppoe_tag *host_uniq, const struct pppoe_tag *relay_sid, int code, int tag_type)
@@ -778,7 +771,7 @@ static void pppoe_send_err(struct pppoe_serv_t *serv, const uint8_t *addr, const
 		print_packet(pack);
 	}
 
-	pppoe_send(serv->hnd.fd, pack);
+	pppoe_send(serv->ifindex, pack);
 }
 
 static void pppoe_send_PADS(struct pppoe_conn_t *conn)
@@ -803,7 +796,7 @@ static void pppoe_send_PADS(struct pppoe_conn_t *conn)
 	}
 
 	__sync_add_and_fetch(&stat_PADS_sent, 1);
-	pppoe_send(conn->disc_sock, pack);
+	pppoe_send(conn->serv->ifindex, pack);
 }
 
 static void pppoe_send_PADT(struct pppoe_conn_t *conn)
@@ -824,7 +817,7 @@ static void pppoe_send_PADT(struct pppoe_conn_t *conn)
 		print_packet(pack);
 	}
 
-	pppoe_send(conn->disc_sock, pack);
+	pppoe_send(conn->serv->ifindex, pack);
 }
 
 static void free_delayed_pado(struct delayed_pado_t *pado)
@@ -1174,92 +1167,37 @@ static void pppoe_recv_PADT(struct pppoe_serv_t *serv, uint8_t *pack)
 	pthread_mutex_unlock(&serv->lock);
 }
 
-static int pppoe_serv_read(struct triton_md_handler_t *h)
+void pppoe_serv_read(uint8_t *data)
 {
-	struct pppoe_serv_t *serv = container_of(h, typeof(*serv), hnd);
-	uint8_t pack[ETHER_MAX_LEN];
-	struct ethhdr *ethhdr = (struct ethhdr *)pack;
+	struct pppoe_serv_t *serv = container_of(triton_context_self(), typeof(*serv), ctx);
+	uint8_t *pack = data + 4;
 	struct pppoe_hdr *hdr = (struct pppoe_hdr *)(pack + ETH_HLEN);
-	int n;
-
-	while (1) {
-		n = read(h->fd, pack, sizeof(pack));
-		if (n < 0) {
-			if (errno == EAGAIN)
-				break;
-			if (errno == ENETDOWN) {
-				_server_stop(serv);
-				return 1;
-			}
-			log_error("pppoe: read: %s\n", strerror(errno));
-			return 0;
-		}
-
-		if (n < ETH_HLEN + sizeof(*hdr)) {
-			if (conf_verbose)
-				log_warn("pppoe: short packet received (%i)\n", n);
-			continue;
-		}
-
-		if (mac_filter_check(ethhdr->h_source)) {
-			__sync_add_and_fetch(&stat_filtered, 1);
-			continue;
-		}
-
-		if (memcmp(ethhdr->h_dest, bc_addr, ETH_ALEN) && memcmp(ethhdr->h_dest, serv->hwaddr, ETH_ALEN))
-			continue;
+	int n = *(int *)data;
 
-		if (!memcmp(ethhdr->h_source, bc_addr, ETH_ALEN)) {
-			if (conf_verbose)
-				log_warn("pppoe: discarding packet (host address is broadcast)\n");
-			continue;
-		}
-
-		if ((ethhdr->h_source[0] & 1) != 0) {
-			if (conf_verbose)
-				log_warn("pppoe: discarding packet (host address is not unicast)\n");
-			continue;
-		}
-
-		if (n < ETH_HLEN + sizeof(*hdr) + ntohs(hdr->length)) {
-			if (conf_verbose)
-				log_warn("pppoe: short packet received\n");
-			continue;
-		}
-
-		if (hdr->ver != 1) {
-			if (conf_verbose)
-				log_warn("pppoe: discarding packet (unsupported version %i)\n", hdr->ver);
-			continue;
-		}
-
-		if (hdr->type != 1) {
-			if (conf_verbose)
-				log_warn("pppoe: discarding packet (unsupported type %i)\n", hdr->type);
-		}
-
-		switch (hdr->code) {
-			case CODE_PADI:
-				pppoe_recv_PADI(serv, pack, n);
-				break;
-			case CODE_PADR:
-				pppoe_recv_PADR(serv, pack, n);
-				break;
-			case CODE_PADT:
-				pppoe_recv_PADT(serv, pack);
-				break;
-		}
+	switch (hdr->code) {
+		case CODE_PADI:
+			pppoe_recv_PADI(serv, pack, n);
+			break;
+		case CODE_PADR:
+			pppoe_recv_PADR(serv, pack, n);
+			break;
+		case CODE_PADT:
+			pppoe_recv_PADT(serv, pack);
+			break;
 	}
-	return 0;
+
+	mempool_free(data);
 }
 
 static void pppoe_serv_close(struct triton_context_t *ctx)
 {
 	struct pppoe_serv_t *serv = container_of(ctx, typeof(*serv), ctx);
 
-	triton_md_disable_handler(&serv->hnd, MD_MODE_READ | MD_MODE_WRITE);
+	if (serv->stopping)
+		return;
 
 	serv->stopping = 1;
+	pppoe_disc_stop(serv);
 
 	pthread_mutex_lock(&serv->lock);
 	if (!serv->conn_cnt) {
@@ -1366,10 +1304,7 @@ void pppoe_server_start(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;
-	int sock;
-	int f = 1;
 	struct ifreq ifr;
-	struct sockaddr_ll sa;
 	int padi_limit = conf_padi_limit;
 
 	if (parse_server(opt, &padi_limit)) {
@@ -1400,26 +1335,8 @@ static void __pppoe_server_start(const char *ifname, const char *opt, void *cli,
 		return;
 	}
 
-	sock = socket(PF_PACKET, SOCK_RAW, htons(ETH_P_PPP_DISC));
-	if (sock < 0) {
-		if (cli)
-			cli_sendv(cli, "socket: %s\r\n", strerror(errno));
-		log_error("pppoe: socket: %s\n", strerror(errno));
-		_free(serv);
-		return;
-	}
-
-	fcntl(sock, F_SETFD, fcntl(sock, F_GETFD) | FD_CLOEXEC);
-
-	if (setsockopt(sock, SOL_SOCKET, SO_BROADCAST, &f, sizeof(f))) {
-		if (cli)
-			cli_sendv(cli, "setsockopt(SO_BROADCAST): %s\r\n", strerror(errno));
-		log_error("pppoe: setsockopt(SO_BROADCAST): %s\n", strerror(errno));
-		goto out_err;
-	}
-
 	strncpy(ifr.ifr_name, ifname, sizeof(ifr.ifr_name));
-	if (ioctl(sock, SIOCGIFHWADDR, &ifr)) {
+	if (ioctl(sock_fd, SIOCGIFHWADDR, &ifr)) {
 		if (cli)
 			cli_sendv(cli, "ioctl(SIOCGIFHWADDR): %s\r\n", strerror(errno));
 		log_error("pppoe: ioctl(SIOCGIFHWADDR): %s\n", strerror(errno));
@@ -1442,7 +1359,7 @@ static void __pppoe_server_start(const char *ifname, const char *opt, void *cli,
 
 	memcpy(serv->hwaddr, ifr.ifr_hwaddr.sa_data, ETH_ALEN);
 
-	if (ioctl(sock, SIOCGIFMTU, &ifr)) {
+	if (ioctl(sock_fd, SIOCGIFMTU, &ifr)) {
 		if (cli)
 			cli_sendv(cli, "ioctl(SIOCGIFMTU): %s\r\n", strerror(errno));
 		log_error("pppoe: ioctl(SIOCGIFMTU): %s\n", strerror(errno));
@@ -1455,39 +1372,17 @@ static void __pppoe_server_start(const char *ifname, const char *opt, void *cli,
 		log_error("pppoe: interface %s has MTU of %i, should be %i\n", ifname, ifr.ifr_mtu, ETH_DATA_LEN);
 	}
 
-	if (ioctl(sock, SIOCGIFINDEX, &ifr)) {
+	if (ioctl(sock_fd, SIOCGIFINDEX, &ifr)) {
 		if (cli)
 			cli_sendv(cli, "ioctl(SIOCGIFINDEX): %s\r\n", strerror(errno));
 		log_error("pppoe: ioctl(SIOCGIFINDEX): %s\n", strerror(errno));
 		goto out_err;
 	}
 
-	serv->ifindex = ifr.ifr_ifindex;
-
-	memset(&sa, 0, sizeof(sa));
-	sa.sll_family = AF_PACKET;
-	sa.sll_protocol = htons(ETH_P_PPP_DISC);
-	sa.sll_ifindex = ifr.ifr_ifindex;
-
-	if (bind(sock, (struct sockaddr *)&sa, sizeof(sa))) {
-		if (cli)
-			cli_sendv(cli, "bind: %s\n", strerror(errno));
-		log_error("pppoe: bind: %s\n", strerror(errno));
-		goto out_err;
-	}
-
-	if (fcntl(sock, F_SETFL, O_NONBLOCK)) {
-		if (cli)
-			cli_sendv(cli, "failed to set nonblocking mode: %s\n", strerror(errno));
-    log_error("pppoe: failed to set nonblocking mode: %s\n", strerror(errno));
-		goto out_err;
-	}
-
 	serv->ctx.close = pppoe_serv_close;
 	serv->ctx.before_switch = log_switch;
-	serv->hnd.fd = sock;
-	serv->hnd.read = pppoe_serv_read;
 	serv->ifname = _strdup(ifname);
+	serv->ifindex = ifr.ifr_ifindex;
 	pthread_mutex_init(&serv->lock, NULL);
 
 	INIT_LIST_HEAD(&serv->conn_list);
@@ -1507,18 +1402,17 @@ static void __pppoe_server_start(const char *ifname, const char *opt, void *cli,
 		}
 	}
 
-	triton_md_register_handler(&serv->ctx, &serv->hnd);
-	triton_md_enable_handler(&serv->hnd, MD_MODE_READ);
-	triton_context_wakeup(&serv->ctx);
-
 	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;
 
 out_err:
-	close(sock);
 	_free(serv);
 }
 
@@ -1527,7 +1421,7 @@ static void _conn_stop(struct pppoe_conn_t *conn)
 	ap_session_terminate(&conn->ppp.ses, TERM_ADMIN_RESET, 0);
 }
 
-static void _server_stop(struct pppoe_serv_t *serv)
+void _server_stop(struct pppoe_serv_t *serv)
 {
 	struct pppoe_conn_t *conn;
 
@@ -1535,7 +1429,7 @@ static void _server_stop(struct pppoe_serv_t *serv)
 		return;
 
 	serv->stopping = 1;
-	triton_md_disable_handler(&serv->hnd, MD_MODE_READ | MD_MODE_WRITE);
+	pppoe_disc_stop(serv);
 
 	pthread_mutex_lock(&serv->lock);
 	if (!serv->conn_cnt) {
@@ -1564,8 +1458,6 @@ void pppoe_server_free(struct pppoe_serv_t *serv)
 	if (serv->timer.tpd)
 		triton_timer_del(&serv->timer);
 
-	triton_md_unregister_handler(&serv->hnd, 1);
-
 	if (serv->vid) {
 		log_info2("ipoe: remove vlan %s\n", serv->ifname);
 		iplink_vlan_del(serv->ifindex);
diff --git a/accel-pppd/ctrl/pppoe/pppoe.h b/accel-pppd/ctrl/pppoe/pppoe.h
index 50d2d9b..d044e4c 100644
--- a/accel-pppd/ctrl/pppoe/pppoe.h
+++ b/accel-pppd/ctrl/pppoe/pppoe.h
@@ -6,6 +6,7 @@
 #include <linux/if.h>
 #include <linux/if_pppox.h>
 
+#include "rbtree.h"
 #include "crypto.h"
 
 /* PPPoE codes */
@@ -66,7 +67,8 @@ struct pppoe_serv_t
 {
 	struct list_head entry;
 	struct triton_context_t ctx;
-	struct triton_md_handler_t hnd;
+	struct rb_node node;
+
 	uint8_t hwaddr[ETH_ALEN];
 	char *ifname;
 	int ifindex;
@@ -111,9 +113,15 @@ extern unsigned long stat_filtered;
 extern pthread_rwlock_t serv_lock;
 extern struct list_head serv_list;
 
+extern int disc_sock;
+
 int mac_filter_check(const uint8_t *addr);
 void pppoe_server_start(const char *intf, void *client);
 void pppoe_server_stop(const char *intf);
+void pppoe_serv_read(uint8_t *data);
+void _server_stop(struct pppoe_serv_t *s);
+void pppoe_disc_start(struct pppoe_serv_t *serv);
+void pppoe_disc_stop(struct pppoe_serv_t *serv);
 
 extern int pado_delay;
 void dpado_check_next(int conn_cnt);