diff --git a/l2tpns.c b/l2tpns.c
index 3e7c9d6..a80589d 100644
--- a/l2tpns.c
+++ b/l2tpns.c
@@ -253,6 +253,7 @@ struct Tstats *_statistics = NULL;
 struct Tringbuffer *ringbuffer = NULL;
 #endif
 
+static int setupif(int ifidx, uint32_t mru, int config_addr);
 static ssize_t rtnetlink_send(struct nlmsghdr *nh);
 static ssize_t genetlink_send(struct nlmsghdr *nh);
 static ssize_t genetlink_recv(void *buf, ssize_t len);
@@ -1510,41 +1511,51 @@ static void inittun(void)
 		LOG(0, 0, 0, "Can't get tun interface index\n");
 		exit(1);
 	}
-
+	if (setupif(tunidx, MRU, 1) < 0)
 	{
-		struct {
-			// interface setting
-			struct nlmsghdr nh;
-			union {
-				struct ifinfomsg ifinfo;
-				struct ifaddrmsg ifaddr;
-			} ifmsg;
-			char rtdata[32]; // 32 should be enough
-		} req;
-		uint32_t txqlen, mtu;
-		in_addr_t ip;
+		LOG(0, 0, 0, "Error while setting up tun device: %s\n", strerror(errno));
+		exit(1);
+	}
+}
 
-		memset(&req, 0, sizeof(req));
+//
+// Set up an interface for serving as gateway
+static int setupif(int ifidx, uint32_t mru, int config_addr)
+{
+	struct {
+		// interface setting
+		struct nlmsghdr nh;
+		union {
+			struct ifinfomsg ifinfo;
+			struct ifaddrmsg ifaddr;
+		} ifmsg;
+		char rtdata[32]; // 32 should be enough
+	} req;
+	uint32_t txqlen;
+	in_addr_t ip;
 
-		req.nh.nlmsg_type = RTM_NEWLINK;
-		req.nh.nlmsg_flags = NLM_F_REQUEST | NLM_F_MULTI;
-		req.nh.nlmsg_len = NLMSG_LENGTH(sizeof(req.ifmsg.ifinfo));
+	memset(&req, 0, sizeof(req));
 
-		req.ifmsg.ifinfo.ifi_family = AF_UNSPEC;
-		req.ifmsg.ifinfo.ifi_index = tunidx;
-		req.ifmsg.ifinfo.ifi_flags |= IFF_UP; // set interface up
-		req.ifmsg.ifinfo.ifi_change = IFF_UP; // only change this flag
+	req.nh.nlmsg_type = RTM_NEWLINK;
+	req.nh.nlmsg_flags = NLM_F_REQUEST | NLM_F_MULTI;
+	req.nh.nlmsg_len = NLMSG_LENGTH(sizeof(req.ifmsg.ifinfo));
 
-		/* Bump up the qlen to deal with bursts from the network */
-		txqlen = 1000;
-		rtnetlink_addattr(&req.nh, IFLA_TXQLEN, &txqlen, sizeof(txqlen));
-		/* set MTU to modem MRU */
-		mtu = MRU;
-		rtnetlink_addattr(&req.nh, IFLA_MTU, &mtu, sizeof(mtu));
+	req.ifmsg.ifinfo.ifi_family = AF_UNSPEC;
+	req.ifmsg.ifinfo.ifi_index = ifidx;
+	req.ifmsg.ifinfo.ifi_flags |= IFF_UP; // set interface up
+	req.ifmsg.ifinfo.ifi_change = IFF_UP; // only change this flag
 
-		if (rtnetlink_send(&req.nh) < 0)
-			goto senderror;
+	/* Bump up the qlen to deal with bursts from the network */
+	txqlen = 1000;
+	rtnetlink_addattr(&req.nh, IFLA_TXQLEN, &txqlen, sizeof(txqlen));
+	/* set MTU to modem MRU */
+	rtnetlink_addattr(&req.nh, IFLA_MTU, &mru, sizeof(mru));
 
+	if (rtnetlink_send(&req.nh) < 0)
+		return -1;
+
+	if (config_addr)
+	{
 		memset(&req, 0, sizeof(req));
 
 		req.nh.nlmsg_type = RTM_NEWADDR;
@@ -1554,7 +1565,7 @@ static void inittun(void)
 		req.ifmsg.ifaddr.ifa_family = AF_INET;
 		req.ifmsg.ifaddr.ifa_prefixlen = 32;
 		req.ifmsg.ifaddr.ifa_scope = RT_SCOPE_UNIVERSE;
-		req.ifmsg.ifaddr.ifa_index = tunidx;
+		req.ifmsg.ifaddr.ifa_index = ifidx;
 
 		if (config->nbmultiaddress > 1)
 		{
@@ -1564,7 +1575,7 @@ static void inittun(void)
 				ip = config->iftun_n_address[i];
 				rtnetlink_addattr(&req.nh, IFA_LOCAL, &ip, sizeof(ip));
 				if (rtnetlink_send(&req.nh) < 0)
-					goto senderror;
+					return -1;
 			}
 		}
 		else
@@ -1576,77 +1587,71 @@ static void inittun(void)
 			rtnetlink_addattr(&req.nh, IFA_LOCAL, &ip, sizeof(ip));
 
 			if (rtnetlink_send(&req.nh) < 0)
-				goto senderror;
+				return -1;
 		}
