Added functionality into capwap data channel kernel module

openwrt/mac80211_patchs/920-mac80211_packet_tunnel.patch

@@ -1,6 +1,7 @@
---- a/include/net/mac80211.h	2014-07-10 19:19:55.000000000 +0200
+diff -ur a/include/net/mac80211.h b/include/net/mac80211.h
-+++ b/include/net/mac80211.h	2014-07-10 20:52:02.000000000 +0200
+--- a/include/net/mac80211.h	2014-12-23 18:25:24.000000000 +0100
-@@ -4772,4 +4772,24 @@ int ieee80211_parse_p2p_noa(const struct
++++ b/include/net/mac80211.h	2014-12-10 21:42:20.000000000 +0100
+@@ -4772,4 +4772,29 @@
   */
  void ieee80211_update_p2p_noa(struct ieee80211_noa_data *data, u32 tsf);
  
@@ -23,11 +24,17 @@
 + *
 + */
 +int ieee80211_pcktunnel_deregister(struct net_device *dev, struct ieee80211_pcktunnel *handler);
++
++/**
++ *
++ */
++netdev_tx_t ieee80211_inject_xmit(struct sk_buff* skb, struct net_device* dev);
 +
  #endif /* MAC80211_H */
---- a/net/mac80211/ieee80211_i.h	2014-07-10 19:19:55.000000000 +0200
+diff -ur a/net/mac80211/ieee80211_i.h b/net/mac80211/ieee80211_i.h
-+++ b/net/mac80211/ieee80211_i.h	2014-07-10 20:47:56.000000000 +0200
+--- a/net/mac80211/ieee80211_i.h	2014-12-23 18:25:24.000000000 +0100
-@@ -165,6 +165,7 @@ typedef unsigned __bitwise__ ieee80211_r
++++ b/net/mac80211/ieee80211_i.h	2014-07-23 21:22:30.000000000 +0200
+@@ -165,6 +165,7 @@
  #define RX_DROP_UNUSABLE	((__force ieee80211_rx_result) 1u)
  #define RX_DROP_MONITOR		((__force ieee80211_rx_result) 2u)
  #define RX_QUEUED		((__force ieee80211_rx_result) 3u)
@@ -35,7 +42,7 @@
  
  /**
   * enum ieee80211_packet_rx_flags - packet RX flags
-@@ -743,6 +744,9 @@ struct ieee80211_sub_if_data {
+@@ -743,6 +744,9 @@
  
  	char name[IFNAMSIZ];
  
@@ -45,9 +52,10 @@
  	/* Fragment table for host-based reassembly */
  	struct ieee80211_fragment_entry	fragments[IEEE80211_FRAGMENT_MAX];
  	unsigned int fragment_next;
---- a/net/mac80211/iface.c	2014-07-10 19:19:55.000000000 +0200
+diff -ur a/net/mac80211/iface.c b/net/mac80211/iface.c
-+++ b/net/mac80211/iface.c	2014-07-10 20:51:28.000000000 +0200
+--- a/net/mac80211/iface.c	2014-12-23 18:25:24.000000000 +0100
-@@ -1844,3 +1844,45 @@ void ieee80211_iface_exit(void)
++++ b/net/mac80211/iface.c	2014-07-23 21:22:30.000000000 +0200
+@@ -1844,3 +1844,45 @@
  {
  	unregister_netdevice_notifier(&mac80211_netdev_notifier);
  }
@@ -93,9 +101,10 @@
 +}
 +EXPORT_SYMBOL(ieee80211_pcktunnel_deregister);
 +
---- a/net/mac80211/rx.c	2014-07-10 19:19:55.000000000 +0200
+diff -ur a/net/mac80211/rx.c b/net/mac80211/rx.c
-+++ b/net/mac80211/rx.c	2014-07-10 21:01:19.000000000 +0200
+--- a/net/mac80211/rx.c	2014-12-23 18:25:24.000000000 +0100
-@@ -2831,6 +2831,51 @@ ieee80211_rx_h_mgmt(struct ieee80211_rx_
++++ b/net/mac80211/rx.c	2014-07-23 21:22:30.000000000 +0200
+@@ -2831,6 +2831,51 @@
  	return RX_QUEUED;
  }
  
@@ -147,7 +156,7 @@
  /* TODO: use IEEE80211_RX_FRAGMENTED */
  static void ieee80211_rx_cooked_monitor(struct ieee80211_rx_data *rx,
  					struct ieee80211_rate *rate)
-@@ -2910,6 +2955,7 @@ static void ieee80211_rx_handlers_result
+@@ -2910,6 +2955,7 @@
  		if (rx->sta)
  			rx->sta->rx_dropped++;
  		/* fall through */
