ctrl: implemented L2TPv2 server (without IPsec)

14 files changed, 867 insertions, 250 deletions

diff --git a/README b/README
index 95d047cd..e17ba1de 100644
--- a/README
+++ b/README
@@ -1,6 +1,6 @@
 Overview
 --------
-The ACCEL-PPTP v1.0 is completly new implementation of PPTP which was written from null.
+The ACCEL-PPTP v1.0 is completly new implementation of PPTP/PPPoE/L2TP which was written from null.
 Userspace daemon has its own PPP implementation, so it does not uses pppd and one process (multi-threaded) manages all connections.
 ACCEL-PPTP uses kernel module to increase performance and decrease system usage.
 It is not fully tested yet so not recomended to use in production.
@@ -13,6 +13,7 @@ Features
 2.  High-performance multi-threaded I/O core
 3.  Supported PPTP
 4.  Supported PPPoE
+5.  Supported L2TPv2 (without IPsec)
 5.  Radius authentication/accounting
 6.  Radius DM/CoA extention
 7.  Supported authentication types: PAP, CHAP (md5), Microsoft CHAP Extentions (including version 2), not supported - EAP
@@ -31,6 +32,7 @@ Requirment
 2. kernel-2.6.25 or later
 3. glibc-2.8 or later
 4. cmake-2.6 or later
+5. libnl-1.1 or probably later
 
 
 Compilation and instalation
diff --git a/accel-pptpd/accel-pptp.conf b/accel-pptpd/accel-pptp.conf
index bf8cdd42..62a7c886 100644
--- a/accel-pptpd/accel-pptp.conf
+++ b/accel-pptpd/accel-pptp.conf
@@ -2,9 +2,9 @@
 #path=/usr/local/lib/accel-pptp
 log_file
 #log_pgsql
-#pptp
+pptp
 #pppoe
-l2tp
+#l2tp
 auth_pap
 auth_chap_md5
 auth_mschap_v1
@@ -32,13 +32,18 @@ echo-failure=3
 echo-interval=30
 verbose=1
 
-[ltp]
-#dictionary=/usr/local/share/accel-pptp/dictionary
-
 [pppoe]
 interface=eth0
 verbose=1
 
+[l2tp]
+#dictionary=/usr/local/share/accel-pptp/dictionary
+#hello_interval=60
+#timeout=60
+#rtimeout=5
+#retransmit=5
+verbose=1
+
 [dns]
 #dns1=172.16.0.1
 #dns2=172.16.1.1
diff --git a/accel-pptpd/accel-pptp.conf.5 b/accel-pptpd/accel-pptp.conf.5
index c630ac17..e82b1335 100644
--- a/accel-pptpd/accel-pptp.conf.5
+++ b/accel-pptpd/accel-pptp.conf.5
@@ -156,6 +156,34 @@ options.
 If this option is given and 
 .B n
 is greater of zero then pppoe module will produce verbose logging.
+.TP
+.SH [l2tp]
+.br
+Configuration of L2TP module.
+.TP
+.BI "bind=" x.x.x.x
+Specifies IP address to bind.
+.TP
+.BI "host-name=" string
+This name will be sent to clients in Host-Name attribute.
+.TP
+.BI "hello-interval=" n
+Specifies interval (in seconds) to send Hello control message. Its used for keep alive connection. If peer will not respond to Hello connection will be terminated.
+.TP
+.BI "timeout=" n
+Specifies timeout (in seconds) to wait peer completes tunnel and session negotiation.
+.TP
+.BI "rtimeout=" n
+Specifies timeout (in seconds) to wait message acknowledge, if elapsed message retransmition will be performed.
+.TP
+.BI "retransmit=" n
+Specifies maximum number of message retransmission, if exceeds connection will be terminated.
+.TP
+.BI "verbose=" n
+If this option is given and 
+.B n
+is greater of zero then l2tp module will produce verbose logging.
+.TP
 .SH [radius]
 .br
 Configuration of RADIUS module.
diff --git a/accel-pptpd/ctrl/CMakeLists.txt b/accel-pptpd/ctrl/CMakeLists.txt
index fa7acdc2..6b37bc4a 100644
--- a/accel-pptpd/ctrl/CMakeLists.txt
+++ b/accel-pptpd/ctrl/CMakeLists.txt
@@ -1,3 +1,3 @@
 ADD_SUBDIRECTORY(pptp)
 ADD_SUBDIRECTORY(pppoe)
-#ADD_SUBDIRECTORY(l2tp)
+ADD_SUBDIRECTORY(l2tp)
diff --git a/accel-pptpd/ctrl/l2tp/CMakeLists.txt b/accel-pptpd/ctrl/l2tp/CMakeLists.txt
index 8a64d5f0..edff55dc 100644
--- a/accel-pptpd/ctrl/l2tp/CMakeLists.txt
+++ b/accel-pptpd/ctrl/l2tp/CMakeLists.txt
@@ -6,6 +6,8 @@ ADD_LIBRARY(l2tp SHARED
 	l2tp.c
 	dict.c
 	packet.c
+	netlink.c
 )
+TARGET_LINK_LIBRARIES(l2tp nl)
 
 INSTALL(TARGETS l2tp LIBRARY DESTINATION usr/lib/accel-pptp)
diff --git a/accel-pptpd/ctrl/l2tp/l2tp.c b/accel-pptpd/ctrl/l2tp/l2tp.c
index 7548411a..29961272 100644
--- a/accel-pptpd/ctrl/l2tp/l2tp.c
+++ b/accel-pptpd/ctrl/l2tp/l2tp.c
@@ -10,6 +10,9 @@
 #include <arpa/inet.h>
 #include <netinet/in.h>
 #include <sys/socket.h>
+#include <linux/if.h>
+#include <linux/if_ether.h>
+#include <linux/if_pppox.h>
 
 #include "triton.h"
 #include "mempool.h"
@@ -17,6 +20,7 @@
 #include "ppp.h"
 #include "events.h"
 #include "utils.h"
+#include "iprange.h"
 
 #include "memdebug.h"
 
@@ -24,14 +28,20 @@
 #include "attr_defs.h"
 
 #define STATE_WAIT_SCCCN 1
-#define STATE_WAIT_OCRQ  2
-#define STATE_WAIT_OCCN  3
-#define STATE_PPP        4
+#define STATE_WAIT_ICRQ  2
+#define STATE_WAIT_ICCN  3
+#define STATE_WAIT_OCRP  4
+#define STATE_WAIT_OCCN  5
+#define STATE_ESTB       6
+#define STATE_PPP        7
+#define STATE_FIN        8
 #define STATE_CLOSE      0
 
 int conf_verbose = 0;
-int conf_timeout = 5;
+int conf_timeout = 60;
+int conf_rtimeout = 5;
 int conf_retransmit = 5;
+int conf_hello_interval = 60;
 const char *conf_host_name = "accel-pptp";
 
 struct l2tp_serv_t