+	}
 
-
-
-		// Only setup IPv6 on the tun device if we have a configured prefix
-		if (config->ipv6_prefix.s6_addr[0]) {
-			struct in6_addr ip6;
-
-			memset(&req, 0, sizeof(req));
-
-			req.nh.nlmsg_type = RTM_NEWADDR;
-			req.nh.nlmsg_flags = NLM_F_REQUEST | NLM_F_CREATE | NLM_F_REPLACE | NLM_F_MULTI;
-			req.nh.nlmsg_len = NLMSG_LENGTH(sizeof(req.ifmsg.ifaddr));
-
-			req.ifmsg.ifaddr.ifa_family = AF_INET6;
-			req.ifmsg.ifaddr.ifa_prefixlen = 64;
-			req.ifmsg.ifaddr.ifa_scope = RT_SCOPE_LINK;
-			req.ifmsg.ifaddr.ifa_index = tunidx;
-
-			// Link local address is FE80::1
-			memset(&ip6, 0, sizeof(ip6));
-			ip6.s6_addr[0] = 0xFE;
-			ip6.s6_addr[1] = 0x80;
-			ip6.s6_addr[15] = 1;
-			rtnetlink_addattr(&req.nh, IFA_LOCAL, &ip6, sizeof(ip6));
-
-			if (rtnetlink_send(&req.nh) < 0)
-				goto senderror;
-
-			memset(&req, 0, sizeof(req));
-
-			req.nh.nlmsg_type = RTM_NEWADDR;
-			req.nh.nlmsg_flags = NLM_F_REQUEST | NLM_F_CREATE | NLM_F_REPLACE | NLM_F_MULTI;
-			req.nh.nlmsg_len = NLMSG_LENGTH(sizeof(req.ifmsg.ifaddr));
-
-			req.ifmsg.ifaddr.ifa_family = AF_INET6;
-			req.ifmsg.ifaddr.ifa_prefixlen = 64;
-			req.ifmsg.ifaddr.ifa_scope = RT_SCOPE_UNIVERSE;
-			req.ifmsg.ifaddr.ifa_index = tunidx;
-
-			// Global address is prefix::1
-			ip6 = config->ipv6_prefix;
-			ip6.s6_addr[15] = 1;
-			rtnetlink_addattr(&req.nh, IFA_LOCAL, &ip6, sizeof(ip6));
-
-			if (rtnetlink_send(&req.nh) < 0)
-				goto senderror;
-		}
+	// Only setup IPv6 on the tun device if we have a configured prefix
+	if (config->ipv6_prefix.s6_addr[0]) {
+		struct in6_addr ip6;
 
 		memset(&req, 0, sizeof(req));
 
-		req.nh.nlmsg_type = NLMSG_DONE;
-		req.nh.nlmsg_len = NLMSG_LENGTH(0);
+		req.nh.nlmsg_type = RTM_NEWADDR;
+		req.nh.nlmsg_flags = NLM_F_REQUEST | NLM_F_CREATE | NLM_F_REPLACE | NLM_F_MULTI;
+		req.nh.nlmsg_len = NLMSG_LENGTH(sizeof(req.ifmsg.ifaddr));
+
+		req.ifmsg.ifaddr.ifa_family = AF_INET6;
+		req.ifmsg.ifaddr.ifa_prefixlen = 64;
+		req.ifmsg.ifaddr.ifa_scope = RT_SCOPE_LINK;
+		req.ifmsg.ifaddr.ifa_index = ifidx;
+
+		// Link local address is FE80::1
+		memset(&ip6, 0, sizeof(ip6));
+		ip6.s6_addr[0] = 0xFE;
+		ip6.s6_addr[1] = 0x80;
+		ip6.s6_addr[15] = 1;
+		rtnetlink_addattr(&req.nh, IFA_LOCAL, &ip6, sizeof(ip6));
 
 		if (rtnetlink_send(&req.nh) < 0)
-			goto senderror;
+			return -1;
 
-		// if we get an error for seqnum < min_initok_nlseqnum,
-		// we must exit as initialization went wrong
-		if (config->ipv6_prefix.s6_addr[0])
-			min_initok_rtnlseqnum = 5 + 1; // idx + if + addr + 2*addr6
-		else
-			min_initok_rtnlseqnum = 3 + 1; // idx + if + addr
+		memset(&req, 0, sizeof(req));
+
+		req.nh.nlmsg_type = RTM_NEWADDR;
+		req.nh.nlmsg_flags = NLM_F_REQUEST | NLM_F_CREATE | NLM_F_REPLACE | NLM_F_MULTI;
+		req.nh.nlmsg_len = NLMSG_LENGTH(sizeof(req.ifmsg.ifaddr));
+
+		req.ifmsg.ifaddr.ifa_family = AF_INET6;
+		req.ifmsg.ifaddr.ifa_prefixlen = 64;
+		req.ifmsg.ifaddr.ifa_scope = RT_SCOPE_UNIVERSE;
+		req.ifmsg.ifaddr.ifa_index = ifidx;
+
+		// Global address is prefix::1
+		ip6 = config->ipv6_prefix;
+		ip6.s6_addr[15] = 1;
+		rtnetlink_addattr(&req.nh, IFA_LOCAL, &ip6, sizeof(ip6));
+
+		if (rtnetlink_send(&req.nh) < 0)
+			return -1;
 	}
 
-	return;
+	memset(&req, 0, sizeof(req));
 
-senderror:
-	LOG(0, 0, 0, "Error while setting up tun device: %s\n", strerror(errno));
-	exit(1);
+	req.nh.nlmsg_type = NLMSG_DONE;
+	req.nh.nlmsg_len = NLMSG_LENGTH(0);
+
+	if (rtnetlink_send(&req.nh) < 0)
+		return -1;
+
+	// if we get an error for seqnum < min_initok_nlseqnum,
+	// we must exit as initialization went wrong
+	if (config->ipv6_prefix.s6_addr[0])
+		min_initok_rtnlseqnum = 5 + 1; // idx + if + addr + 2*addr6
+	else
+		min_initok_rtnlseqnum = 3 + 1; // idx + if + addr
+
+	return 0;
 }
 
 // set up LAC UDP ports