Fix fragment error

Add stub for virtual interfaces of data packet

.hgignore

@@ -1,8 +1,6 @@
 # use glob syntax
 syntax: glob
 
-build.sh
-
 Makefile
 Makefile.in
 aclocal.m4
@@ -15,6 +13,8 @@ configure
 depcomp
 install-sh
 missing
+Module.symvers
+modules.order
 build/config.h
 build/config.h.in
 build/stamp-h1
@@ -22,6 +22,8 @@ build/stamp-h1
 build/ac/ac
 build/wtp/wtp
 
+src/ac/kmod/.tmp_versions
+
 *~
 *.o
 *.lo
@@ -29,3 +31,6 @@ build/wtp/wtp
 m4
 .deps
 .libs
+*.mod.c
+*.ko
+*.cmd

src/ac/ac_kmod.c

@@ -413,6 +413,82 @@ int ac_kmod_delete_datasession(struct sockaddr_storage* sockaddr, struct capwap_
 	return result;
 }
 
+/* */
+static int cb_kmod_create_iface(struct nl_msg* msg, void* data) {
+	struct nlattr* tb_msg[NLSMARTCAPWAP_ATTR_MAX + 1];
+	struct genlmsghdr* gnlh = nlmsg_data(nlmsg_hdr(msg));
+	uint32_t* ifindex = (uint32_t*)data;
+
+	nla_parse(tb_msg, NLSMARTCAPWAP_ATTR_MAX, genlmsg_attrdata(gnlh, 0), genlmsg_attrlen(gnlh, 0), NULL);
+
+	if (tb_msg[NLSMARTCAPWAP_ATTR_IFPHY_INDEX]) {
+		*ifindex = nla_get_u32(tb_msg[NLSMARTCAPWAP_ATTR_IFPHY_INDEX]);
+	}
+
+	return NL_SKIP;
+}
+
+/* */
+int ac_kmod_create_iface(const char* ifname, uint16_t mtu) {
+	int result;
+	struct nl_msg* msg;
+	uint32_t ifindex = 0;
+
+	ASSERT(ifname != NULL);
+	ASSERT(mtu > 0);
+
+	/* */
+	msg = nlmsg_alloc();
+	if (!msg) {
+		return -1;
+	}
+
+	/* */
+	genlmsg_put(msg, 0, 0, g_ac.kmodhandle.nlsmartcapwap_id, 0, 0, NLSMARTCAPWAP_CMD_ADD_IFACE, 0);
+	nla_put_string(msg, NLSMARTCAPWAP_ATTR_IFPHY_NAME, ifname);
+	nla_put_u16(msg, NLSMARTCAPWAP_ATTR_MTU, mtu);
+
+	/* */
+	result = ac_kmod_send_and_recv_msg(msg, cb_kmod_create_iface, &ifindex);
+	if (!result) {
+		result = (ifindex ? (int)ifindex : -1);
+	} else {
+		capwap_logging_error("Unable to create data session: %d", result);
+	}
+
+	/* */
+	nlmsg_free(msg);
+	return result;
+}
+
+/* */
+int ac_kmod_delete_iface(int ifindex) {
+	int result;
+	struct nl_msg* msg;
+
+	ASSERT(ifindex > 0);
+
+	/* */
+	msg = nlmsg_alloc();
+	if (!msg) {
+		return -1;
+	}
+
+	/* */
+	genlmsg_put(msg, 0, 0, g_ac.kmodhandle.nlsmartcapwap_id, 0, 0, NLSMARTCAPWAP_CMD_DELETE_IFACE, 0);
+	nla_put_u32(msg, NLSMARTCAPWAP_ATTR_IFPHY_INDEX, (uint32_t)ifindex);
+
+	/* */
+	result = ac_kmod_send_and_recv_msg(msg, NULL, NULL);
+	if (result && (result != ENOENT)) {
+		capwap_logging_error("Unable to delete interface: %d", result);
+	}
+
+	/* */
+	nlmsg_free(msg);
+	return result;
+}
+
 /* */
 int ac_kmod_init(uint32_t hash, uint32_t threads) {
 	int result;

src/ac/ac_kmod.h

@@ -46,6 +46,10 @@ int ac_kmod_createdatachannel(int family, unsigned short port);
 int ac_kmod_send_keepalive(struct sockaddr_storage* sockaddr);
 int ac_kmod_send_data(struct sockaddr_storage* sockaddr, uint8_t radioid, uint8_t binding, const uint8_t* data, int length);
 
+/* */
+int ac_kmod_create_iface(const char* ifname, uint16_t mtu);
+int ac_kmod_delete_iface(int ifindex);
+
 /* */
 int ac_kmod_new_datasession(struct capwap_sessionid_element* sessionid, uint16_t mtu);
 int ac_kmod_delete_datasession(struct sockaddr_storage* sockaddr, struct capwap_sessionid_element* sessionid);

src/ac/kmod/Makefile

@@ -7,7 +7,8 @@ smartcapwap-y := \
 	netlinkapp.o \
 	capwap.o \
 	capwap_private.o \
-	socket.o
+	socket.o \
+	iface.o
 
 all:
 	make -C /lib/modules/$(KVERSION)/build M="$(PWD)" modules

src/ac/kmod/capwap.c

@@ -557,7 +557,7 @@ int sc_capwap_forwarddata(struct sc_capwap_session* session, uint8_t radioid, ui
 		}
 
 		/* */
-		header = (struct sc_capwap_header*)((uint8_t*)header + (size + length));
+		header = (struct sc_capwap_header*)(((uint8_t*)header + size + length) - sizeof(struct sc_capwap_header));
 		fragmentoffset += length;
 		packetlength -= length;
 	}