@@ -44,12 +54,11 @@ struct l2tp_serv_t
 struct l2tp_conn_t
 {
 	struct triton_context_t ctx;
+	struct triton_md_handler_t hnd;
 	struct triton_timer_t timeout_timer;
-	struct triton_timer_t echo_timer;
-
-	pthread_mutex_t lock;
+	struct triton_timer_t rtimeout_timer;
+	struct triton_timer_t hello_timer;
 
-	int sock;
 	struct sockaddr_in addr;
 	uint16_t tid;
 	uint16_t sid;
@@ -57,13 +66,13 @@ struct l2tp_conn_t
 	uint16_t peer_sid;
 	uint32_t framing_cap;
 
-	struct l2tp_packet_t *last_pack;
 	int retransmit;
 	uint16_t Ns, Nr;
-	struct list_head recv_queue;
 	struct list_head send_queue;
 
 	int state;
+	int state1;
+	int state2;
 
 	struct ppp_ctrl_t ctrl;
 	struct ppp_t ppp;
@@ -76,41 +85,39 @@ static uint16_t l2tp_tid;
 static mempool_t l2tp_conn_pool;
 
 static void l2tp_timeout(struct triton_timer_t *t);
+static void l2tp_rtimeout(struct triton_timer_t *t);
+static void l2tp_send_HELLO(struct triton_timer_t *t);
 static void l2tp_send_SCCRP(struct l2tp_conn_t *conn);
-static void l2tp_send(struct l2tp_conn_t *conn, struct l2tp_packet_t *pack);
+static int l2tp_send(struct l2tp_conn_t *conn, struct l2tp_packet_t *pack);
+static int l2tp_conn_read(struct triton_md_handler_t *);
 
-static struct l2tp_conn_t *l2tp_conn_lookup(uint16_t tid)
+static void l2tp_disconnect(struct l2tp_conn_t *conn)
 {
-	struct l2tp_conn_t *conn;
-	pthread_mutex_lock(&conn->lock);
-	conn = l2tp_conn[tid];
-	if (conn)
-		pthread_mutex_lock(&conn->lock);
-	pthread_mutex_unlock(&conn->lock);
+	struct l2tp_packet_t *pack;
 
-	return conn;
-}
+	if (conn->tid)
+		l2tp_nl_delete_tunnel(conn->tid);
 
-static void l2tp_disconnect(struct l2tp_conn_t *conn)
-{
-	close(conn->sock);
+	triton_md_unregister_handler(&conn->hnd);
+	close(conn->hnd.fd);
 
 	if (conn->timeout_timer.tpd)
 		triton_timer_del(&conn->timeout_timer);
 
-	if (conn->echo_timer.tpd)
-		triton_timer_del(&conn->echo_timer);
+	if (conn->rtimeout_timer.tpd)
+		triton_timer_del(&conn->rtimeout_timer);
+
+	if (conn->hello_timer.tpd)
+		triton_timer_del(&conn->hello_timer);
 
 	if (conn->state == STATE_PPP) {
-		conn->state = STATE_CLOSE;
+		conn->state = STATE_FIN;
 		ppp_terminate(&conn->ppp, 1);
 	}
 
 	pthread_mutex_lock(&l2tp_lock);
-	pthread_mutex_lock(&conn->lock);
 	l2tp_conn[conn->tid] = NULL;
 	pthread_mutex_unlock(&l2tp_lock);
-	pthread_mutex_unlock(&conn->lock);
 
 	triton_event_fire(EV_CTRL_FINISHED, &conn->ppp);
 	
@@ -119,8 +126,11 @@ static void l2tp_disconnect(struct l2tp_conn_t *conn)
 
 	triton_context_unregister(&conn->ctx);
 
-	if (conn->last_pack)
-		l2tp_packet_free(conn->last_pack);
+	while (!list_empty(&conn->send_queue)) {
+		pack = list_entry(conn->send_queue.next, typeof(*pack), entry);
+		list_del(&pack->entry);
+		l2tp_packet_free(pack);
+	}
 
 	if (conn->ppp.chan_name)
 		_free(conn->ppp.chan_name);
@@ -131,43 +141,115 @@ static void l2tp_disconnect(struct l2tp_conn_t *conn)
 	mempool_free(conn);
 }
 
-static void l2tp_terminate(struct l2tp_conn_t *conn, int res, int err)
+static int l2tp_terminate(struct l2tp_conn_t *conn, int res, int err)
 {
 	struct l2tp_packet_t *pack;
 	struct l2tp_avp_result_code rc = {res, err};
 
+	log_ppp_debug("l2tp: terminate (%i, %i)\n", res, err);
+
 	pack = l2tp_packet_alloc(2, Message_Type_Stop_Ctrl_Conn_Notify, &conn->addr);
-	if (!pack) {
-		l2tp_disconnect(conn);
-		return;
-	}
+	if (!pack)
+		return -1;
 	
-	if (l2tp_packet_add_int16(pack, Assigned_Tunnel_ID, conn->tid))
+	if (l2tp_packet_add_int16(pack, Assigned_Tunnel_ID, conn->tid, 1))
 		goto out_err;
-	if (l2tp_packet_add_octets(pack, Result_Code, (uint8_t *)&rc, sizeof(rc)))
+	if (l2tp_packet_add_octets(pack, Result_Code, (uint8_t *)&rc, sizeof(rc), 0))
 		goto out_err;
 
 	l2tp_send(conn, pack);
 
-	return;
+	conn->state = STATE_FIN;
+
+	return 0;
 
 out_err:
 	l2tp_packet_free(pack);
-	l2tp_disconnect(conn);
+	return -1;
+}
+
+static void l2tp_ppp_started(struct ppp_t *ppp)
+{
+	struct l2tp_conn_t *conn = container_of(ppp, typeof(*conn), ppp);
+	
+	log_ppp_debug("l2tp: ppp started\n");
+	
+	if (conf_hello_interval)
+		triton_timer_add(&conn->ctx, &conn->hello_timer, 0);
+}
+
+static void l2tp_ppp_finished(struct ppp_t *ppp)
+{
+	struct l2tp_conn_t *conn = container_of(ppp, typeof(*conn), ppp);
+
+	log_ppp_debug("l2tp: ppp finished\n");
+
+	if (conn->state != STATE_FIN) {
+		if (l2tp_terminate(conn, 0, 0))
+			triton_context_call(&conn->ctx, (triton_event_func)l2tp_disconnect, conn);
+	}
+}
+
+static void l2tp_conn_close(struct triton_context_t *ctx)
+{
+	struct l2tp_conn_t *conn = container_of(ctx, typeof(*conn), ctx);
+	
+	if (conn->state == STATE_PPP) {
+		conn->state = STATE_FIN;
+		ppp_terminate(&conn->ppp, 1);
+	}
+	
+	if (l2tp_terminate(conn, 0, 0))
+		l2tp_disconnect(conn);
 }
 
-static void l2tp_alloc(struct l2tp_serv_t *serv, struct l2tp_packet_t *pack, struct l2tp_attr_t *assigned_tid, struct l2tp_attr_t *framing_cap)
+static int l2tp_tunnel_alloc(struct l2tp_serv_t *serv, struct l2tp_packet_t *pack, struct l2tp_attr_t *assigned_tid, struct l2tp_attr_t *framing_cap)
 {
 	struct l2tp_conn_t *conn;
+	struct sockaddr_in addr;
 	uint16_t tid;
+	char *opt;
 
 	conn = mempool_alloc(l2tp_conn_pool);
 	if (!conn) {
 		log_emerg("l2tp: out of memory\n");
-		return;
+		return -1;
 	}
 
 	memset(conn, 0, sizeof(*conn));
+	INIT_LIST_HEAD(&conn->send_queue);
+
+	conn->hnd.fd = socket(PF_INET, SOCK_DGRAM, 0);
+	if (conn->hnd.fd < 0) {
+		log_error("l2tp: socket: %s\n", strerror(errno));
+		mempool_free(conn);
+		return -1;
+	}
+
+	memset(&addr, 0, sizeof(addr));
+	addr.sin_family = AF_INET;
+	addr.sin_port = 0;
+
+	opt = conf_get_opt("l2tp", "bind");
+	if (opt)
+		addr.sin_addr.s_addr = inet_addr(opt);
+	else
+		addr.sin_addr.s_addr = INADDR_ANY;
+	
+	if (bind(conn->hnd.fd, (struct sockaddr *)&addr, sizeof(addr))) {
+		log_error("l2tp: bind: %s\n", strerror(errno));
+		goto out_err;
+	}
+
+	if (connect(conn->hnd.fd, (struct sockaddr *)&pack->addr, sizeof(addr))) {
+		log_error("l2tp: connect: %s\n", strerror(errno));
+		goto out_err;
+	}
+ 
+	if (fcntl(conn->hnd.fd, F_SETFL, O_NONBLOCK)) {
+    log_emerg("l2tp: failed to set nonblocking mode: %s\n", strerror(errno));
+		goto out_err;
+	}
 
 	pthread_mutex_lock(&l2tp_lock);
 	for (tid = l2tp_tid + 1; tid != l2tp_tid; tid++) {
@@ -185,21 +267,29 @@ static void l2tp_alloc(struct l2tp_serv_t *serv, struct l2tp_packet_t *pack, str
 		if (conf_verbose)
 			log_warn("l2tp: no free tid available\n");
 		mempool_free(conn);
-		return;
+		return -1;
 	}
 
-	INIT_LIST_HEAD(&conn->recv_queue);
-	INIT_LIST_HEAD(&conn->send_queue);
+	conn->sid = 1;
 
-	conn->sock = dup(serv->hnd.fd);
 	memcpy(&conn->addr, &pack->addr, sizeof(pack->addr));
 	conn->peer_tid = assigned_tid->val.uint16;
 	conn->framing_cap = framing_cap->val.uint32;
 
+	conn->ctx.before_switch = log_switch;
+	conn->ctx.close = l2tp_conn_close;
+	conn->hnd.read = l2tp_conn_read;
 	conn->timeout_timer.expire = l2tp_timeout;
 	conn->timeout_timer.period = conf_timeout * 1000;
+	conn->rtimeout_timer.expire = l2tp_rtimeout;
+	conn->rtimeout_timer.period = conf_rtimeout * 1000;
+	conn->hello_timer.expire = l2tp_send_HELLO;
+	conn->hello_timer.period = conf_hello_interval * 1000;
 	conn->ctrl.ctx = &conn->ctx;
 	conn->ctrl.name = "l2tp";
+	conn->ctrl.started = l2tp_ppp_started;
+	conn->ctrl.finished = l2tp_ppp_finished;
+	conn->ctrl.max_mtu = 1420;
 
 	conn->ctrl.calling_station_id = _malloc(17);
 	conn->ctrl.called_station_id = _malloc(17);
@@ -210,6 +300,8 @@ static void l2tp_alloc(struct l2tp_serv_t *serv, struct l2tp_packet_t *pack, str
 	conn->ppp.ctrl = &conn->ctrl;
 
 	triton_context_register(&conn->ctx, &conn->ppp);
+	triton_md_register_handler(&conn->ctx, &conn->hnd);
+	triton_md_enable_handler(&conn->hnd, MD_MODE_READ);
 	triton_context_wakeup(&conn->ctx);
 
 	if (conf_verbose) {
@@ -219,17 +311,66 @@ static void l2tp_alloc(struct l2tp_serv_t *serv, struct l2tp_packet_t *pack, str
 	}
 
 	triton_context_call(&conn->ctx, (triton_event_func)l2tp_send_SCCRP, conn);
+
+	return 0;
+
+out_err:
+	close(conn->hnd.fd);
+	mempool_free(conn);
+	return -1;
 }
 
 static int l2tp_connect(struct l2tp_conn_t *conn)
 {
+	struct sockaddr_pppol2tp pppox_addr;
+	
+	pppox_addr.sa_family = AF_PPPOX;
+	pppox_addr.sa_protocol = PX_PROTO_OL2TP;
+	pppox_addr.pppol2tp.fd = conn->hnd.fd;
+	memcpy(&pppox_addr.pppol2tp.addr, &conn->addr, sizeof(conn->addr));
+	pppox_addr.pppol2tp.s_tunnel = conn->tid;
+	pppox_addr.pppol2tp.s_session = conn->sid;
+	pppox_addr.pppol2tp.d_tunnel = conn->peer_tid;
+	pppox_addr.pppol2tp.d_session = conn->peer_sid;
+
+	l2tp_nl_create_tunnel(conn->hnd.fd, conn->tid, conn->peer_tid);
+	l2tp_nl_create_session(conn->tid, conn->sid, conn->peer_sid);
+
+	conn->ppp.fd = socket(AF_PPPOX, SOCK_DGRAM, PX_PROTO_OL2TP);
+	if (!conn->ppp.fd) {
+		log_ppp_error("l2tp: socket(AF_PPPOX): %s\n", strerror(errno));
+		return -1;
+	}
+
+	if (connect(conn->ppp.fd, (struct sockaddr *)&pppox_addr, sizeof(pppox_addr))) {
+		log_ppp_error("l2tp: connect: %s\n", strerror(errno));
+		close(conn->ppp.fd);
+		return -1;
+	}
+
+	/*if (setsockopt(conn->ppp.fd, SOL_SOCKET, PPPOL2TP_SO_LNSMODE, &conn->ppp.fd, sizeof(conn->ppp.fd))) {
+		log_ppp_error("l2tp: setsockopt: %s\n", strerror(errno));
+		close(conn->ppp.fd);
+		return -1;
+	}*/
 
+	conn->ppp.chan_name = _strdup(inet_ntoa(conn->addr.sin_addr));
+	
+	triton_event_fire(EV_CTRL_STARTED, &conn->ppp);
+
+	if (establish_ppp(&conn->ppp)) {
+		close(conn->ppp.fd);
+		return -1;
+	}
+
+	conn->state = STATE_PPP;
+	
 	return 0;
 }
 
-static void l2tp_timeout(struct triton_timer_t *t)
+static void l2tp_rtimeout(struct triton_timer_t *t)
 {
-	struct l2tp_conn_t *conn = container_of(t, typeof(*conn), timeout_timer);
+	struct l2tp_conn_t *conn = container_of(t, typeof(*conn), rtimeout_timer);
 	struct l2tp_packet_t *pack;
 
 	if (!list_empty(&conn->send_queue)) {
@@ -239,59 +380,82 @@ static void l2tp_timeout(struct triton_timer_t *t)
 			pack->hdr.Nr = htons(conn->Nr + 1);
 			if (conf_verbose) {
 				log_ppp_info("send ");
-				l2tp_packet_print(conn->last_pack);
+				l2tp_packet_print(pack);
 			}
-			if (l2tp_packet_send(conn->sock, conn->last_pack) == 0)
+			if (l2tp_packet_send(conn->hnd.fd, pack) == 0)
 				return;
-		}
+		} else
+			l2tp_disconnect(conn);
 	}
-	
+}
+
+static void l2tp_timeout(struct triton_timer_t *t)
+{
+	struct l2tp_conn_t *conn = container_of(t, typeof(*conn), timeout_timer);
+	log_ppp_debug("l2tp: timeout\n");
 	l2tp_disconnect(conn);
 }
 
-static void l2tp_send(struct l2tp_conn_t *conn, struct l2tp_packet_t *pack)
+static int l2tp_send(struct l2tp_conn_t *conn, struct l2tp_packet_t *pack)
 {
 	conn->retransmit = 0;
 
 	pack->hdr.tid = htons(conn->peer_tid);
-	pack->hdr.sid = htons(conn->peer_sid);
+	//pack->hdr.sid = htons(conn->peer_sid);
 	pack->hdr.Nr = htons(conn->Nr + 1);
-	pack->hdr.Ns = htons(conn->Ns++);
+	pack->hdr.Ns = htons(conn->Ns);
+
+	if (!list_empty(&pack->attrs))
+		conn->Ns++;
 
 	if (conf_verbose) {
 		log_ppp_info("send ");
-		l2tp_packet_print(conn->last_pack);
+		l2tp_packet_print(pack);
 	}
 
-	if (l2tp_packet_send(conn->sock, pack))
+	if (l2tp_packet_send(conn->hnd.fd, pack))
 		goto out_err;
 
-	if (!conn->timeout_timer.tpd)
-		triton_timer_add(&conn->ctx, &conn->timeout_timer, 0);
-
-	if (!list_empty(&pack->attrs))
+	if (!list_empty(&pack->attrs)) {
 		list_add_tail(&pack->entry, &conn->send_queue);
+		if (!conn->rtimeout_timer.tpd)
+			triton_timer_add(&conn->ctx, &conn->rtimeout_timer, 0);
+	}
 	
-	return;
+	return 0;
 
 out_err:
 	l2tp_packet_free(pack);
-	l2tp_disconnect(conn);
+	return -1;
 }
 
-static void l2tp_send_ZLB(struct l2tp_conn_t *conn)
+static int l2tp_send_ZLB(struct l2tp_conn_t *conn)
 {
 	struct l2tp_packet_t *pack;
 
 	pack = l2tp_packet_alloc(2, 0, &conn->addr);
+	if (!pack)
+		return -1;
+
+	if (l2tp_send(conn, pack))
+		return -1;
+
+	return 0;
+}
+
+static void l2tp_send_HELLO(struct triton_timer_t *t)
+{
+	struct l2tp_conn_t *conn = container_of(t, typeof(*conn), hello_timer);
+	struct l2tp_packet_t *pack;
+
+	pack = l2tp_packet_alloc(2, Message_Type_Hello, &conn->addr);
 	if (!pack) {
 		l2tp_disconnect(conn);
 		return;
 	}
 
-	l2tp_send(conn, pack);
-
-	return;
+	if (l2tp_send(conn, pack))
+		l2tp_disconnect(conn);
 }
 
 static void l2tp_send_SCCRP(struct l2tp_conn_t *conn)
@@ -299,22 +463,25 @@ static void l2tp_send_SCCRP(struct l2tp_conn_t *conn)
 	struct l2tp_packet_t *pack;
 
 	pack = l2tp_packet_alloc(2, Message_Type_Start_Ctrl_Conn_Reply, &conn->addr);
-	if (!pack) {
-		l2tp_disconnect(conn);
-		return;
-	}
+	if (!pack)
+		goto out;
 	
-	if (l2tp_packet_add_int16(pack, Protocol_Version, L2TP_V2_PROTOCOL_VERSION))
+	if (l2tp_packet_add_int16(pack, Protocol_Version, L2TP_V2_PROTOCOL_VERSION, 1))
 		goto out_err;
-	if (l2tp_packet_add_string(pack, Host_Name, conf_host_name))
+	if (l2tp_packet_add_string(pack, Host_Name, conf_host_name, 1))
 		goto out_err;
-	if (l2tp_packet_add_int32(pack, Framing_Capabilities, conn->framing_cap))
+	if (l2tp_packet_add_int32(pack, Framing_Capabilities, conn->framing_cap, 1))
 		goto out_err;
-	if (l2tp_packet_add_int16(pack, Assigned_Tunnel_ID, conn->tid))
+	if (l2tp_packet_add_int16(pack, Assigned_Tunnel_ID, conn->tid, 1))
 		goto out_err;
 
+	if (l2tp_send(conn, pack))
+		goto out;
 
-	l2tp_send(conn, pack);
+	if (!conn->timeout_timer.tpd)
+		triton_timer_add(&conn->ctx, &conn->timeout_timer, 0);
+	else
+		triton_timer_mod(&conn->timeout_timer, 0);
 
 	conn->state = STATE_WAIT_SCCCN;
 
@@ -322,34 +489,83 @@ static void l2tp_send_SCCRP(struct l2tp_conn_t *conn)
 
 out_err:
 	l2tp_packet_free(pack);
+out:
 	l2tp_disconnect(conn);
 }
 
-static void l2tp_send_OCRP(struct l2tp_conn_t *conn)
+static int l2tp_send_ICRP(struct l2tp_conn_t *conn)
 {
 	struct l2tp_packet_t *pack;
 
-	pack = l2tp_packet_alloc(2, Message_Type_Outgoing_Call_Reply, &conn->addr);
-	if (!pack) {
-		l2tp_disconnect(conn);
-		return;
-	}
-	
-	conn->sid = 1;
+	pack = l2tp_packet_alloc(2, Message_Type_Incoming_Call_Reply, &conn->addr);
+	if (!pack)
+		return -1;
 
-	if (l2tp_packet_add_int16(pack, Assigned_Session_ID, conn->sid))
+	pack->hdr.sid = htons(conn->peer_sid);
+	
+	if (l2tp_packet_add_int16(pack, Assigned_Session_ID, conn->sid, 1))
 		goto out_err;
 
 	l2tp_send(conn, pack);
 
-	return;
+	if (!conn->timeout_timer.tpd)
+		triton_timer_add(&conn->ctx, &conn->timeout_timer, 0);
+	else
+		triton_timer_mod(&conn->timeout_timer, 0);
+	
+	conn->state1 = STATE_WAIT_ICCN;
+	
+	return 0;
 
 out_err:
 	l2tp_packet_free(pack);
-	l2tp_disconnect(conn);
+	return -1;
+}
+
+static int l2tp_send_OCRQ(struct l2tp_conn_t *conn)
+{
+	struct l2tp_packet_t *pack;
+
+	pack = l2tp_packet_alloc(2, Message_Type_Outgoing_Call_Request, &conn->addr);
+	if (!pack)
+		return -1;
+	
+	pack->hdr.sid = htons(conn->peer_sid);
+
+	if (l2tp_packet_add_int16(pack, Assigned_Session_ID, conn->sid, 1))
+		goto out_err;
+	if (l2tp_packet_add_int32(pack, Call_Serial_Number, 0, 1))
+		goto out_err;
+	if (l2tp_packet_add_int32(pack, Minimum_BPS, 100, 1))
+		goto out_err;
+	if (l2tp_packet_add_int32(pack, Maximum_BPS, 100000, 1))
+		goto out_err;
+	if (l2tp_packet_add_int32(pack, Bearer_Type, 3, 1))
+		goto out_err;
+	if (l2tp_packet_add_int32(pack, Framing_Type, 3, 1))
+		goto out_err;
+	if (l2tp_packet_add_string(pack, Called_Number, "", 1))
+		goto out_err;
+
+	if (l2tp_send(conn, pack))
+		return -1;
+
+	if (!conn->timeout_timer.tpd)
+		triton_timer_add(&conn->ctx, &conn->timeout_timer, 0);
+	else
+		triton_timer_mod(&conn->timeout_timer, 0);
+	
+	conn->state2 = STATE_WAIT_OCRP;
+	
+	return 0;
+
+out_err:
+	l2tp_packet_free(pack);
+	return -1;
 }
 
-static void l2tp_recv_SCCRQ(struct l2tp_serv_t *serv, struct l2tp_packet_t *pack)
+
+static int l2tp_recv_SCCRQ(struct l2tp_serv_t *serv, struct l2tp_packet_t *pack)
 {
 	struct l2tp_attr_t *attr;
 	struct l2tp_attr_t *protocol_version = NULL;
@@ -372,7 +588,7 @@ static void l2tp_recv_SCCRQ(struct l2tp_serv_t *serv, struct l2tp_packet_t *pack
 			case Challenge:
 				if (conf_verbose)
 					log_warn("l2tp: Challenge in SCCRQ is not supported\n");
-				return;
+				return -1;
 			case Assigned_Connection_ID:
 				assigned_cid = attr;
 				break;
@@ -382,7 +598,7 @@ static void l2tp_recv_SCCRQ(struct l2tp_serv_t *serv, struct l2tp_packet_t *pack
 			case Message_Digest:
 				if (conf_verbose)
 					log_warn("l2tp: Message-Digest is not supported\n");
-				return;
+				return -1;
 		}
 	}
 
@@ -390,59 +606,69 @@ static void l2tp_recv_SCCRQ(struct l2tp_serv_t *serv, struct l2tp_packet_t *pack
 		if (!protocol_version) {
 			if (conf_verbose)
 				log_warn("l2tp: SCCRQ: no Protocol-Version present in message\n");
-			return;
+			return -1;
 		}
 		if (protocol_version->val.uint16 != L2TP_V2_PROTOCOL_VERSION) {
 			if (conf_verbose)
 				log_warn("l2tp: protocol version %02x is not supported\n", protocol_version->val.uint16);
-			return;
+			return -1;
 		}
 		if (!framing_cap) {
 			if (conf_verbose)
 				log_warn("l2tp: SCCRQ: no Framing-Capabilities present in message\n");
-			return;
+			return -1;
 		}
 		
-		l2tp_alloc(serv, pack, assigned_tid, framing_cap);
+		if (l2tp_tunnel_alloc(serv, pack, assigned_tid, framing_cap))
+			return -1;
 
 	} else if (assigned_cid) {
 		// not yet implemented
-		return;
+		return 0;
 	} else {
 		if (conf_verbose)
 			log_warn("l2tp: SCCRQ: no Assigned-Tunnel-ID or Assigned-Connection-ID present in message\n");
-		return;
+		return -1;
 	}
+
+	return 0;
 }
 
-static void l2tp_recv_SCCCN(struct l2tp_conn_t *conn, struct l2tp_packet_t *pack)
+static int l2tp_recv_SCCCN(struct l2tp_conn_t *conn, struct l2tp_packet_t *pack)
 {
 	if (conn->state == STATE_WAIT_SCCCN) {
-		l2tp_send_ZLB(conn);
-		conn->state = STATE_WAIT_OCRQ;
+		triton_timer_mod(&conn->timeout_timer, 0);
+		conn->state = STATE_ESTB;
+		conn->state1 = STATE_WAIT_ICRQ;
 	}
 	else
 		log_ppp_warn("l2tp: unexpected SCCCN\n");
+	
+	return 0;
 }
 
-static void l2tp_recv_StopCCN(struct l2tp_conn_t *conn, struct l2tp_packet_t *pack)
+static int l2tp_recv_StopCCN(struct l2tp_conn_t *conn, struct l2tp_packet_t *pack)
 {
-
+	l2tp_send_ZLB(conn);
+	return -1;
 }
 
-static void l2tp_recv_HELLO(struct l2tp_conn_t *conn, struct l2tp_packet_t *pack)
+static int l2tp_recv_HELLO(struct l2tp_conn_t *conn, struct l2tp_packet_t *pack)
 {
-
+	if (l2tp_send_ZLB(conn))
+		return -1;
+	
+	return 0;
 }
 
-static void l2tp_recv_OCRQ(struct l2tp_conn_t *conn, struct l2tp_packet_t *pack)
+static int l2tp_recv_ICRQ(struct l2tp_conn_t *conn, struct l2tp_packet_t *pack)
 {
 	struct l2tp_attr_t *attr;
 	struct l2tp_attr_t *assigned_sid = NULL;
 
-	if (conn->state != STATE_WAIT_OCRQ) {
-		log_ppp_warn("l2tp: unexpected OCRQ\n");
-		return;
+	if (conn->state1 != STATE_WAIT_ICRQ) {
+		log_ppp_warn("l2tp: unexpected ICRQ\n");
+		return 0;
 	}
 
 	list_for_each_entry(attr, &pack->attrs, entry) {
@@ -450,20 +676,21 @@ static void l2tp_recv_OCRQ(struct l2tp_conn_t *conn, struct l2tp_packet_t *pack)
 			case Assigned_Session_ID:
 				assigned_sid = attr;
 				break;
+			case Message_Type:
 			case Call_Serial_Number:
-			case Minimum_BPS:
-			case Maximum_BPS:
 			case Bearer_Type:
-			case Framing_Type:
+			case Calling_Number:
 			case Called_Number:
 			case Sub_Address:
+			case Physical_Channel_ID:
 				break;
 			default:
 				if (attr->M) {
 					if (conf_verbose) {
-						log_ppp_warn("l2tp: OCRQ: unknown attribute %i\n", attr->attr->id);
-						l2tp_terminate(conn, 2, 8);
-						return;
+						log_ppp_warn("l2tp: ICRQ: unknown attribute %i\n", attr->attr->id);
+						if (l2tp_terminate(conn, 2, 8))
+							return -1;
+						return 0;
 					}
 				}
 		}
@@ -471,132 +698,222 @@ static void l2tp_recv_OCRQ(struct l2tp_conn_t *conn, struct l2tp_packet_t *pack)
 
 	if (!assigned_sid) {
 		if (conf_verbose)
-			log_ppp_warn("l2tp: OCRQ: no Assigned-Session-ID attribute present in message\n");
-		l2tp_terminate(conn, 2, 0);
-		return;
+			log_ppp_warn("l2tp: ICRQ: no Assigned-Session-ID attribute present in message\n");
+		if (l2tp_terminate(conn, 2, 0))
+			return -1;
 	}
 
-	l2tp_send_OCRP(conn);
-
 	conn->peer_sid = assigned_sid->val.uint16;
 
-	l2tp_connect(conn);
+	if (l2tp_send_ICRP(conn))
+		return -1;
+		
+	if (l2tp_send_OCRQ(conn))
+		return -1;
+	
+	return 0;
 }
 
-static void l2tp_recv_OCCN(struct l2tp_conn_t *conn, struct l2tp_packet_t *pack)
+static int l2tp_recv_ICCN(struct l2tp_conn_t *conn, struct l2tp_packet_t *pack)
 {
+	if (conn->state1 != STATE_WAIT_ICCN) {
+		log_ppp_warn("l2tp: unexpected ICCN\n");
+		return 0;
+	}
+
+	conn->state1 = STATE_ESTB;
 
+	if (l2tp_connect(conn)) {
+		if (l2tp_terminate(conn, 2, 0))
+			return -1;
+		return 0;
+	}
+
+	if (l2tp_send_ZLB(conn))
+		return -1;
+	
+	triton_timer_del(&conn->timeout_timer);
+
+	return 0;
 }
 
-static void l2tp_recv_CDN(struct l2tp_conn_t *conn, struct l2tp_packet_t *pack)
+static int  l2tp_recv_OCRP(struct l2tp_conn_t *conn, struct l2tp_packet_t *pack)
 {
+	if (conn->state2 != STATE_WAIT_OCRP) {
+		log_ppp_warn("l2tp: unexpected OCRP\n");
+		return 0;
+	}
+
+	conn->state2 = STATE_WAIT_OCCN;
 
+	return 0;
 }
 
-static void l2tp_recv_SLI(struct l2tp_conn_t *conn, struct l2tp_packet_t *pack)
+static int l2tp_recv_OCCN(struct l2tp_conn_t *conn, struct l2tp_packet_t *pack)
 {
+	if (conn->state2 != STATE_WAIT_OCCN) {
+		log_ppp_warn("l2tp: unexpected OCCN\n");
+		return 0;
+	}
+
+	conn->state2 = STATE_ESTB;
 
+	return 0;
 }
 
-static void l2tp_ctx_recv(struct l2tp_conn_t *conn)
+static int l2tp_recv_CDN(struct l2tp_conn_t *conn, struct l2tp_packet_t *pack)
 {
-	struct l2tp_packet_t *pack;
-	struct l2tp_attr_t *msg_type;
-
-	pthread_mutex_lock(&conn->lock);
-	if (list_empty(&conn->recv_queue)) {
-		pthread_mutex_unlock(&conn->lock);
-		return;
+	if (ntohs(pack->hdr.sid) != conn->sid) {
+		if (conf_verbose)
+			log_warn("l2tp: sid %i is incorrect\n", ntohs(pack->hdr.sid));
+		return 0;
 	}
-	pack = list_entry(conn->recv_queue.next, typeof(*pack), entry);
-	list_del(&pack->entry);
-	pthread_mutex_unlock(&conn->lock);
 
-	if (conf_verbose) {
-		log_ppp_info("recv ");
-		l2tp_packet_print(pack);
+	if (conn->state == STATE_PPP) {
+		conn->state = STATE_FIN;
+		ppp_terminate(&conn->ppp, 1);
 	}
+	
+	if (l2tp_terminate(conn, 0, 0))
+		return -1;
+	
+	return 0;
+}
 
-	if (ntohs(pack->hdr.Ns) == conn->Nr + 1) {
-		conn->Nr++;
-		if (!list_empty(&conn->send_queue)) {
-			pack = list_entry(conn->send_queue.next, typeof(*pack), entry);
-			list_del(&pack->entry);
+static int l2tp_recv_SLI(struct l2tp_conn_t *conn, struct l2tp_packet_t *pack)
+{
+	return 0;
+}
+
+static int l2tp_conn_read(struct triton_md_handler_t *h)
+{
+	struct l2tp_conn_t *conn = container_of(h, typeof(*conn), hnd);
+	struct l2tp_packet_t *pack, *p;
+	struct l2tp_attr_t *msg_type;
+
+	while (1) {
+		pack = NULL;
+
+		if (l2tp_recv(h->fd, &pack))
+			return 0;
+
+		if (!pack)
+			continue;
+
+		if (ntohs(pack->hdr.tid) != conn->tid) {
+			if (conf_verbose)
+				log_warn("l2tp: incorrect tid %i in tunnel %i\n", ntohs(pack->hdr.tid), conn->tid);
 			l2tp_packet_free(pack);
-			conn->retransmit = 0;
+			continue;
 		}
-		if (!list_empty(&conn->send_queue))
-			triton_timer_mod(&conn->timeout_timer, 0);
-		else if (conn->timeout_timer.tpd)
-			triton_timer_del(&conn->timeout_timer);
-	} else {
-		if (ntohs(pack->hdr.Ns) < conn->Nr + 1 || (ntohs(pack->hdr.Ns > 32767 && conn->Nr + 1 < 32767))) {
-			log_ppp_debug("duplicate packet\n");
-			l2tp_send_ZLB(conn);
-		} else
-			log_ppp_debug("reordered packet\n");
-		l2tp_packet_free(pack);
-		return;
-	}
 
-	if (list_empty(&pack->attrs)) {
-		l2tp_packet_free(pack);
-		return;
-	}
+		if (conf_verbose) {
+			log_ppp_info("recv ");
+			l2tp_packet_print(pack);
+		}
 
-	msg_type = list_entry(pack->attrs.next, typeof(*msg_type), entry);
+		if (ntohs(pack->hdr.Ns) == conn->Nr + 1) {
+			if (!list_empty(&pack->attrs))
+				conn->Nr++;
+			if (!list_empty(&conn->send_queue)) {
+				p = list_entry(conn->send_queue.next, typeof(*pack), entry);
+				list_del(&p->entry);
+				l2tp_packet_free(p);
+				conn->retransmit = 0;
+			}
+			if (!list_empty(&conn->send_queue))
+				triton_timer_mod(&conn->rtimeout_timer, 0);
+			else {
+				if (conn->rtimeout_timer.tpd)
+					triton_timer_del(&conn->rtimeout_timer);
+				if (conn->state == STATE_FIN)
+					goto drop;
+			}
+		} else {
+			if (ntohs(pack->hdr.Ns) < conn->Nr + 1 || (ntohs(pack->hdr.Ns > 32767 && conn->Nr + 1 < 32767))) {
+				log_ppp_debug("duplicate packet\n");
+				if (l2tp_send_ZLB(conn))
+					goto drop;
+			} else
+				log_ppp_debug("reordered packet\n");
+			l2tp_packet_free(pack);
+			continue;
+		}
 
-	if (msg_type->attr->id != Message_Type) {
-		if (conf_verbose)
-			log_ppp_error("l2tp: first attribute is not Message-Type, dropping connection...\n");
-		goto drop;
-	}
+		if (list_empty(&pack->attrs)) {
+			l2tp_packet_free(pack);
+			continue;
+		}
 
-	switch (msg_type->val.uint16) {
-		case Message_Type_Start_Ctrl_Conn_Connected:
-			l2tp_recv_SCCCN(conn, pack);
-			break;
-		case Message_Type_Stop_Ctrl_Conn_Notify:
-			l2tp_recv_StopCCN(conn, pack);
-			break;
-		case Message_Type_Hello:
-			l2tp_recv_HELLO(conn, pack);
-			break;
-		case Message_Type_Outgoing_Call_Request:
-			l2tp_recv_OCRQ(conn, pack);
-			break;
-		case Message_Type_Outgoing_Call_Connected:
-			l2tp_recv_OCCN(conn, pack);
-			break;
-		case Message_Type_Call_Disconnect_Notify:
-			l2tp_recv_CDN(conn, pack);
-			break;
-		case Message_Type_Set_Link_Info:
-			l2tp_recv_SLI(conn, pack);
-			break;
-		case Message_Type_Start_Ctrl_Conn_Reply:
-		case Message_Type_Outgoing_Call_Reply:
-		case Message_Type_Incoming_Call_Request:
-		case Message_Type_Incoming_Call_Reply:
-		case Message_Type_Incoming_Call_Connected:
-		case Message_Type_WAN_Error_Notify:
-			if (conf_verbose)
-				log_warn("l2tp: unexpected Message-Type %i\n", msg_type->val.uint16);
-			break;
-		default:
+		msg_type = list_entry(pack->attrs.next, typeof(*msg_type), entry);
+
+		if (msg_type->attr->id != Message_Type) {
 			if (conf_verbose)
-				log_warn("l2tp: unknown Message-Type %i\n", msg_type->val.uint16);
-			if (msg_type->M)
-				l2tp_terminate(conn, 2, 8);
-	}
+				log_ppp_error("l2tp: first attribute is not Message-Type, dropping connection...\n");
+			goto drop;
+		}
 
-	l2tp_packet_free(pack);
+		switch (msg_type->val.uint16) {
+			case Message_Type_Start_Ctrl_Conn_Connected:
+				if (l2tp_recv_SCCCN(conn, pack))
+					goto drop;
+				break;
+			case Message_Type_Stop_Ctrl_Conn_Notify:
+				if (l2tp_recv_StopCCN(conn, pack))
+					goto drop;
+				break;
+			case Message_Type_Hello:
+				if (l2tp_recv_HELLO(conn, pack))
+					goto drop;
+				break;
+			case Message_Type_Incoming_Call_Request:
+				if (l2tp_recv_ICRQ(conn, pack))
+					goto drop;
+				break;
+			case Message_Type_Incoming_Call_Connected:
+				if (l2tp_recv_ICCN(conn, pack))
+					goto drop;
+				break;
+			case Message_Type_Outgoing_Call_Reply:
+				if (l2tp_recv_OCRP(conn, pack))
+					goto drop;
+				break;
+			case Message_Type_Outgoing_Call_Connected:
+				if (l2tp_recv_OCCN(conn, pack))
+					goto drop;
+				break;
+			case Message_Type_Call_Disconnect_Notify:
+				if (l2tp_recv_CDN(conn, pack))
+					goto drop;
+				break;
+			case Message_Type_Set_Link_Info:
+				if (l2tp_recv_SLI(conn, pack))
+					goto drop;
+				break;
+			case Message_Type_Start_Ctrl_Conn_Request:
+			case Message_Type_Start_Ctrl_Conn_Reply:
+			case Message_Type_Outgoing_Call_Request:
+			case Message_Type_Incoming_Call_Reply:
+			case Message_Type_WAN_Error_Notify:
+				if (conf_verbose)
+					log_warn("l2tp: unexpected Message-Type %i\n", msg_type->val.uint16);
+				break;
+			default:
+				if (conf_verbose)
+					log_warn("l2tp: unknown Message-Type %i\n", msg_type->val.uint16);
+				if (msg_type->M) {
+					if (l2tp_terminate(conn, 2, 8))
+						goto drop;
+				}
+		}
 
-	return;
+		l2tp_packet_free(pack);
+	}
 
 drop:
 	l2tp_packet_free(pack);
 	l2tp_disconnect(conn);
+	return -1;
 }
 
 static int l2tp_udp_read(struct triton_md_handler_t *h)
@@ -604,7 +921,6 @@ static int l2tp_udp_read(struct triton_md_handler_t *h)
 	struct l2tp_serv_t *serv = container_of(h, typeof(*serv), hnd);
 	struct l2tp_packet_t *pack;
 	struct l2tp_attr_t *msg_type;
-	struct l2tp_conn_t *conn = NULL;
 
 	while (1) {
 		pack = NULL;
@@ -615,20 +931,14 @@ static int l2tp_udp_read(struct triton_md_handler_t *h)
 		if (!pack)
 			continue;
 
-		if (pack->hdr.ver == 2 && pack->hdr.tid) {
-				conn = l2tp_conn_lookup(ntohs(pack->hdr.tid));
-				if (!conn) {
-					if (conf_verbose)
-						log_warn("l2tp: tunnel %i not found\n", ntohs(pack->hdr.tid));
-					goto skip;
-				}
-				
-				list_add_tail(&pack->entry, &conn->recv_queue);
-				triton_context_call(&conn->ctx, (triton_event_func)l2tp_ctx_recv, conn);
-				pthread_mutex_unlock(&conn->lock);
-				continue;
+		if (iprange_client_check(pack->addr.sin_addr.s_addr)) {
+			log_warn("l2tp: IP is out of client-ip-range, droping connection...\n");
+			goto skip;
 		}
 
+		if (pack->hdr.tid)
+			goto skip;
+
 		if (list_empty(&pack->attrs)) {
 			if (conf_verbose)
 				log_warn("l2tp: to Message-Type attribute present\n");
@@ -643,7 +953,7 @@ static int l2tp_udp_read(struct triton_md_handler_t *h)
 		}
 
 		if (msg_type->val.uint16 == Message_Type_Start_Ctrl_Conn_Request)
-				l2tp_recv_SCCRQ(serv, pack);
+			l2tp_recv_SCCRQ(serv, pack);
 		else {
 			if (conf_verbose) {
 				log_warn("recv (unexpected) ");
@@ -660,6 +970,9 @@ skip:
 static void l2tp_udp_close(struct triton_context_t *ctx)
 {
 	struct l2tp_serv_t *serv = container_of(ctx, typeof(*serv), ctx);
+	triton_md_unregister_handler(&serv->hnd);
+	close(serv->hnd.fd);
+	triton_context_unregister(&serv->ctx);
 }
 
 static struct l2tp_serv_t udp_serv =
@@ -693,7 +1006,9 @@ static void start_udp_server(void)
 	else
 		addr.sin_addr.s_addr = htonl(INADDR_ANY);
   
-  setsockopt(udp_serv.hnd.fd, SOL_SOCKET, SO_REUSEADDR, &udp_serv.hnd.fd, 4);  
+  setsockopt(udp_serv.hnd.fd, SOL_SOCKET, SO_REUSEADDR, &udp_serv.hnd.fd, sizeof(udp_serv.hnd.fd));
+  setsockopt(udp_serv.hnd.fd, SOL_SOCKET, SO_NO_CHECK, &udp_serv.hnd.fd, sizeof(udp_serv.hnd.fd));
+
   if (bind (udp_serv.hnd.fd, (struct sockaddr *) &addr, sizeof (addr)) < 0) {
     log_emerg("l2tp: failed to bind socket: %s\n", strerror(errno));
 		close(udp_serv.hnd.fd);
@@ -727,6 +1042,26 @@ static void __init l2tp_init(void)
 	if (opt && atoi(opt) > 0)
 		conf_verbose = 1;
 
+	opt = conf_get_opt("l2tp", "hello_interval");
+	if (opt && atoi(opt) > 0)
+		conf_hello_interval = atoi(opt);
+
+	opt = conf_get_opt("l2tp", "timeout");
+	if (opt && atoi(opt) > 0)
+		conf_timeout = atoi(opt);
+
+	opt = conf_get_opt("l2tp", "rtimeout");
+	if (opt && atoi(opt) > 0)
+		conf_rtimeout = atoi(opt);
+
+	opt = conf_get_opt("l2tp", "retransmit");
+	if (opt && atoi(opt) > 0)
+		conf_retransmit = atoi(opt);
+
+	opt = conf_get_opt("l2tp", "host-name");
+	if (opt && atoi(opt) > 0)
+		conf_host_name = opt;
+
 	start_udp_server();
 }
 
diff --git a/accel-pptpd/ctrl/l2tp/l2tp.h b/accel-pptpd/ctrl/l2tp/l2tp.h
index a99bdc35..d6ef5f8c 100644
--- a/accel-pptpd/ctrl/l2tp/l2tp.h
+++ b/accel-pptpd/ctrl/l2tp/l2tp.h
@@ -76,9 +76,13 @@ void l2tp_packet_free(struct l2tp_packet_t *);
 void l2tp_packet_print(struct l2tp_packet_t *);
 struct l2tp_packet_t *l2tp_packet_alloc(int ver, int msg_type, struct sockaddr_in *addr);
 int l2tp_packet_send(int sock, struct l2tp_packet_t *);
-int l2tp_packet_add_int16(struct l2tp_packet_t *pack, int id, int16_t val);
-int l2tp_packet_add_int32(struct l2tp_packet_t *pack, int id, int32_t val);
-int l2tp_packet_add_string(struct l2tp_packet_t *pack, int id, const char *val);
-int l2tp_packet_add_octets(struct l2tp_packet_t *pack, int id, const uint8_t *val, int size);
+int l2tp_packet_add_int16(struct l2tp_packet_t *pack, int id, int16_t val, int M);
+int l2tp_packet_add_int32(struct l2tp_packet_t *pack, int id, int32_t val, int M);
+int l2tp_packet_add_string(struct l2tp_packet_t *pack, int id, const char *val, int M);
+int l2tp_packet_add_octets(struct l2tp_packet_t *pack, int id, const uint8_t *val, int size, int M);
+
+void l2tp_nl_create_tunnel(int fd, int tid, int peer_tid);
+void l2tp_nl_create_session(int tid, int sid, int peer_sid);
+void l2tp_nl_delete_tunnel(int tid);
 
 #endif
diff --git a/accel-pptpd/ctrl/l2tp/l2tp_kernel.h b/accel-pptpd/ctrl/l2tp/l2tp_kernel.h
new file mode 100644
index 00000000..4bdb31df
--- /dev/null
+++ b/accel-pptpd/ctrl/l2tp/l2tp_kernel.h
@@ -0,0 +1,163 @@
+/*
+ * L2TP-over-IP socket for L2TPv3.
+ *
+ * Author: James Chapman <jchapman@katalix.com>
+ */
+
+#ifndef _LINUX_L2TP_H_
+#define _LINUX_L2TP_H_
+
+#include <linux/types.h>
+#ifdef __KERNEL__
+#include <linux/socket.h>
+#include <linux/in.h>
+#else
+#include <netinet/in.h>
+#endif
+
+#define IPPROTO_L2TP		115
+
+/**
+ * struct sockaddr_l2tpip - the sockaddr structure for L2TP-over-IP sockets
+ * @l2tp_family:  address family number AF_L2TPIP.
+ * @l2tp_addr:    protocol specific address information
+ * @l2tp_conn_id: connection id of tunnel
+ */
+#define __SOCK_SIZE__	16		/* sizeof(struct sockaddr)	*/
+struct sockaddr_l2tpip {
+	/* The first fields must match struct sockaddr_in */
+	sa_family_t	l2tp_family;	/* AF_INET */
+	__be16		l2tp_unused;	/* INET port number (unused) */
+	struct in_addr	l2tp_addr;	/* Internet address */
+
+	__u32		l2tp_conn_id;	/* Connection ID of tunnel */
+
+	/* Pad to size of `struct sockaddr'. */
+	unsigned char	__pad[sizeof(struct sockaddr) - sizeof(sa_family_t) -
+			      sizeof(__be16) - sizeof(struct in_addr) -
+			      sizeof(__u32)];
+};
+
+/*****************************************************************************
+ *  NETLINK_GENERIC netlink family.
+ *****************************************************************************/
+
+/*
+ * Commands.
+ * Valid TLVs of each command are:-
+ * TUNNEL_CREATE	- CONN_ID, pw_type, netns, ifname, ipinfo, udpinfo, udpcsum, vlanid
+ * TUNNEL_DELETE	- CONN_ID
+ * TUNNEL_MODIFY	- CONN_ID, udpcsum
+ * TUNNEL_GETSTATS	- CONN_ID, (stats)
+ * TUNNEL_GET		- CONN_ID, (...)
+ * SESSION_CREATE	- SESSION_ID, PW_TYPE, offset, data_seq, cookie, peer_cookie, offset, l2spec
+ * SESSION_DELETE	- SESSION_ID
+ * SESSION_MODIFY	- SESSION_ID, data_seq
+ * SESSION_GET		- SESSION_ID, (...)
+ * SESSION_GETSTATS	- SESSION_ID, (stats)
+ *
+ */
+enum {
+	L2TP_CMD_NOOP,
+	L2TP_CMD_TUNNEL_CREATE,
+	L2TP_CMD_TUNNEL_DELETE,
+	L2TP_CMD_TUNNEL_MODIFY,
+	L2TP_CMD_TUNNEL_GET,
+	L2TP_CMD_SESSION_CREATE,
+	L2TP_CMD_SESSION_DELETE,
+	L2TP_CMD_SESSION_MODIFY,
+	L2TP_CMD_SESSION_GET,
+	__L2TP_CMD_MAX,
+};
+
+#define L2TP_CMD_MAX			(__L2TP_CMD_MAX - 1)
+
+/*
+ * ATTR types defined for L2TP
+ */
+enum {
+	L2TP_ATTR_NONE,			/* no data */
+	L2TP_ATTR_PW_TYPE,		/* u16, enum l2tp_pwtype */
+	L2TP_ATTR_ENCAP_TYPE,		/* u16, enum l2tp_encap_type */
+	L2TP_ATTR_OFFSET,		/* u16 */
+	L2TP_ATTR_DATA_SEQ,		/* u16 */
+	L2TP_ATTR_L2SPEC_TYPE,		/* u8, enum l2tp_l2spec_type */
+	L2TP_ATTR_L2SPEC_LEN,		/* u8, enum l2tp_l2spec_type */
+	L2TP_ATTR_PROTO_VERSION,	/* u8 */
+	L2TP_ATTR_IFNAME,		/* string */
+	L2TP_ATTR_CONN_ID,		/* u32 */
+	L2TP_ATTR_PEER_CONN_ID,		/* u32 */
+	L2TP_ATTR_SESSION_ID,		/* u32 */
+	L2TP_ATTR_PEER_SESSION_ID,	/* u32 */
+	L2TP_ATTR_UDP_CSUM,		/* u8 */
+	L2TP_ATTR_VLAN_ID,		/* u16 */
+	L2TP_ATTR_COOKIE,		/* 0, 4 or 8 bytes */
+	L2TP_ATTR_PEER_COOKIE,		/* 0, 4 or 8 bytes */
+	L2TP_ATTR_DEBUG,		/* u32 */
+	L2TP_ATTR_RECV_SEQ,		/* u8 */
+	L2TP_ATTR_SEND_SEQ,		/* u8 */
+	L2TP_ATTR_LNS_MODE,		/* u8 */
+	L2TP_ATTR_USING_IPSEC,		/* u8 */
+	L2TP_ATTR_RECV_TIMEOUT,		/* msec */
+	L2TP_ATTR_FD,			/* int */
+	L2TP_ATTR_IP_SADDR,		/* u32 */
+	L2TP_ATTR_IP_DADDR,		/* u32 */
+	L2TP_ATTR_UDP_SPORT,		/* u16 */
+	L2TP_ATTR_UDP_DPORT,		/* u16 */
+	L2TP_ATTR_MTU,			/* u16 */
+	L2TP_ATTR_MRU,			/* u16 */
+	L2TP_ATTR_STATS,		/* nested */
+	__L2TP_ATTR_MAX,
+};
+
+#define L2TP_ATTR_MAX			(__L2TP_ATTR_MAX - 1)
+
+/* Nested in L2TP_ATTR_STATS */
+enum {
+	L2TP_ATTR_STATS_NONE,		/* no data */
+	L2TP_ATTR_TX_PACKETS,		/* u64 */
+	L2TP_ATTR_TX_BYTES,		/* u64 */
+	L2TP_ATTR_TX_ERRORS,		/* u64 */
+	L2TP_ATTR_RX_PACKETS,		/* u64 */
+	L2TP_ATTR_RX_BYTES,		/* u64 */
+	L2TP_ATTR_RX_SEQ_DISCARDS,	/* u64 */
+	L2TP_ATTR_RX_OOS_PACKETS,	/* u64 */
+	L2TP_ATTR_RX_ERRORS,		/* u64 */
+	__L2TP_ATTR_STATS_MAX,
+};
+
+#define L2TP_ATTR_STATS_MAX		(__L2TP_ATTR_STATS_MAX - 1)
+
+enum l2tp_pwtype {
+	L2TP_PWTYPE_NONE = 0x0000,
+	L2TP_PWTYPE_ETH_VLAN = 0x0004,
+	L2TP_PWTYPE_ETH = 0x0005,
+	L2TP_PWTYPE_PPP = 0x0007,
+	L2TP_PWTYPE_PPP_AC = 0x0008,
+	L2TP_PWTYPE_IP = 0x000b,
+	__L2TP_PWTYPE_MAX
+};
+
+enum l2tp_l2spec_type {
+	L2TP_L2SPECTYPE_NONE,
+	L2TP_L2SPECTYPE_DEFAULT,
+};
+
+enum l2tp_encap_type {
+	L2TP_ENCAPTYPE_UDP,
+	L2TP_ENCAPTYPE_IP,
+};
+
+enum l2tp_seqmode {
+	L2TP_SEQ_NONE = 0,
+	L2TP_SEQ_IP = 1,
+	L2TP_SEQ_ALL = 2,
+};
+
+/*
+ * NETLINK_GENERIC related info
+ */
+#define L2TP_GENL_NAME		"l2tp"
+#define L2TP_GENL_VERSION	0x1
+
+#endif
diff --git a/accel-pptpd/ctrl/l2tp/netlink.c b/accel-pptpd/ctrl/l2tp/netlink.c
new file mode 100644
index 00000000..a50e9b2b
--- /dev/null
+++ b/accel-pptpd/ctrl/l2tp/netlink.c
@@ -0,0 +1,70 @@
+#include <netlink/netlink.h>
+#include <netlink/genl/genl.h>
+#include <netlink/genl/ctrl.h>
+
+#include "l2tp_kernel.h"
+#include "triton.h"
+
+static struct nl_handle *nl_sock;
+static int family;
+
+void l2tp_nl_delete_tunnel(int tid)
+{
+	struct nl_msg *msg = nlmsg_alloc();
+	
+	genlmsg_put(msg, NL_AUTO_PID, NL_AUTO_SEQ, family, 0, NLM_F_REQUEST, L2TP_CMD_TUNNEL_DELETE, L2TP_GENL_VERSION);
+	nla_put_u32(msg, L2TP_ATTR_CONN_ID, tid);
+
+	nl_send_auto_complete(nl_sock, msg);
+	nl_recvmsgs_default(nl_sock);
+
+	nlmsg_free(msg);
+
+}
+
+void l2tp_nl_create_tunnel(int fd, int tid, int peer_tid)
+{
+	struct nl_msg *msg = nlmsg_alloc();
+
+	l2tp_nl_delete_tunnel(tid);
+	
+	genlmsg_put(msg, NL_AUTO_PID, NL_AUTO_SEQ, family, 0, NLM_F_REQUEST, L2TP_CMD_TUNNEL_CREATE, L2TP_GENL_VERSION);
+	nla_put_u16(msg, L2TP_ATTR_ENCAP_TYPE, L2TP_ENCAPTYPE_UDP);
+	nla_put_u8(msg, L2TP_ATTR_PROTO_VERSION, 2);
+	nla_put_u32(msg, L2TP_ATTR_CONN_ID, tid);
+	nla_put_u32(msg, L2TP_ATTR_PEER_CONN_ID, peer_tid);
+	nla_put_u32(msg, L2TP_ATTR_FD, fd);
+	//nla_put_u32(msg, L2TP_ATTR_DEBUG, 0xffffffff);
+
+	nl_send_auto_complete(nl_sock, msg);
+	nl_recvmsgs_default(nl_sock);
+
+	nlmsg_free(msg);
+}
+
+void l2tp_nl_create_session(int tid, int sid, int peer_sid)
+{
+	struct nl_msg *msg = nlmsg_alloc();
+	
+	genlmsg_put(msg, NL_AUTO_PID, NL_AUTO_SEQ, family, 0, NLM_F_REQUEST, L2TP_CMD_SESSION_CREATE, L2TP_GENL_VERSION);
+	nla_put_u32(msg, L2TP_ATTR_CONN_ID, tid);
+	nla_put_u32(msg, L2TP_ATTR_SESSION_ID, sid);
+	nla_put_u32(msg, L2TP_ATTR_PEER_SESSION_ID, peer_sid);
+	nla_put_u16(msg, L2TP_ATTR_PW_TYPE, L2TP_PWTYPE_PPP);
+	nla_put_u8(msg, L2TP_ATTR_LNS_MODE, 1);
+	//nla_put_u32(msg, L2TP_ATTR_DEBUG, 0xffffffff);
+
+	nl_send_auto_complete(nl_sock, msg);
+	nl_recvmsgs_default(nl_sock);
+
+	nlmsg_free(msg);
+}
+
+static void __init init(void)
+{
+	nl_sock = nl_handle_alloc();
+	
+	genl_connect(nl_sock);
+
+	family = genl_ctrl_resolve(nl_sock, L2TP_GENL_NAME);
+}
diff --git a/accel-pptpd/ctrl/l2tp/packet.c b/accel-pptpd/ctrl/l2tp/packet.c
index e0ff65ad..c127619b 100644
--- a/accel-pptpd/ctrl/l2tp/packet.c
+++ b/accel-pptpd/ctrl/l2tp/packet.c
@@ -26,7 +26,7 @@ void l2tp_packet_print(struct l2tp_packet_t *pack)
 
 	if (pack->hdr.ver == 2)
 		log_ppp_info("[L2TP tid=%i sid=%i Ns=%i Nr=%i",
-			pack->hdr.tid, pack->hdr.sid, pack->hdr.Ns, pack->hdr.Nr);
+			ntohs(pack->hdr.tid), ntohs(pack->hdr.sid), ntohs(pack->hdr.Ns), ntohs(pack->hdr.Nr));
 	else
 		log_ppp_info("[L2TP cid=%u Ns=%i Nr=%i",
 			pack->hdr.cid, pack->hdr.Ns, pack->hdr.Nr);
@@ -70,7 +70,7 @@ struct l2tp_packet_t *l2tp_packet_alloc(int ver, int msg_type, struct sockaddr_i
 	memcpy(&pack->addr, addr, sizeof(*addr));
 
 	if (msg_type) {
-		if (l2tp_packet_add_int16(pack, Message_Type, msg_type)) {
+		if (l2tp_packet_add_int16(pack, Message_Type, msg_type, 1)) {
 			mempool_free(pack);
 			return NULL;
 		}
@@ -115,8 +115,10 @@ int l2tp_recv(int fd, struct l2tp_packet_t **p)
 	n = recvfrom(fd, buf, L2TP_MAX_PACKET_SIZE, 0, &addr, &len);
 
 	if (n < 0) {
-		if (errno == EAGAIN)
+		if (errno == EAGAIN) {
+			mempool_free(buf);
 			return -1;
+		}
 		log_error("l2tp: recv: %s\n", strerror(errno));
 		return 0;
 	}
@@ -332,7 +334,7 @@ int l2tp_packet_send(int sock, struct l2tp_packet_t *pack)
 	pack->hdr.length = htons(len);
 	memcpy(buf, &pack->hdr, sizeof(pack->hdr));
 
-	n = sendto(sock, buf, ntohs(pack->hdr.length), 0, &pack->addr, sizeof(pack->addr));
+	n = write(sock, buf, ntohs(pack->hdr.length));
 	
 	mempool_free(buf);
 
@@ -355,7 +357,7 @@ int l2tp_packet_send(int sock, struct l2tp_packet_t *pack)
 	return 0;
 }
 
-static struct l2tp_attr_t *attr_alloc(int id)
+static struct l2tp_attr_t *attr_alloc(int id, int M)
 {
 	struct l2tp_attr_t *attr;
 	struct l2tp_dict_attr_t *da;
@@ -376,15 +378,18 @@ static struct l2tp_attr_t *attr_alloc(int id)
 
 	if (da->M != -1)
 		attr->M = da->M;
-	if (da->H != -1)
-		attr->H = da->H;
+	else
+		attr->M = M;
+
+	//if (da->H != -1)
+	//attr->H = da->H;
 
 	return attr;
 }
 
-int l2tp_packet_add_int16(struct l2tp_packet_t *pack, int id, int16_t val)
+int l2tp_packet_add_int16(struct l2tp_packet_t *pack, int id, int16_t val, int M)
 {
-	struct l2tp_attr_t *attr = attr_alloc(id);
+	struct l2tp_attr_t *attr = attr_alloc(id, M);
 
 	if (!attr)
 		return -1;
@@ -395,9 +400,9 @@ int l2tp_packet_add_int16(struct l2tp_packet_t *pack, int id, int16_t val)
 
 	return 0;
 }
-int l2tp_packet_add_int32(struct l2tp_packet_t *pack, int id, int32_t val)
+int l2tp_packet_add_int32(struct l2tp_packet_t *pack, int id, int32_t val, int M)
 {
-	struct l2tp_attr_t *attr = attr_alloc(id);
+	struct l2tp_attr_t *attr = attr_alloc(id, M);
 
 	if (!attr)
 		return -1;
@@ -408,9 +413,9 @@ int l2tp_packet_add_int32(struct l2tp_packet_t *pack, int id, int32_t val)
 
 	return 0;
 }
-int l2tp_packet_add_string(struct l2tp_packet_t *pack, int id, const char *val)
+int l2tp_packet_add_string(struct l2tp_packet_t *pack, int id, const char *val, int M)
 {
-	struct l2tp_attr_t *attr = attr_alloc(id);
+	struct l2tp_attr_t *attr = attr_alloc(id, M);
 
 	if (!attr)
 		return -1;
@@ -428,9 +433,9 @@ int l2tp_packet_add_string(struct l2tp_packet_t *pack, int id, const char *val)
 	return 0;
 }
 
-int l2tp_packet_add_octets(struct l2tp_packet_t *pack, int id, const uint8_t *val, int size)
+int l2tp_packet_add_octets(struct l2tp_packet_t *pack, int id, const uint8_t *val, int size, int M)
 {
-	struct l2tp_attr_t *attr = attr_alloc(id);
+	struct l2tp_attr_t *attr = attr_alloc(id, M);
 
 	if (!attr)
 		return -1;
diff --git a/accel-pptpd/ctrl/pppoe/pppoe.c b/accel-pptpd/ctrl/pppoe/pppoe.c
index 30769b53..68ecbdc7 100644
--- a/accel-pptpd/ctrl/pppoe/pppoe.c
+++ b/accel-pptpd/ctrl/pppoe/pppoe.c
@@ -170,6 +170,7 @@ static struct pppoe_conn_t *allocate_channel(struct pppoe_serv_t *serv, const ui
 		memcpy(conn->relay_sid, relay_sid, sizeof(*relay_sid) + ntohs(relay_sid->tag_len));
 	}
 
+	conn->ctx.before_switch = log_switch;
 	conn->ctx.close = pppoe_conn_close;
 	conn->ctrl.ctx = &conn->ctx;
 	conn->ctrl.started = ppp_started;
diff --git a/accel-pptpd/ppp/ppp_ipcp.c b/accel-pptpd/ppp/ppp_ipcp.c
index 15103e08..ca8896c5 100644
--- a/accel-pptpd/ppp/ppp_ipcp.c
+++ b/accel-pptpd/ppp/ppp_ipcp.c
@@ -154,10 +154,10 @@ static void ipcp_layer_down(struct ppp_fsm_t *fsm)
 
 	log_ppp_debug("ipcp_layer_finished\n");
 
-	ppp_layer_finished(ipcp->ppp, &ipcp->ld);
-	if (ipcp->started)
+	if (ipcp->started) {
 		ipcp->started = 0;
-	else
+	  ppp_layer_finished(ipcp->ppp, &ipcp->ld);
+	} else
 		ppp_terminate(ipcp->ppp, 1);
 }
 
diff --git a/accel-pptpd/ppp/ppp_lcp.c b/accel-pptpd/ppp/ppp_lcp.c
index 2bbfd56a..0eae8fc9 100644
--- a/accel-pptpd/ppp/ppp_lcp.c
+++ b/accel-pptpd/ppp/ppp_lcp.c
@@ -176,10 +176,10 @@ static void lcp_layer_finished(struct ppp_fsm_t *fsm)
 	log_ppp_debug("lcp_layer_finished\n");
 
 	stop_echo(lcp);
-	ppp_layer_finished(lcp->ppp, &lcp->ld);
-	if (lcp->started)
+	if (lcp->started) {
 		lcp->started = 0;
-	else
+		ppp_layer_finished(lcp->ppp, &lcp->ld);
+	} else
 		ppp_terminate(lcp->ppp, 1);
 }
 
diff --git a/contrib/gentoo/net-dialup/accel-pptp/accel-pptp-9999.ebuild b/contrib/gentoo/net-dialup/accel-pptp/accel-pptp-9999.ebuild
index 2c30a60b..06ae2b81 100644
--- a/contrib/gentoo/net-dialup/accel-pptp/accel-pptp-9999.ebuild
+++ b/contrib/gentoo/net-dialup/accel-pptp/accel-pptp-9999.ebuild
@@ -18,9 +18,11 @@ IUSE="postgres debug"
 
 DEPEND="dev-libs/openssl
 	dev-libs/libaio
+	dev-libs/libnl
 	postgres? ( >=dev-db/postgresql-base-8.1 )"
 
-RDEPEND="virtual/modutils"
+RDEPEND="$DEPEND
+         virtual/modutils"
 
 BUILD_TARGETS="default"
 BUILD_PARAMS="KDIR=${KERNEL_DIR}"