@@ -155,7 +164,7 @@
  	case RX_CONTINUE: {
  		struct ieee80211_rate *rate = NULL;
  		struct ieee80211_supported_band *sband;
-@@ -2938,7 +2984,9 @@ static void ieee80211_rx_handlers_result
+@@ -2938,7 +2984,9 @@
  }
  
  static void ieee80211_rx_handlers(struct ieee80211_rx_data *rx,
@@ -166,7 +175,7 @@
  {
  	ieee80211_rx_result res = RX_DROP_MONITOR;
  	struct sk_buff *skb;
-@@ -2971,6 +3019,11 @@ static void ieee80211_rx_handlers(struct
+@@ -2971,6 +3019,11 @@
  		if (ieee80211_vif_is_mesh(&rx->sdata->vif))
  			CALL_RXH(ieee80211_rx_h_mesh_fwding);
  #endif
@@ -178,7 +187,7 @@
  		CALL_RXH(ieee80211_rx_h_amsdu)
  		CALL_RXH(ieee80211_rx_h_data)
  
-@@ -2994,7 +3047,8 @@ static void ieee80211_rx_handlers(struct
+@@ -2994,7 +3047,8 @@
  	spin_unlock_bh(&rx->local->rx_path_lock);
  }
  
@@ -188,7 +197,7 @@
  {
  	struct sk_buff_head reorder_release;
  	ieee80211_rx_result res = RX_DROP_MONITOR;
-@@ -3012,7 +3066,7 @@ static void ieee80211_invoke_rx_handlers
+@@ -3012,7 +3066,7 @@
  
  	ieee80211_rx_reorder_ampdu(rx, &reorder_release);
  
@@ -197,7 +206,7 @@
  	return;
  
   rxh_next:
-@@ -3049,7 +3103,7 @@ void ieee80211_release_reorder_timeout(s
+@@ -3049,7 +3103,7 @@
  	ieee80211_sta_reorder_release(sta->sdata, tid_agg_rx, &frames);
  	spin_unlock(&tid_agg_rx->reorder_lock);
  
@@ -206,7 +215,7 @@
  }
  
  /* main receive path */
-@@ -3163,7 +3217,9 @@ static bool prepare_for_handlers(struct 
+@@ -3163,7 +3217,9 @@
   * or not the skb was consumed.
   */
  static bool ieee80211_prepare_and_rx_handle(struct ieee80211_rx_data *rx,
@@ -217,7 +226,7 @@
  {
  	struct ieee80211_local *local = rx->local;
  	struct ieee80211_sub_if_data *sdata = rx->sdata;
-@@ -3189,7 +3245,7 @@ static bool ieee80211_prepare_and_rx_han
+@@ -3189,7 +3245,7 @@
  		rx->skb = skb;
  	}
  
@@ -226,7 +235,7 @@
  	return true;
  }
  
-@@ -3198,7 +3254,8 @@ static bool ieee80211_prepare_and_rx_han
+@@ -3198,7 +3254,8 @@
   * be called with rcu_read_lock protection.
   */
  static void __ieee80211_rx_handle_packet(struct ieee80211_hw *hw,
@@ -236,7 +245,7 @@
  {
  	struct ieee80211_local *local = hw_to_local(hw);
  	struct ieee80211_sub_if_data *sdata;
-@@ -3251,7 +3308,7 @@ static void __ieee80211_rx_handle_packet
+@@ -3251,7 +3308,7 @@
  
  			rx.sta = prev_sta;
  			rx.sdata = prev_sta->sdata;
@@ -245,7 +254,7 @@
  
  			prev_sta = sta;
  		}
-@@ -3260,7 +3317,7 @@ static void __ieee80211_rx_handle_packet
+@@ -3260,7 +3317,7 @@
  			rx.sta = prev_sta;
  			rx.sdata = prev_sta->sdata;
  
@@ -254,7 +263,7 @@
  				return;
  			goto out;
  		}
-@@ -3289,7 +3346,7 @@ static void __ieee80211_rx_handle_packet
+@@ -3289,7 +3346,7 @@
  
  		rx.sta = sta_info_get_bss(prev, hdr->addr2);
  		rx.sdata = prev;
@@ -263,7 +272,7 @@
  
  		prev = sdata;
  	}
-@@ -3298,7 +3355,7 @@ static void __ieee80211_rx_handle_packet
+@@ -3298,7 +3355,7 @@
  		rx.sta = sta_info_get_bss(prev, hdr->addr2);
  		rx.sdata = prev;
  
@@ -272,7 +281,7 @@
  			return;
  	}
  
-@@ -3409,7 +3466,7 @@ void ieee80211_rx(struct ieee80211_hw *h
+@@ -3409,7 +3466,7 @@
  	ieee80211_tpt_led_trig_rx(local,
  			((struct ieee80211_hdr *)skb->data)->frame_control,
  			skb->len);
@@ -281,3 +290,121 @@
  
  	rcu_read_unlock();
  
+diff -ur a/net/mac80211/tx.c b/net/mac80211/tx.c
+--- a/net/mac80211/tx.c	2014-12-23 18:25:24.000000000 +0100
++++ b/net/mac80211/tx.c	2014-12-21 17:51:34.000000000 +0100
+@@ -3050,3 +3050,114 @@
+ 	ieee80211_xmit(sdata, skb, band);
+ 	local_bh_enable();
+ }
++
++netdev_tx_t ieee80211_inject_xmit(struct sk_buff* skb, struct net_device* dev) {
++	int hdrlen;
++	int multicast;
++	uint16_t info_id = 0;
++	uint32_t info_flags = 0;
++	struct ieee80211_chanctx_conf* chanctx_conf;
++	struct ieee80211_sub_if_data* sdata = IEEE80211_DEV_TO_SUB_IF(dev);
++	struct ieee80211_hdr* hdr = (struct ieee80211_hdr*)skb->data;
++	struct ieee80211_tx_info* info = IEEE80211_SKB_CB(skb);
++
++	rcu_read_lock();
++
++	/* */
++	if (sdata->vif.type != NL80211_IFTYPE_AP) {
++		goto error;
++	}
++
++	/* */
++	hdrlen = ieee80211_hdrlen(hdr->frame_control);
++	if (skb->len < hdrlen) {
++		goto error;
++	}
++
++	/* */
++	chanctx_conf = rcu_dereference(sdata->vif.chanctx_conf);
++	if (!chanctx_conf) {
++		goto error;
++	}
++
++	/* */
++	multicast = is_multicast_ether_addr(hdr->addr1);
++	if (!multicast) {
++		struct sta_info* sta = sta_info_get(sdata, hdr->addr1);
++		if (!sta || !test_sta_flag(sta, WLAN_STA_AUTHORIZED)) {
++			goto error;
++		}
++
++	}
++
++	/* */
++	if (unlikely(!multicast && skb->sk && (skb_shinfo(skb)->tx_flags & SKBTX_WIFI_STATUS))) {
++		struct sk_buff *orig_skb = skb;
++
++		skb = skb_clone(skb, GFP_ATOMIC);
++		if (skb) {
++			int id;
++			unsigned long flags;
++			struct ieee80211_local* local = sdata->local;
++
++			spin_lock_irqsave(&local->ack_status_lock, flags);
++			id = idr_alloc(&local->ack_status_frames, orig_skb, 1, 0x10000, GFP_ATOMIC);
++			spin_unlock_irqrestore(&local->ack_status_lock, flags);
++
++			if (id >= 0) {
++				info_id = id;
++				info_flags |= IEEE80211_TX_CTL_REQ_TX_STATUS;
++			} else if (skb_shared(skb)) {
++				kfree_skb(orig_skb);
++			} else {
++				kfree_skb(skb);
++				skb = orig_skb;
++			}
++		} else {
++			skb = orig_skb;		/* couldn't clone -- lose tx status ... */
++		}
++	}
++
++	/* If the skb is shared we need to obtain our own copy. */
++	if (skb_shared(skb)) {
++		struct sk_buff *tmp_skb = skb;
++
++		/* can't happen -- skb is a clone if info_id != 0 */
++		WARN_ON(info_id);
++
++		skb = skb_clone(skb, GFP_ATOMIC);
++		kfree_skb(tmp_skb);
++
++		if (!skb) {
++			goto error;
++		}
++	}
++
++	/* */
++	dev->stats.tx_packets++;
++	dev->stats.tx_bytes += skb->len;
++
++	/* */
++	skb_reset_mac_header(skb);
++	skb_reset_network_header(skb);
++	skb_reset_transport_header(skb);
++
++	/* */
++	memset(info, 0, sizeof(struct ieee80211_tx_info));
++	dev->trans_start = jiffies;
++	info->flags = info_flags;
++	info->ack_frame_id = info_id;
++
++	/* */
++	ieee80211_xmit(sdata, skb, chanctx_conf->def.chan->band);
++	rcu_read_unlock();
++
++	return NETDEV_TX_OK;
++
++error:
++	rcu_read_unlock();
++	dev_kfree_skb(skb);
++	return NETDEV_TX_OK;
++}
++EXPORT_SYMBOL(ieee80211_inject_xmit);
++

src/ac/ac.c

@@ -25,7 +25,7 @@ static char g_configurationfile[260] = AC_DEFAULT_CONFIGURATION_FILE;
 static unsigned long ac_stations_item_gethash(const void* key, unsigned long hashsize) {
 	uint8_t* macaddress = (uint8_t*)key;
 
-	return ((((unsigned long)macaddress[4] << 8) | (unsigned long)macaddress[5]) ^ ((unsigned long)macaddress[3] << 4));
+	return (((((unsigned long)macaddress[4] << 8) | (unsigned long)macaddress[5]) ^ ((unsigned long)macaddress[3] << 4)) % AC_STATIONS_HASH_SIZE);
 }
 
 /* */
@@ -859,7 +859,7 @@ int main(int argc, char** argv) {
 		result = ac_configure();
 		if (result == CAPWAP_SUCCESSFUL) {
 			/* Connect AC to kernel module */
-			if (!ac_kmod_init(16, 4)) {		/* TODO change static value with param */
+			if (!ac_kmod_init()) {
 				/* Bind data channel */
 				if (!ac_kmod_createdatachannel(g_ac.net.localaddr.ss.ss_family, CAPWAP_GET_NETWORK_PORT(&g_ac.net.localaddr) + 1)) {
 					capwap_logging_info("SmartCAPWAP kernel module connected");

src/ac/ac.h

@@ -24,6 +24,8 @@
 #define AC_DEFAULT_MAXSTATION				128
 #define AC_DEFAULT_MAXSESSIONS				128
 
+#define VLAN_MAX							4096
+
 /* AC runtime error return code */
 #define AC_ERROR_SYSTEM_FAILER				-1000
 #define AC_ERROR_LOAD_CONFIGURATION			-1001
@@ -125,7 +127,7 @@ struct ac_t {
 	/* Sessions message queue */
 	int fdmsgsessions[2];
 
-	/* */
+	/* Kernel module */
 	struct ac_kmod_handle kmodhandle;
 
 	/* Sessions */

src/ac/ac_dfa_datacheck.c

@@ -171,7 +171,7 @@ void ac_dfa_state_datacheck(struct ac_session_t* session, struct capwap_parsed_p
 
 		/* Create data session */
 		if (CAPWAP_RESULTCODE_OK(result)) {
-			if (ac_kmod_new_datasession(&session->sessionid, session->mtu)) {
+			if (ac_kmod_new_datasession(&session->sessionid, (uint8_t)session->binding, session->mtu)) {
 				result = CAPWAP_RESULTCODE_FAILURE;
 			}
 		}

src/ac/ac_dfa_run.c

@@ -83,10 +83,19 @@ static void execute_ieee80211_wlan_configuration_addwlan(struct ac_session_t* se
 	/* Get BSSID */
 	assignbssid = (struct capwap_80211_assignbssid_element*)capwap_get_message_element_data(packet, CAPWAP_ELEMENT_80211_ASSIGN_BSSID);
 	if (assignbssid && (assignbssid->radioid == addwlan->radioid) && (assignbssid->wlanid == addwlan->wlanid)) {
-		wlan = ac_wlans_create_bssid(&session->wlans->devices[assignbssid->radioid - 1], assignbssid->wlanid, assignbssid->bssid, addwlan);
+		if (!ac_kmod_addwlan(&session->sessionid, assignbssid->radioid, assignbssid->wlanid, assignbssid->bssid, addwlan->macmode, addwlan->tunnelmode)) {
+			wlan = ac_wlans_create_bssid(&session->wlans->devices[assignbssid->radioid - 1], assignbssid->wlanid, assignbssid->bssid, addwlan);
 
-		/* Assign BSSID to session */
+			/* Assign BSSID to session */
-		ac_wlans_assign_bssid(session, wlan);
+			if (ac_wlans_assign_bssid(session, wlan)) {
+				capwap_logging_warning("Unable to add new wlan with radioid: %d, wlanid: %d", (int)assignbssid->radioid, (int)assignbssid->wlanid);
+				ac_wlans_free_bssid(wlan);
+
+				/* TODO: add remove wlan from wtp */
+			}
+		} else {
+			/* TODO: add remove wlan from wtp */
+		}
 	}
 }
 

src/ac/ac_execute.c

@@ -72,7 +72,7 @@ static int ac_update_configuration_datachannelinterfaces(void* data, void* param
 			if (jsonindex && (json_object_get_type(jsonindex) == json_type_int)) {
 				if (iface->index == (unsigned long)json_object_get_int(jsonindex)) {
 					if (!ac_update_configuration_getdatachannel_params(jsonvalue, &mtu, &bridge)) {
-						/* TODO */
+						/* TODO update interface */
 					}
 
 					/* Interface found */

src/ac/ac_ieee80211_data.c

@@ -94,7 +94,7 @@ static void ac_ieee80211_mgmt_authentication_packet(struct ac_session_t* session
 			responselength = ieee80211_create_authentication_response(buffer, sizeof(buffer), &ieee80211_params);
 			if (responselength > 0) {
 				/* Send authentication response */
-				if (!ac_kmod_send_data(&session->sockaddrdata.ss, wlan->device->radioid, session->binding, buffer, responselength)) {
+				if (!ac_kmod_send_data(&session->sessionid, wlan->device->radioid, session->binding, buffer, responselength)) {
 					capwap_logging_info("Sent IEEE802.11 Authentication Response to %s station with %d status code", station->addrtext, (int)responsestatuscode);
 					station->flags |= AC_STATION_FLAGS_AUTHENTICATED;
 				} else {
@@ -236,7 +236,7 @@ static void ac_ieee80211_mgmt_association_request_packet(struct ac_session_t* se
 				responselength = ieee80211_create_associationresponse_response(buffer, sizeof(buffer), &ieee80211_params);
 				if (responselength > 0) {
 					/* Send association response */
-					if (!ac_kmod_send_data(&session->sockaddrdata.ss, wlan->device->radioid, session->binding, buffer, responselength)) {
+					if (!ac_kmod_send_data(&session->sessionid, wlan->device->radioid, session->binding, buffer, responselength)) {
 						capwap_logging_info("Sent IEEE802.11 Association Response to %s station with %d status code", station->addrtext, (int)resultstatuscode);
 
 						/* Active Station */

src/ac/ac_kmod.c

@@ -61,23 +61,27 @@ static struct nl_sock* nl_create_handle(struct nl_cb* cb) {
 
 /* */
 static int ac_kmod_no_seq_check(struct nl_msg* msg, void* arg) {
+	capwap_logging_debug("Call ac_kmod_no_seq_check");
 	return NL_OK;
 }
 
 /* */
 static int ac_kmod_error_handler(struct sockaddr_nl* nla, struct nlmsgerr* err, void* arg) {
+	capwap_logging_debug("Call ac_kmod_error_handler %d", err->error);
 	*((int*)arg) = err->error;
 	return NL_STOP;
 }
 
 /* */
 static int ac_kmod_finish_handler(struct nl_msg* msg, void* arg) {
+	capwap_logging_debug("Call ac_kmod_finish_handler");
 	*((int*)arg) = 0;
 	return NL_SKIP;
 }
 
 /* */
 static int ac_kmod_ack_handler(struct nl_msg* msg, void* arg) {
+	capwap_logging_debug("Call ac_kmod_ack_handler");
 	*((int*)arg) = 0;
 	return NL_STOP;
 }
@@ -86,18 +90,12 @@ static int ac_kmod_ack_handler(struct nl_msg* msg, void* arg) {
 static int ac_kmod_event_handler(struct genlmsghdr* gnlh, struct nlattr** tb_msg, void* data) {
 	switch (gnlh->cmd) {
 		case NLSMARTCAPWAP_CMD_RECV_KEEPALIVE: {
-			if (tb_msg[NLSMARTCAPWAP_ATTR_ADDRESS] && tb_msg[NLSMARTCAPWAP_ATTR_SESSION_ID]) {
+			if (tb_msg[NLSMARTCAPWAP_ATTR_SESSION_ID]) {
-				struct ac_session_t* session = ac_search_session_from_sessionid((struct capwap_sessionid_element*)nla_data(tb_msg[NLSMARTCAPWAP_ATTR_SESSION_ID]));
+				struct capwap_sessionid_element* sessionid = (struct capwap_sessionid_element*)nla_data(tb_msg[NLSMARTCAPWAP_ATTR_SESSION_ID]);
+				struct ac_session_t* session = ac_search_session_from_sessionid(sessionid);
 
 				if (session) {
-					/* Save data channel address */
+					ac_kmod_send_keepalive(sessionid);
-					if (session->sockaddrdata.ss.ss_family == AF_UNSPEC)  {
-						capwap_lock_enter(&session->sessionlock);
-						memcpy(&session->sockaddrdata.ss, nla_data(tb_msg[NLSMARTCAPWAP_ATTR_ADDRESS]), sizeof(struct sockaddr_storage));
-						capwap_lock_exit(&session->sessionlock);
-					}
-
-					/* Notify keep-alive */
 					ac_session_send_action(session, AC_SESSION_ACTION_RECV_KEEPALIVE, 0, NULL, 0);
 					ac_session_release_reference(session);
 				}
@@ -171,11 +169,11 @@ static int ac_kmod_send_and_recv(struct nl_sock* nl, struct nl_cb* nl_cb, struct
 
 /* */
 static int ac_kmod_send_and_recv_msg(struct nl_msg* msg, ac_kmod_valid_cb valid_cb, void* data) {
-	return ac_kmod_send_and_recv(g_ac.kmodhandle.nl, g_ac.kmodhandle.nl_cb, msg, valid_cb, data);
+	return ac_kmod_send_and_recv(g_ac.kmodhandle.nlmsg, g_ac.kmodhandle.nlmsg_cb, msg, valid_cb, data);
 }
 
 /* */
-static int ac_kmod_link(uint32_t hash, uint32_t threads) {
+static int ac_kmod_link(void) {
 	int result;
 	struct nl_msg* msg;
 
@@ -187,11 +185,9 @@ static int ac_kmod_link(uint32_t hash, uint32_t threads) {
 
 	/* */
 	genlmsg_put(msg, 0, 0, g_ac.kmodhandle.nlsmartcapwap_id, 0, 0, NLSMARTCAPWAP_CMD_LINK, 0);
-	nla_put_u32(msg, NLSMARTCAPWAP_ATTR_HASH_SESSION_BITFIELD, hash);
-	nla_put_u32(msg, NLSMARTCAPWAP_ATTR_SESSION_THREADS_COUNT, threads);
 
 	/* */
-	result = ac_kmod_send_and_recv_msg(msg, NULL, NULL);
+	result = ac_kmod_send_and_recv(g_ac.kmodhandle.nl, g_ac.kmodhandle.nl_cb, msg, NULL, NULL);
 	if (result) {
 		if (result == -EALREADY) {
 			result = 0;
@@ -221,11 +217,11 @@ static void ac_kmod_event_receive(int fd, void** params, int paramscount) {
 }
 
 /* */
-int ac_kmod_send_keepalive(struct sockaddr_storage* sockaddr) {
+int ac_kmod_send_keepalive(struct capwap_sessionid_element* sessionid) {
 	int result;
 	struct nl_msg* msg;
 
-	ASSERT(sockaddr != NULL);
+	ASSERT(sessionid != NULL);
 
 	/* */
 	msg = nlmsg_alloc();
@@ -235,13 +231,15 @@ int ac_kmod_send_keepalive(struct sockaddr_storage* sockaddr) {
 
 	/* */
 	genlmsg_put(msg, 0, 0, g_ac.kmodhandle.nlsmartcapwap_id, 0, 0, NLSMARTCAPWAP_CMD_SEND_KEEPALIVE, 0);
-	nla_put(msg, NLSMARTCAPWAP_ATTR_ADDRESS, sizeof(struct sockaddr_storage), sockaddr);
+	nla_put(msg, NLSMARTCAPWAP_ATTR_SESSION_ID, sizeof(struct capwap_sessionid_element), sessionid);
 
 	/* */
+	capwap_logging_debug("Prepare to send keep-alive");
 	result = ac_kmod_send_and_recv_msg(msg, NULL, NULL);
 	if (result) {
 		capwap_logging_error("Unable to send keep-alive: %d", result);
 	}
+	capwap_logging_debug("Sent keep-alive");
 
 	/* */
 	nlmsg_free(msg);
@@ -249,11 +247,11 @@ 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_send_data(struct capwap_sessionid_element* sessionid, uint8_t radioid, uint8_t binding, const uint8_t* data, int length) {
 	int result;
 	struct nl_msg* msg;
 
-	ASSERT(sockaddr != NULL);
+	ASSERT(sessionid != NULL);
 	ASSERT(data != NULL);
 	ASSERT(length > 0);
 
@@ -265,7 +263,7 @@ int ac_kmod_send_data(struct sockaddr_storage* sockaddr, uint8_t radioid, uint8_
 
 	/* */
 	genlmsg_put(msg, 0, 0, g_ac.kmodhandle.nlsmartcapwap_id, 0, 0, NLSMARTCAPWAP_CMD_SEND_DATA, 0);
-	nla_put(msg, NLSMARTCAPWAP_ATTR_ADDRESS, sizeof(struct sockaddr_storage), sockaddr);
+	nla_put(msg, NLSMARTCAPWAP_ATTR_SESSION_ID, sizeof(struct capwap_sessionid_element), sessionid);
 	nla_put_u8(msg, NLSMARTCAPWAP_ATTR_RADIOID, radioid);
 	nla_put_u8(msg, NLSMARTCAPWAP_ATTR_BINDING, binding);
 	nla_put(msg, NLSMARTCAPWAP_ATTR_DATA_FRAME, length, data);
@@ -354,7 +352,7 @@ int ac_kmod_createdatachannel(int family, unsigned short port) {
 }
 
 /* */
-int ac_kmod_new_datasession(struct capwap_sessionid_element* sessionid, uint16_t mtu) {
+int ac_kmod_new_datasession(struct capwap_sessionid_element* sessionid, uint8_t binding, uint16_t mtu) {
 	int result;
 	struct nl_msg* msg;
 
@@ -369,6 +367,7 @@ int ac_kmod_new_datasession(struct capwap_sessionid_element* sessionid, uint16_t
 	/* */
 	genlmsg_put(msg, 0, 0, g_ac.kmodhandle.nlsmartcapwap_id, 0, 0, NLSMARTCAPWAP_CMD_NEW_SESSION, 0);
 	nla_put(msg, NLSMARTCAPWAP_ATTR_SESSION_ID, sizeof(struct capwap_sessionid_element), sessionid);
+	nla_put_u16(msg, NLSMARTCAPWAP_ATTR_BINDING, binding);
 	nla_put_u16(msg, NLSMARTCAPWAP_ATTR_MTU, mtu);
 
 	/* */
@@ -410,6 +409,70 @@ int ac_kmod_delete_datasession(struct capwap_sessionid_element* sessionid) {
 	return result;
 }
 
+/* */
+int ac_kmod_addwlan(struct capwap_sessionid_element* sessionid, uint8_t radioid, uint8_t wlanid, const uint8_t* bssid, uint8_t macmode, uint8_t tunnelmode) {
+	int result;
+	struct nl_msg* msg;
+
+	ASSERT(sessionid != NULL);
+	ASSERT(IS_VALID_RADIOID(radioid));
+	ASSERT(IS_VALID_WLANID(wlanid));
+	ASSERT(bssid != NULL);
+
+	/* */
+	msg = nlmsg_alloc();
+	if (!msg) {
+		return -1;
+	}
+
+	/* */
+	genlmsg_put(msg, 0, 0, g_ac.kmodhandle.nlsmartcapwap_id, 0, 0, NLSMARTCAPWAP_CMD_ADD_WLAN, 0);
+	nla_put(msg, NLSMARTCAPWAP_ATTR_SESSION_ID, sizeof(struct capwap_sessionid_element), sessionid);
+	nla_put_u8(msg, NLSMARTCAPWAP_ATTR_RADIOID, radioid);
+	nla_put_u8(msg, NLSMARTCAPWAP_ATTR_WLANID, wlanid);
+	nla_put(msg, NLSMARTCAPWAP_ATTR_MACADDRESS, MACADDRESS_EUI48_LENGTH, bssid);
+	nla_put_u8(msg, NLSMARTCAPWAP_ATTR_MACMODE, macmode);
+	nla_put_u8(msg, NLSMARTCAPWAP_ATTR_TUNNELMODE, tunnelmode);
+
+	/* */
+	result = ac_kmod_send_and_recv_msg(msg, NULL, NULL);
+	if (result) {
+		capwap_logging_error("Unable to add wlan: %d", result);
+	}
+
+	/* */
+	nlmsg_free(msg);
+	return result;
+}
+
+/* */
+int ac_kmod_removewlan(struct capwap_sessionid_element* sessionid) {
+	int result;
+	struct nl_msg* msg;
+
+	ASSERT(sessionid != NULL);
+
+	/* */
+	msg = nlmsg_alloc();
+	if (!msg) {
+		return -1;
+	}
+
+	/* */
+	genlmsg_put(msg, 0, 0, g_ac.kmodhandle.nlsmartcapwap_id, 0, 0, NLSMARTCAPWAP_CMD_REMOVE_WLAN, 0);
+	nla_put(msg, NLSMARTCAPWAP_ATTR_SESSION_ID, sizeof(struct capwap_sessionid_element), sessionid);
+
+	/* */
+	result = ac_kmod_send_and_recv_msg(msg, NULL, NULL);
+	if (result && (result != ENOENT)) {
+		capwap_logging_error("Unable to remove wlan: %d", result);
+	}
+
+	/* */
+	nlmsg_free(msg);
+	return result;
+}
+
 /* */
 static int cb_kmod_create_iface(struct nl_msg* msg, void* data) {
 	struct nlattr* tb_msg[NLSMARTCAPWAP_ATTR_MAX + 1];
@@ -487,7 +550,77 @@ int ac_kmod_delete_iface(int ifindex) {
 }
 
 /* */
-int ac_kmod_init(uint32_t hash, uint32_t threads) {
+int ac_kmod_authorize_station(struct capwap_sessionid_element* sessionid, const uint8_t* macaddress, int ifindex, uint8_t radioid, uint8_t wlanid, uint16_t vlan) {
+	int result;
+	struct nl_msg* msg;
+
+	ASSERT(sessionid != NULL);
+	ASSERT(macaddress != NULL);
+	ASSERT(ifindex >= 0);
+	ASSERT(IS_VALID_RADIOID(radioid));
+	ASSERT(vlan < VLAN_MAX);
+
+	/* */
+	msg = nlmsg_alloc();
+	if (!msg) {
+		return -1;
+	}
+
+	/* */
+	genlmsg_put(msg, 0, 0, g_ac.kmodhandle.nlsmartcapwap_id, 0, 0, NLSMARTCAPWAP_CMD_AUTH_STATION, 0);
+	nla_put(msg, NLSMARTCAPWAP_ATTR_SESSION_ID, sizeof(struct capwap_sessionid_element), sessionid);
+	nla_put(msg, NLSMARTCAPWAP_ATTR_MACADDRESS, MACADDRESS_EUI48_LENGTH, macaddress);
+	nla_put_u32(msg, NLSMARTCAPWAP_ATTR_IFPHY_INDEX, (unsigned long)ifindex);
+	nla_put_u8(msg, NLSMARTCAPWAP_ATTR_RADIOID, radioid);
+	nla_put_u8(msg, NLSMARTCAPWAP_ATTR_WLANID, wlanid);
+
+	if (vlan > 0) {
+		nla_put_u16(msg, NLSMARTCAPWAP_ATTR_VLAN, ifindex);
+	}
+
+	/* */
+	result = ac_kmod_send_and_recv_msg(msg, NULL, NULL);
+	if (result) {
+		capwap_logging_error("Unable to authorize station: %d", result);
+	}
+
+	/* */
+	nlmsg_free(msg);
+	return result;
+}
+
+/* */
+int ac_kmod_deauthorize_station(struct capwap_sessionid_element* sessionid, const uint8_t* macaddress) {
+	int result;
+	struct nl_msg* msg;
+
+	ASSERT(sessionid != NULL);
+	ASSERT(macaddress != NULL);
+
+	/* */
+	msg = nlmsg_alloc();
+	if (!msg) {
+		return -1;
+	}
+
+	/* */
+	genlmsg_put(msg, 0, 0, g_ac.kmodhandle.nlsmartcapwap_id, 0, 0, NLSMARTCAPWAP_CMD_DEAUTH_STATION, 0);
+	nla_put(msg, NLSMARTCAPWAP_ATTR_SESSION_ID, sizeof(struct capwap_sessionid_element), sessionid);
+	nla_put(msg, NLSMARTCAPWAP_ATTR_MACADDRESS, MACADDRESS_EUI48_LENGTH, macaddress);
+
+	/* */
+	result = ac_kmod_send_and_recv_msg(msg, NULL, NULL);
+	if (result) {
+		capwap_logging_error("Unable to deauthorize station: %d", result);
+	}
+
+	/* */
+	nlmsg_free(msg);
+	return result;
+}
+
+/* */
+int ac_kmod_init(void) {
 	int result;
 
 	/* Configure netlink callback */
@@ -519,12 +652,26 @@ int ac_kmod_init(uint32_t hash, uint32_t threads) {
 	nl_cb_set(g_ac.kmodhandle.nl_cb, NL_CB_VALID, NL_CB_CUSTOM, ac_kmod_valid_handler, NULL);
 
 	/* Link to kernel module */
-	result = ac_kmod_link(hash, threads);
+	result = ac_kmod_link();
 	if (result) {
 		ac_kmod_free();
 		return result;
 	}
 
+	/* Configure netlink message socket */
+	g_ac.kmodhandle.nlmsg_cb = nl_cb_alloc(NL_CB_DEFAULT);
+	if (!g_ac.kmodhandle.nlmsg_cb) {
+		ac_kmod_free();
+		return -1;
+	}
+
+	/* */
+	g_ac.kmodhandle.nlmsg = nl_create_handle(g_ac.kmodhandle.nlmsg_cb);
+	if (!g_ac.kmodhandle.nlmsg) {
+		ac_kmod_free();
+		return -1;
+	}
+
 	return 0;
 }
 
@@ -538,6 +685,14 @@ void ac_kmod_free(void) {
 		nl_cb_put(g_ac.kmodhandle.nl_cb);
 	}
 
+	if (g_ac.kmodhandle.nlmsg) {
+		nl_socket_free(g_ac.kmodhandle.nlmsg);
+	}
+
+	if (g_ac.kmodhandle.nlmsg_cb) {
+		nl_cb_put(g_ac.kmodhandle.nlmsg_cb);
+	}
+
 	/* */
 	memset(&g_ac.kmodhandle, 0, sizeof(struct ac_kmod_handle));
 }

src/ac/ac_kmod.h

@@ -17,10 +17,15 @@
 
 /* */
 struct ac_kmod_handle {
+	/* Callback */
 	struct nl_sock* nl;
 	int nl_fd;
 	struct nl_cb* nl_cb;
 	int nlsmartcapwap_id;
+
+	/* Send message */
+	struct nl_sock* nlmsg;
+	struct nl_cb* nlmsg_cb;
 };
 
 /* */
@@ -32,7 +37,7 @@ struct ac_kmod_event {
 };
 
 /* */
-int ac_kmod_init(uint32_t hash, uint32_t threads);
+int ac_kmod_init(void);
 void ac_kmod_free(void);
 
 /* */
@@ -43,15 +48,23 @@ int ac_kmod_getfd(struct pollfd* fds, struct ac_kmod_event* events, int count);
 int ac_kmod_createdatachannel(int family, unsigned short port);
 
 /* */
-int ac_kmod_send_keepalive(struct sockaddr_storage* sockaddr);
+int ac_kmod_send_keepalive(struct capwap_sessionid_element* sessionid);
-int ac_kmod_send_data(struct sockaddr_storage* sockaddr, uint8_t radioid, uint8_t binding, const uint8_t* data, int length);
+int ac_kmod_send_data(struct capwap_sessionid_element* sessionid, 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_new_datasession(struct capwap_sessionid_element* sessionid, uint8_t binding, uint16_t mtu);
 int ac_kmod_delete_datasession(struct capwap_sessionid_element* sessionid);
 
+/* */
+int ac_kmod_addwlan(struct capwap_sessionid_element* sessionid, uint8_t radioid, uint8_t wlanid, const uint8_t* bssid, uint8_t macmode, uint8_t tunnelmode);
+int ac_kmod_removewlan(struct capwap_sessionid_element* sessionid);
+
+/* */
+int ac_kmod_authorize_station(struct capwap_sessionid_element* sessionid, const uint8_t* macaddress, int ifindex, uint8_t radioid, uint8_t wlanid, uint16_t vlan);
+int ac_kmod_deauthorize_station(struct capwap_sessionid_element* sessionid, const uint8_t* macaddress);
+
 #endif /* __AC_KMOD_HEADER__ */

src/ac/ac_session.c

@@ -53,11 +53,26 @@ static struct ac_soap_response* ac_session_action_authorizestation_request(struc
 }
 
 /* */
-static int ac_session_action_authorizestation_response(struct ac_session_t* session, struct ac_soap_response* response) {
+static int ac_session_action_authorizestation_response(struct ac_session_t* session, struct ac_soap_response* response, struct ac_notify_station_configuration_ieee8011_add_station* notify) {
+	int result = -1;
+	int ifindex = -1;
+	uint16_t vlan = 0;
+	struct ac_if_datachannel* datachannel;
+	struct ac_wlan* wlan;
 	struct json_object* jsonroot;
+	struct json_object* jsonsection;
+	struct json_object* jsonelement;
+	struct capwap_header_data capwapheader;
+	struct capwap_packet_txmng* txmngpacket;
+	struct capwap_addstation_element addstation;
+	struct capwap_80211_station_element station;
 
 	/* Receive SOAP response with JSON result
 		{
+			DataChannelInterface: {
+				Index: [int],
+				VLAN: [int/string]
+			},
 		}
 	*/
 
@@ -67,11 +82,106 @@ static int ac_session_action_authorizestation_response(struct ac_session_t* sess
 		return -1;
 	}
 
-	/* TODO */
+	/* */
+	jsonsection = compat_json_object_object_get(jsonroot, "DataChannelInterface");
+	if (jsonsection && (json_object_get_type(jsonsection) == json_type_object)) {
+		jsonelement = compat_json_object_object_get(jsonsection, "Index");
+		if (jsonelement && (json_object_get_type(jsonelement) == json_type_int)) {
+			unsigned long index = (unsigned long)json_object_get_int(jsonelement);
+
+			/* Retrieve interface index */
+			capwap_rwlock_rdlock(&g_ac.ifdatachannellock);
+
+			datachannel = (struct ac_if_datachannel*)capwap_hash_search(g_ac.ifdatachannel, &index);
+			if (datachannel) {
+				ifindex = datachannel->ifindex;
+			}
+
+			capwap_rwlock_unlock(&g_ac.ifdatachannellock);
+
+			/* Prepare request */
+			if (ifindex >= 0) {
+				wlan = ac_wlans_get_bssid_with_wlanid(session, notify->radioid, notify->wlanid);
+				if (wlan) {
+					memset(&addstation, 0, sizeof(struct capwap_addstation_element));
+					addstation.radioid = notify->radioid;
+					addstation.length = MACADDRESS_EUI48_LENGTH;
+					addstation.address = notify->address;
+					if (wlan->tunnelmode == CAPWAP_ADD_WLAN_TUNNELMODE_LOCAL) {
+						jsonelement = compat_json_object_object_get(jsonsection, "VLAN");
+						if (jsonelement && (json_object_get_type(jsonelement) == json_type_string)) {
+							const char* wtpvlan = json_object_get_string(jsonelement);
+							if (wtpvlan && (strlen(wtpvlan) < CAPWAP_ADDSTATION_VLAN_MAX_LENGTH)) {
+								addstation.vlan = (uint8_t*)wtpvlan;		/* Free with jsonroot */
+							}
+						}
+					}
+
+					/* */
+					memset(&station, 0, sizeof(struct capwap_80211_station_element));
+					station.radioid = notify->radioid;
+					station.associationid = notify->associationid;
+					memcpy(station.address, notify->address, MACADDRESS_EUI48_LENGTH);
+					station.capabilities = notify->capabilities;
+					station.wlanid = notify->wlanid;
+					station.supportedratescount = notify->supportedratescount;
+					memcpy(station.supportedrates, notify->supportedrates, station.supportedratescount);
+
+					/* Build packet */
+					capwap_header_init(&capwapheader, CAPWAP_RADIOID_NONE, session->binding);
+					txmngpacket = capwap_packet_txmng_create_ctrl_message(&capwapheader, CAPWAP_STATION_CONFIGURATION_REQUEST, session->localseqnumber++, session->mtu);
+
+					/* Add message element */
+					capwap_packet_txmng_add_message_element(txmngpacket, CAPWAP_ELEMENT_ADDSTATION, &addstation);
+					capwap_packet_txmng_add_message_element(txmngpacket, CAPWAP_ELEMENT_80211_STATION, &station);
+
+					/* CAPWAP_ELEMENT_VENDORPAYLOAD */				/* TODO */
+
+					/* Station Configuration Request complete, get fragment packets */
+					capwap_packet_txmng_get_fragment_packets(txmngpacket, session->requestfragmentpacket, session->fragmentid);
+					if (session->requestfragmentpacket->count > 1) {
+						session->fragmentid++;
+					}
+
+					/* Free packets manager */
+					capwap_packet_txmng_free(txmngpacket);
+
+					/* Send Station Configuration Request to WTP */
+					if (capwap_crypt_sendto_fragmentpacket(&session->dtls, session->requestfragmentpacket)) {
+						/* Retrive VLAN */
+						if (wlan->tunnelmode != CAPWAP_ADD_WLAN_TUNNELMODE_LOCAL) {
+							jsonelement = compat_json_object_object_get(jsonroot, "DataChannelInterface.VLAN");
+							if (jsonelement && (json_object_get_type(jsonelement) == json_type_int)) {
+								int acvlan = json_object_get_int(jsonelement);
+								if ((acvlan > 0) && (acvlan < VLAN_MAX)) {
+									vlan = (uint16_t)acvlan;
+								}
+							}
+						}
+
+						/* Authorize station also into kernel module */
+						if (!ac_kmod_authorize_station(&session->sessionid, addstation.address, ifindex, notify->radioid, notify->wlanid, vlan)) {
+							result = 0;
+							session->retransmitcount = 0;
+							capwap_timeout_set(session->timeout, session->idtimercontrol, AC_RETRANSMIT_INTERVAL, ac_dfa_retransmition_timeout, session, NULL);
+						} else {
+							capwap_logging_warning("Unable to authorize station into kernel module data channel");
+							ac_free_reference_last_request(session);
+							ac_session_teardown(session);
+						}
+					} else {
+						capwap_logging_debug("Warning: error to send Station Configuration Request packet");
+						ac_free_reference_last_request(session);
+						ac_session_teardown(session);
+					}
+				}
+			}
+		}
+	}
 
 	/* */
 	json_object_put(jsonroot);
-	return 0;
+	return result;
 }
 
 /* */
@@ -179,10 +289,6 @@ static int ac_session_action_addwlan(struct ac_session_t* session, struct ac_not
 
 /* */
 static int ac_session_action_station_configuration_ieee8011_add_station(struct ac_session_t* session, struct ac_notify_station_configuration_ieee8011_add_station* notify) {
-	struct capwap_header_data capwapheader;
-	struct capwap_packet_txmng* txmngpacket;
-	struct capwap_addstation_element addstation;
-	struct capwap_80211_station_element station;
 	struct ac_soap_response* response;
 
 	ASSERT(session->requestfragmentpacket->count == 0);
@@ -192,57 +298,11 @@ static int ac_session_action_station_configuration_ieee8011_add_station(struct a
 		return AC_NO_ERROR;
 	}
 
-	/* */
+	/* Need authorization of Director */
 	response = ac_session_action_authorizestation_request(session, notify->radioid, notify->wlanid, notify->address);
 	if (response) {
-		if (!ac_session_action_authorizestation_response(session, response)) {
+		if (ac_session_action_authorizestation_response(session, response, notify)) {
-			memset(&addstation, 0, sizeof(struct capwap_addstation_element));
+			capwap_logging_info("Station is not authorized");
-			addstation.radioid = notify->radioid;
-			addstation.length = MACADDRESS_EUI48_LENGTH;
-			addstation.address = notify->address;
-			if (notify->vlan[0]) {
-				addstation.vlan = notify->vlan;
-			}
-
-			/* */
-			memset(&station, 0, sizeof(struct capwap_80211_station_element));
-			station.radioid = notify->radioid;
-			station.associationid = notify->associationid;
-			memcpy(station.address, notify->address, MACADDRESS_EUI48_LENGTH);
-			station.capabilities = notify->capabilities;
-			station.wlanid = notify->wlanid;
-			station.supportedratescount = notify->supportedratescount;
-			memcpy(station.supportedrates, notify->supportedrates, station.supportedratescount);
-
-			/* Build packet */
-			capwap_header_init(&capwapheader, CAPWAP_RADIOID_NONE, session->binding);
-			txmngpacket = capwap_packet_txmng_create_ctrl_message(&capwapheader, CAPWAP_STATION_CONFIGURATION_REQUEST, session->localseqnumber++, session->mtu);
-		
-			/* Add message element */
-			capwap_packet_txmng_add_message_element(txmngpacket, CAPWAP_ELEMENT_ADDSTATION, &addstation);
-			capwap_packet_txmng_add_message_element(txmngpacket, CAPWAP_ELEMENT_80211_STATION, &station);
-
-			/* CAPWAP_ELEMENT_VENDORPAYLOAD */				/* TODO */
-
-			/* Station Configuration Request complete, get fragment packets */
-			capwap_packet_txmng_get_fragment_packets(txmngpacket, session->requestfragmentpacket, session->fragmentid);
-			if (session->requestfragmentpacket->count > 1) {
-				session->fragmentid++;
-			}
-
-			/* Free packets manager */
-			capwap_packet_txmng_free(txmngpacket);
-
-			/* Send Station Configuration Request to WTP */
-			if (capwap_crypt_sendto_fragmentpacket(&session->dtls, session->requestfragmentpacket)) {
-				session->retransmitcount = 0;
-				capwap_timeout_set(session->timeout, session->idtimercontrol, AC_RETRANSMIT_INTERVAL, ac_dfa_retransmition_timeout, session, NULL);
-			} else {
-				capwap_logging_debug("Warning: error to send Station Configuration Request packet");
-				ac_free_reference_last_request(session);
-				ac_session_teardown(session);
-			}
-		} else {
 			/* TODO kickoff station */
 		}
 
@@ -343,7 +403,7 @@ static int ac_session_action_execute(struct ac_session_t* session, struct ac_ses
 			}
 #endif
 			/* Send keep-alive response */
-			ac_kmod_send_keepalive(&session->sockaddrdata.ss);
+			//ac_kmod_send_keepalive(&session->sessionid);
 			capwap_timeout_set(session->timeout, session->idtimerkeepalivedead, AC_MAX_DATA_KEEPALIVE_INTERVAL, ac_dfa_teardown_timeout, session, NULL);
 
 			/* */

src/ac/ac_session.h

@@ -79,7 +79,6 @@ struct ac_notify_addwlan_t {
 struct ac_notify_station_configuration_ieee8011_add_station {
 	uint8_t radioid;
 	uint8_t address[MACADDRESS_EUI48_LENGTH];
-	uint8_t vlan[CAPWAP_ADDSTATION_VLAN_MAX_LENGTH];
 
 	uint8_t wlanid;
 	uint16_t associationid;
@@ -125,8 +124,6 @@ struct ac_session_t {
 	unsigned short mtu;
 	struct capwap_dtls dtls;
 
-	union sockaddr_capwap sockaddrdata;
-
 	struct capwap_timeout* timeout;
 	unsigned long idtimercontrol;
 	unsigned long idtimerkeepalivedead;

src/ac/ac_wlans.c

@@ -72,7 +72,7 @@ static void ac_stations_destroy_station(struct ac_session_t* session, struct ac_
 static unsigned long ac_wlans_item_gethash(const void* key, unsigned long hashsize) {
 	uint8_t* macaddress = (uint8_t*)key;
 
-	return (unsigned long)(macaddress[3] ^ macaddress[4] ^ macaddress[5]);
+	return ((unsigned long)(macaddress[3] ^ macaddress[4] ^ macaddress[5]) % AC_WLANS_STATIONS_HASH_SIZE);
 }
 
 /* */
@@ -151,7 +151,7 @@ int ac_wlans_assign_bssid(struct ac_session_t* session, struct ac_wlan* wlan) {
 
 	/* */
 	if (ac_wlans_get_bssid(session, wlan->device->radioid, wlan->address)) {
-		return 0;
+		return -1;
 	}
 
 	/* */
@@ -167,7 +167,7 @@ int ac_wlans_assign_bssid(struct ac_session_t* session, struct ac_wlan* wlan) {
 
 	/* */
 	capwap_logging_info("Added new wlan with radioid: %d, wlanid: %d, bssid: %s", (int)wlan->device->radioid, (int)wlan->wlanid, capwap_printf_macaddress(buffer, wlan->address, MACADDRESS_EUI48_LENGTH));
-	return 1;
+	return 0;
 }
 
 /* */
@@ -265,9 +265,7 @@ struct ac_wlan* ac_wlans_get_bssid_with_wlanid(struct ac_session_t* session, uin
 }
 
 /* */
-static void ac_wlans_destroy_bssid(struct ac_session_t* session, struct ac_wlan* wlan) {
+void ac_wlans_free_bssid(struct ac_wlan* wlan) {
-	ASSERT(session != NULL);
-	ASSERT(session->wlans != NULL);
 	ASSERT(wlan != NULL);
 
 	/* Free capability */
@@ -275,13 +273,9 @@ static void ac_wlans_destroy_bssid(struct ac_session_t* session, struct ac_wlan*
 		capwap_free(wlan->key);
 	}
 
-	/* Remove stations */
-	while (wlan->stations->first) {
-		ac_stations_destroy_station(session, (struct ac_station*)wlan->stations->first->item);
-	}
-
 	/* */
 	capwap_list_free(wlan->stations);
+	capwap_itemlist_free(wlan->wlanitem);
 }
 
 /* */
@@ -296,11 +290,17 @@ void ac_wlans_delete_bssid(struct ac_session_t* session, uint8_t radioid, const
 	/* */
 	if (session->wlans->devices[radioid - 1].wlans) {
 		for (search = session->wlans->devices[radioid - 1].wlans->first; search; search = search->next) {
-			struct ac_wlan* item = (struct ac_wlan*)search->item;
+			struct ac_wlan* wlan = (struct ac_wlan*)search->item;
 
-			if (!memcmp(bssid, item->address, MACADDRESS_EUI48_LENGTH)) {
+			if (!memcmp(bssid, wlan->address, MACADDRESS_EUI48_LENGTH)) {
-				ac_wlans_destroy_bssid(session, item);
+				/* Remove stations */
-				capwap_itemlist_free(capwap_itemlist_remove(session->wlans->devices[radioid - 1].wlans, search));
+				while (wlan->stations->first) {
+					ac_stations_destroy_station(session, (struct ac_station*)wlan->stations->first->item);
+				}
+
+				/* */
+				capwap_itemlist_remove(session->wlans->devices[radioid - 1].wlans, search);
+				ac_wlans_free_bssid(wlan);
 				break;
 			}
 		}
@@ -465,7 +465,7 @@ void ac_stations_deauthorize_station(struct ac_session_t* session, struct ac_sta
 		responselength = ieee80211_create_deauthentication(buffer, IEEE80211_MTU, &ieee80211_params);
 		if (responselength > 0) {
 			station->flags &= ~(AC_STATION_FLAGS_AUTHENTICATED | AC_STATION_FLAGS_ASSOCIATE);
-			ac_kmod_send_data(&session->sockaddrdata.ss, station->wlan->device->radioid, session->binding, buffer, responselength);
+			ac_kmod_send_data(&session->sessionid, station->wlan->device->radioid, session->binding, buffer, responselength);
 		}
 	}
 }

src/ac/ac_wlans.h

@@ -113,6 +113,7 @@ int ac_wlans_assign_bssid(struct ac_session_t* session, struct ac_wlan* wlan);
 struct ac_wlan* ac_wlans_get_bssid(struct ac_session_t* session, uint8_t radioid, const uint8_t* bssid);
 struct ac_wlan* ac_wlans_get_bssid_with_wlanid(struct ac_session_t* session, uint8_t radioid, uint8_t wlanid);
 void ac_wlans_delete_bssid(struct ac_session_t* session, uint8_t radioid, const uint8_t* bssid);
+void ac_wlans_free_bssid(struct ac_wlan* wlan);
 
 /* Management Stations */
 struct ac_station* ac_stations_create_station(struct ac_session_t* session, uint8_t radioid, const uint8_t* bssid, const uint8_t* address);

src/ac/kmod/Makefile

@@ -7,6 +7,7 @@ smartcapwap-y := \
 	netlinkapp.o \
 	capwap.o \
 	capwap_private.o \
+	station.o \
 	socket.o \
 	iface.o
 

src/ac/kmod/capwap.c

@@ -1,5 +1,7 @@
 #include "config.h"
 #include <linux/module.h>
+#include <linux/if_ether.h>
+#include <linux/etherdevice.h>
 #include <linux/ieee80211.h>
 #include "socket.h"
 #include "capwap.h"
@@ -12,6 +14,12 @@
 /* */
 union capwap_addr sc_localaddr;
 
+/* Ethernet-II snap header (RFC1042 for most EtherTypes) */
+static unsigned char sc_rfc1042_header[] = { 0xaa, 0xaa, 0x03, 0x00, 0x00, 0x00 };
+
+/* Bridge-Tunnel header (for EtherTypes ETH_P_AARP and ETH_P_IPX) */
+static unsigned char sc_bridge_tunnel_header[] = { 0xaa, 0xaa, 0x03, 0x00, 0x00, 0xf8 };
+
 /* */
 static void sc_capwap_fragment_free(struct sc_capwap_fragment* fragment) {
 	TRACEKMOD("### sc_capwap_fragment_free\n");
@@ -32,7 +40,6 @@ static void sc_capwap_fragment_free(struct sc_capwap_fragment* fragment) {
 /* */
 static void sc_capwap_defrag_evictor(struct sc_capwap_session* session, ktime_t now) {
 	ktime_t delta;
-	unsigned long flags;
 	struct sc_capwap_fragment* fragment;
 
 	TRACEKMOD("### sc_capwap_defrag_evictor\n");
@@ -45,7 +52,7 @@ static void sc_capwap_defrag_evictor(struct sc_capwap_session* session, ktime_t
 
 	/* Remove last old fragment */
 	if (!list_empty(&session->fragments.lru_list)) {
-		spin_lock_irqsave(&session->fragments.lock, flags);
+		spin_lock(&session->fragments.lock);
 
 		fragment = list_first_entry(&session->fragments.lru_list, struct sc_capwap_fragment, lru_list);
 		if (fragment) {
@@ -58,7 +65,7 @@ static void sc_capwap_defrag_evictor(struct sc_capwap_session* session, ktime_t
 			}
 		}
 
-		spin_unlock_irqrestore(&session->fragments.lock, flags);
+		spin_unlock(&session->fragments.lock);
 	}
 }
 
@@ -104,7 +111,6 @@ static struct sk_buff* sc_capwap_reasm(struct sc_capwap_fragment* fragment) {
 
 /* */
 static struct sk_buff* sc_capwap_defrag(struct sc_capwap_session* session, struct sk_buff* skb) {
-	unsigned long flags;
 	uint16_t headersize;
 	uint16_t frag_id;
 	struct sk_buff* prev;
@@ -132,7 +138,7 @@ static struct sk_buff* sc_capwap_defrag(struct sc_capwap_session* session, struc
 	cb->frag_length = skb->len - headersize;
 
 	/* */
-	spin_lock_irqsave(&session->fragments.lock, flags);
+	spin_lock(&session->fragments.lock);
 
 	/* Get fragment */
 	fragment = &session->fragments.queues[frag_id % CAPWAP_FRAGMENT_QUEUE];
@@ -219,18 +225,158 @@ static struct sk_buff* sc_capwap_defrag(struct sc_capwap_session* session, struc
 		}
 	}
 
-	spin_unlock_irqrestore(&session->fragments.lock, flags);
+	spin_unlock(&session->fragments.lock);
 
 	return skb_defrag;
 
 error2:
-	spin_unlock_irqrestore(&session->fragments.lock, flags);
+	spin_unlock(&session->fragments.lock);
 
 error:
 	kfree_skb(skb);
 	return NULL;
 }
 
+/* */
+static unsigned int sc_capwap_80211_hdrlen(__le16 fc) {
+	unsigned int hdrlen = 24;
+
+	TRACEKMOD("### sc_capwap_80211_hdrlen\n");
+
+	if (ieee80211_is_data(fc)) {
+		if (ieee80211_has_a4(fc)) {
+			hdrlen = 30;
+		}
+
+		if (ieee80211_is_data_qos(fc)) {
+			hdrlen += IEEE80211_QOS_CTL_LEN;
+			if (ieee80211_has_order(fc)) {
+				hdrlen += IEEE80211_HT_CTL_LEN;
+			}
+		}
+	} else if (ieee80211_is_ctl(fc)) {
+		if ((fc & cpu_to_le16(0x00E0)) == cpu_to_le16(0x00C0)) {
+			hdrlen = 10;
+		} else {
+			hdrlen = 16;
+		}
+	}
+
+	return hdrlen;
+}
+
+/* */
+int sc_capwap_8023_to_80211(struct sk_buff* skb, const uint8_t* bssid) {
+	uint16_t hdrlen;
+	int head_need;
+	struct ieee80211_hdr hdr;
+	int skip_header_bytes;
+	uint8_t* encaps_data;
+	int encaps_len;
+	struct ethhdr* eh = (struct ethhdr*)skb->data;
+	uint16_t ethertype = ntohs(eh->h_proto);
+
+	TRACEKMOD("### sc_capwap_8023_to_80211\n");
+
+	/* IEEE 802.11 header */
+	hdrlen = 24;
+	hdr.frame_control = cpu_to_le16(IEEE80211_FTYPE_DATA | IEEE80211_STYPE_DATA | IEEE80211_FCTL_FROMDS);
+	memcpy(hdr.addr1, eh->h_dest, ETH_ALEN);
+	memcpy(hdr.addr2, bssid, ETH_ALEN);
+	memcpy(hdr.addr3, eh->h_source, ETH_ALEN);
+	hdr.duration_id = 0;
+	hdr.seq_ctrl = 0;
+
+	/* */
+	skip_header_bytes = ETH_HLEN;
+	if ((ethertype == ETH_P_AARP) || (ethertype == ETH_P_IPX)) {
+		encaps_data = sc_bridge_tunnel_header;
+		encaps_len = sizeof(sc_bridge_tunnel_header);
+		skip_header_bytes -= 2;
+	} else if (ethertype >= ETH_P_802_3_MIN) {
+		encaps_data = sc_rfc1042_header;
+		encaps_len = sizeof(sc_rfc1042_header);
+		skip_header_bytes -= 2;
+	} else {
+		encaps_data = NULL;
+		encaps_len = 0;
+	}
+
+	/* Remove IEEE 802.3 header */
+	skb_pull(skb, skip_header_bytes);
+
+	/* Check headroom */
+	head_need = hdrlen + encaps_len - skb_headroom(skb);
+	if ((head_need > 0) || skb_cloned(skb)) {
+		head_need = max(head_need, 0);
+		if (head_need) {
+			skb_orphan(skb);
+		}
+
+		TRACEKMOD("*** Expand headroom skb of: %d\n", head_need);
+		if (pskb_expand_head(skb, head_need, 0, GFP_ATOMIC)) {
+			return -ENOMEM;
+		}
+
+		skb->truesize += head_need;
+	}
+
+	/* Add LLC header */
+	if (encaps_data) {
+		memcpy(skb_push(skb, encaps_len), encaps_data, encaps_len);
+	}
+
+	/* Add IEEE 802.11 header */
+	memcpy(skb_push(skb, hdrlen), &hdr, hdrlen);
+	skb_reset_mac_header(skb);
+
+	return 0;
+}
+
+/* */
+int sc_capwap_80211_to_8023(struct sk_buff* skb) {
+	struct ieee80211_hdr* hdr = (struct ieee80211_hdr*)skb->data;
+	uint16_t hdrlen;
+	uint16_t ethertype;
+	uint8_t* payload;
+	uint8_t dst[ETH_ALEN];
+	uint8_t src[ETH_ALEN] __aligned(2);
+
+	TRACEKMOD("### sc_capwap_80211_to_8023\n");
+
+	/* */
+	hdrlen = sc_capwap_80211_hdrlen(hdr->frame_control);
+	memcpy(dst, ieee80211_get_DA(hdr), ETH_ALEN);
+	memcpy(src, ieee80211_get_SA(hdr), ETH_ALEN);
+
+	/* */
+	if (!pskb_may_pull(skb, hdrlen + 8)) {
+		return -1;
+	}
+
+	/* */
+	payload = skb->data + hdrlen;
+	ethertype = (payload[6] << 8) | payload[7];
+
+	if (likely((ether_addr_equal(payload, sc_rfc1042_header) && (ethertype != ETH_P_AARP) && (ethertype != ETH_P_IPX)) || ether_addr_equal(payload, sc_bridge_tunnel_header))) {
+		skb_pull(skb, hdrlen + 6);
+		memcpy(skb_push(skb, ETH_ALEN), src, ETH_ALEN);
+		memcpy(skb_push(skb, ETH_ALEN), dst, ETH_ALEN);
+	} else {
+		struct ethhdr *ehdr;
+		__be16 len;
+
+		skb_pull(skb, hdrlen);
+		len = htons(skb->len);
+		ehdr = (struct ethhdr *) skb_push(skb, sizeof(struct ethhdr));
+		memcpy(ehdr->h_dest, dst, ETH_ALEN);
+		memcpy(ehdr->h_source, src, ETH_ALEN);
+		ehdr->h_proto = len;
+	}
+
+	return 0;
+}
+
 /* */
 int sc_capwap_bind(union capwap_addr* sockaddr) {
 	int ret;
@@ -275,13 +421,12 @@ void sc_capwap_freesession(struct sc_capwap_session* session) {
 /* */
 uint16_t sc_capwap_newfragmentid(struct sc_capwap_session* session) {
 	uint16_t fragmentid;
-	unsigned long flags;
 
 	TRACEKMOD("### sc_capwap_newfragmentid\n");
 
-	spin_lock_irqsave(&session->fragmentid_lock, flags);
+	spin_lock(&session->fragmentid_lock);
 	fragmentid = session->fragmentid++;
-	spin_unlock_irqrestore(&session->fragmentid_lock, flags);
+	spin_unlock(&session->fragmentid_lock);
 
 	return fragmentid;
 }
@@ -454,8 +599,6 @@ int sc_capwap_parsingpacket(struct sc_capwap_session* session, const union capwa
 			}
 		}
 
-		/* Parsing complete */
-		kfree_skb(skb);
 		return 0;
 	}
 
@@ -464,6 +607,7 @@ int sc_capwap_parsingpacket(struct sc_capwap_session* session, const union capwa
 
 /* */
 int sc_capwap_forwarddata(struct sc_capwap_session* session, uint8_t radioid, uint8_t binding, struct sk_buff* skb, uint32_t flags, struct sc_capwap_radio_addr* radioaddr, int radioaddrlength, struct sc_capwap_wireless_information* winfo, int winfolength) {
+	int err;
 	int size;
 	int length;
 	int reserve;
@@ -482,7 +626,7 @@ int sc_capwap_forwarddata(struct sc_capwap_session* session, uint8_t radioid, ui
 	reserve = sizeof(struct sc_capwap_header) + radioaddrlength + winfolength;
 	if (skb_is_nonlinear(skb) || (headroom < reserve)) {
 		printk("*** Expand socket buffer\n");
-		clone = skb_copy_expand(skb, max_t(int, headroom, reserve), skb_tailroom(skb), GFP_KERNEL);
+		clone = skb_copy_expand(skb, max(headroom, reserve), skb_tailroom(skb), GFP_KERNEL);
 		if (!clone) {
 			printk("*** Unable to expand socket buffer\n");
 			return -ENOMEM;
@@ -551,7 +695,9 @@ int sc_capwap_forwarddata(struct sc_capwap_session* session, uint8_t radioid, ui
 		}
 
 		/* Send packet */
-		if (sc_socket_send(SOCKET_UDP, (uint8_t*)header, (size + length), &session->peeraddr) < 0) {
+		err = sc_socket_send(SOCKET_UDP, (uint8_t*)header, (size + length), &session->peeraddr);
+		TRACEKMOD("*** Send packet result: %d\n", err);
+		if (err < 0) {
 			break;
 		}
 
@@ -593,18 +739,18 @@ struct sc_capwap_radio_addr* sc_capwap_setradiomacaddress(uint8_t* buffer, int s
 	radioaddr = (struct sc_capwap_radio_addr*)buffer;
 	radioaddr->length = MACADDRESS_EUI48_LENGTH;
 
-	addr = (struct sc_capwap_macaddress_eui48*)(buffer + sizeof(struct sc_capwap_radio_addr));
+	addr = (struct sc_capwap_macaddress_eui48*)radioaddr->addr;
 	memcpy(addr->addr, bssid, MACADDRESS_EUI48_LENGTH);
 
 	return radioaddr;
 }
 
 /* */
-struct sc_capwap_wireless_information* sc_capwap_setwirelessinformation(uint8_t* buffer, int size, uint8_t rssi, uint8_t snr, uint16_t rate) {
+struct sc_capwap_wireless_information* sc_capwap_setwinfo_frameinfo(uint8_t* buffer, int size, uint8_t rssi, uint8_t snr, uint16_t rate) {
 	struct sc_capwap_wireless_information* winfo;
 	struct sc_capwap_ieee80211_frame_info* frameinfo;
 
-	TRACEKMOD("### sc_capwap_setwirelessinformation\n");
+	TRACEKMOD("### sc_capwap_setwinfo_frameinfo\n");
 
 	memset(buffer, 0, size);
 
@@ -618,3 +764,21 @@ struct sc_capwap_wireless_information* sc_capwap_setwirelessinformation(uint8_t*
 
 	return winfo;
 }
+
+/* */
+struct sc_capwap_wireless_information* sc_capwap_setwinfo_destwlans(uint8_t* buffer, int size, uint16_t wlanidbitmap) {
+	struct sc_capwap_wireless_information* winfo;
+	struct sc_capwap_destination_wlans* destwlans;
+
+	TRACEKMOD("### sc_capwap_setwinfo_destwlans\n");
+
+	memset(buffer, 0, size);
+
+	winfo = (struct sc_capwap_wireless_information*)buffer;
+	winfo->length = sizeof(struct sc_capwap_destination_wlans);
+
+	destwlans = (struct sc_capwap_destination_wlans*)(buffer + sizeof(struct sc_capwap_wireless_information));
+	destwlans->wlanidbitmap = cpu_to_be16(wlanidbitmap);
+
+	return winfo;
+}

src/ac/kmod/capwap.h

@@ -21,27 +21,25 @@
 /* */
 #define SKB_CAPWAP_FLAG_FROM_DATA_CHANNEL			0x0001
 #define SKB_CAPWAP_FLAG_FROM_USER_SPACE				0x0002
-#define SKB_CAPWAP_FLAG_FROM_IEEE80211				0x0004
+#define SKB_CAPWAP_FLAG_FROM_AC_TAP					0x0004
-
+#define SKB_CAPWAP_FLAG_FROM_IEEE80211				0x0008
-#define SKB_CAPWAP_FLAG_PEERADDRESS					0x0010
-#define SKB_CAPWAP_FLAG_RADIOID						0x0020
-#define SKB_CAPWAP_FLAG_BINDING						0x0040
-#define SKB_CAPWAP_FLAG_RADIOADDRESS				0x0080
-#define SKB_CAPWAP_FLAG_WIRELESSINFORMATION			0x0100
 
+#define SKB_CAPWAP_FLAG_SESSIONID					0x0100
+#define SKB_CAPWAP_FLAG_RADIOID						0x0200
+#define SKB_CAPWAP_FLAG_BINDING						0x0400
+#define SKB_CAPWAP_FLAG_WIRELESSINFORMATION			0x0800
 #define SKB_CAPWAP_FLAG_FRAGMENT					0x1000
 
 struct sc_skb_capwap_cb {
 	uint16_t flags;
-	struct capwap_addr_little peeraddr;
+
+	/* Session ID */
+	struct sc_capwap_sessionid_element sessionid;
 
 	/* Capwap information */
 	uint8_t radioid;
 	uint8_t binding;
 
-	/* Radio Address */
-	uint8_t radioaddr_addr[MACADDRESS_EUI48_LENGTH];
-
 	/* Wireless Information */
 	uint8_t winfo_rssi;
 	uint8_t winfo_snr;
@@ -106,16 +104,17 @@ void sc_capwap_initsession(struct sc_capwap_session* session);
 void sc_capwap_freesession(struct sc_capwap_session* session);
 uint16_t sc_capwap_newfragmentid(struct sc_capwap_session* session);
 
-void sc_capwap_sessionid_printf(const struct sc_capwap_sessionid_element* sessionid, char* string);
+int sc_capwap_8023_to_80211(struct sk_buff* skb, const uint8_t* bssid);
+int sc_capwap_80211_to_8023(struct sk_buff* skb);
 
-struct sc_capwap_packet* sc_capwap_poppacketqueue(struct sc_capwap_session* session);
+void sc_capwap_sessionid_printf(const struct sc_capwap_sessionid_element* sessionid, char* string);
-void sc_capwap_pushpacketqueue(struct sc_capwap_session* session, struct sc_capwap_packet* packet);
 
 int sc_capwap_createkeepalive(struct sc_capwap_sessionid_element* sessionid, uint8_t* buffer, int size);
 int sc_capwap_parsingpacket(struct sc_capwap_session* session, const union capwap_addr* sockaddr, struct sk_buff* skb);
 
 struct sc_capwap_radio_addr* sc_capwap_setradiomacaddress(uint8_t* buffer, int size, uint8_t* bssid);
-struct sc_capwap_wireless_information* sc_capwap_setwirelessinformation(uint8_t* buffer, int size, uint8_t rssi, uint8_t snr, uint16_t rate);
+struct sc_capwap_wireless_information* sc_capwap_setwinfo_frameinfo(uint8_t* buffer, int size, uint8_t rssi, uint8_t snr, uint16_t rate);
+struct sc_capwap_wireless_information* sc_capwap_setwinfo_destwlans(uint8_t* buffer, int size, uint16_t wlanidbitmap);
 
 int sc_capwap_forwarddata(struct sc_capwap_session* session, uint8_t radioid, uint8_t binding, struct sk_buff* skb, uint32_t flags, struct sc_capwap_radio_addr* radioaddr, int radioaddrlength, struct sc_capwap_wireless_information* winfo, int winfolength);
 

src/ac/kmod/capwap_private.c

@@ -1,65 +1,93 @@
 #include "config.h"
 #include <linux/module.h>
 #include <linux/hash.h>
+#include <linux/slab.h>
 #include <linux/kthread.h>
+#include <linux/if_ether.h>
+#include <linux/if_vlan.h>
+#include <linux/etherdevice.h>
+#include <linux/smp.h>
+#include <linux/lockdep.h>
 #include <net/ipv6.h>
+#include <net/cfg80211.h>
 #include "socket.h"
 #include "capwap.h"
 #include "nlsmartcapwap.h"
 #include "netlinkapp.h"
 #include "iface.h"
+#include "station.h"
+
+/* */
+#define SESSION_HASH_SIZE_SHIFT				16
+#define SESSION_HASH_SIZE					(1 << SESSION_HASH_SIZE_SHIFT)
+#define MAX_WORKER_THREAD					32
+
+/* */
+static DEFINE_MUTEX(sc_session_update_mutex);
 
 /* Sessions */
-static DEFINE_MUTEX(sc_session_mutex);
 static struct list_head sc_session_setup_list;
 static struct list_head sc_session_running_list;
 
-static uint32_t sc_session_hash_size;
+static struct sc_capwap_session_priv* __rcu sc_session_hash_ipaddr[SESSION_HASH_SIZE];
-static uint32_t sc_session_hash_size_shift;
+static struct sc_capwap_session_priv* __rcu sc_session_hash_sessionid[SESSION_HASH_SIZE];
-static struct sc_capwap_session_priv** __rcu sc_session_hash_ipaddr;
-static struct sc_capwap_session_priv** __rcu sc_session_hash_sessionid;
 
 /* Threads */
 static DEFINE_SPINLOCK(sc_session_threads_lock);
 static uint32_t sc_session_threads_pos;
 static uint32_t sc_session_threads_count;
-static struct sc_capwap_workthread* sc_session_threads;
+static struct sc_capwap_workthread sc_session_threads[MAX_WORKER_THREAD];
 
 /* */
 static uint32_t sc_capwap_hash_ipaddr(const union capwap_addr* peeraddr) {
 	TRACEKMOD("### sc_capwap_hash_ipaddr\n");
 
-	return hash_32(((peeraddr->ss.ss_family == AF_INET) ? peeraddr->sin.sin_addr.s_addr : ipv6_addr_hash(&peeraddr->sin6.sin6_addr)), sc_session_hash_size_shift);
+	return hash_32(((peeraddr->ss.ss_family == AF_INET) ? peeraddr->sin.sin_addr.s_addr : ipv6_addr_hash(&peeraddr->sin6.sin6_addr)), SESSION_HASH_SIZE_SHIFT);
 }
 
 /* */
 static uint32_t sc_capwap_hash_sessionid(const struct sc_capwap_sessionid_element* sessionid) {
 	TRACEKMOD("### sc_capwap_hash_sessionid\n");
 
-	return (sessionid->id32[0] ^ sessionid->id32[1] ^ sessionid->id32[2] ^ sessionid->id32[3]) % sc_session_hash_size;
+	return ((sessionid->id32[0] ^ sessionid->id32[1] ^ sessionid->id32[2] ^ sessionid->id32[3]) % SESSION_HASH_SIZE);
 }
 
 /* */
 static void sc_capwap_closesession(struct sc_capwap_session_priv* sessionpriv) {
 	uint32_t hash;
 	struct sc_capwap_session_priv* search;
+	struct sc_capwap_station* temp;
+	struct sc_capwap_station* station;
 
 	TRACEKMOD("### sc_capwap_closesession\n");
 
-	lockdep_assert_held(&sc_session_mutex);
+	/* Close stations */
+	list_for_each_entry_safe(station, temp, &sessionpriv->list_stations, list_session) {
+		sc_stations_releaseconnection(station);
+		sc_stations_free(station);
+	}
+
+	/* */
+	if (!list_empty(&sessionpriv->list_stations)) {
+		TRACEKMOD("*** Bug: the list stations of session is not empty\n");
+	}
+
+	if (!list_empty(&sessionpriv->list_connections)) {
+		TRACEKMOD("*** Bug: the list connections of session is not empty\n");
+	}
 
 	/* Remove session from list reference */
 	if (sessionpriv->session.peeraddr.ss.ss_family != AF_UNSPEC) {
 		/* IP Address */
 		hash = sc_capwap_hash_ipaddr(&sessionpriv->session.peeraddr);
-		search = rcu_dereference_protected(sc_session_hash_ipaddr[hash], lockdep_is_held(&sc_session_mutex));
+		search = rcu_dereference_protected(sc_session_hash_ipaddr[hash], sc_capwap_update_lock_is_locked());
 
 		if (search) {
 			if (search == sessionpriv) {
 				rcu_assign_pointer(sc_session_hash_ipaddr[hash], sessionpriv->next_ipaddr);
 			} else {
 				while (rcu_access_pointer(search->next_ipaddr) && (rcu_access_pointer(search->next_ipaddr) != sessionpriv)) {
-					search = rcu_dereference_protected(search->next_ipaddr, lockdep_is_held(&sc_session_mutex));
+					search = rcu_dereference_protected(search->next_ipaddr, sc_capwap_update_lock_is_locked());
 				}
 
 				if (rcu_access_pointer(search->next_ipaddr)) {
@@ -70,14 +98,14 @@ static void sc_capwap_closesession(struct sc_capwap_session_priv* sessionpriv) {
 
 		/* Session ID */
 		hash = sc_capwap_hash_sessionid(&sessionpriv->session.sessionid);
-		search = rcu_dereference_protected(sc_session_hash_sessionid[hash], lockdep_is_held(&sc_session_mutex));
+		search = rcu_dereference_protected(sc_session_hash_sessionid[hash], sc_capwap_update_lock_is_locked());
 
 		if (search) {
 			if (search == sessionpriv) {
 				rcu_assign_pointer(sc_session_hash_sessionid[hash], sessionpriv->next_sessionid);
 			} else {
 				while (rcu_access_pointer(search->next_sessionid) && (rcu_access_pointer(search->next_sessionid) != sessionpriv)) {
-					search = rcu_dereference_protected(search->next_sessionid, lockdep_is_held(&sc_session_mutex));
+					search = rcu_dereference_protected(search->next_sessionid, sc_capwap_update_lock_is_locked());
 				}
 
 				if (rcu_access_pointer(search->next_sessionid)) {
@@ -94,8 +122,6 @@ static void sc_capwap_closesession(struct sc_capwap_session_priv* sessionpriv) {
 	/* Free memory */
 	sc_capwap_freesession(&sessionpriv->session);
 	kfree(sessionpriv);
-
-	TRACEKMOD("*** Free session\n");
 }
 
 /* */
@@ -104,38 +130,22 @@ static void sc_capwap_closesessions(void) {
 	struct sc_capwap_session_priv* temp;
 
 	TRACEKMOD("### sc_capwap_closesessions\n");
-	TRACEKMOD("*** Delete all sessions\n");
 
 	/* */
-	mutex_lock(&sc_session_mutex);
+	sc_capwap_update_lock();
 
 	/* */
 	list_for_each_entry_safe(sessionpriv, temp, &sc_session_setup_list, list) {
-#ifdef DEBUGKMOD
-		do {
-			char sessionname[33];
-			sc_capwap_sessionid_printf(&sessionpriv->session.sessionid, sessionname);
-			TRACEKMOD("*** Delete setup session: %s\n", sessionname);
-		} while(0);
-#endif
 		sc_capwap_closesession(sessionpriv);
 	}
 
 	/* */
 	list_for_each_entry_safe(sessionpriv, temp, &sc_session_running_list, list) {
-#ifdef DEBUGKMOD
-		do {
-			char sessionname[33];
-			sc_capwap_sessionid_printf(&sessionpriv->session.sessionid, sessionname);
-			TRACEKMOD("*** Delete running session: %s\n", sessionname);
-		} while(0);
-#endif
 		sc_capwap_closesession(sessionpriv);
 	}
 
 	/* */
-	synchronize_net();
+	sc_capwap_update_unlock();
-	mutex_unlock(&sc_session_mutex);
 }
 
 /* */
@@ -145,14 +155,14 @@ static struct sc_capwap_session_priv* sc_capwap_getsession_ipaddr(const union ca
 	TRACEKMOD("### sc_capwap_getsession_ipaddr\n");
 
 	/* */
-	sessionpriv = rcu_dereference_check(sc_session_hash_ipaddr[sc_capwap_hash_ipaddr(sockaddr)], lockdep_is_held(&sc_session_mutex));
+	sessionpriv = rcu_dereference_check(sc_session_hash_ipaddr[sc_capwap_hash_ipaddr(sockaddr)], sc_capwap_update_lock_is_locked());
 	while (sessionpriv) {
 		if (!sc_addr_compare(sockaddr, &sessionpriv->session.peeraddr)) {
 			break;
 		}
 
 		/* */
-		sessionpriv = rcu_dereference_check(sessionpriv->next_ipaddr, lockdep_is_held(&sc_session_mutex));
+		sessionpriv = rcu_dereference_check(sessionpriv->next_ipaddr, sc_capwap_update_lock_is_locked());
 	}
 
 	return sessionpriv;
@@ -165,14 +175,14 @@ static struct sc_capwap_session_priv* sc_capwap_getsession_sessionid(const struc
 	TRACEKMOD("### sc_capwap_getsession_sessionid\n");
 
 	/* */
-	sessionpriv = rcu_dereference_check(sc_session_hash_ipaddr[sc_capwap_hash_sessionid(sessionid)], lockdep_is_held(&sc_session_mutex));
+	sessionpriv = rcu_dereference_check(sc_session_hash_sessionid[sc_capwap_hash_sessionid(sessionid)], sc_capwap_update_lock_is_locked());
 	while (sessionpriv) {
 		if (!memcmp(&sessionpriv->session.sessionid, sessionid, sizeof(struct sc_capwap_sessionid_element))) {
 			break;
 		}
 
 		/* */
-		sessionpriv = rcu_dereference_check(sessionpriv->next_sessionid, lockdep_is_held(&sc_session_mutex));
+		sessionpriv = rcu_dereference_check(sessionpriv->next_sessionid, sc_capwap_update_lock_is_locked());
 	}
 
 	return sessionpriv;
@@ -186,7 +196,7 @@ static int sc_capwap_deletesetupsession(const struct sc_capwap_sessionid_element
 	TRACEKMOD("### sc_capwap_deletesetupsession\n");
 
 	/* */
-	mutex_lock(&sc_session_mutex);
+	sc_capwap_update_lock();
 
 	list_for_each_entry(sessionpriv, &sc_session_setup_list, list) {
 		if (!memcmp(&sessionpriv->session.sessionid, sessionid, sizeof(struct sc_capwap_sessionid_element))) {
@@ -204,7 +214,7 @@ static int sc_capwap_deletesetupsession(const struct sc_capwap_sessionid_element
 	}
 
 	/* */
-	mutex_unlock(&sc_session_mutex);
+	sc_capwap_update_unlock();
 	return ret;
 }
 
@@ -216,7 +226,7 @@ static int sc_capwap_deleterunningsession(const struct sc_capwap_sessionid_eleme
 	TRACEKMOD("### sc_capwap_deleterunningsession\n");
 
 	/* */
-	mutex_lock(&sc_session_mutex);
+	sc_capwap_update_lock();
 
 	/* Search session with address hash */
 	sessionpriv = sc_capwap_getsession_sessionid(sessionid);
@@ -233,14 +243,121 @@ static int sc_capwap_deleterunningsession(const struct sc_capwap_sessionid_eleme
 	}
 
 	/* */
-	mutex_unlock(&sc_session_mutex);
+	sc_capwap_update_unlock();
 	return ret;
 }
 
+/* */
+static int sc_capwap_restrictbroadcastpacket(struct sk_buff* skb, int is80211) {
+	TRACEKMOD("### sc_capwap_restrictbroadcastpacket\n");
+
+	/* TODO: limit some broadcast packet (DHCP) */
+
+	return 0;
+}
+
+/* */
+static int sc_capwap_sendpacket_wtp(struct sc_capwap_session_priv* sessionpriv, uint8_t radioid, uint8_t wlanid, struct sk_buff* skb, int is80211) {
+	uint32_t flags = 0;
+	struct sc_capwap_radio_addr* radioaddr = NULL;
+	uint8_t radioaddrbuffer[CAPWAP_RADIO_EUI48_LENGTH_PADDED];
+	struct sc_capwap_wlan* wlan = &sessionpriv->wlans[radioid - 1][wlanid - 1];
+
+	TRACEKMOD("### sc_capwap_sendpacket_wtp\n");
+
+	/* */
+	if (!wlan->used) {
+		return -EINVAL;
+	}
+
+	/* Datalink header convertion */
+	if (is80211 && (wlan->tunnelmode == CAPWAP_ADD_WLAN_TUNNELMODE_8023)) {
+		sc_capwap_80211_to_8023(skb);
+		flags |= NLSMARTCAPWAP_FLAGS_TUNNEL_8023;
+		radioaddr = sc_capwap_setradiomacaddress(radioaddrbuffer, CAPWAP_RADIO_EUI48_LENGTH_PADDED, wlan->bssid);
+	} else if (!is80211 && (wlan->tunnelmode == CAPWAP_ADD_WLAN_TUNNELMODE_80211)) {
+		sc_capwap_8023_to_80211(skb, wlan->bssid);
+	}
+
+	/* Forward packet */
+	return sc_capwap_forwarddata(&sessionpriv->session, radioid, sessionpriv->binding, skb, flags, radioaddr, (radioaddr ? CAPWAP_RADIO_EUI48_LENGTH_PADDED : 0), NULL, 0);
+}
+
+/* */
+static void sc_capwap_sendbroadcastpacket_wtp(struct sc_netdev_priv* netpriv, uint16_t vlan, struct sk_buff* skb, struct sc_capwap_session_priv* ignore) {
+	struct sk_buff* clone;
+	struct sc_capwap_connection* connection;
+	struct sc_capwap_wireless_information* winfo;
+	uint8_t buffer[CAPWAP_WINFO_DESTWLAN_LENGTH_PADDED];
+	int headroom = sizeof(struct sc_capwap_header) + CAPWAP_WINFO_DESTWLAN_LENGTH_PADDED;
+
+	TRACEKMOD("### sc_capwap_sendbroadcastpacket_wtp\n");
+
+	/* */
+	if (headroom < skb_headroom(skb)) {
+		headroom = skb_headroom(skb);
+	}
+
+	/* Send packet for every connection */
+	list_for_each_entry_rcu(connection, &netpriv->list_connections, list_dev) {
+		if ((connection->vlan == vlan) && (connection->sessionpriv != ignore)) {
+			clone = skb_copy_expand(skb, headroom, skb_tailroom(skb), GFP_KERNEL);
+			if (!clone) {
+				break;
+			}
+
+			/* Forward packet */
+			winfo = sc_capwap_setwinfo_destwlans(buffer, CAPWAP_WINFO_DESTWLAN_LENGTH_PADDED, connection->wlanidmask);
+			sc_capwap_forwarddata(&connection->sessionpriv->session, connection->radioid, connection->sessionpriv->binding, clone, NLSMARTCAPWAP_FLAGS_TUNNEL_8023, NULL, 0, winfo, CAPWAP_WINFO_DESTWLAN_LENGTH_PADDED);
+			kfree_skb(clone);
+		}
+	}
+}
+
+/* */
+static void sc_capwap_sendpacket_iface(struct sc_capwap_station* station, struct sk_buff* skb) {
+	struct sc_netdev_priv* devpriv = rcu_dereference(station->devpriv);
+
+	TRACEKMOD("### sc_capwap_sendpacket_iface\n");
+
+	/* */
+	if (devpriv->dev->flags & IFF_UP) {
+		if (station->vlan) {
+			skb = vlan_insert_tag(skb, htons(ETH_P_8021Q), station->vlan & VLAN_VID_MASK);
+			if (!skb) {
+				/* Unable add VLAN id */
+				spin_lock(&devpriv->lock);
+				devpriv->dev->stats.rx_dropped++;
+				spin_unlock(&devpriv->lock);
+				return;
+			}
+		}
+
+		/* Prepare to send packet */
+		skb_reset_mac_header(skb);
+		skb->protocol = eth_type_trans(skb, devpriv->dev);
+
+		/* Send packet */
+		netif_rx_ni(skb);
+
+		/* Update stats */
+		spin_lock(&devpriv->lock);
+		devpriv->dev->stats.rx_packets++;
+		devpriv->dev->stats.rx_bytes += skb->len;
+		spin_unlock(&devpriv->lock);
+	} else {
+		/* Drop packet */
+		kfree_skb(skb);
+
+		spin_lock(&devpriv->lock);
+		devpriv->dev->stats.rx_dropped++;
+		spin_unlock(&devpriv->lock);
+	}
+}
+
 /* */
 static int sc_capwap_thread_recvpacket(struct sk_buff* skb) {
 	int ret = 1;
-	union capwap_addr peeraddr;
 	struct sc_capwap_session_priv* sessionpriv;
 	struct sc_skb_capwap_cb* cb = CAPWAP_SKB_CB(skb);
 
@@ -250,14 +367,10 @@ static int sc_capwap_thread_recvpacket(struct sk_buff* skb) {
 	if (cb->flags & SKB_CAPWAP_FLAG_FROM_USER_SPACE) {
 		TRACEKMOD("*** Receive SKB_CAPWAP_FLAG_FROM_USER_SPACE\n");
 
-		/* Get peer address */
-		sc_addr_fromlittle(&cb->peeraddr, &peeraddr);
-		TRACEKMOD("*** Address %d %x %x\n", peeraddr.ss.ss_family, (int)peeraddr.sin.sin_addr.s_addr, (int)peeraddr.sin.sin_port);
-
 		/* Send packet*/
 		rcu_read_lock();
 
-		sessionpriv = sc_capwap_getsession_ipaddr(&peeraddr);
+		sessionpriv = sc_capwap_getsession_sessionid(&cb->sessionid);
 		if (sessionpriv) {
 			if (sc_capwap_forwarddata(&sessionpriv->session, cb->radioid, cb->binding, skb, 0, NULL, 0, NULL, 0)) {
 				TRACEKMOD("*** Unable send packet from sc_netlink_send_data function\n");
@@ -268,25 +381,63 @@ static int sc_capwap_thread_recvpacket(struct sk_buff* skb) {
 
 		rcu_read_unlock();
 	} else if (cb->flags & SKB_CAPWAP_FLAG_FROM_DATA_CHANNEL) {
+		union capwap_addr peeraddr;
+
 		TRACEKMOD("*** Receive SKB_CAPWAP_FLAG_FROM_DATA_CHANNEL\n");
 
 		/* Get peer address */
-		if (sc_socket_getpeeraddr(skb, &peeraddr)) {
+		if (!sc_socket_getpeeraddr(skb, &peeraddr)) {
+			if (skb_pull(skb, sizeof(struct udphdr))) {
+				rcu_read_lock();
+
+				sessionpriv = sc_capwap_getsession_ipaddr(&peeraddr);
+				ret = sc_capwap_parsingpacket((sessionpriv ? &sessionpriv->session : NULL), &peeraddr, skb);
+
+				rcu_read_unlock();
+			} else {
+				TRACEKMOD("*** Invalid packet\n");
+				ret = -EOVERFLOW;
+			}
+		} else {
 			TRACEKMOD("*** Unable get address from packet\n");
-			return -EINVAL;
+			ret = -EINVAL;
+		}
+	} else if (cb->flags & SKB_CAPWAP_FLAG_FROM_AC_TAP) {
+		uint16_t vlan = 0;
+		struct ethhdr* eh = eth_hdr(skb);
+		struct sc_capwap_station* station;
+
+		TRACEKMOD("*** Receive SKB_CAPWAP_FLAG_FROM_AC_TAP\n");
+
+		/* Retrieve VLAN */
+		if (vlan_tx_tag_present(skb)) {
+			vlan = vlan_tx_tag_get_id(skb);
+		} else if (eh->h_proto == htons(ETH_P_8021Q)) {
+			vlan = ntohs(vlan_eth_hdr(skb)->h_vlan_TCI) & VLAN_VID_MASK;
+
+			/* Remove 802.1q from packet */
+			memmove(skb->data + VLAN_HLEN, skb->data, 2 * ETH_ALEN);
+			skb_pull(skb, VLAN_HLEN);
+			skb_reset_mac_header(skb);
 		}
 
-		/* Remove UDP header */
-		if (!skb_pull(skb, sizeof(struct udphdr))) {
-			TRACEKMOD("*** Invalid packet\n");
-			return -EOVERFLOW;
-		}
-
-		/* */
 		rcu_read_lock();
 
-		sessionpriv = sc_capwap_getsession_ipaddr(&peeraddr);
+		if (is_multicast_ether_addr(eh->h_dest)) {
-		ret = sc_capwap_parsingpacket(&sessionpriv->session, &peeraddr, skb);
+			TRACEKMOD("*** Receive broadcast/multicast packet\n");
+
+			if (!sc_capwap_restrictbroadcastpacket(skb, 0)) {
+				sc_capwap_sendbroadcastpacket_wtp((struct sc_netdev_priv*)netdev_priv(skb->dev), vlan, skb, NULL);
+			}
+		} else {
+			station = sc_stations_search(eh->h_dest);
+			if (station && (station->vlan == vlan)) {
+				sc_capwap_sendpacket_wtp(rcu_dereference(station->sessionpriv), station->radioid, station->wlanid, skb, 0);
+			} else {
+				TRACEKMOD("*** Unable to found station from macaddress\n");
+				ret = -EINVAL;
+			}
+		}
 
 		rcu_read_unlock();
 	}
@@ -300,7 +451,7 @@ static int sc_capwap_thread(void* data) {
 	struct sc_capwap_workthread* thread = (struct sc_capwap_workthread*)data;
 
 	TRACEKMOD("### sc_capwap_thread\n");
-	TRACEKMOD("*** Thread start\n");
+	TRACEKMOD("*** Thread start: %d\n", smp_processor_id());
 
 	for (;;) {
 		wait_event_interruptible(thread->waitevent, (skb_queue_len(&thread->queue) > 0) || kthread_should_stop());
@@ -311,23 +462,44 @@ static int sc_capwap_thread(void* data) {
 		/* Get packet */
 		skb = skb_dequeue(&thread->queue);
 		if (!skb) {
+			TRACEKMOD("*** Nothing from thread %d\n", smp_processor_id());
 			continue;
 		}
 
 		/* */
-		TRACEKMOD("*** Thread receive packet\n");
+		TRACEKMOD("*** Thread receive packet %d\n", smp_processor_id());
 		if (sc_capwap_thread_recvpacket(skb)) {
 			TRACEKMOD("*** Free packet\n");
 			kfree_skb(skb);
 		}
 	}
 
-	TRACEKMOD("*** Thread end\n");
+	/* Purge queue */
+	skb_queue_purge(&thread->queue);
+
+	TRACEKMOD("*** Thread end: %d\n", smp_processor_id());
 	return 0;
 }
 
 /* */
-int sc_capwap_sendkeepalive(const union capwap_addr* peeraddr) {
+void sc_capwap_update_lock(void) {
+	mutex_lock(&sc_session_update_mutex);
+}
+
+/* */
+void sc_capwap_update_unlock(void) {
+	mutex_unlock(&sc_session_update_mutex);
+}
+
+/* */
+#ifdef CONFIG_PROVE_LOCKING
+int sc_capwap_update_lock_is_locked(void) {
+	return lockdep_is_held(&sc_session_update_mutex);
+}
+#endif
+
+/* */
+int sc_capwap_sendkeepalive(const struct sc_capwap_sessionid_element* sessionid) {
 	int ret;
 	int length;
 	struct sc_capwap_session_priv* sessionpriv;
@@ -339,7 +511,7 @@ int sc_capwap_sendkeepalive(const union capwap_addr* peeraddr) {
 	rcu_read_lock();
 
 	/* Get session */
-	sessionpriv = sc_capwap_getsession_ipaddr(peeraddr);
+	sessionpriv = sc_capwap_getsession_sessionid(sessionid);
 	if (!sessionpriv) {
 		TRACEKMOD("*** Unknown keep-alive session\n");
 		ret = -ENOENT;
@@ -359,6 +531,7 @@ int sc_capwap_sendkeepalive(const union capwap_addr* peeraddr) {
 
 	/* Send packet */
 	ret = sc_socket_send(SOCKET_UDP, buffer, length, &sessionpriv->session.peeraddr);
+	TRACEKMOD("*** Send keep-alive result: %d\n", ret);
 	if (ret > 0) {
 		ret = 0;
 	}
@@ -369,7 +542,7 @@ done:
 }
 
 /* */
-int sc_capwap_newsession(const struct sc_capwap_sessionid_element* sessionid, uint16_t mtu) {
+int sc_capwap_newsession(const struct sc_capwap_sessionid_element* sessionid, uint8_t binding, uint16_t mtu) {
 	struct sc_capwap_session_priv* sessionpriv;
 
 	TRACEKMOD("### sc_capwap_newsession\n");
@@ -383,7 +556,7 @@ int sc_capwap_newsession(const struct sc_capwap_sessionid_element* sessionid, ui
 #endif
 
 	/* */
-	sessionpriv = kzalloc(sizeof(struct sc_capwap_session_priv), GFP_KERNEL);
+	sessionpriv = (struct sc_capwap_session_priv*)kzalloc(sizeof(struct sc_capwap_session_priv), GFP_KERNEL);
 	if (!sessionpriv) {
 		TRACEKMOD("*** Unable to create session\n");
 		return -ENOMEM;
@@ -392,75 +565,68 @@ int sc_capwap_newsession(const struct sc_capwap_sessionid_element* sessionid, ui
 	/* Initialize session */
 	sc_capwap_initsession(&sessionpriv->session);
 	memcpy(&sessionpriv->session.sessionid, sessionid, sizeof(struct sc_capwap_sessionid_element));
+	sessionpriv->binding = binding;
 	sessionpriv->session.mtu = mtu;
-	INIT_LIST_HEAD(&sessionpriv->list);
+	INIT_LIST_HEAD(&sessionpriv->list_stations);
+	INIT_LIST_HEAD(&sessionpriv->list_connections);
 
 	/* Add to setup session list */
-	mutex_lock(&sc_session_mutex);
+	sc_capwap_update_lock();
 	list_add_rcu(&sessionpriv->list, &sc_session_setup_list);
-	mutex_unlock(&sc_session_mutex);
+	sc_capwap_update_unlock();
 
 	TRACEKMOD("*** Create session\n");
 	return 0;
 }
 
 /* */
-int sc_capwap_init(uint32_t hash, uint32_t threads) {
+int sc_capwap_init(void) {
-	uint32_t i;
+	unsigned long i;
+	unsigned long cpu;
 	int err = -ENOMEM;
 
 	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));
-	INIT_LIST_HEAD(&sc_session_running_list);
 	INIT_LIST_HEAD(&sc_session_setup_list);
+	INIT_LIST_HEAD(&sc_session_running_list);
 
 	/* */
-	sc_session_hash_size_shift = hash;
+	memset(sc_session_hash_ipaddr, 0, sizeof(struct sc_capwap_session_priv*) * SESSION_HASH_SIZE);
-	sc_session_hash_size = 1 << hash;
+	memset(sc_session_hash_sessionid, 0, sizeof(struct sc_capwap_session_priv*) * SESSION_HASH_SIZE);
-
-	sc_session_hash_ipaddr = (struct sc_capwap_session_priv**)kzalloc(sizeof(struct sc_capwap_session_priv*) * sc_session_hash_size, GFP_KERNEL);
-	if (!sc_session_hash_ipaddr) {
-		goto error;
-	}
-
-	sc_session_hash_sessionid = (struct sc_capwap_session_priv**)kzalloc(sizeof(struct sc_capwap_session_priv*) * sc_session_hash_size, GFP_KERNEL);
-	if (!sc_session_hash_sessionid) {
-		goto error1;
-	}
 
 	/* Create threads */
 	sc_session_threads_pos = 0;
-	sc_session_threads_count = threads;
+	sc_session_threads_count = 0;
-	sc_session_threads = (struct sc_capwap_workthread*)kzalloc(sizeof(struct sc_capwap_workthread) * threads, GFP_KERNEL);
+	for_each_online_cpu(cpu) {
-	if (!sc_session_threads) {
+		memset(&sc_session_threads[sc_session_threads_count], 0, sizeof(struct sc_capwap_workthread));
-		goto error2;
-	}
 
-	for (i = 0; i < threads; i++) {
+		/* Create thread and bind to cpu */
-		sc_session_threads[i].thread = kthread_create(sc_capwap_thread, &sc_session_threads[i], "smartcapwap/%u", i);
+		sc_session_threads[sc_session_threads_count].thread = kthread_create(sc_capwap_thread, &sc_session_threads[sc_session_threads_count], "smartcapwap/%u", sc_session_threads_count);
-		if (IS_ERR(sc_session_threads[i].thread)) {
+		if (!IS_ERR(sc_session_threads[sc_session_threads_count].thread)) {
-			err = PTR_ERR(sc_session_threads[i].thread);
+			kthread_bind(sc_session_threads[sc_session_threads_count].thread, cpu);
-			sc_session_threads[i].thread = NULL;
+
-			goto error3;
+			/* */
+			sc_session_threads_count++;
+			if (sc_session_threads_count == MAX_WORKER_THREAD) {
+				break;
+			}
+		} else {
+			err = PTR_ERR(sc_session_threads[sc_session_threads_count].thread);
+			sc_session_threads[sc_session_threads_count].thread = NULL;
+			goto error;
 		}
 	}
 
 	/* Init sockect */
 	err = sc_socket_init();
 	if (err) {
-		goto error3;
+		goto error;
 	}
 
 	/* Start threads */
-	for (i = 0; i < threads; i++) {
+	for (i = 0; i < sc_session_threads_count; i++) {
 		skb_queue_head_init(&sc_session_threads[i].queue);
 		init_waitqueue_head(&sc_session_threads[i].waitevent);
 		wake_up_process(sc_session_threads[i].thread);
@@ -468,22 +634,13 @@ int sc_capwap_init(uint32_t hash, uint32_t threads) {
 
 	return 0;
 
-error3:
+error:
-	for (i = 0; i < threads; i++) {
+	for (i = 0; i < sc_session_threads_count; i++) {
 		if (sc_session_threads[i].thread) {
 			kthread_stop(sc_session_threads[i].thread);
 		}
 	}
 
-	kfree(sc_session_threads);
-
-error2:
-	kfree(sc_session_hash_sessionid);
-
-error1:
-	kfree(sc_session_hash_ipaddr);
-
-error:
 	return err;
 }
 
@@ -492,27 +649,16 @@ void sc_capwap_close(void) {
 	uint32_t i;
 
 	TRACEKMOD("### sc_capwap_close\n");
-	TRACEKMOD("*** Closing capwap module\n");
 
-	/* */
+	/* Close */
 	sc_socket_close();
+	sc_capwap_closesessions();
+	sc_iface_closeall();
 
-	/* */
+	/* Terminate threads */
 	for (i = 0; i < sc_session_threads_count; i++) {
 		kthread_stop(sc_session_threads[i].thread);
 	}
-
-	kfree(sc_session_threads);
-
-	/* */
-	sc_capwap_closesessions();
-	kfree(sc_session_hash_ipaddr);
-	kfree(sc_session_hash_sessionid);
-
-	/* */
-	sc_iface_closeall();
-
-	TRACEKMOD("*** Close capwap module\n");
 }
 
 /* */
@@ -556,17 +702,64 @@ int sc_capwap_deletesession(const struct sc_capwap_sessionid_element* sessionid)
 	return -ENOENT;
 }
 
+/* */
+int sc_capwap_addwlan(const struct sc_capwap_sessionid_element* sessionid, uint8_t radioid, uint8_t wlanid, const uint8_t* bssid, uint8_t macmode, uint8_t tunnelmode) {
+	int err = -ENOENT;
+	struct sc_capwap_session_priv* sessionpriv;
+
+	TRACEKMOD("### sc_capwap_addwlan\n");
+
+	/* */
+	sc_capwap_update_lock();
+
+	/* Search session and interface */
+	sessionpriv = sc_capwap_getsession_sessionid(sessionid);
+	if (sessionpriv) {
+		struct sc_capwap_wlan* wlan = &sessionpriv->wlans[radioid - 1][wlanid - 1];
+
+		memcpy(wlan->bssid, bssid, MACADDRESS_EUI48_LENGTH);
+		wlan->macmode = macmode;
+		wlan->tunnelmode = tunnelmode;
+		wlan->used = 1;
+		err = 0;
+	}
+
+	sc_capwap_update_unlock();
+
+	return err;
+}
+
+/* */
+int sc_capwap_removewlan(const struct sc_capwap_sessionid_element* sessionid, uint8_t radioid, uint8_t wlanid) {
+	int err = -ENOENT;
+	struct sc_capwap_session_priv* sessionpriv;
+
+	TRACEKMOD("### sc_capwap_removewlan\n");
+
+	/* */
+	sc_capwap_update_lock();
+
+	/* Search session and interface */
+	sessionpriv = sc_capwap_getsession_sessionid(sessionid);
+	if (sessionpriv) {
+		sessionpriv->wlans[radioid - 1][wlanid - 1].used = 0;
+	}
+
+	sc_capwap_update_unlock();
+
+	return err;
+}
+
 /* */
 void sc_capwap_recvpacket(struct sk_buff* skb) {
 	uint32_t pos;
-	unsigned long flags;
 
 	TRACEKMOD("### sc_capwap_recvpacket\n");
 
-	spin_lock_irqsave(&sc_session_threads_lock, flags);
+	spin_lock(&sc_session_threads_lock);
 	sc_session_threads_pos = ((sc_session_threads_pos + 1) % sc_session_threads_count);
 	pos = sc_session_threads_pos;
-	spin_unlock_irqrestore(&sc_session_threads_lock, flags);
+	spin_unlock(&sc_session_threads_lock);
 
 	TRACEKMOD("*** Add packet to thread: %u\n", pos);
 
@@ -583,9 +776,6 @@ 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];
@@ -596,7 +786,7 @@ struct sc_capwap_session* sc_capwap_recvunknownkeepalive(const union capwap_addr
 
 	/* Change read lock to update lock */
 	rcu_read_unlock();
-	mutex_lock(&sc_session_mutex);
+	sc_capwap_update_lock();
 
 	/* Search and remove from setup session */
 	list_for_each_entry(search, &sc_session_setup_list, list) {
@@ -622,27 +812,31 @@ struct sc_capwap_session* sc_capwap_recvunknownkeepalive(const union capwap_addr
 
 	/* */
 	hash = sc_capwap_hash_ipaddr(sockaddr);
-	sessionpriv->next_ipaddr = sc_session_hash_ipaddr[hash];
+	sessionpriv->next_ipaddr = rcu_dereference_protected(sc_session_hash_ipaddr[hash], sc_capwap_update_lock_is_locked());
 	rcu_assign_pointer(sc_session_hash_ipaddr[hash], sessionpriv);
 
 	/* */
 	hash = sc_capwap_hash_sessionid(sessionid);
-	sessionpriv->next_sessionid = sc_session_hash_sessionid[hash];
+	sessionpriv->next_sessionid = rcu_dereference_protected(sc_session_hash_sessionid[hash], sc_capwap_update_lock_is_locked());
 	rcu_assign_pointer(sc_session_hash_sessionid[hash], sessionpriv);
 
 done:
 	rcu_read_lock();
-	mutex_unlock(&sc_session_mutex);
+	sc_capwap_update_unlock();
 
 	/* */
 	return (sessionpriv ? &sessionpriv->session : NULL);
 }
 
-
 /* */
 void sc_capwap_parsingdatapacket(struct sc_capwap_session* session, struct sk_buff* skb) {
 	uint8_t* pos;
+	uint8_t* srcaddress;
+	uint8_t* dstaddress;
+	struct sc_capwap_station* srcstation;
+	struct sc_capwap_station* dststation;
 	struct sc_capwap_header* header = (struct sc_capwap_header*)skb->data;
+	int is80211 = (IS_FLAG_T_HEADER(header) ? 1 : 0);
 	struct sc_capwap_radio_addr* radioaddr = NULL;
 	int radioaddrsize = 0;
 	struct sc_capwap_wireless_information* winfo = NULL;
@@ -660,11 +854,67 @@ void sc_capwap_parsingdatapacket(struct sc_capwap_session* session, struct sk_bu
 
 	if (IS_FLAG_W_HEADER(header)) {
 		winfo = (struct sc_capwap_wireless_information*)pos;
-		radioaddrsize = (sizeof(struct sc_capwap_wireless_information) + winfo->length + 3) & ~3;
+		winfosize = (sizeof(struct sc_capwap_wireless_information) + winfo->length + 3) & ~3;
 		pos += winfosize;
 	}
 
-	/* TODO */
+	/* Body packet */
+	skb_pull(skb, GET_HLEN_HEADER(header) * 4);
+	srcaddress = (is80211 ? ieee80211_get_SA((struct ieee80211_hdr*)skb->data) : (uint8_t*)((struct ethhdr*)skb->data)->h_source);
+	dstaddress = (is80211 ? ieee80211_get_DA((struct ieee80211_hdr*)skb->data) : (uint8_t*)((struct ethhdr*)skb->data)->h_dest);
+
+	/* Search source station */
+	srcstation = sc_stations_search(srcaddress);
+	if (srcstation) {
+		struct sc_capwap_session_priv* srcsessionpriv = rcu_dereference(srcstation->sessionpriv);
+		struct sc_capwap_wlan* wlan = &srcsessionpriv->wlans[srcstation->radioid - 1][srcstation->wlanid - 1];
+
+		if (wlan->used) {
+			/* Check tunnel mode */
+			if (wlan->tunnelmode != CAPWAP_ADD_WLAN_TUNNELMODE_LOCAL) {
+				if (is_multicast_ether_addr(dstaddress)) {
+					if (is80211) {
+						sc_capwap_80211_to_8023(skb);
+					}
+
+					/* Forward to any session with same connection */
+					if (!srcsessionpriv->isolation && !sc_capwap_restrictbroadcastpacket(skb, is80211)) {
+						sc_capwap_sendbroadcastpacket_wtp(rcu_dereference(srcstation->devpriv), srcstation->vlan, skb, srcsessionpriv);
+					}
+
+					/* Forward to physical interface */
+					sc_capwap_sendpacket_iface(srcstation, skb);
+				} else {
+					/* Search destination station */
+					dststation = sc_stations_search(dstaddress);
+					if (dststation) {
+						/* Forward packet */
+						if (!srcsessionpriv->isolation && (srcsessionpriv != rcu_access_pointer(dststation->sessionpriv))) {
+							sc_capwap_sendpacket_wtp(rcu_dereference(dststation->sessionpriv), dststation->radioid, dststation->wlanid, skb, is80211);
+						}
+
+						kfree_skb(skb);
+					} else {
+						if (is80211) {
+							sc_capwap_80211_to_8023(skb);
+						}
+
+						/* Forward to physical interface */
+						sc_capwap_sendpacket_iface(srcstation, skb);
+					}
+				}
+			} else {
+				TRACEKMOD("*** Receive packet from local tunnel mode wlan session\n");
+				kfree_skb(skb);
+			}
+		} else {
+			TRACEKMOD("*** Receive packet from disable wlan\n");
+			kfree_skb(skb);
+		}
+	} else {
+		TRACEKMOD("*** Receive packet from unknown station\n");
+		kfree_skb(skb);
+	}
 }
 
 /* */
@@ -673,4 +923,119 @@ void sc_capwap_parsingmgmtpacket(struct sc_capwap_session* session, struct sk_bu
 
 	/* Send packet with capwap header into userspace */
 	sc_netlink_notify_recv_data(&session->sessionid, skb->data, skb->len);
+	kfree_skb(skb);
+}
+
+/* */
+int sc_capwap_authstation(const struct sc_capwap_sessionid_element* sessionid, const uint8_t* address, uint32_t ifindex, uint8_t radioid, uint8_t wlanid, uint16_t vlan) {
+	int err = 0;
+	struct sc_capwap_station* station;
+	struct sc_netdev_priv* devpriv;
+	struct sc_capwap_session_priv* sessionpriv;
+
+	TRACEKMOD("### sc_capwap_authstation\n");
+
+	if (!IS_VALID_RADIOID(radioid)) {
+		return -EINVAL;
+	}
+
+	/* */
+	sc_capwap_update_lock();
+
+	/* Search session and interface */
+	sessionpriv = sc_capwap_getsession_sessionid(sessionid);
+	if (sessionpriv) {
+		devpriv = sc_iface_search(ifindex);
+		if (devpriv) {
+			/* Create or Update Station */
+			station = sc_stations_search(address);
+			if (station) {
+				/* Release old connection */
+				sc_stations_releaseconnection(station);
+
+				/* */
+				station->vlan = vlan;
+				station->radioid = radioid;
+				station->wlanid = wlanid;
+
+				/* Update interface */
+				if (rcu_access_pointer(station->devpriv) != devpriv) {
+					rcu_assign_pointer(station->devpriv, devpriv);
+					list_replace(&station->list_dev, &devpriv->list_stations);
+				}
+
+				/* Update session */
+				if (rcu_access_pointer(station->sessionpriv) != sessionpriv) {
+					rcu_assign_pointer(station->sessionpriv, sessionpriv);
+					list_replace(&station->list_session, &sessionpriv->list_stations);
+				}
+			} else {
+				station = (struct sc_capwap_station*)kzalloc(sizeof(struct sc_capwap_station), GFP_KERNEL);
+				if (station) {
+					memcpy(station->address, address, MACADDRESS_EUI48_LENGTH);
+					station->vlan = vlan;
+					station->radioid = radioid;
+					station->wlanid = wlanid;
+
+					/* Assign interface */
+					rcu_assign_pointer(station->devpriv, devpriv);
+					list_add(&station->list_dev, &devpriv->list_stations);
+
+					/* Assign session */
+					rcu_assign_pointer(station->sessionpriv, sessionpriv);
+					list_add(&station->list_session, &sessionpriv->list_stations);
+
+					/* Add station */
+					sc_stations_add(station);
+				} else {
+					TRACEKMOD("*** Unable to create station\n");
+					err = -ENOMEM;
+				}
+			}
+
+			/* Set new connection */
+			if (!err && station) {
+				err = sc_stations_setconnection(station);
+				if (err) {
+					TRACEKMOD("*** Unable to set connection\n");
+					sc_stations_free(station);
+				}
+			}
+		} else {
+			TRACEKMOD("*** Unable to find interface\n");
+			err = -EINVAL;
+		}
+	} else {
+		TRACEKMOD("*** Unable to find session\n");
+		err = -EINVAL;
+	}
+
+	sc_capwap_update_unlock();
+
+	return err;
+}
+
+/* */
+int sc_capwap_deauthstation(const struct sc_capwap_sessionid_element* sessionid, const uint8_t* address) {
+	int err = -ENOENT;
+	struct sc_capwap_station* station;
+	struct sc_capwap_session_priv* sessionpriv;
+
+	TRACEKMOD("### sc_capwap_deauthstation\n");
+
+	sc_capwap_update_lock();
+
+	sessionpriv = sc_capwap_getsession_sessionid(sessionid);
+	if (sessionpriv) {
+		station = sc_stations_search(address);
+		if (station && (rcu_access_pointer(station->sessionpriv) == sessionpriv)) {
+			sc_stations_releaseconnection(station);
+			sc_stations_free(station);
+			err = 0;
+		}
+	}
+
+	sc_capwap_update_unlock();
+
+	return err;
 }

src/ac/kmod/capwap_private.h

@@ -1,6 +1,15 @@
 #ifndef __KMOD_CAPWAP_PRIVATE_HEADER__
 #define __KMOD_CAPWAP_PRIVATE_HEADER__
 
+/* */
+struct sc_capwap_wlan {
+	int used;
+
+	uint8_t bssid[MACADDRESS_EUI48_LENGTH];
+	uint8_t macmode;
+	uint8_t tunnelmode;
+};
+
 /* */
 struct sc_capwap_session_priv {
 	struct sc_capwap_session session;
@@ -8,6 +17,14 @@ struct sc_capwap_session_priv {
 	struct list_head list;
 	struct sc_capwap_session_priv* __rcu next_ipaddr;
 	struct sc_capwap_session_priv* __rcu next_sessionid;
+
+	struct list_head list_stations;
+	struct list_head list_connections;
+
+	/* */
+	int isolation;
+	uint8_t binding;
+	struct sc_capwap_wlan wlans[CAPWAP_RADIOID_MAX_COUNT][CAPWAP_WLANID_MAX_COUNT];
 };
 
 /* */
@@ -19,14 +36,32 @@ struct sc_capwap_workthread {
 };
 
 /* */
-int sc_capwap_init(uint32_t hash, uint32_t threads);
+int sc_capwap_init(void);
 void sc_capwap_close(void);
 
 /* */
-int sc_capwap_sendkeepalive(const union capwap_addr* peeraddr);
+void sc_capwap_update_lock(void);
+void sc_capwap_update_unlock(void);
+
+#ifdef CONFIG_PROVE_LOCKING
+int sc_capwap_update_lock_is_locked(void);
+#else
+static inline int sc_capwap_update_lock_is_locked(void) { return 1; }
+#endif
 
 /* */
-int sc_capwap_newsession(const struct sc_capwap_sessionid_element* sessionid, uint16_t mtu);
+int sc_capwap_sendkeepalive(const struct sc_capwap_sessionid_element* sessionid);
+
+/* */
+int sc_capwap_newsession(const struct sc_capwap_sessionid_element* sessionid, uint8_t binding, uint16_t mtu);
 int sc_capwap_deletesession(const struct sc_capwap_sessionid_element* sessionid);
 
+/* */
+int sc_capwap_addwlan(const struct sc_capwap_sessionid_element* sessionid, uint8_t radioid, uint8_t wlanid, const uint8_t* bssid, uint8_t macmode, uint8_t tunnelmode);
+int sc_capwap_removewlan(const struct sc_capwap_sessionid_element* sessionid, uint8_t radioid, uint8_t wlanid);
+
+/* */
+int sc_capwap_authstation(const struct sc_capwap_sessionid_element* sessionid, const uint8_t* address, uint32_t ifindex, uint8_t radioid, uint8_t wlanid, uint16_t vlan);
+int sc_capwap_deauthstation(const struct sc_capwap_sessionid_element* sessionid, const uint8_t* address);
+
 #endif /* __KMOD_CAPWAP_PRIVATE_HEADER__ */

src/ac/kmod/capwap_rfc.h

@@ -79,6 +79,7 @@ struct sc_capwap_header {
 #define CAPWAP_RADIO_MAX_LENGTH_PADDED				12
 struct sc_capwap_radio_addr {
 	uint8_t length;
+	uint8_t addr[0];
 } __packed;
 
 /* Wireless Information */
@@ -96,6 +97,12 @@ struct sc_capwap_ieee80211_frame_info {
 	__be16 rate;
 } __packed;
 
+/* Destination WLANs */
+struct sc_capwap_destination_wlans {
+	__be16 wlanidbitmap;
+	__be16 reserved;
+} __packed;
+
 /* */
 #define CAPWAP_HEADER_MAX_LENGTH					(sizeof(struct sc_capwap_header) + CAPWAP_RADIO_MAX_LENGTH_PADDED + CAPWAP_WINFO_MAX_LENGTH_PADDED)
 
@@ -172,4 +179,9 @@ struct sc_capwap_macaddress_eui64 {
 #define IS_FLAG_K_HEADER(x)					((x)->flag_k)
 #define SET_FLAG_K_HEADER(x, y)				((x)->flag_k = ((y) ? 1 : 0))
 
+/* IEEE 802.11 Add WLAN */
+#define CAPWAP_ADD_WLAN_TUNNELMODE_LOCAL			0
+#define CAPWAP_ADD_WLAN_TUNNELMODE_8023				1
+#define CAPWAP_ADD_WLAN_TUNNELMODE_80211			2
+
 #endif /* __KMOD_CAPWAP_RFC_HEADER__ */

src/ac/kmod/iface.c

@@ -3,67 +3,80 @@
 #include <linux/kernel.h>
 #include <linux/netdevice.h>
 #include <linux/etherdevice.h>
+#include <linux/smp.h>
 #include "iface.h"
-
+#include "station.h"
-/* */
+#include "capwap.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 LIST_HEAD(sc_iface_list);
-static DEFINE_SPINLOCK(sc_iface_lock);
+static struct sc_netdev_priv* __rcu sc_iface_hash[CAPWAP_IFACE_COUNT];
-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;
+	struct sc_capwap_station* temp;
+	struct sc_capwap_station* station;
 	int hash = CAPWAP_IFACE_HASH(dev->ifindex);
 	struct sc_netdev_priv* priv = (struct sc_netdev_priv*)netdev_priv(dev);
 
 	TRACEKMOD("### sc_iface_netdev_uninit\n");
 
-	/* Remove interface from hash */
+	sc_capwap_update_lock();
-	spin_lock_irqsave(&sc_iface_lock, flags);
 
-	search  = sc_iface_hash[hash];
+	/* Close stations */
+	list_for_each_entry_safe(station, temp, &priv->list_stations, list_dev) {
+		sc_stations_releaseconnection(station);
+		sc_stations_free(station);
+	}
+
+	/* */
+	if (!list_empty(&priv->list_stations)) {
+		TRACEKMOD("*** Bug: the list stations of interface is not empty\n");
+	}
+
+	if (!list_empty(&priv->list_connections)) {
+		TRACEKMOD("*** Bug: the list connections of interface is not empty\n");
+	}
+
+	/* Remove interface from hash */
+	search  = rcu_dereference_protected(sc_iface_hash[hash], sc_capwap_update_lock_is_locked());
 	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;
+			rcu_assign_pointer(sc_iface_hash[hash], priv->next);
+
+			list_del_rcu(&priv->list);
+			synchronize_net();
 
 			dev_put(dev);
-			sc_iface_count--;
 		} else {
-			while (search->next && (search->next != priv)) {
+			while (rcu_access_pointer(search->next) && (rcu_access_pointer(search->next) != priv)) {
-				search = search->next;
+				search = rcu_dereference_protected(search->next, sc_capwap_update_lock_is_locked());
 			}
 
-			if (search->next) {
+			if (rcu_access_pointer(search->next)) {
 				netif_tx_lock_bh(dev);
 				netif_carrier_off(dev);
 				netif_tx_unlock_bh(dev);
 
-				search->next = priv->next;
+				rcu_assign_pointer(search->next, priv->next);
+
+				list_del_rcu(&priv->list);
+				synchronize_net();
 
 				dev_put(dev);
-				sc_iface_count--;
 			}
 		}
 	}
 
-	spin_unlock_irqrestore(&sc_iface_lock, flags);
+	sc_capwap_update_unlock();
-
-	/* Close stations with link to this device */
-	/* TODO */
 }
 
 /* */
@@ -84,9 +97,40 @@ static int sc_iface_netdev_stop(struct net_device* dev) {
 
 /* */
 static int sc_iface_netdev_tx(struct sk_buff* skb, struct net_device* dev) {
-	TRACEKMOD("### sc_iface_netdev_tx\n");
+	struct sc_netdev_priv* priv = (struct sc_netdev_priv*)netdev_priv(dev);
+
+	TRACEKMOD("### sc_iface_netdev_tx %d\n", smp_processor_id());
+
+	if (dev->flags & IFF_UP) {
+		/* Ignore 802.1ad */
+		if (skb->vlan_proto == htons(ETH_P_8021AD) || (eth_hdr(skb)->h_proto == htons(ETH_P_8021AD))) {
+			goto drop;
+		}
+
+		/* */
+		spin_lock(&priv->lock);
+		dev->stats.tx_packets++;
+		dev->stats.tx_bytes += skb->len;
+		spin_unlock(&priv->lock);
+
+		/* */
+		CAPWAP_SKB_CB(skb)->flags = SKB_CAPWAP_FLAG_FROM_AC_TAP;
+		sc_capwap_recvpacket(skb);
+	} else {
+		goto drop;
+	}
+
+	return 0;
+
+drop:
+	/* Drop packet */
+	kfree_skb(skb);
+
+	/* */
+	spin_lock(&priv->lock);
+	dev->stats.rx_dropped++;
+	spin_unlock(&priv->lock);
 
-	/* TODO */
 	return 0;
 }
 
@@ -100,13 +144,16 @@ static int sc_iface_netdev_change_mtu(struct net_device* dev, int new_mtu) {
 
 /* */
 static void sc_iface_netdev_setup(struct net_device* dev) {
-	struct sc_netdev_priv* priv = (struct sc_netdev_priv*)netdev_priv(dev);
+	struct sc_netdev_priv* devpriv = (struct sc_netdev_priv*)netdev_priv(dev);
 
 	TRACEKMOD("### sc_iface_netdev_setup\n");
 
 	/* */
-	memset(priv, 0, sizeof(struct sc_netdev_priv));
+	memset(devpriv, 0, sizeof(struct sc_netdev_priv));
-	priv->dev = dev;
+	devpriv->dev = dev;
+	spin_lock_init(&devpriv->lock);
+	INIT_LIST_HEAD(&devpriv->list_stations);
+	INIT_LIST_HEAD(&devpriv->list_connections);
 }
 
 /* */
@@ -122,7 +169,6 @@ static const struct net_device_ops capwap_netdev_ops = {
 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;
 
@@ -156,14 +202,16 @@ int sc_iface_create(const char* ifname, uint16_t mtu) {
 	/* */
 	hash = CAPWAP_IFACE_HASH(dev->ifindex);
 
-	spin_lock_irqsave(&sc_iface_lock, flags);
+	/* */
+	sc_capwap_update_lock();
 
-	sc_iface_count++;
+	list_add_rcu(&priv->list, &sc_iface_list);
-	priv->next = sc_iface_hash[hash];
+
-	sc_iface_hash[hash] = priv;
+	priv->next = rcu_dereference_protected(sc_iface_hash[hash], sc_capwap_update_lock_is_locked());
+	rcu_assign_pointer(sc_iface_hash[hash], priv);
 	dev_hold(dev);
 
-	spin_unlock_irqrestore(&sc_iface_lock, flags);
+	sc_capwap_update_unlock();
 
 	/* Enable carrier */
 	netif_tx_lock_bh(dev);
@@ -175,60 +223,77 @@ int sc_iface_create(const char* ifname, uint16_t mtu) {
 
 /* */
 int sc_iface_delete(uint32_t ifindex) {
-	unsigned long flags;
 	struct sc_netdev_priv* priv;
+	struct net_device* dev = NULL;
 
 	TRACEKMOD("### sc_iface_delete\n");
 
-	/* */
+	rcu_read_lock();
-	spin_lock_irqsave(&sc_iface_lock, flags);
 
-	priv  = sc_iface_hash[CAPWAP_IFACE_HASH(ifindex)];
+	/* Search device */
-	while (priv) {
+	priv = sc_iface_search(ifindex);
-		if (priv->dev->ifindex == ifindex) {
+	if (priv) {
-			break;
+		dev = priv->dev;
-		}
-
-		priv = priv->next;
 	}
 
-	spin_unlock_irqrestore(&sc_iface_lock, flags);
+	rcu_read_unlock();
 
 	/* */
-	if (!priv) {
+	if (!dev) {
 		return -ENOENT;
 	}
 
-	/* */
+	/* Unregister device */
-	unregister_netdev(priv->dev);
+	unregister_netdev(dev);
-	free_netdev(priv->dev);
+	free_netdev(dev);
 
 	return 0;
 }
 
+/* */
+struct sc_netdev_priv* sc_iface_search(uint32_t ifindex) {
+	struct sc_netdev_priv* priv;
+
+	TRACEKMOD("### sc_iface_search\n");
+
+	priv = rcu_dereference_check(sc_iface_hash[CAPWAP_IFACE_HASH(ifindex)], lockdep_is_held(&sc_iface_mutex));
+	while (priv) {
+		if (priv->dev->ifindex == ifindex) {
+			break;
+		}
+
+		/* */
+		priv = rcu_dereference_check(priv->next, lockdep_is_held(&sc_iface_mutex));
+	}
+
+	return priv;
+}
+
 /* */
 void sc_iface_closeall(void) {
-	int i;
+	struct sc_netdev_priv* priv;
-	unsigned long flags;
 
 	TRACEKMOD("### sc_iface_closeall\n");
 
-	while (sc_iface_count) {
+	for (;;) {
 		struct net_device* dev = NULL;
 
-		spin_lock_irqsave(&sc_iface_lock, flags);
+		rcu_read_lock();
 
-		for (i = 0; i < CAPWAP_IFACE_COUNT; i++) {
+		/* Get device */
-			if (sc_iface_hash[i]) {
+		priv = list_first_or_null_rcu(&sc_iface_list, struct sc_netdev_priv, list);
-				dev = sc_iface_hash[i]->dev;
+		if (priv) {
-				break;
+			dev = priv->dev;
-			}
 		}
 
-		spin_unlock_irqrestore(&sc_iface_lock, flags);
+		rcu_read_unlock();
 
 		/* */
-		BUG_ON(!dev);
+		if (!dev) {
+			break;
+		}
+
+		/* Unregister device */
 		unregister_netdev(dev);
 		free_netdev(dev);
 	}

src/ac/kmod/iface.h

@@ -1,10 +1,27 @@
 #ifndef __KMOD_AC_IFACE_HEADER__
 #define __KMOD_AC_IFACE_HEADER__
 
+/* */
+struct sc_netdev_priv {
+	struct list_head list;
+	struct net_device* dev;
+
+	spinlock_t lock;
+
+	struct list_head list_stations;
+	struct list_head list_connections;
+
+	struct sc_netdev_priv* __rcu next;
+};
+
 /* */
 int sc_iface_create(const char* ifname, uint16_t mtu);
 int sc_iface_delete(uint32_t ifindex);
 
+/* */
+struct sc_netdev_priv* sc_iface_search(uint32_t ifindex);
+
+/* */
 void sc_iface_closeall(void);
 
 #endif /* __KMOD_AC_IFACE_HEADER__ */

src/ac/kmod/netlinkapp.c

@@ -18,6 +18,10 @@
 /* */
 static u32 sc_netlink_usermodeid;
 
+/* */
+static int sc_netlink_pre_doit(struct genl_ops* ops, struct sk_buff* skb, struct genl_info* info);
+static void sc_netlink_post_doit(struct genl_ops* ops, struct sk_buff* skb, struct genl_info* info);
+
 /* Netlink Family */
 static struct genl_family sc_netlink_family = {
 	.id = GENL_ID_GENERATE,
@@ -26,8 +30,22 @@ 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,
 };
 
+/* */
+static int sc_netlink_pre_doit(struct genl_ops* ops, struct sk_buff* skb, struct genl_info* info) {
+	TRACEKMOD("### sc_netlink_pre_doit: %d\n", (int)ops->cmd);
+
+	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: %d\n", (int)ops->cmd);
+}
+
 /* */
 static int sc_netlink_bind(struct sk_buff* skb, struct genl_info* info) {
 	union capwap_addr sockaddr;
@@ -35,7 +53,7 @@ static int sc_netlink_bind(struct sk_buff* skb, struct genl_info* info) {
 	TRACEKMOD("### sc_netlink_bind\n");
 
 	/* Check Link */
-	if (sc_netlink_usermodeid != info->snd_portid) {
+	if (!sc_netlink_usermodeid) {
 		return -ENOLINK;
 	}
 
@@ -55,58 +73,37 @@ static int sc_netlink_bind(struct sk_buff* skb, struct genl_info* info) {
 
 /* */
 static int sc_netlink_send_keepalive(struct sk_buff* skb, struct genl_info* info) {
-	int ret;
-	union capwap_addr sockaddr;
-
 	TRACEKMOD("### sc_netlink_send_keepalive\n");
 
 	/* Check Link */
-	if (sc_netlink_usermodeid != info->snd_portid) {
+	if (!sc_netlink_usermodeid) {
 		return -ENOLINK;
 	}
 
-	/* Check Session address */
+	/* Check Session ID */
-	if (!info->attrs[NLSMARTCAPWAP_ATTR_ADDRESS]) {
+	if (!info->attrs[NLSMARTCAPWAP_ATTR_SESSION_ID]) {
-		return -EINVAL;
-	}
-
-	/* */
-	memcpy(&sockaddr.ss, nla_data(info->attrs[NLSMARTCAPWAP_ATTR_ADDRESS]), sizeof(struct sockaddr_storage));
-	if ((sockaddr.ss.ss_family != AF_INET) && (sockaddr.ss.ss_family != AF_INET6)) {
 		return -EINVAL;
 	}
 
 	/* Send keep-alive packet */
-	ret = sc_capwap_sendkeepalive(&sockaddr);
+	return sc_capwap_sendkeepalive((struct sc_capwap_sessionid_element*)nla_data(info->attrs[NLSMARTCAPWAP_ATTR_SESSION_ID]));
-	if (ret < 0) {
-		return ret;
-	}
-
-	return 0;
 }
 
 /* */
 static int sc_netlink_send_data(struct sk_buff* skb, struct genl_info* info) {
 	int length;
 	struct sk_buff* skbdata;
-	union capwap_addr sockaddr;
 	struct sc_skb_capwap_cb* cb;
 
 	TRACEKMOD("### sc_netlink_send_data\n");
 
 	/* Check Link */
-	if (sc_netlink_usermodeid != info->snd_portid) {
+	if (!sc_netlink_usermodeid) {
 		return -ENOLINK;
 	}
 
 	/* */
-	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_SESSION_ID] || !info->attrs[NLSMARTCAPWAP_ATTR_RADIOID] || !info->attrs[NLSMARTCAPWAP_ATTR_BINDING] || !info->attrs[NLSMARTCAPWAP_ATTR_DATA_FRAME]) {
-		return -EINVAL;
-	}
-
-	/* */
-	memcpy(&sockaddr.ss, nla_data(info->attrs[NLSMARTCAPWAP_ATTR_ADDRESS]), sizeof(struct sockaddr_storage));
-	if ((sockaddr.ss.ss_family != AF_INET) && (sockaddr.ss.ss_family != AF_INET6)) {
 		return -EINVAL;
 	}
 
@@ -125,8 +122,8 @@ static int sc_netlink_send_data(struct sk_buff* skb, struct genl_info* info) {
 
 	/* */
 	cb = CAPWAP_SKB_CB(skbdata);
-	cb->flags = SKB_CAPWAP_FLAG_FROM_USER_SPACE | SKB_CAPWAP_FLAG_PEERADDRESS | SKB_CAPWAP_FLAG_RADIOID | SKB_CAPWAP_FLAG_BINDING;
+	cb->flags = SKB_CAPWAP_FLAG_FROM_USER_SPACE | SKB_CAPWAP_FLAG_SESSIONID | SKB_CAPWAP_FLAG_RADIOID | SKB_CAPWAP_FLAG_BINDING;
-	sc_addr_tolittle(&sockaddr, &cb->peeraddr);
+	memcpy(&cb->sessionid, nla_data(info->attrs[NLSMARTCAPWAP_ATTR_SESSION_ID]), sizeof(struct sc_capwap_sessionid_element));
 	cb->radioid = nla_get_u8(info->attrs[NLSMARTCAPWAP_ATTR_RADIOID]);
 	cb->binding = nla_get_u8(info->attrs[NLSMARTCAPWAP_ATTR_BINDING]);
 
@@ -142,12 +139,12 @@ static int sc_netlink_new_session(struct sk_buff* skb, struct genl_info* info) {
 	TRACEKMOD("### sc_netlink_new_session\n");
 
 	/* Check Link */
-	if (sc_netlink_usermodeid != info->snd_portid) {
+	if (!sc_netlink_usermodeid) {
 		return -ENOLINK;
 	}
 
-	/* Check Session ID */
+	/* Check Session ID & Binding */
-	if (!info->attrs[NLSMARTCAPWAP_ATTR_SESSION_ID]) {
+	if (!info->attrs[NLSMARTCAPWAP_ATTR_SESSION_ID] || !info->attrs[NLSMARTCAPWAP_ATTR_BINDING]) {
 		return -EINVAL;
 	}
 
@@ -160,7 +157,7 @@ static int sc_netlink_new_session(struct sk_buff* skb, struct genl_info* info) {
 	}
 
 	/* New session */
-	return sc_capwap_newsession((struct sc_capwap_sessionid_element*)nla_data(info->attrs[NLSMARTCAPWAP_ATTR_SESSION_ID]), mtu);
+	return sc_capwap_newsession((struct sc_capwap_sessionid_element*)nla_data(info->attrs[NLSMARTCAPWAP_ATTR_SESSION_ID]), nla_get_u8(info->attrs[NLSMARTCAPWAP_ATTR_BINDING]), mtu);
 }
 
 /* */
@@ -168,7 +165,7 @@ static int sc_netlink_delete_session(struct sk_buff* skb, struct genl_info* info
 	TRACEKMOD("### sc_netlink_delete_session\n");
 
 	/* Check Link */
-	if (sc_netlink_usermodeid != info->snd_portid) {
+	if (!sc_netlink_usermodeid) {
 		return -ENOLINK;
 	}
 
@@ -181,17 +178,119 @@ static int sc_netlink_delete_session(struct sk_buff* skb, struct genl_info* info
 	return sc_capwap_deletesession((struct sc_capwap_sessionid_element*)nla_data(info->attrs[NLSMARTCAPWAP_ATTR_SESSION_ID]));
 }
 
+/* */
+static int sc_netlink_add_wlan(struct sk_buff* skb, struct genl_info* info) {
+	uint8_t radioid;
+	uint8_t wlanid;
+
+	TRACEKMOD("### sc_netlink_add_wlan\n");
+
+	/* Check Link */
+	if (!sc_netlink_usermodeid) {
+		return -ENOLINK;
+	}
+
+	/* Check params */
+	if (!info->attrs[NLSMARTCAPWAP_ATTR_SESSION_ID] || !info->attrs[NLSMARTCAPWAP_ATTR_RADIOID] || !info->attrs[NLSMARTCAPWAP_ATTR_WLANID] || !info->attrs[NLSMARTCAPWAP_ATTR_MACADDRESS] || !info->attrs[NLSMARTCAPWAP_ATTR_MACMODE] || !info->attrs[NLSMARTCAPWAP_ATTR_TUNNELMODE]) {
+		return -EINVAL;
+	}
+
+	/* */
+	radioid = nla_get_u8(info->attrs[NLSMARTCAPWAP_ATTR_RADIOID]);
+	wlanid = nla_get_u8(info->attrs[NLSMARTCAPWAP_ATTR_WLANID]);
+	if (!IS_VALID_RADIOID(radioid) || !IS_VALID_WLANID(wlanid)) {
+		return -EINVAL;
+	}
+
+	/* */
+	return sc_capwap_addwlan((struct sc_capwap_sessionid_element*)nla_data(info->attrs[NLSMARTCAPWAP_ATTR_SESSION_ID]), radioid, wlanid, (uint8_t*)nla_data(info->attrs[NLSMARTCAPWAP_ATTR_MACADDRESS]), nla_get_u8(info->attrs[NLSMARTCAPWAP_ATTR_MACMODE]), nla_get_u8(info->attrs[NLSMARTCAPWAP_ATTR_TUNNELMODE]));
+}
+
+/* */
+static int sc_netlink_remove_wlan(struct sk_buff* skb, struct genl_info* info) {
+	uint8_t radioid;
+	uint8_t wlanid;
+
+	TRACEKMOD("### sc_netlink_remove_wlan\n");
+
+	/* Check Link */
+	if (!sc_netlink_usermodeid) {
+		return -ENOLINK;
+	}
+
+	/* Check Session ID */
+	if (!info->attrs[NLSMARTCAPWAP_ATTR_SESSION_ID] || !info->attrs[NLSMARTCAPWAP_ATTR_RADIOID] || !info->attrs[NLSMARTCAPWAP_ATTR_WLANID]) {
+		return -EINVAL;
+	}
+
+	/* */
+	radioid = nla_get_u8(info->attrs[NLSMARTCAPWAP_ATTR_RADIOID]);
+	wlanid = nla_get_u8(info->attrs[NLSMARTCAPWAP_ATTR_WLANID]);
+	if (!IS_VALID_RADIOID(radioid) || !IS_VALID_WLANID(wlanid)) {
+		return -EINVAL;
+	}
+
+	return sc_capwap_removewlan((struct sc_capwap_sessionid_element*)nla_data(info->attrs[NLSMARTCAPWAP_ATTR_SESSION_ID]), radioid, wlanid);
+}
+
+/* */
+static int sc_netlink_auth_station(struct sk_buff* skb, struct genl_info* info) {
+	uint8_t radioid;
+	uint8_t wlanid;
+	uint16_t vlan = 0;
+
+	TRACEKMOD("### sc_netlink_auth_station\n");
+
+	/* Check Link */
+	if (!sc_netlink_usermodeid) {
+		return -ENOLINK;
+	}
+
+	/* Check params */
+	if (!info->attrs[NLSMARTCAPWAP_ATTR_SESSION_ID] || !info->attrs[NLSMARTCAPWAP_ATTR_MACADDRESS] || !info->attrs[NLSMARTCAPWAP_ATTR_IFPHY_INDEX] || !info->attrs[NLSMARTCAPWAP_ATTR_RADIOID] || !info->attrs[NLSMARTCAPWAP_ATTR_WLANID]) {
+		return -EINVAL;
+	}
+
+	/* */
+	radioid = nla_get_u8(info->attrs[NLSMARTCAPWAP_ATTR_RADIOID]);
+	wlanid = nla_get_u8(info->attrs[NLSMARTCAPWAP_ATTR_WLANID]);
+	if (!IS_VALID_RADIOID(radioid) || !IS_VALID_WLANID(wlanid)) {
+		return -EINVAL;
+	}
+
+	/* VLAN */
+	if (info->attrs[NLSMARTCAPWAP_ATTR_VLAN]) {
+		vlan = nla_get_u16(info->attrs[NLSMARTCAPWAP_ATTR_VLAN]);
+	}
+
+	/* */
+	return sc_capwap_authstation((struct sc_capwap_sessionid_element*)nla_data(info->attrs[NLSMARTCAPWAP_ATTR_SESSION_ID]), (uint8_t*)nla_data(info->attrs[NLSMARTCAPWAP_ATTR_MACADDRESS]), nla_get_u32(info->attrs[NLSMARTCAPWAP_ATTR_IFPHY_INDEX]), radioid, wlanid, vlan);
+}
+
+/* */
+static int sc_netlink_deauth_station(struct sk_buff* skb, struct genl_info* info) {
+	TRACEKMOD("### sc_netlink_deauth_station\n");
+
+	/* Check Link */
+	if (!sc_netlink_usermodeid) {
+		return -ENOLINK;
+	}
+
+	/* Check params */
+	if (!info->attrs[NLSMARTCAPWAP_ATTR_SESSION_ID] || !info->attrs[NLSMARTCAPWAP_ATTR_MACADDRESS]) {
+		return -EINVAL;
+	}
+
+	/* */
+	return sc_capwap_deauthstation((struct sc_capwap_sessionid_element*)nla_data(info->attrs[NLSMARTCAPWAP_ATTR_SESSION_ID]), (uint8_t*)nla_data(info->attrs[NLSMARTCAPWAP_ATTR_MACADDRESS]));
+}
+
 /* */
 static int sc_netlink_link(struct sk_buff* skb, struct genl_info* info) {
 	int ret;
 
 	TRACEKMOD("### sc_netlink_link\n");
 
-	if (!info->attrs[NLSMARTCAPWAP_ATTR_HASH_SESSION_BITFIELD] || !info->attrs[NLSMARTCAPWAP_ATTR_SESSION_THREADS_COUNT]) {
-		TRACEKMOD("*** Invalid link argument\n");
-		return -EINVAL;
-	}
-
 	/* */
 	if (sc_netlink_usermodeid) {
 		TRACEKMOD("*** Busy kernel link\n");
@@ -199,7 +298,7 @@ static int sc_netlink_link(struct sk_buff* skb, struct genl_info* info) {
 	}
 
 	/* 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]));
+	ret = sc_capwap_init();
 	if (ret) {
 		return ret;
 	}
@@ -222,7 +321,7 @@ static int sc_netlink_add_iface(struct sk_buff* skb, struct genl_info* info) {
 	TRACEKMOD("### sc_netlink_add_iface\n");
 
 	/* Check Link */
-	if (sc_netlink_usermodeid != info->snd_portid) {
+	if (!sc_netlink_usermodeid) {
 		return -ENOLINK;
 	}
 
@@ -277,7 +376,7 @@ 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) {
+	if (!sc_netlink_usermodeid) {
 		return -ENOLINK;
 	}
 
@@ -311,14 +410,18 @@ static const struct nla_policy sc_netlink_policy[NLSMARTCAPWAP_ATTR_MAX + 1] = {
 	[NLSMARTCAPWAP_ATTR_FLAGS] = { .type = NLA_U32 },
 	[NLSMARTCAPWAP_ATTR_SESSION_ID] = { .type = NLA_BINARY, .len = sizeof(struct sc_capwap_sessionid_element) },
 	[NLSMARTCAPWAP_ATTR_RADIOID] = { .type = NLA_U8 },
+	[NLSMARTCAPWAP_ATTR_WLANID] = { .type = NLA_U8 },
 	[NLSMARTCAPWAP_ATTR_BINDING] = { .type = NLA_U8 },
+	[NLSMARTCAPWAP_ATTR_MACMODE] = { .type = NLA_U8 },
+	[NLSMARTCAPWAP_ATTR_TUNNELMODE] = { .type = NLA_U8 },
 	[NLSMARTCAPWAP_ATTR_ADDRESS] = { .type = NLA_BINARY, .len = sizeof(struct sockaddr_storage) },
 	[NLSMARTCAPWAP_ATTR_DATA_FRAME] = { .type = NLA_BINARY, .len = IEEE80211_MAX_DATA_LEN + CAPWAP_HEADER_MAX_LENGTH },
 	[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 },
+	[NLSMARTCAPWAP_ATTR_MACADDRESS] = { .type = NLA_BINARY, .len = MACADDRESS_EUI48_LENGTH },
+	[NLSMARTCAPWAP_ATTR_BSSID] = { .type = NLA_BINARY, .len = MACADDRESS_EUI48_LENGTH },
+	[NLSMARTCAPWAP_ATTR_VLAN] = { .type = NLA_U16 },
 };
 
 /* Netlink Ops */
@@ -371,6 +474,30 @@ static struct genl_ops sc_netlink_ops[] = {
 		.policy = sc_netlink_policy,
 		.flags = GENL_ADMIN_PERM,
 	},
+	{
+		.cmd = NLSMARTCAPWAP_CMD_ADD_WLAN,
+		.doit = sc_netlink_add_wlan,
+		.policy = sc_netlink_policy,
+		.flags = GENL_ADMIN_PERM,
+	},
+	{
+		.cmd = NLSMARTCAPWAP_CMD_REMOVE_WLAN,
+		.doit = sc_netlink_remove_wlan,
+		.policy = sc_netlink_policy,
+		.flags = GENL_ADMIN_PERM,
+	},
+	{
+		.cmd = NLSMARTCAPWAP_CMD_AUTH_STATION,
+		.doit = sc_netlink_auth_station,
+		.policy = sc_netlink_policy,
+		.flags = GENL_ADMIN_PERM,
+	},
+	{
+		.cmd = NLSMARTCAPWAP_CMD_DEAUTH_STATION,
+		.doit = sc_netlink_deauth_station,
+		.policy = sc_netlink_policy,
+		.flags = GENL_ADMIN_PERM,
+	},
 };
 
 /* Netlink notify */
@@ -398,8 +525,7 @@ int sc_netlink_notify_recv_keepalive(const union capwap_addr* sockaddr, struct s
 	}
 
 	/* */
-	if (nla_put(sk_msg, NLSMARTCAPWAP_ATTR_ADDRESS, sizeof(struct sockaddr_storage), &sockaddr->ss) || 
+	if (nla_put(sk_msg, NLSMARTCAPWAP_ATTR_SESSION_ID, sizeof(struct sc_capwap_sessionid_element), sessionid)) {
-		nla_put(sk_msg, NLSMARTCAPWAP_ATTR_SESSION_ID, sizeof(struct sc_capwap_sessionid_element), sessionid)) {
 		goto error2;
 	}
 

src/ac/kmod/nlsmartcapwap.h

@@ -16,19 +16,24 @@ enum sc_netlink_attrs {
 	NLSMARTCAPWAP_ATTR_SESSION_ID,
 
 	NLSMARTCAPWAP_ATTR_RADIOID,
+	NLSMARTCAPWAP_ATTR_WLANID,
 	NLSMARTCAPWAP_ATTR_BINDING,
+	NLSMARTCAPWAP_ATTR_MACMODE,
+	NLSMARTCAPWAP_ATTR_TUNNELMODE,
 
 	NLSMARTCAPWAP_ATTR_ADDRESS,
 	NLSMARTCAPWAP_ATTR_MTU,
 
 	NLSMARTCAPWAP_ATTR_DATA_FRAME,
 
-	NLSMARTCAPWAP_ATTR_HASH_SESSION_BITFIELD,
-	NLSMARTCAPWAP_ATTR_SESSION_THREADS_COUNT,
-
 	NLSMARTCAPWAP_ATTR_IFPHY_NAME,
 	NLSMARTCAPWAP_ATTR_IFPHY_INDEX,
 
+	NLSMARTCAPWAP_ATTR_MACADDRESS,
+	NLSMARTCAPWAP_ATTR_BSSID,
+
+	NLSMARTCAPWAP_ATTR_VLAN,
+
 	/* Last attribute */
 	__NLSMARTCAPWAP_ATTR_AFTER_LAST,
 	NLSMARTCAPWAP_ATTR_MAX = __NLSMARTCAPWAP_ATTR_AFTER_LAST - 1
@@ -51,9 +56,15 @@ enum sc_netlink_commands {
 	NLSMARTCAPWAP_CMD_NEW_SESSION,
 	NLSMARTCAPWAP_CMD_DELETE_SESSION,
 
+	NLSMARTCAPWAP_CMD_ADD_WLAN,
+	NLSMARTCAPWAP_CMD_REMOVE_WLAN,
+
 	NLSMARTCAPWAP_CMD_SEND_DATA,
 	NLSMARTCAPWAP_CMD_RECV_DATA,
 
+	NLSMARTCAPWAP_CMD_AUTH_STATION,
+	NLSMARTCAPWAP_CMD_DEAUTH_STATION,
+
 	/* Last command */
 	__NLSMARTCAPWAP_CMD_AFTER_LAST,
 	NLSMARTCAPWAP_CMD_MAX = __NLSMARTCAPWAP_CMD_AFTER_LAST - 1

src/ac/kmod/socket.c

@@ -225,29 +225,3 @@ int sc_addr_compare(const union capwap_addr* addr1, const union capwap_addr* add
 
 	return -1;
 }
-
-/* */
-void sc_addr_tolittle(const union capwap_addr* addr, struct capwap_addr_little* little) {
-	little->family = (uint8_t)addr->ss.ss_family;
-	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 (addr->ss.ss_family == AF_INET6) {
-		memcpy(&little->addr6, &addr->sin6.sin6_addr, sizeof(struct in6_addr));
-		little->port = addr->sin6.sin6_port;
-	}
-}
-
-/* */
-void sc_addr_fromlittle(const struct capwap_addr_little* little, union capwap_addr* addr) {
-	memset(addr, 0, sizeof(union capwap_addr));
-
-	addr->ss.ss_family = little->family;
-	if (little->family == AF_INET) {
-		memcpy(&addr->sin.sin_addr, &little->addr4, sizeof(struct in_addr));
-		addr->sin.sin_port = little->port;
-	} else if (little->family == AF_INET6) {
-		memcpy(&addr->sin6.sin6_addr, &little->addr6, sizeof(struct in6_addr));
-		addr->sin6.sin6_port = little->port;
-	}
-}

src/ac/kmod/socket.h

@@ -10,16 +10,6 @@
 #define SOCKET_UDP					0
 #define SOCKET_UDPLITE				1
 
-/* Little socket address */
-struct capwap_addr_little {
-	uint8_t family;
-	union {
-		struct in_addr addr4;
-		struct in6_addr addr6;
-	};
-	uint16_t port;
-};
-
 /* Universal socket address */
 union capwap_addr {
 	struct sockaddr sa;
@@ -35,11 +25,11 @@ void sc_socket_close(void);
 /* */
 int sc_socket_bind(union capwap_addr* sockaddr);
 int sc_socket_send(int type, uint8_t* buffer, int length, union capwap_addr* sockaddr);
+
+/* */
 int sc_socket_getpeeraddr(struct sk_buff* skb, union capwap_addr* peeraddr);
 
 /* */
 int sc_addr_compare(const union capwap_addr* addr1, const union capwap_addr* addr2);
-void sc_addr_tolittle(const union capwap_addr* addr, struct capwap_addr_little* little);
-void sc_addr_fromlittle(const struct capwap_addr_little* little, union capwap_addr* addr);
 
 #endif /* __KMOD_SOCKET_HEADER__ */

src/ac/kmod/station.c

@@ -0,0 +1,157 @@
+#include "config.h"
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/netdevice.h>
+#include "station.h"
+#include "capwap.h"
+#include "iface.h"
+
+/* */
+#define STATION_HASH_SIZE					65536
+
+/* */
+static LIST_HEAD(sc_station_list);
+static struct sc_capwap_station* __rcu sc_station_hash_addr[STATION_HASH_SIZE];
+
+/* */
+static uint32_t sc_stations_hash_addr(const uint8_t* macaddress) {
+	TRACEKMOD("### sc_stations_hash_addr\n");
+
+	return (((((uint32_t)macaddress[4] << 8) | (uint32_t)macaddress[5]) ^ ((uint32_t)macaddress[3] << 4)) % STATION_HASH_SIZE);
+}
+
+/* */
+static struct sc_capwap_connection* sc_stations_searchconnection(struct sc_capwap_station* station) {
+	struct sc_capwap_connection* connection;
+	struct sc_capwap_session_priv* sessionpriv = rcu_access_pointer(station->sessionpriv);
+	struct sc_netdev_priv* devpriv = rcu_access_pointer(station->devpriv);
+
+	TRACEKMOD("### sc_stations_searchconnection\n");
+
+	list_for_each_entry(connection, &sessionpriv->list_connections, list_session) {
+		if ((connection->devpriv == devpriv) && (connection->radioid == station->radioid) && (connection->vlan == station->vlan)) {
+			return connection;
+		}
+	}
+
+	return NULL;
+}
+
+/* */
+void sc_stations_add(struct sc_capwap_station* station) {
+	uint32_t hash;
+
+	TRACEKMOD("### sc_stations_add\n");
+
+	/* */
+	list_add_rcu(&station->list, &sc_station_list);
+
+	hash = sc_stations_hash_addr(station->address);
+	station->next_addr = rcu_dereference_protected(sc_station_hash_addr[hash], sc_capwap_update_lock_is_locked());
+	rcu_assign_pointer(sc_station_hash_addr[hash], station);
+}
+
+/* */
+struct sc_capwap_station* sc_stations_search(const uint8_t* macaddress) {
+	struct sc_capwap_station* station;
+
+	TRACEKMOD("### sc_stations_search\n");
+
+	/* */
+	station = rcu_dereference_check(sc_station_hash_addr[sc_stations_hash_addr(macaddress)], sc_capwap_update_lock_is_locked());
+	while (station) {
+		if (!memcmp(&station->address, macaddress, MACADDRESS_EUI48_LENGTH)) {
+			break;
+		}
+
+		/* */
+		station = rcu_dereference_check(station->next_addr, sc_capwap_update_lock_is_locked());
+	}
+
+	return station;
+}
+
+/* */
+void sc_stations_free(struct sc_capwap_station* station) {
+	uint32_t hash;
+	struct sc_capwap_station* search;
+
+	TRACEKMOD("### sc_stations_free\n");
+
+	/* */
+	hash = sc_stations_hash_addr(station->address);
+	search = rcu_dereference_protected(sc_station_hash_addr[hash], sc_capwap_update_lock_is_locked());
+
+	if (search) {
+		if (search == station) {
+			rcu_assign_pointer(sc_station_hash_addr[hash], station->next_addr);
+		} else {
+			while (rcu_access_pointer(search->next_addr) && (rcu_access_pointer(search->next_addr) != station)) {
+				search = rcu_dereference_protected(search->next_addr, sc_capwap_update_lock_is_locked());
+			}
+
+			if (rcu_access_pointer(search->next_addr)) {
+				rcu_assign_pointer(search->next_addr, station->next_addr);
+			}
+		}
+	}
+
+	/* */
+	list_del_rcu(&station->list_dev);
+	list_del_rcu(&station->list_session);
+	synchronize_net();
+
+	kfree(station);
+}
+
+/* */
+int sc_stations_setconnection(struct sc_capwap_station* station) {
+	struct sc_capwap_connection* connection;
+
+	TRACEKMOD("### sc_stations_setconnection\n");
+
+	/* */
+	connection = sc_stations_searchconnection(station);
+	if (!connection) {
+		connection = (struct sc_capwap_connection*)kzalloc(sizeof(struct sc_capwap_connection), GFP_KERNEL);
+		if (!connection) {
+			TRACEKMOD("*** Unable to create connection\n");
+			return -ENOMEM;
+		}
+
+		/* */
+		connection->sessionpriv = rcu_access_pointer(station->sessionpriv);
+		list_add_rcu(&connection->list_session, &connection->sessionpriv->list_connections);
+		connection->devpriv = rcu_access_pointer(station->devpriv);
+		list_add_rcu(&connection->list_dev, &connection->devpriv->list_connections);
+		connection->radioid = station->radioid;
+		connection->vlan = station->vlan;
+	}
+
+	/* */
+	connection->count++;
+	connection->wlanidmask |= 1 << (station->wlanid - 1);
+	return 0;
+}
+
+/* */
+void sc_stations_releaseconnection(struct sc_capwap_station* station) {
+	struct sc_capwap_connection* connection;
+
+	TRACEKMOD("### sc_stations_releaseconnection\n");
+
+	connection = sc_stations_searchconnection(station);
+	if (connection) {
+		TRACEKMOD("*** Release connection reference %d\n", connection->count);
+
+		connection->count--;
+		if (!connection->count) {
+			list_del_rcu(&connection->list_session);
+			list_del_rcu(&connection->list_dev);
+			synchronize_net();
+
+			kfree(connection);
+		}
+	}
+}

src/ac/kmod/station.h

@@ -0,0 +1,58 @@
+#ifndef __KMOD_AC_STATION_HEADER__
+#define __KMOD_AC_STATION_HEADER__
+
+#include "capwap_rfc.h"
+
+/* */
+struct sc_capwap_connection {
+	int count;
+
+	/* Session */
+	struct sc_capwap_session_priv* sessionpriv;
+	struct list_head list_session;
+
+	/* Interface */
+	struct sc_netdev_priv* devpriv;
+	struct list_head list_dev;
+
+	/* */
+	uint16_t vlan;
+	uint8_t radioid;
+	uint8_t wlanidmask;
+};
+
+/* */
+struct sc_capwap_station {
+	struct list_head list;
+
+	/* */
+	uint8_t address[MACADDRESS_EUI48_LENGTH];
+	struct sc_capwap_station* __rcu next_addr;
+
+	/* Session */
+	struct sc_capwap_session_priv* __rcu sessionpriv;
+	struct list_head list_session;
+
+	/* Interface */
+	struct sc_netdev_priv* __rcu devpriv;
+	struct list_head list_dev;
+
+	/* */
+	uint8_t bssid[MACADDRESS_EUI48_LENGTH];
+	uint16_t vlan;
+	uint8_t radioid;
+	uint8_t wlanid;
+};
+
+/* */
+void sc_stations_add(struct sc_capwap_station* station);
+void sc_stations_free(struct sc_capwap_station* station);
+
+/* */
+struct sc_capwap_station* sc_stations_search(const uint8_t* macaddress);
+
+/* */
+int sc_stations_setconnection(struct sc_capwap_station* station);
+void sc_stations_releaseconnection(struct sc_capwap_station* station);
+
+#endif /* __KMOD_AC_STATION_HEADER__ */

src/wtp/kmod/capwap.c

@@ -1,5 +1,7 @@
 #include "config.h"
 #include <linux/module.h>
+#include <linux/if_ether.h>
+#include <linux/etherdevice.h>
 #include <linux/ieee80211.h>
 #include "socket.h"
 #include "capwap.h"
@@ -12,6 +14,12 @@
 /* */
 union capwap_addr sc_localaddr;
 
+/* Ethernet-II snap header (RFC1042 for most EtherTypes) */
+static unsigned char sc_rfc1042_header[] = { 0xaa, 0xaa, 0x03, 0x00, 0x00, 0x00 };
+
+/* Bridge-Tunnel header (for EtherTypes ETH_P_AARP and ETH_P_IPX) */
+static unsigned char sc_bridge_tunnel_header[] = { 0xaa, 0xaa, 0x03, 0x00, 0x00, 0xf8 };
+
 /* */
 static void sc_capwap_fragment_free(struct sc_capwap_fragment* fragment) {
 	TRACEKMOD("### sc_capwap_fragment_free\n");
@@ -32,7 +40,6 @@ static void sc_capwap_fragment_free(struct sc_capwap_fragment* fragment) {
 /* */
 static void sc_capwap_defrag_evictor(struct sc_capwap_session* session, ktime_t now) {
 	ktime_t delta;
-	unsigned long flags;
 	struct sc_capwap_fragment* fragment;
 
 	TRACEKMOD("### sc_capwap_defrag_evictor\n");
@@ -45,7 +52,7 @@ static void sc_capwap_defrag_evictor(struct sc_capwap_session* session, ktime_t
 
 	/* Remove last old fragment */
 	if (!list_empty(&session->fragments.lru_list)) {
-		spin_lock_irqsave(&session->fragments.lock, flags);
+		spin_lock(&session->fragments.lock);
 
 		fragment = list_first_entry(&session->fragments.lru_list, struct sc_capwap_fragment, lru_list);
 		if (fragment) {
@@ -58,7 +65,7 @@ static void sc_capwap_defrag_evictor(struct sc_capwap_session* session, ktime_t
 			}
 		}
 
-		spin_unlock_irqrestore(&session->fragments.lock, flags);
+		spin_unlock(&session->fragments.lock);
 	}
 }
 
@@ -104,7 +111,6 @@ static struct sk_buff* sc_capwap_reasm(struct sc_capwap_fragment* fragment) {
 
 /* */
 static struct sk_buff* sc_capwap_defrag(struct sc_capwap_session* session, struct sk_buff* skb) {
-	unsigned long flags;
 	uint16_t headersize;
 	uint16_t frag_id;
 	struct sk_buff* prev;
@@ -132,7 +138,7 @@ static struct sk_buff* sc_capwap_defrag(struct sc_capwap_session* session, struc
 	cb->frag_length = skb->len - headersize;
 
 	/* */
-	spin_lock_irqsave(&session->fragments.lock, flags);
+	spin_lock(&session->fragments.lock);
 
 	/* Get fragment */
 	fragment = &session->fragments.queues[frag_id % CAPWAP_FRAGMENT_QUEUE];
@@ -219,18 +225,158 @@ static struct sk_buff* sc_capwap_defrag(struct sc_capwap_session* session, struc
 		}
 	}
 
-	spin_unlock_irqrestore(&session->fragments.lock, flags);
+	spin_unlock(&session->fragments.lock);
 
 	return skb_defrag;
 
 error2:
-	spin_unlock_irqrestore(&session->fragments.lock, flags);
+	spin_unlock(&session->fragments.lock);
 
 error:
 	kfree_skb(skb);
 	return NULL;
 }
 
+/* */
+static unsigned int sc_capwap_80211_hdrlen(__le16 fc) {
+	unsigned int hdrlen = 24;
+
+	TRACEKMOD("### sc_capwap_80211_hdrlen\n");
+
+	if (ieee80211_is_data(fc)) {
+		if (ieee80211_has_a4(fc)) {
+			hdrlen = 30;
+		}
+
+		if (ieee80211_is_data_qos(fc)) {
+			hdrlen += IEEE80211_QOS_CTL_LEN;
+			if (ieee80211_has_order(fc)) {
+				hdrlen += IEEE80211_HT_CTL_LEN;
+			}
+		}
+	} else if (ieee80211_is_ctl(fc)) {
+		if ((fc & cpu_to_le16(0x00E0)) == cpu_to_le16(0x00C0)) {
+			hdrlen = 10;
+		} else {
+			hdrlen = 16;
+		}
+	}
+
+	return hdrlen;
+}
+
+/* */
+int sc_capwap_8023_to_80211(struct sk_buff* skb, const uint8_t* bssid) {
+	uint16_t hdrlen;
+	int head_need;
+	struct ieee80211_hdr hdr;
+	int skip_header_bytes;
+	uint8_t* encaps_data;
+	int encaps_len;
+	struct ethhdr* eh = (struct ethhdr*)skb->data;
+	uint16_t ethertype = ntohs(eh->h_proto);
+
+	TRACEKMOD("### sc_capwap_8023_to_80211\n");
+
+	/* IEEE 802.11 header */
+	hdrlen = 24;
+	hdr.frame_control = cpu_to_le16(IEEE80211_FTYPE_DATA | IEEE80211_STYPE_DATA | IEEE80211_FCTL_FROMDS);
+	memcpy(hdr.addr1, eh->h_dest, ETH_ALEN);
+	memcpy(hdr.addr2, bssid, ETH_ALEN);
+	memcpy(hdr.addr3, eh->h_source, ETH_ALEN);
+	hdr.duration_id = 0;
+	hdr.seq_ctrl = 0;
+
+	/* */
+	skip_header_bytes = ETH_HLEN;
+	if ((ethertype == ETH_P_AARP) || (ethertype == ETH_P_IPX)) {
+		encaps_data = sc_bridge_tunnel_header;
+		encaps_len = sizeof(sc_bridge_tunnel_header);
+		skip_header_bytes -= 2;
+	} else if (ethertype >= ETH_P_802_3_MIN) {
+		encaps_data = sc_rfc1042_header;
+		encaps_len = sizeof(sc_rfc1042_header);
+		skip_header_bytes -= 2;
+	} else {
+		encaps_data = NULL;
+		encaps_len = 0;
+	}
+
+	/* Remove IEEE 802.3 header */
+	skb_pull(skb, skip_header_bytes);
+
+	/* Check headroom */
+	head_need = hdrlen + encaps_len - skb_headroom(skb);
+	if ((head_need > 0) || skb_cloned(skb)) {
+		head_need = max(head_need, 0);
+		if (head_need) {
+			skb_orphan(skb);
+		}
+
+		TRACEKMOD("*** Expand headroom skb of: %d\n", head_need);
+		if (pskb_expand_head(skb, head_need, 0, GFP_ATOMIC)) {
+			return -ENOMEM;
+		}
+
+		skb->truesize += head_need;
+	}
+
+	/* Add LLC header */
+	if (encaps_data) {
+		memcpy(skb_push(skb, encaps_len), encaps_data, encaps_len);
+	}
+
+	/* Add IEEE 802.11 header */
+	memcpy(skb_push(skb, hdrlen), &hdr, hdrlen);
+	skb_reset_mac_header(skb);
+
+	return 0;
+}
+
+/* */
+int sc_capwap_80211_to_8023(struct sk_buff* skb) {
+	struct ieee80211_hdr* hdr = (struct ieee80211_hdr*)skb->data;
+	uint16_t hdrlen;
+	uint16_t ethertype;
+	uint8_t* payload;
+	uint8_t dst[ETH_ALEN];
+	uint8_t src[ETH_ALEN] __aligned(2);
+
+	TRACEKMOD("### sc_capwap_80211_to_8023\n");
+
+	/* */
+	hdrlen = sc_capwap_80211_hdrlen(hdr->frame_control);
+	memcpy(dst, ieee80211_get_DA(hdr), ETH_ALEN);
+	memcpy(src, ieee80211_get_SA(hdr), ETH_ALEN);
+
+	/* */
+	if (!pskb_may_pull(skb, hdrlen + 8)) {
+		return -1;
+	}
+
+	/* */
+	payload = skb->data + hdrlen;
+	ethertype = (payload[6] << 8) | payload[7];
+
+	if (likely((ether_addr_equal(payload, sc_rfc1042_header) && (ethertype != ETH_P_AARP) && (ethertype != ETH_P_IPX)) || ether_addr_equal(payload, sc_bridge_tunnel_header))) {
+		skb_pull(skb, hdrlen + 6);
+		memcpy(skb_push(skb, ETH_ALEN), src, ETH_ALEN);
+		memcpy(skb_push(skb, ETH_ALEN), dst, ETH_ALEN);
+	} else {
+		struct ethhdr *ehdr;
+		__be16 len;
+
+		skb_pull(skb, hdrlen);
+		len = htons(skb->len);
+		ehdr = (struct ethhdr *) skb_push(skb, sizeof(struct ethhdr));
+		memcpy(ehdr->h_dest, dst, ETH_ALEN);
+		memcpy(ehdr->h_source, src, ETH_ALEN);
+		ehdr->h_proto = len;
+	}
+
+	return 0;
+}
+
 /* */
 int sc_capwap_bind(union capwap_addr* sockaddr) {
 	int ret;
@@ -275,13 +421,12 @@ void sc_capwap_freesession(struct sc_capwap_session* session) {
 /* */
 uint16_t sc_capwap_newfragmentid(struct sc_capwap_session* session) {
 	uint16_t fragmentid;
-	unsigned long flags;
 
 	TRACEKMOD("### sc_capwap_newfragmentid\n");
 
-	spin_lock_irqsave(&session->fragmentid_lock, flags);
+	spin_lock(&session->fragmentid_lock);
 	fragmentid = session->fragmentid++;
-	spin_unlock_irqrestore(&session->fragmentid_lock, flags);
+	spin_unlock(&session->fragmentid_lock);
 
 	return fragmentid;
 }
@@ -454,8 +599,6 @@ int sc_capwap_parsingpacket(struct sc_capwap_session* session, const union capwa
 			}
 		}
 
-		/* Parsing complete */
-		kfree_skb(skb);
 		return 0;
 	}
 
@@ -464,6 +607,7 @@ int sc_capwap_parsingpacket(struct sc_capwap_session* session, const union capwa
 
 /* */
 int sc_capwap_forwarddata(struct sc_capwap_session* session, uint8_t radioid, uint8_t binding, struct sk_buff* skb, uint32_t flags, struct sc_capwap_radio_addr* radioaddr, int radioaddrlength, struct sc_capwap_wireless_information* winfo, int winfolength) {
+	int err;
 	int size;
 	int length;
 	int reserve;
@@ -482,7 +626,7 @@ int sc_capwap_forwarddata(struct sc_capwap_session* session, uint8_t radioid, ui
 	reserve = sizeof(struct sc_capwap_header) + radioaddrlength + winfolength;
 	if (skb_is_nonlinear(skb) || (headroom < reserve)) {
 		printk("*** Expand socket buffer\n");
-		clone = skb_copy_expand(skb, max_t(int, headroom, reserve), skb_tailroom(skb), GFP_KERNEL);
+		clone = skb_copy_expand(skb, max(headroom, reserve), skb_tailroom(skb), GFP_KERNEL);
 		if (!clone) {
 			printk("*** Unable to expand socket buffer\n");
 			return -ENOMEM;
@@ -551,7 +695,9 @@ int sc_capwap_forwarddata(struct sc_capwap_session* session, uint8_t radioid, ui
 		}
 
 		/* Send packet */
-		if (sc_socket_send(SOCKET_UDP, (uint8_t*)header, (size + length), &session->peeraddr) < 0) {
+		err = sc_socket_send(SOCKET_UDP, (uint8_t*)header, (size + length), &session->peeraddr);
+		TRACEKMOD("*** Send packet result: %d\n", err);
+		if (err < 0) {
 			break;
 		}
 
@@ -593,18 +739,18 @@ struct sc_capwap_radio_addr* sc_capwap_setradiomacaddress(uint8_t* buffer, int s
 	radioaddr = (struct sc_capwap_radio_addr*)buffer;
 	radioaddr->length = MACADDRESS_EUI48_LENGTH;
 
-	addr = (struct sc_capwap_macaddress_eui48*)(buffer + sizeof(struct sc_capwap_radio_addr));
+	addr = (struct sc_capwap_macaddress_eui48*)radioaddr->addr;
 	memcpy(addr->addr, bssid, MACADDRESS_EUI48_LENGTH);
 
 	return radioaddr;
 }
 
 /* */
-struct sc_capwap_wireless_information* sc_capwap_setwirelessinformation(uint8_t* buffer, int size, uint8_t rssi, uint8_t snr, uint16_t rate) {
+struct sc_capwap_wireless_information* sc_capwap_setwinfo_frameinfo(uint8_t* buffer, int size, uint8_t rssi, uint8_t snr, uint16_t rate) {
 	struct sc_capwap_wireless_information* winfo;
 	struct sc_capwap_ieee80211_frame_info* frameinfo;
 
-	TRACEKMOD("### sc_capwap_setwirelessinformation\n");
+	TRACEKMOD("### sc_capwap_setwinfo_frameinfo\n");
 
 	memset(buffer, 0, size);
 
@@ -618,3 +764,21 @@ struct sc_capwap_wireless_information* sc_capwap_setwirelessinformation(uint8_t*
 
 	return winfo;
 }
+
+/* */
+struct sc_capwap_wireless_information* sc_capwap_setwinfo_destwlans(uint8_t* buffer, int size, uint16_t wlanidbitmap) {
+	struct sc_capwap_wireless_information* winfo;
+	struct sc_capwap_destination_wlans* destwlans;
+
+	TRACEKMOD("### sc_capwap_setwinfo_destwlans\n");
+
+	memset(buffer, 0, size);
+
+	winfo = (struct sc_capwap_wireless_information*)buffer;
+	winfo->length = sizeof(struct sc_capwap_destination_wlans);
+
+	destwlans = (struct sc_capwap_destination_wlans*)(buffer + sizeof(struct sc_capwap_wireless_information));
+	destwlans->wlanidbitmap = cpu_to_be16(wlanidbitmap);
+
+	return winfo;
+}

src/wtp/kmod/capwap.h

@@ -21,27 +21,25 @@
 /* */
 #define SKB_CAPWAP_FLAG_FROM_DATA_CHANNEL			0x0001
 #define SKB_CAPWAP_FLAG_FROM_USER_SPACE				0x0002
-#define SKB_CAPWAP_FLAG_FROM_IEEE80211				0x0004
+#define SKB_CAPWAP_FLAG_FROM_AC_TAP					0x0004
-
+#define SKB_CAPWAP_FLAG_FROM_IEEE80211				0x0008
-#define SKB_CAPWAP_FLAG_PEERADDRESS					0x0010
-#define SKB_CAPWAP_FLAG_RADIOID						0x0020
-#define SKB_CAPWAP_FLAG_BINDING						0x0040
-#define SKB_CAPWAP_FLAG_RADIOADDRESS				0x0080
-#define SKB_CAPWAP_FLAG_WIRELESSINFORMATION			0x0100
 
+#define SKB_CAPWAP_FLAG_SESSIONID					0x0100
+#define SKB_CAPWAP_FLAG_RADIOID						0x0200
+#define SKB_CAPWAP_FLAG_BINDING						0x0400
+#define SKB_CAPWAP_FLAG_WIRELESSINFORMATION			0x0800
 #define SKB_CAPWAP_FLAG_FRAGMENT					0x1000
 
 struct sc_skb_capwap_cb {
 	uint16_t flags;
-	struct capwap_addr_little peeraddr;
+
+	/* Session ID */
+	struct sc_capwap_sessionid_element sessionid;
 
 	/* Capwap information */
 	uint8_t radioid;
 	uint8_t binding;
 
-	/* Radio Address */
-	uint8_t radioaddr_addr[MACADDRESS_EUI48_LENGTH];
-
 	/* Wireless Information */
 	uint8_t winfo_rssi;
 	uint8_t winfo_snr;
@@ -106,16 +104,17 @@ void sc_capwap_initsession(struct sc_capwap_session* session);
 void sc_capwap_freesession(struct sc_capwap_session* session);
 uint16_t sc_capwap_newfragmentid(struct sc_capwap_session* session);
 
-void sc_capwap_sessionid_printf(const struct sc_capwap_sessionid_element* sessionid, char* string);
+int sc_capwap_8023_to_80211(struct sk_buff* skb, const uint8_t* bssid);
+int sc_capwap_80211_to_8023(struct sk_buff* skb);
 
-struct sc_capwap_packet* sc_capwap_poppacketqueue(struct sc_capwap_session* session);
+void sc_capwap_sessionid_printf(const struct sc_capwap_sessionid_element* sessionid, char* string);
-void sc_capwap_pushpacketqueue(struct sc_capwap_session* session, struct sc_capwap_packet* packet);
 
 int sc_capwap_createkeepalive(struct sc_capwap_sessionid_element* sessionid, uint8_t* buffer, int size);
 int sc_capwap_parsingpacket(struct sc_capwap_session* session, const union capwap_addr* sockaddr, struct sk_buff* skb);
 
 struct sc_capwap_radio_addr* sc_capwap_setradiomacaddress(uint8_t* buffer, int size, uint8_t* bssid);
-struct sc_capwap_wireless_information* sc_capwap_setwirelessinformation(uint8_t* buffer, int size, uint8_t rssi, uint8_t snr, uint16_t rate);
+struct sc_capwap_wireless_information* sc_capwap_setwinfo_frameinfo(uint8_t* buffer, int size, uint8_t rssi, uint8_t snr, uint16_t rate);
+struct sc_capwap_wireless_information* sc_capwap_setwinfo_destwlans(uint8_t* buffer, int size, uint16_t wlanidbitmap);
 
 int sc_capwap_forwarddata(struct sc_capwap_session* session, uint8_t radioid, uint8_t binding, struct sk_buff* skb, uint32_t flags, struct sc_capwap_radio_addr* radioaddr, int radioaddrlength, struct sc_capwap_wireless_information* winfo, int winfolength);
 

src/wtp/kmod/capwap_private.c

@@ -1,6 +1,9 @@
 #include "config.h"
 #include <linux/module.h>
 #include <linux/kthread.h>
+#include <linux/etherdevice.h>
+#include <linux/ieee80211.h>
+#include <net/mac80211.h>
 #include <net/ipv6.h>
 #include "capwap.h"
 #include "nlsmartcapwap.h"
@@ -77,6 +80,7 @@ int sc_capwap_sendkeepalive(void) {
 
 	/* Send packet */
 	ret = sc_socket_send(SOCKET_UDP, buffer, length, &sc_acsession.peeraddr);
+	TRACEKMOD("*** Send keep-alive result: %d\n", ret);
 	if (ret > 0) {
 		ret = 0;
 	}
@@ -151,8 +155,119 @@ struct sc_capwap_session* sc_capwap_recvunknownkeepalive(const union capwap_addr
 
 /* */
 void sc_capwap_parsingdatapacket(struct sc_capwap_session* session, struct sk_buff* skb) {
+	uint8_t* pos;
+	uint8_t* dstaddress;
+	struct net_device* dev;
+	struct sc_capwap_header* header = (struct sc_capwap_header*)skb->data;
+	int is80211 = (IS_FLAG_T_HEADER(header) ? 1 : 0);
+	struct sc_capwap_radio_addr* radioaddr = NULL;
+	int radioaddrsize = 0;
+	struct sc_capwap_wireless_information* winfo = NULL;
+	struct sc_capwap_destination_wlans* destwlan = NULL;
+	int winfosize = 0;
+
 	TRACEKMOD("### sc_capwap_parsingdatapacket\n");
 
+	/* Retrieve optional attribute */
+	pos = skb->data + sizeof(struct sc_capwap_header);
+	if (IS_FLAG_M_HEADER(header)) {
+		radioaddr = (struct sc_capwap_radio_addr*)pos;
+		radioaddrsize = (sizeof(struct sc_capwap_radio_addr) + radioaddr->length + 3) & ~3;
+		pos += radioaddrsize;
+	}
+
+	if (IS_FLAG_W_HEADER(header)) {
+		winfo = (struct sc_capwap_wireless_information*)pos;
+		destwlan = (struct sc_capwap_destination_wlans*)(pos + sizeof(struct sc_capwap_wireless_information));
+		winfosize = (sizeof(struct sc_capwap_wireless_information) + winfo->length + 3) & ~3;
+		pos += winfosize;
+	}
+
+	/* Body packet */
+	skb_pull(skb, GET_HLEN_HEADER(header) * 4);
+
+	dstaddress = (is80211 ? ieee80211_get_DA((struct ieee80211_hdr*)skb->data) : (uint8_t*)((struct ethhdr*)skb->data)->h_dest);
+	if (is_multicast_ether_addr(dstaddress)) {
+		/* Accept only broadcast packet with wireless information */
+		if (winfo) {
+			uint8_t wlanid = 1;
+			uint16_t bitmask = be16_to_cpu(destwlan->wlanidbitmap);
+			while (bitmask) {
+				if (bitmask & 0x01) {
+					dev = sc_netlink_getdev_from_wlanid(GET_RID_HEADER(header), wlanid);
+					if (dev) {
+						struct sk_buff* clone = skb_copy_expand(skb, skb_headroom(skb), skb_tailroom(skb), GFP_KERNEL);
+						if (!clone) {
+							goto error;
+						}
+
+						TRACEKMOD("****************\n");
+						print_hex_dump(KERN_ERR, "", DUMP_PREFIX_OFFSET, 16, 1, clone->data, sizeof(struct ethhdr), 1);
+						TRACEKMOD("++++++++++++++++\n");
+
+						/* */
+						if (!is80211) { 
+							if (sc_capwap_8023_to_80211(clone, dev->dev_addr)) {
+								kfree_skb(clone);
+								goto error;
+							}
+						}
+
+						print_hex_dump(KERN_ERR, "", DUMP_PREFIX_OFFSET, 16, 1, clone->data, sizeof(struct ieee80211_hdr), 1);
+						TRACEKMOD("****************\n");
+
+						TRACEKMOD("** Send broadcast packet to interface: %d\n", dev->ifindex);
+
+						/* Send packet */
+						local_bh_disable();
+						ieee80211_inject_xmit(clone, dev);
+						local_bh_enable();
+					} else {
+						TRACEKMOD("*** Unknown wlanid: %d\n", (int)wlanid);
+					}
+				}
+
+				/* Next */
+				wlanid++;
+				bitmask >>= 1;
+			}
+		} else {
+			TRACEKMOD("*** Invalid broadcast packet\n");
+		}
+
+		/* Free broadcast packet */
+		kfree_skb(skb);
+	} else {
+		/* Accept only 802.11 frame or 802.3 frame with radio address */
+		if (is80211 || (radioaddr && (radioaddr->length == MACADDRESS_EUI48_LENGTH))){
+			if (!is80211) { 
+				if (sc_capwap_8023_to_80211(skb, radioaddr->addr)) {
+					goto error;
+				}
+			}
+
+			/* */
+			dev = sc_netlink_getdev_from_bssid(GET_RID_HEADER(header), ((struct ieee80211_hdr*)skb->data)->addr2);
+			if (!dev) {
+				goto error;
+			}
+
+			TRACEKMOD("** Send packet to interface: %d\n", dev->ifindex);
+
+			/* Send packet */
+			local_bh_disable();
+			ieee80211_inject_xmit(skb, dev);
+			local_bh_enable();
+		} else {
+			goto error;
+		}
+	}
+
+	return;
+
+error:
+	TRACEKMOD("*** Invalid packet\n");
+	kfree_skb(skb);
 }
 
 /* */

src/wtp/kmod/capwap_rfc.h

@@ -79,6 +79,7 @@ struct sc_capwap_header {
 #define CAPWAP_RADIO_MAX_LENGTH_PADDED				12
 struct sc_capwap_radio_addr {
 	uint8_t length;
+	uint8_t addr[0];
 } __packed;
 
 /* Wireless Information */
@@ -96,6 +97,12 @@ struct sc_capwap_ieee80211_frame_info {
 	__be16 rate;
 } __packed;
 
+/* Destination WLANs */
+struct sc_capwap_destination_wlans {
+	__be16 wlanidbitmap;
+	__be16 reserved;
+} __packed;
+
 /* */
 #define CAPWAP_HEADER_MAX_LENGTH					(sizeof(struct sc_capwap_header) + CAPWAP_RADIO_MAX_LENGTH_PADDED + CAPWAP_WINFO_MAX_LENGTH_PADDED)
 
@@ -112,7 +119,10 @@ struct sc_capwap_message_element {
 
 /* Session id message element */
 struct sc_capwap_sessionid_element {
-	uint8_t id[16];
+	union {
+		uint8_t id[16];
+		uint32_t id32[4];
+	};
 } __packed;
 
 /* */
@@ -169,4 +179,9 @@ struct sc_capwap_macaddress_eui64 {
 #define IS_FLAG_K_HEADER(x)					((x)->flag_k)
 #define SET_FLAG_K_HEADER(x, y)				((x)->flag_k = ((y) ? 1 : 0))
 
+/* IEEE 802.11 Add WLAN */
+#define CAPWAP_ADD_WLAN_TUNNELMODE_LOCAL			0
+#define CAPWAP_ADD_WLAN_TUNNELMODE_8023				1
+#define CAPWAP_ADD_WLAN_TUNNELMODE_80211			2
+
 #endif /* __KMOD_CAPWAP_RFC_HEADER__ */

src/wtp/kmod/netlinkapp.c

@@ -68,8 +68,8 @@ static int sc_netlink_handler(uint32_t ifindex, struct sk_buff* skb, int sig_dbm
 	if (ieee80211_is_data(hdr->frame_control)) {
 		int err;
 		struct sc_capwap_session* session;
-		unsigned char radioaddrbuffer[CAPWAP_RADIO_EUI48_LENGTH_PADDED];
+		uint8_t radioaddrbuffer[CAPWAP_RADIO_EUI48_LENGTH_PADDED];
-		unsigned char winfobuffer[CAPWAP_WINFO_FRAMEINFO_LENGTH_PADDED];
+		uint8_t winfobuffer[CAPWAP_WINFO_FRAMEINFO_LENGTH_PADDED];
 		struct sc_capwap_radio_addr* radioaddr = NULL;
 		struct sc_capwap_wireless_information* winfo = NULL;
 
@@ -97,7 +97,7 @@ static int sc_netlink_handler(uint32_t ifindex, struct sk_buff* skb, int sig_dbm
 
 		/* Create Wireless Information */
 		if (sig_dbm || rate) {
-			winfo = sc_capwap_setwirelessinformation(winfobuffer, CAPWAP_WINFO_FRAMEINFO_LENGTH_PADDED, (uint8_t)sig_dbm, 0, ((uint16_t)rate) * 5);
+			winfo = sc_capwap_setwinfo_frameinfo(winfobuffer, CAPWAP_WINFO_FRAMEINFO_LENGTH_PADDED, (uint8_t)sig_dbm, 0, ((uint16_t)rate) * 5);
 		}
 
 		/* */
@@ -455,7 +455,7 @@ static int sc_netlink_send_data(struct sk_buff* skb, struct genl_info* info) {
 
 	/* Create Wireless Information */
 	if (rssi || snr || rate) {
-		winfo = sc_capwap_setwirelessinformation(winfobuffer, CAPWAP_WINFO_FRAMEINFO_LENGTH_PADDED, rssi, snr, rate);
+		winfo = sc_capwap_setwinfo_frameinfo(winfobuffer, CAPWAP_WINFO_FRAMEINFO_LENGTH_PADDED, rssi, snr, rate);
 	}
 
 	/* Create socket buffer */
@@ -728,6 +728,38 @@ error:
 	return -ENOMEM;
 }
 
+/* */
+struct net_device* sc_netlink_getdev_from_wlanid(uint8_t radioid, uint8_t wlanid) {
+	struct sc_netlink_device* nldev;
+
+	TRACEKMOD("### sc_netlink_getdev_from_wlanid\n");
+
+	/* Search */
+	list_for_each_entry(nldev, &sc_netlink_dev_list, list) {
+		if ((nldev->radioid == radioid) && (nldev->wlanid == wlanid)) {
+			return nldev->dev;
+		}
+	}
+
+	return NULL;
+}
+
+/* */
+struct net_device* sc_netlink_getdev_from_bssid(uint8_t radioid, const uint8_t* addr) {
+	struct sc_netlink_device* nldev;
+
+	TRACEKMOD("### sc_netlink_getdev_from_bssid\n");
+
+	/* Search */
+	list_for_each_entry(nldev, &sc_netlink_dev_list, list) {
+		if ((nldev->radioid == radioid) && !memcmp(nldev->dev->dev_addr, addr, MACADDRESS_EUI48_LENGTH)) {
+			return nldev->dev;
+		}
+	}
+
+	return NULL;
+}
+
 /* */
 int sc_netlink_init(void) {
 	int ret;

src/wtp/kmod/netlinkapp.h

@@ -8,6 +8,10 @@
 int sc_netlink_init(void);
 void sc_netlink_exit(void);
 
+/* */
+struct net_device* sc_netlink_getdev_from_wlanid(uint8_t radioid, uint8_t wlanid);
+struct net_device* sc_netlink_getdev_from_bssid(uint8_t radioid, const uint8_t* addr);
+
 /* */
 int sc_netlink_notify_recv_keepalive(const union capwap_addr* sockaddr, struct sc_capwap_sessionid_element* sessionid);
 int sc_netlink_notify_recv_data(uint8_t* packet, int length);

src/wtp/kmod/socket.c

@@ -225,29 +225,3 @@ int sc_addr_compare(const union capwap_addr* addr1, const union capwap_addr* add
 
 	return -1;
 }
-
-/* */
-void sc_addr_tolittle(const union capwap_addr* addr, struct capwap_addr_little* little) {
-	little->family = (uint8_t)addr->ss.ss_family;
-	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 (addr->ss.ss_family == AF_INET6) {
-		memcpy(&little->addr6, &addr->sin6.sin6_addr, sizeof(struct in6_addr));
-		little->port = addr->sin6.sin6_port;
-	}
-}
-
-/* */
-void sc_addr_fromlittle(const struct capwap_addr_little* little, union capwap_addr* addr) {
-	memset(addr, 0, sizeof(union capwap_addr));
-
-	addr->ss.ss_family = little->family;
-	if (little->family == AF_INET) {
-		memcpy(&addr->sin.sin_addr, &little->addr4, sizeof(struct in_addr));
-		addr->sin.sin_port = little->port;
-	} else if (little->family == AF_INET6) {
-		memcpy(&addr->sin6.sin6_addr, &little->addr6, sizeof(struct in6_addr));
-		addr->sin6.sin6_port = little->port;
-	}
-}

src/wtp/kmod/socket.h

@@ -10,16 +10,6 @@
 #define SOCKET_UDP					0
 #define SOCKET_UDPLITE				1
 
-/* Little socket address */
-struct capwap_addr_little {
-	uint8_t family;
-	union {
-		struct in_addr addr4;
-		struct in6_addr addr6;
-	};
-	uint16_t port;
-};
-
 /* Universal socket address */
 union capwap_addr {
 	struct sockaddr sa;
@@ -35,11 +25,11 @@ void sc_socket_close(void);
 /* */
 int sc_socket_bind(union capwap_addr* sockaddr);
 int sc_socket_send(int type, uint8_t* buffer, int length, union capwap_addr* sockaddr);
+
+/* */
 int sc_socket_getpeeraddr(struct sk_buff* skb, union capwap_addr* peeraddr);
 
 /* */
 int sc_addr_compare(const union capwap_addr* addr1, const union capwap_addr* addr2);
-void sc_addr_tolittle(const union capwap_addr* addr, struct capwap_addr_little* little);
-void sc_addr_fromlittle(const struct capwap_addr_little* little, union capwap_addr* addr);
 
 #endif /* __KMOD_SOCKET_HEADER__ */