src/ac/kmod/capwap_private.c

@@ -7,9 +7,10 @@
 #include "capwap.h"
 #include "nlsmartcapwap.h"
 #include "netlinkapp.h"
+#include "iface.h"
 
 /* Sessions */
-static struct mutex sc_wtpsession_mutex;
+static DEFINE_MUTEX(sc_wtpsession_mutex);
 static struct list_head sc_wtpsession_list;
 static struct list_head sc_wtpsession_setup_list;
 
@@ -214,8 +215,7 @@ static int sc_capwap_thread_recvpacket(struct sk_buff* skb) {
 		/* */
 		rcu_read_lock();
 
-		session = sc_capwap_getsession(&peeraddr);
+		ret = sc_capwap_parsingpacket(sc_capwap_getsession(&peeraddr), &peeraddr, skb);
-		ret = sc_capwap_parsingpacket(session, &peeraddr, skb);
 
 		rcu_read_unlock();
 	}
@@ -340,13 +340,17 @@ int sc_capwap_init(uint32_t hash, uint32_t threads) {
 	TRACEKMOD("### sc_capwap_init\n");
 	TRACEKMOD("*** Init capwap module - hash bitfield: %u - threads: %u\n", hash, threads);
 
+	/* */
+	if (!hash || !threads) {
+		return -EINVAL;
+	}
+
 	/* Init session */
 	memset(&sc_localaddr, 0, sizeof(union capwap_addr));
-	mutex_init(&sc_wtpsession_mutex);
-
 	INIT_LIST_HEAD(&sc_wtpsession_list);
 	INIT_LIST_HEAD(&sc_wtpsession_setup_list);
 
+	/* */
 	sc_wtpsession_size_shift = hash;
 	sc_wtpsession_size = 1 << hash;
 	sc_wtpsession_hash = (struct sc_capwap_session**)kzalloc(sizeof(struct sc_capwap_session*) * sc_wtpsession_size, GFP_KERNEL);
@@ -409,6 +413,7 @@ void sc_capwap_close(void) {
 	TRACEKMOD("### sc_capwap_close\n");
 	TRACEKMOD("*** Closing capwap module\n");
 
+	/* */
 	sc_socket_close();
 
 	/* */
@@ -420,9 +425,11 @@ void sc_capwap_close(void) {
 
 	/* */
 	sc_capwap_closewtpsessions();
-	mutex_destroy(&sc_wtpsession_mutex);
 	kfree(sc_wtpsession_hash);
 
+	/* */
+	sc_iface_closeall();
+
 	TRACEKMOD("*** Close capwap module\n");
 }
 
@@ -451,14 +458,6 @@ int sc_capwap_deletesession(const union capwap_addr* sockaddr, const struct sc_c
 		return sc_capwap_deleterunningsession(sockaddr);
 	} else {
 		list_for_each_entry_rcu(wtpsession, &sc_wtpsession_list, list) {
-#ifdef DEBUGKMOD
-			do {
-				char sessionname[33];
-				sc_capwap_sessionid_printf(&wtpsession->sessionid, sessionname);
-				TRACEKMOD("*** Check running session for delete: %s\n", sessionname);
-			} while(0);
-#endif
-
 			if (!memcmp(&wtpsession->sessionid, sessionid, sizeof(struct sc_capwap_sessionid_element))) {
 				union capwap_addr peeraddr;
 
@@ -474,14 +473,6 @@ int sc_capwap_deletesession(const union capwap_addr* sockaddr, const struct sc_c
 
 	/* Search into setup session list */
 	list_for_each_entry_rcu(wtpsession, &sc_wtpsession_setup_list, list) {
-#ifdef DEBUGKMOD
-		do {
-			char sessionname[33];
-			sc_capwap_sessionid_printf(&wtpsession->sessionid, sessionname);
-			TRACEKMOD("*** Check setup session for delete: %s\n", sessionname);
-		} while(0);
-#endif
-
 		if (!memcmp(&wtpsession->sessionid, sessionid, sizeof(struct sc_capwap_sessionid_element))) {
 			rcu_read_unlock();
 
@@ -543,6 +534,9 @@ struct sc_capwap_session* sc_capwap_recvunknownkeepalive(const union capwap_addr
 
 	TRACEKMOD("### sc_capwap_recvunknownkeepalive\n");
 
+	/* Must be called under rcu_read_lock() */
+	 rcu_lockdep_assert(rcu_read_lock_held(), "sc_capwap_recvunknownkeepalive() needs rcu_read_lock() protection");
+
 #ifdef DEBUGKMOD
 	do {
 		char sessionname[33];
@@ -557,14 +551,6 @@ struct sc_capwap_session* sc_capwap_recvunknownkeepalive(const union capwap_addr
 
 	/* Search and remove from setup session */
 	list_for_each_entry(search, &sc_wtpsession_setup_list, list) {
-#ifdef DEBUGKMOD
-		do {
-			char sessionname[33];
-			sc_capwap_sessionid_printf(&search->sessionid, sessionname);
-			TRACEKMOD("*** Check setup session: %s\n", sessionname);
-		} while(0);
-#endif
-
 		if (!memcmp(&search->sessionid, sessionid, sizeof(struct sc_capwap_sessionid_element))) {
 			wtpsession = search;
 			break;
@@ -572,15 +558,15 @@ struct sc_capwap_session* sc_capwap_recvunknownkeepalive(const union capwap_addr
 	}
 
 	/* */
-	if (wtpsession) {
+	if (!wtpsession) {
-		TRACEKMOD("*** Setup session found\n");
-		list_del_rcu(&wtpsession->list);
-		synchronize_net();
-	} else {
 		TRACEKMOD("*** Setup session not found\n");
 		goto done;
 	}
 
+	/* */
+	list_del_rcu(&wtpsession->list);
+	synchronize_net();
+
 	/* */
 	hash = sc_capwap_hash(sockaddr);
 	memcpy(&wtpsession->peeraddr, sockaddr, sizeof(union capwap_addr));

src/ac/kmod/iface.c

@@ -0,0 +1,217 @@
+#include "config.h"
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/netdevice.h>
+#include <linux/etherdevice.h>
+#include "iface.h"
+
+/* */
+struct sc_netdev_priv {
+	struct net_device* dev;
+	struct sc_netdev_priv* next;
+};
+
+/* */
+#define CAPWAP_IFACE_COUNT				8
+#define CAPWAP_IFACE_HASH(x)			((x) % CAPWAP_IFACE_COUNT)
+
+static uint32_t sc_iface_count;
+static DEFINE_SPINLOCK(sc_iface_lock);
+static struct sc_netdev_priv* sc_iface_hash[CAPWAP_IFACE_COUNT];
+
+/* */
+static void sc_iface_netdev_uninit(struct net_device* dev) {
+	unsigned long flags;
+	struct sc_netdev_priv* search;
+	int hash = CAPWAP_IFACE_HASH(dev->ifindex);
+	struct sc_netdev_priv* priv = (struct sc_netdev_priv*)netdev_priv(dev);
+
+	/* Remove interface from hash */
+	spin_lock_irqsave(&sc_iface_lock, flags);
+
+	search  = sc_iface_hash[hash];
+	if (search) {
+		if (priv == search) {
+			netif_tx_lock_bh(dev);
+			netif_carrier_off(dev);
+			netif_tx_unlock_bh(dev);
+
+			sc_iface_hash[hash] = priv->next;
+
+			dev_put(dev);
+			sc_iface_count--;
+		} else {
+			while (search->next && (search->next != priv)) {
+				search = search->next;
+			}
+
+			if (search->next) {
+				netif_tx_lock_bh(dev);
+				netif_carrier_off(dev);
+				netif_tx_unlock_bh(dev);
+
+				search->next = priv->next;
+
+				dev_put(dev);
+				sc_iface_count--;
+			}
+		}
+	}
+
+	spin_unlock_irqrestore(&sc_iface_lock, flags);
+
+	/* Close stations with link to this device */
+	/* TODO */
+}
+
+/* */
+static int sc_iface_netdev_open(struct net_device* dev) {
+	netif_start_queue(dev);
+	return 0;
+}
+
+/* */
+static int sc_iface_netdev_stop(struct net_device* dev) {
+	netif_stop_queue(dev);
+	return 0;
+}
+
+/* */
+static int sc_iface_netdev_tx(struct sk_buff* skb, struct net_device* dev) {
+	/* TODO */
+	return 0;
+}
+
+/* */
+static int sc_iface_netdev_change_mtu(struct net_device* dev, int new_mtu) {
+	/* TODO */
+	return 0;
+}
+
+/* */
+static void sc_iface_netdev_setup(struct net_device* dev) {
+	struct sc_netdev_priv* priv = (struct sc_netdev_priv*)netdev_priv(dev);
+
+	/* */
+	memset(priv, 0, sizeof(struct sc_netdev_priv));
+	priv->dev = dev;
+}
+
+/* */
+static const struct net_device_ops capwap_netdev_ops = {
+	.ndo_uninit = sc_iface_netdev_uninit,
+	.ndo_open = sc_iface_netdev_open,
+	.ndo_stop = sc_iface_netdev_stop,
+	.ndo_start_xmit = sc_iface_netdev_tx,
+	.ndo_change_mtu = sc_iface_netdev_change_mtu,
+};
+
+/* */
+int sc_iface_create(const char* ifname, uint16_t mtu) {
+	int err;
+	int hash;
+	unsigned long flags;
+	struct net_device* dev;
+	struct sc_netdev_priv* priv;
+
+	/* Create interface */
+	dev = alloc_netdev(sizeof(struct sc_netdev_priv), ifname, sc_iface_netdev_setup);
+	if (!dev) {
+		return -ENOMEM;
+	}
+
+	/* */
+	priv = (struct sc_netdev_priv*)netdev_priv(dev);
+	dev->netdev_ops = &capwap_netdev_ops;
+	ether_setup(dev);
+
+	eth_hw_addr_random(dev);
+
+	dev->mtu = mtu;
+
+	dev->hw_features = NETIF_F_HW_CSUM;
+	dev->features = dev->hw_features;
+
+	/* */
+	err = register_netdev(dev);
+	if (err) {
+		free_netdev(dev);
+		return err;
+	}
+
+	/* */
+	hash = CAPWAP_IFACE_HASH(dev->ifindex);
+
+	spin_lock_irqsave(&sc_iface_lock, flags);
+
+	sc_iface_count++;
+	priv->next = sc_iface_hash[hash];
+	sc_iface_hash[hash] = priv;
+	dev_hold(dev);
+
+	spin_unlock_irqrestore(&sc_iface_lock, flags);
+
+	/* Enable carrier */
+	netif_tx_lock_bh(dev);
+	netif_carrier_on(dev);
+	netif_tx_unlock_bh(dev);
+
+	return dev->ifindex;
+}
+
+/* */
+int sc_iface_delete(uint32_t ifindex) {
+	unsigned long flags;
+	struct sc_netdev_priv* priv;
+
+	/* */
+	spin_lock_irqsave(&sc_iface_lock, flags);
+
+	priv  = sc_iface_hash[CAPWAP_IFACE_HASH(ifindex)];
+	while (priv) {
+		if (priv->dev->ifindex == ifindex) {
+			break;
+		}
+
+		priv = priv->next;
+	}
+
+	spin_unlock_irqrestore(&sc_iface_lock, flags);
+
+	/* */
+	if (!priv) {
+		return -ENOENT;
+	}
+
+	/* */
+	unregister_netdev(priv->dev);
+	free_netdev(priv->dev);
+
+	return 0;
+}
+
+/* */
+void sc_iface_closeall(void) {
+	int i;
+	unsigned long flags;
+
+	while (sc_iface_count) {
+		struct net_device* dev = NULL;
+
+		spin_lock_irqsave(&sc_iface_lock, flags);
+
+		for (i = 0; i < CAPWAP_IFACE_COUNT; i++) {
+			if (sc_iface_hash[i]) {
+				dev = sc_iface_hash[i]->dev;
+				break;
+			}
+		}
+
+		spin_unlock_irqrestore(&sc_iface_lock, flags);
+
+		/* */
+		BUG_ON(!dev);
+		unregister_netdev(dev);
+		free_netdev(dev);
+	}
+}

src/ac/kmod/iface.h

@@ -0,0 +1,10 @@
+#ifndef __KMOD_AC_IFACE_HEADER__
+#define __KMOD_AC_IFACE_HEADER__
+
+/* */
+int sc_iface_create(const char* ifname, uint16_t mtu);
+int sc_iface_delete(uint32_t ifindex);
+
+void sc_iface_closeall(void);
+
+#endif /* __KMOD_AC_IFACE_HEADER__ */

src/ac/kmod/netlinkapp.c

@@ -13,25 +13,11 @@
 #include "nlsmartcapwap.h"
 #include "netlinkapp.h"
 #include "capwap.h"
+#include "iface.h"
 
 /* */
 static u32 sc_netlink_usermodeid;
 
-/* */
-static int sc_netlink_pre_doit(struct genl_ops* ops, struct sk_buff* skb, struct genl_info* info) {
-	TRACEKMOD("### sc_netlink_pre_doit\n");
-
-	rtnl_lock();
-	return 0;
-}
-
-/* */
-static void sc_netlink_post_doit(struct genl_ops* ops, struct sk_buff* skb, struct genl_info* info) {
-	TRACEKMOD("### sc_netlink_post_doit\n");
-
-	rtnl_unlock();
-}
-
 /* Netlink Family */
 static struct genl_family sc_netlink_family = {
 	.id = GENL_ID_GENERATE,
@@ -40,8 +26,6 @@ static struct genl_family sc_netlink_family = {
 	.version = 1,
 	.maxattr = NLSMARTCAPWAP_ATTR_MAX,
 	.netnsok = true,
-	.pre_doit = sc_netlink_pre_doit,
-	.post_doit = sc_netlink_post_doit,
 };
 
 /* */
@@ -56,7 +40,7 @@ static int sc_netlink_bind(struct sk_buff* skb, struct genl_info* info) {
 	}
 
 	/* Get bind address */
-	if (!info->attrs[NLSMARTCAPWAP_ATTR_ADDRESS] || (nla_len(info->attrs[NLSMARTCAPWAP_ATTR_ADDRESS]) != sizeof(struct sockaddr_storage))) {
+	if (!info->attrs[NLSMARTCAPWAP_ATTR_ADDRESS]) {
 		return -EINVAL;
 	}
 
@@ -113,7 +97,10 @@ static int sc_netlink_send_data(struct sk_buff* skb, struct genl_info* info) {
 	/* Check Link */
 	if (sc_netlink_usermodeid != info->snd_portid) {
 		return -ENOLINK;
-	} else if (!info->attrs[NLSMARTCAPWAP_ATTR_ADDRESS] || !info->attrs[NLSMARTCAPWAP_ATTR_RADIOID] || !info->attrs[NLSMARTCAPWAP_ATTR_BINDING] || !info->attrs[NLSMARTCAPWAP_ATTR_DATA_FRAME]) {
+	}
+
+	/* */
+	if (!info->attrs[NLSMARTCAPWAP_ATTR_ADDRESS] || !info->attrs[NLSMARTCAPWAP_ATTR_RADIOID] || !info->attrs[NLSMARTCAPWAP_ATTR_BINDING] || !info->attrs[NLSMARTCAPWAP_ATTR_DATA_FRAME]) {
 		return -EINVAL;
 	}
 
@@ -160,7 +147,7 @@ static int sc_netlink_new_session(struct sk_buff* skb, struct genl_info* info) {
 	}
 
 	/* Check Session ID */
-	if (!info->attrs[NLSMARTCAPWAP_ATTR_SESSION_ID] || (nla_len(info->attrs[NLSMARTCAPWAP_ATTR_SESSION_ID]) != sizeof(struct sc_capwap_sessionid_element))) {
+	if (!info->attrs[NLSMARTCAPWAP_ATTR_SESSION_ID]) {
 		return -EINVAL;
 	}
 
@@ -178,7 +165,9 @@ static int sc_netlink_new_session(struct sk_buff* skb, struct genl_info* info) {
 
 /* */
 static int sc_netlink_delete_session(struct sk_buff* skb, struct genl_info* info) {
-	union capwap_addr sockaddr;
+	union capwap_addr sockaddr = {
+		.ss.ss_family = AF_UNSPEC
+	};
 
 	TRACEKMOD("### sc_netlink_delete_session\n");
 
@@ -188,15 +177,13 @@ static int sc_netlink_delete_session(struct sk_buff* skb, struct genl_info* info
 	}
 
 	/* Check Session ID */
-	if (!info->attrs[NLSMARTCAPWAP_ATTR_SESSION_ID] || (nla_len(info->attrs[NLSMARTCAPWAP_ATTR_SESSION_ID]) != sizeof(struct sc_capwap_sessionid_element))) {
+	if (!info->attrs[NLSMARTCAPWAP_ATTR_SESSION_ID]) {
 		return -EINVAL;
 	}
 
 	/* Check Address */
-	if (info->attrs[NLSMARTCAPWAP_ATTR_ADDRESS] && (nla_len(info->attrs[NLSMARTCAPWAP_ATTR_ADDRESS]) == sizeof(struct sockaddr_storage))) {
+	if (info->attrs[NLSMARTCAPWAP_ATTR_ADDRESS]) {
 		memcpy(&sockaddr.ss, nla_data(info->attrs[NLSMARTCAPWAP_ATTR_ADDRESS]), sizeof(struct sockaddr_storage));
-	} else {
-		sockaddr.ss.ss_family = AF_UNSPEC;
 	}
 
 	/* Delete session */
@@ -205,7 +192,7 @@ static int sc_netlink_delete_session(struct sk_buff* skb, struct genl_info* info
 
 /* */
 static int sc_netlink_link(struct sk_buff* skb, struct genl_info* info) {
-	int ret = 0;
+	int ret;
 
 	TRACEKMOD("### sc_netlink_link\n");
 
@@ -214,32 +201,97 @@ static int sc_netlink_link(struct sk_buff* skb, struct genl_info* info) {
 		return -EINVAL;
 	}
 
-	if (!sc_netlink_usermodeid) {
+	/* */
-		uint32_t hash = nla_get_u32(info->attrs[NLSMARTCAPWAP_ATTR_HASH_SESSION_BITFIELD]);
+	if (sc_netlink_usermodeid) {
-		uint32_t threads = nla_get_u32(info->attrs[NLSMARTCAPWAP_ATTR_SESSION_THREADS_COUNT]);
-
-		if (!hash || !threads) {
-			TRACEKMOD("*** Invalid link argument: %u %u\n", hash, threads);
-			return -EINVAL;
-		}
-
-		/* Initialize library */
-		ret = sc_capwap_init(hash, threads);
-		if (!ret) {
-			sc_netlink_usermodeid = info->snd_portid;
-
-			/* Deny unload module */
-			try_module_get(THIS_MODULE);
-		}
-	} else if (sc_netlink_usermodeid == info->snd_portid) {
-		TRACEKMOD("*** Already link\n");
-		ret = -EALREADY;
-	} else {
 		TRACEKMOD("*** Busy kernel link\n");
-		ret = -EBUSY;
+		return -EBUSY;
 	}
 
-	return ret;
+	/* Initialize library */
+	ret = sc_capwap_init(nla_get_u32(info->attrs[NLSMARTCAPWAP_ATTR_HASH_SESSION_BITFIELD]), nla_get_u32(info->attrs[NLSMARTCAPWAP_ATTR_SESSION_THREADS_COUNT]));
+	if (ret) {
+		return ret;
+	}
+
+	/* Deny unload module */
+	try_module_get(THIS_MODULE);
+	sc_netlink_usermodeid = info->snd_portid;
+
+	return 0;
+}
+
+/* */
+static int sc_netlink_add_iface(struct sk_buff* skb, struct genl_info* info) {
+	int err;
+	void* hdr;
+	uint16_t mtu;
+	int ifindex;
+	struct sk_buff *msg;
+
+	TRACEKMOD("### sc_netlink_add_iface\n");
+
+	/* Check Link */
+	if (sc_netlink_usermodeid != info->snd_portid) {
+		return -ENOLINK;
+	}
+
+	/* */
+	if (!info->attrs[NLSMARTCAPWAP_ATTR_IFPHY_NAME] || !info->attrs[NLSMARTCAPWAP_ATTR_MTU]) {
+		return -EINVAL;
+	}
+
+	/* */
+	mtu = nla_get_u16(info->attrs[NLSMARTCAPWAP_ATTR_MTU]);
+	if ((mtu < MIN_MTU) || (mtu > MAX_MTU)) {
+		return -EINVAL;
+	}
+
+	/* */
+	ifindex = sc_iface_create((char*)nla_data(info->attrs[NLSMARTCAPWAP_ATTR_IFPHY_NAME]), mtu);
+	if (ifindex < 0) {
+		return ifindex;
+	}
+
+	/* Send response */
+	msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
+	if (!msg) {
+		err = -ENOMEM;
+		goto error;
+	}
+
+	hdr = genlmsg_put(msg, info->snd_portid, info->snd_seq, &sc_netlink_family, 0, NLSMARTCAPWAP_CMD_ADD_IFACE);
+	if (IS_ERR(hdr)) {
+		err = PTR_ERR(hdr);
+		goto error2;
+	}
+
+	if (nla_put_u32(msg, NLSMARTCAPWAP_ATTR_IFPHY_INDEX, (uint32_t)ifindex)) {
+		err = -ENOBUFS;
+		goto error2;
+	}
+
+	genlmsg_end(msg, hdr);
+	return genlmsg_reply(msg, info);
+
+error2:
+	nlmsg_free(msg);
+
+error:
+	sc_iface_delete((uint32_t)ifindex);
+	return err;
+}
+
+/* */
+static int sc_netlink_delete_iface(struct sk_buff* skb, struct genl_info* info) {
+	TRACEKMOD("### sc_netlink_delete_iface\n");
+
+	/* Check Link */
+	if (sc_netlink_usermodeid != info->snd_portid) {
+		return -ENOLINK;
+	}
+
+	/* */
+	return 0;
 }
 
 /* */
@@ -247,19 +299,13 @@ static int sc_netlink_notify(struct notifier_block* nb, unsigned long state, voi
 	struct netlink_notify* notify = (struct netlink_notify*)_notify;
 
 	/* */
-	if (state == NETLINK_URELEASE) {
+	if ((state == NETLINK_URELEASE) && (sc_netlink_usermodeid == notify->portid)) {
-		rtnl_lock();
+		/* Close capwap engine */
+		sc_capwap_close();
 
-		if (sc_netlink_usermodeid == notify->portid) {
+		/* Allow unload module */
-			/* Close capwap engine */
+		module_put(THIS_MODULE);
-			sc_capwap_close();
+		sc_netlink_usermodeid = 0;
-
-			/* Allow unload module */
-			module_put(THIS_MODULE);
-			sc_netlink_usermodeid = 0;
-		}
-
-		rtnl_unlock();
 	}
 
 	return NOTIFY_DONE;
@@ -276,6 +322,8 @@ static const struct nla_policy sc_netlink_policy[NLSMARTCAPWAP_ATTR_MAX + 1] = {
 	[NLSMARTCAPWAP_ATTR_MTU] = { .type = NLA_U16 },
 	[NLSMARTCAPWAP_ATTR_HASH_SESSION_BITFIELD] = { .type = NLA_U32 },
 	[NLSMARTCAPWAP_ATTR_SESSION_THREADS_COUNT] = { .type = NLA_U32 },
+	[NLSMARTCAPWAP_ATTR_IFPHY_NAME] = { .type = NLA_NUL_STRING, .len = IFNAMSIZ },
+	[NLSMARTCAPWAP_ATTR_IFPHY_INDEX] = { .type = NLA_U32 },
 };
 
 /* Netlink Ops */
@@ -286,6 +334,18 @@ static struct genl_ops sc_netlink_ops[] = {
 		.policy = sc_netlink_policy,
 		.flags = GENL_ADMIN_PERM,
 	},
+	{
+		.cmd = NLSMARTCAPWAP_CMD_ADD_IFACE,
+		.doit = sc_netlink_add_iface,
+		.policy = sc_netlink_policy,
+		.flags = GENL_ADMIN_PERM,
+	},
+	{
+		.cmd = NLSMARTCAPWAP_CMD_DELETE_IFACE,
+		.doit = sc_netlink_delete_iface,
+		.policy = sc_netlink_policy,
+		.flags = GENL_ADMIN_PERM,
+	},
 	{
 		.cmd = NLSMARTCAPWAP_CMD_BIND,
 		.doit = sc_netlink_bind,
@@ -401,9 +461,6 @@ int sc_netlink_init(void) {
 
 	TRACEKMOD("### sc_netlink_init\n");
 
-	/* */
-	sc_netlink_usermodeid = 0;
-
 	/* Register netlink family */
 #if LINUX_VERSION_CODE < KERNEL_VERSION(3,12,0)
 	ret = genl_register_family_with_ops(&sc_netlink_family, sc_netlink_ops, sizeof(sc_netlink_ops) / sizeof(sc_netlink_ops[0]));
@@ -411,21 +468,17 @@ int sc_netlink_init(void) {
 	ret = genl_register_family_with_ops(&sc_netlink_family, sc_netlink_ops);
 #endif
 	if (ret) {
-		goto error;
+		return ret;
 	}
 
 	/* Register netlink notifier */
 	ret = netlink_register_notifier(&sc_netlink_notifier);
 	if (ret) {
-		goto error2;
+		genl_unregister_family(&sc_netlink_family);
+		return ret;
 	}
 
 	return 0;
-
-error2:
-	genl_unregister_family(&sc_netlink_family);
-error:
-	return ret;
 }
 
 /* */

src/ac/kmod/nlsmartcapwap.h

@@ -26,6 +26,9 @@ enum sc_netlink_attrs {
 	NLSMARTCAPWAP_ATTR_HASH_SESSION_BITFIELD,
 	NLSMARTCAPWAP_ATTR_SESSION_THREADS_COUNT,
 
+	NLSMARTCAPWAP_ATTR_IFPHY_NAME,
+	NLSMARTCAPWAP_ATTR_IFPHY_INDEX,
+
 	/* Last attribute */
 	__NLSMARTCAPWAP_ATTR_AFTER_LAST,
 	NLSMARTCAPWAP_ATTR_MAX = __NLSMARTCAPWAP_ATTR_AFTER_LAST - 1
@@ -37,6 +40,9 @@ enum sc_netlink_commands {
 
 	NLSMARTCAPWAP_CMD_LINK,
 
+	NLSMARTCAPWAP_CMD_ADD_IFACE,
+	NLSMARTCAPWAP_CMD_DELETE_IFACE,
+
 	NLSMARTCAPWAP_CMD_BIND,
 
 	NLSMARTCAPWAP_CMD_SEND_KEEPALIVE,

src/ac/kmod/socket.c

@@ -18,12 +18,10 @@ static struct socket* sc_sockets[SOCKET_COUNT];
 
 /* */
 int sc_socket_recvpacket(struct sock* sk, struct sk_buff* skb) {
-	struct sc_skb_capwap_cb* cb = CAPWAP_SKB_CB(skb);
-
 	TRACEKMOD("### sc_socket_recvpacket\n");
 
 	/* */
-	cb->flags = SKB_CAPWAP_FLAG_FROM_DATA_CHANNEL;
+	CAPWAP_SKB_CB(skb)->flags = SKB_CAPWAP_FLAG_FROM_DATA_CHANNEL;
 
 	/* */
 	sc_capwap_recvpacket(skb);
@@ -39,13 +37,13 @@ static int sc_socket_create(int type, union capwap_addr* sockaddr, uint16_t prot
 	/* Create socket */
 	ret = sock_create_kern(sockaddr->ss.ss_family, SOCK_DGRAM, protocol, &sc_sockets[type]);
 	if (ret) {
-		goto failure;
+		return ret;
 	}
 
 	/* Bind to interface */
 	ret = kernel_bind(sc_sockets[type], &sockaddr->sa, sizeof(union capwap_addr));
 	if (ret) {
-		goto failure2;
+		goto failure;
 	}
 
 	/* Set callback */
@@ -60,7 +58,7 @@ static int sc_socket_create(int type, union capwap_addr* sockaddr, uint16_t prot
 		/* Retrieve port */
 		ret = kernel_getsockname(sc_sockets[type], &localaddr.sa, &localaddrsize);
 		if (ret) {
-			goto failure2;
+			goto failure;
 		}
 
 		/* */
@@ -70,17 +68,15 @@ static int sc_socket_create(int type, union capwap_addr* sockaddr, uint16_t prot
 			sockaddr->sin6.sin6_port = localaddr.sin6.sin6_port;
 		} else {
 			ret = -EFAULT;
-			goto failure2;
+			goto failure;
 		}
 	}
 
-	return ret;
+	return 0;
-
-failure2:
-	sock_release(sc_sockets[type]);
-	sc_sockets[type] = 0;
 
 failure:
+	sock_release(sc_sockets[type]);
+	sc_sockets[type] = 0;
 	return ret;
 }
 

src/wtp/kmod/capwap.c

@@ -557,7 +557,7 @@ int sc_capwap_forwarddata(struct sc_capwap_session* session, uint8_t radioid, ui
 		}
 
 		/* */
-		header = (struct sc_capwap_header*)((uint8_t*)header + (size + length));
+		header = (struct sc_capwap_header*)(((uint8_t*)header + size + length) - sizeof(struct sc_capwap_header));
 		fragmentoffset += length;
 		packetlength -= length;
 	}

src/wtp/kmod/socket.c

@@ -18,12 +18,10 @@ static struct socket* sc_sockets[SOCKET_COUNT];
 
 /* */
 int sc_socket_recvpacket(struct sock* sk, struct sk_buff* skb) {
-	struct sc_skb_capwap_cb* cb = CAPWAP_SKB_CB(skb);
-
 	TRACEKMOD("### sc_socket_recvpacket\n");
 
 	/* */
-	cb->flags = SKB_CAPWAP_FLAG_FROM_DATA_CHANNEL;
+	CAPWAP_SKB_CB(skb)->flags = SKB_CAPWAP_FLAG_FROM_DATA_CHANNEL;
 
 	/* */
 	sc_capwap_recvpacket(skb);
@@ -39,13 +37,13 @@ static int sc_socket_create(int type, union capwap_addr* sockaddr, uint16_t prot
 	/* Create socket */
 	ret = sock_create_kern(sockaddr->ss.ss_family, SOCK_DGRAM, protocol, &sc_sockets[type]);
 	if (ret) {
-		goto failure;
+		return ret;
 	}
 
 	/* Bind to interface */
 	ret = kernel_bind(sc_sockets[type], &sockaddr->sa, sizeof(union capwap_addr));
 	if (ret) {
-		goto failure2;
+		goto failure;
 	}
 
 	/* Set callback */
@@ -60,7 +58,7 @@ static int sc_socket_create(int type, union capwap_addr* sockaddr, uint16_t prot
 		/* Retrieve port */
 		ret = kernel_getsockname(sc_sockets[type], &localaddr.sa, &localaddrsize);
 		if (ret) {
-			goto failure2;
+			goto failure;
 		}
 
 		/* */
@@ -70,17 +68,15 @@ static int sc_socket_create(int type, union capwap_addr* sockaddr, uint16_t prot
 			sockaddr->sin6.sin6_port = localaddr.sin6.sin6_port;
 		} else {
 			ret = -EFAULT;
-			goto failure2;
+			goto failure;
 		}
 	}
 
-	return ret;
+	return 0;
-
-failure2:
-	sock_release(sc_sockets[type]);
-	sc_sockets[type] = 0;
 
 failure:
+	sock_release(sc_sockets[type]);
+	sc_sockets[type] = 0;
 	return ret;
 }
 
@@ -233,10 +229,10 @@ int sc_addr_compare(const union capwap_addr* addr1, const union capwap_addr* add
 /* */
 void sc_addr_tolittle(const union capwap_addr* addr, struct capwap_addr_little* little) {
 	little->family = (uint8_t)addr->ss.ss_family;
-	if (little->family == AF_INET) {
+	if (addr->ss.ss_family == AF_INET) {
 		memcpy(&little->addr4, &addr->sin.sin_addr, sizeof(struct in_addr));
 		little->port = addr->sin.sin_port;
-	} else if (little->family == AF_INET6) {
+	} else if (addr->ss.ss_family == AF_INET6) {
 		memcpy(&little->addr6, &addr->sin6.sin6_addr, sizeof(struct in6_addr));
 		little->port = addr->sin6.sin6_port;
 	}