impelment station binding to wlanid

* add a kernel bash hash list to track station to
  radio and wlan id binding
* enforce binding on recv
* configure binding through netlink interface from
  WTP process

src/wtp/kmod/capwap.c

@@ -37,8 +37,10 @@ static void sc_capwap_fragment_free(struct sc_capwap_fragment* fragment)
 /* */
 static void sc_capwap_freesession(struct sc_capwap_session* session)
 {
+	int i;
 	struct sc_capwap_fragment* temp;
 	struct sc_capwap_fragment* fragment;
+	struct sc_station *sta;
 
 	TRACEKMOD("### sc_capwap_freesession\n");
 
@@ -46,6 +48,13 @@ static void sc_capwap_freesession(struct sc_capwap_session* session)
 	list_for_each_entry_safe(fragment, temp, &session->fragments.lru_list, lru_list) {
 		sc_capwap_fragment_free(fragment);
 	}
+
+	for (i = 0; i < STA_HASH_SIZE; i++) {
+		hlist_for_each_entry_rcu(sta, &session->station_list[i], station_list) {
+			hlist_del_rcu(&sta->station_list);
+			kfree_rcu(sta, rcu_head);
+		}
+	}
 }
 
 /* */
@@ -410,6 +419,8 @@ int sc_capwap_bind(struct sc_capwap_session *session, int protocol, struct socka
 	if (ret < 0)
 		goto err_close;
 
+	rcu_assign_sk_user_data(session->socket->sk, session);
+
 	/* Set callback */
 	udp_sk(session->socket->sk)->encap_type = 1;
 	udp_sk(session->socket->sk)->encap_rcv = sc_capwap_recvpacket;

src/wtp/kmod/capwap.h

@@ -8,6 +8,9 @@
 
 #include "capwap_rfc.h"
 
+/* */
+#define STA_HASH_SIZE       16
+
 /* */
 #define MAX_MTU						9000
 #define DEFAULT_MTU 				1450
@@ -42,6 +45,16 @@ union capwap_addr {
 	struct sockaddr_storage ss;
 };
 
+struct sc_station {
+	struct hlist_node station_list;
+
+	uint8_t radioid;
+	uint8_t mac[ETH_ALEN];
+	uint8_t wlanid;
+
+	struct rcu_head rcu_head;
+};
+
 struct sc_skb_capwap_cb {
 	uint16_t flags;
 
@@ -96,8 +109,9 @@ struct sc_capwap_session {
 	struct sc_capwap_sessionid_element sessionid;
 
 	atomic_t fragmentid;
-
 	struct sc_capwap_fragment_queue fragments;
+
+	struct hlist_head station_list[STA_HASH_SIZE];
 };
 
 /* Dipendent implementation function */

src/wtp/kmod/capwap_private.c

@@ -16,8 +16,12 @@
 /* */
 int sc_capwap_init(struct sc_capwap_session *session, struct net *net)
 {
+	int i;
+
 	TRACEKMOD("### sc_capwap_init\n");
 
+	ASSERT_RTNL();
+
 	/* Init session */
 	memset(session, 0, sizeof(struct sc_capwap_session));
 
@@ -28,6 +32,9 @@ int sc_capwap_init(struct sc_capwap_session *session, struct net *net)
 	INIT_LIST_HEAD(&session->fragments.lru_list);
 	spin_lock_init(&session->fragments.lock);
 
+	for (i = 0; i < STA_HASH_SIZE; i++)
+		INIT_HLIST_HEAD(&session->station_list[i]);
+
 	return 0;
 }
 
@@ -225,29 +232,50 @@ void sc_capwap_parsingdatapacket(struct sc_capwap_session* session, struct sk_bu
 		/* Free broadcast packet */
 		kfree_skb(skb);
 	} else {
-		/* Accept only 802.11 frame or 802.3 frame with radio address */
+		uint32_t hash;
-		if (is80211 || (radioaddr && (radioaddr->length == MACADDRESS_EUI48_LENGTH))){
+		struct hlist_head *sta_head;
-			if (!is80211) { 
+		struct sc_station *sta;
-				if (sc_capwap_8023_to_80211(skb, radioaddr->addr)) {
-					goto error;
-				}
-			}
 
-			/* */
+		hash = jhash(dstaddress, ETH_ALEN, GET_RID_HEADER(header)) % STA_HASH_SIZE;
-			dev = sc_netlink_getdev_from_bssid(session->net, GET_RID_HEADER(header), ((struct ieee80211_hdr*)skb->data)->addr2);
+		sta_head = &session->station_list[hash];
-			if (!dev) {
-				goto error;
-			}
 
-			TRACEKMOD("** Send packet to interface: %d\n", dev->ifindex);
+		rcu_read_lock();
 
-			/* Send packet */
+		sta = sc_find_station(sta_head, GET_RID_HEADER(header), dstaddress);
-			local_bh_disable();
+		if (!sta) {
-			ieee80211_inject_xmit(skb, dev);
+			rcu_read_unlock();
-			local_bh_enable();
+			TRACEKMOD("*** Radio Id for STA invalid: %d, %pM\n",
-		} else {
+				  GET_RID_HEADER(header), dstaddress);
 			goto error;
 		}
+
+		dev = sc_netlink_getdev_from_wlanid(session->net, GET_RID_HEADER(header), sta->wlanid);
+		if (!dev) {
+			TRACEKMOD("*** no interface for Radio Id/WLAN Id: %d, %d\n",
+				  GET_RID_HEADER(header), sta->wlanid);
+				rcu_read_unlock();
+				goto error;
+		}
+
+		rcu_read_unlock();
+
+		if (!is80211) {
+			if (sc_capwap_8023_to_80211(skb, dev->dev_addr)) {
+				goto error;
+			}
+		} else {
+			if (memcmp(dev->dev_addr, ((struct ieee80211_hdr*)skb->data)->addr2, ETH_ALEN) != 0) {
+				TRACEKMOD("*** Invalid BSSID in 802.11 packet\n");
+				goto error;
+			}
+		}
+
+		TRACEKMOD("** Send packet to interface: %d\n", dev->ifindex);
+
+		/* Send packet */
+		local_bh_disable();
+		ieee80211_inject_xmit(skb, dev);
+		local_bh_enable();
 	}
 
 	return;

src/wtp/kmod/capwap_private.h

@@ -9,6 +9,9 @@ struct sc_capwap_workthread {
 	wait_queue_head_t waitevent;
 };
 
+/* */
+struct sc_station *sc_find_station(struct hlist_head *sta_head, uint8_t radioid, uint8_t *mac);
+
 /* */
 int sc_capwap_init(struct sc_capwap_session *sc_acsession, struct net *net);
 

src/wtp/kmod/netlinkapp.c

@@ -7,6 +7,7 @@
 #include <linux/rcupdate.h>
 #include <linux/err.h>
 #include <linux/ieee80211.h>
+#include <linux/jhash.h>
 
 #include <net/net_namespace.h>
 #include <net/genetlink.h>
@@ -594,6 +595,104 @@ static int sc_netlink_leave_mac80211_device(struct sk_buff* skb, struct genl_inf
 	return sc_netlink_unregister_device(sn, nla_get_u32(info->attrs[NLSMARTCAPWAP_ATTR_IFINDEX]));
 }
 
+/* */
+struct sc_station *sc_find_station(struct hlist_head *sta_head, uint8_t radioid, uint8_t *mac)
+{
+	struct sc_station *sta;
+
+	hlist_for_each_entry_rcu(sta, sta_head, station_list) {
+		if (sta->radioid == radioid &&
+		    memcmp(&sta->mac, mac, ETH_ALEN) == 0)
+			return sta;
+	}
+
+	return NULL;
+}
+
+/* */
+static int sc_netlink_add_station(struct sk_buff* skb, struct genl_info* info)
+{
+	struct net *net = genl_info_net(info);
+	struct sc_net *sn = net_generic(net, sc_net_id);
+	struct sc_capwap_session *session = &sn->sc_acsession;
+	struct sc_station *sta;
+	uint8_t radioid;
+	uint8_t *mac;
+	uint32_t hash;
+	struct hlist_head *sta_head;
+
+	TRACEKMOD("### sc_netlink_add_station\n");
+
+	/* Check Link */
+	if (!sn->sc_netlink_usermodeid)
+		return -ENOLINK;
+
+	if (!info->attrs[NLSMARTCAPWAP_ATTR_RADIOID] ||
+	    !info->attrs[NLSMARTCAPWAP_ATTR_MAC] ||
+	    !info->attrs[NLSMARTCAPWAP_ATTR_WLANID])
+		return -EINVAL;
+
+	radioid = nla_get_u8(info->attrs[NLSMARTCAPWAP_ATTR_RADIOID]);
+	mac = nla_data(info->attrs[NLSMARTCAPWAP_ATTR_MAC]);
+	hash = jhash(mac, ETH_ALEN, radioid) % STA_HASH_SIZE;
+	sta_head = &session->station_list[hash];
+
+	if (sc_find_station(sta_head, radioid, mac) != NULL)
+		return -EEXIST;
+
+	if (info->nlhdr->nlmsg_flags & NLM_F_REPLACE)
+		return -ENXIO;
+
+	sta = kmalloc(sizeof(struct sc_station), GFP_KERNEL);
+	if (sta == NULL)
+		return -ENOMEM;
+
+	sta->radioid = radioid;
+	memcpy(&sta->mac, mac, ETH_ALEN);
+	sta->wlanid = nla_get_u8(info->attrs[NLSMARTCAPWAP_ATTR_WLANID]);
+
+	hlist_add_head_rcu(&sta->station_list, sta_head);
+
+	return 0;
+}
+
+/* */
+static int sc_netlink_del_station(struct sk_buff* skb, struct genl_info* info)
+{
+	struct net *net = genl_info_net(info);
+	struct sc_net *sn = net_generic(net, sc_net_id);
+	struct sc_capwap_session *session = &sn->sc_acsession;
+	uint8_t radioid;
+	uint8_t *mac;
+	uint32_t hash;
+	struct hlist_head *sta_head;
+	struct sc_station *sta;
+
+	TRACEKMOD("### sc_netlink_del_station\n");
+
+	/* Check Link */
+	if (!sn->sc_netlink_usermodeid)
+		return -ENOLINK;
+
+	if (!info->attrs[NLSMARTCAPWAP_ATTR_RADIOID] ||
+	    !info->attrs[NLSMARTCAPWAP_ATTR_MAC])
+		return -EINVAL;
+
+	radioid = nla_get_u8(info->attrs[NLSMARTCAPWAP_ATTR_RADIOID]);
+	mac = nla_data(info->attrs[NLSMARTCAPWAP_ATTR_MAC]);
+	hash = jhash(mac, ETH_ALEN, radioid) % STA_HASH_SIZE;
+	sta_head = &session->station_list[hash];
+
+	sta = sc_find_station(sta_head, radioid, mac);
+	if (!sta)
+		return -ENOENT;
+
+	hlist_del_rcu(&sta->station_list);
+	kfree_rcu(sta, rcu_head);
+
+	return 0;
+}
+
 /* */
 static int sc_device_event(struct notifier_block* unused,
 			   unsigned long event,
@@ -637,7 +736,7 @@ static const struct nla_policy sc_netlink_policy[NLSMARTCAPWAP_ATTR_MAX + 1] = {
 	[NLSMARTCAPWAP_ATTR_RSSI] = { .type = NLA_U8 },
 	[NLSMARTCAPWAP_ATTR_SNR] = { .type = NLA_U8 },
 	[NLSMARTCAPWAP_ATTR_RATE] = { .type = NLA_U16 },
-
+	[NLSMARTCAPWAP_ATTR_MAC] = { .len = ETH_ALEN },
 };
 
 /* Netlink Ops */
@@ -690,6 +789,18 @@ static const struct genl_ops sc_netlink_ops[] = {
 		.policy = sc_netlink_policy,
 		.flags = GENL_ADMIN_PERM,
 	},
+	{
+		.cmd = NLSMARTCAPWAP_CMD_ADD_STATION,
+		.doit = sc_netlink_add_station,
+		.policy = sc_netlink_policy,
+		.flags = GENL_ADMIN_PERM,
+	},
+	{
+		.cmd = NLSMARTCAPWAP_CMD_DEL_STATION,
+		.doit = sc_netlink_del_station,
+		.policy = sc_netlink_policy,
+		.flags = GENL_ADMIN_PERM,
+	},
 };
 
 /* Netlink notify */
@@ -774,7 +885,7 @@ struct net_device* sc_netlink_getdev_from_wlanid(struct net *net,
 						 uint8_t wlanid)
 {
 	struct sc_net *sn = net_generic(net, sc_net_id);
-	struct sc_netlink_device* nldev;
+	struct sc_netlink_device *nldev;
 
 	TRACEKMOD("### sc_netlink_getdev_from_wlanid\n");
 
@@ -791,29 +902,6 @@ struct net_device* sc_netlink_getdev_from_wlanid(struct net *net,
 	return NULL;
 }
 
-/* */
-struct net_device* sc_netlink_getdev_from_bssid(struct net *net,
-						uint8_t radioid,
-						const uint8_t* addr)
-{
-	struct sc_net *sn = net_generic(net, sc_net_id);
-	struct sc_netlink_device* nldev;
-
-	TRACEKMOD("### sc_netlink_getdev_from_bssid\n");
-
-	/* Search */
-	rcu_read_lock();
-	list_for_each_entry_rcu(nldev, &sn->sc_netlink_dev_list, list) {
-		if ((nldev->radioid == radioid) && !memcmp(nldev->dev->dev_addr, addr, MACADDRESS_EUI48_LENGTH)) {
-			rcu_read_unlock();
-			return nldev->dev;
-		}
-	}
-	rcu_read_unlock();
-
-	return NULL;
-}
-
 static int __net_init sc_net_init(struct net *net)
 {
         struct sc_net *sn = net_generic(net, sc_net_id);

src/wtp/kmod/netlinkapp.h

@@ -10,7 +10,6 @@ void sc_netlink_exit(void);
 
 /* */
 struct net_device* sc_netlink_getdev_from_wlanid(struct net *net, uint8_t radioid, uint8_t wlanid);
-struct net_device* sc_netlink_getdev_from_bssid(struct net *net, uint8_t radioid, const uint8_t* addr);
 
 /* */
 int sc_netlink_notify_recv_keepalive(struct net *net,

src/wtp/kmod/nlsmartcapwap.h

@@ -34,6 +34,8 @@ enum nlsmartcapwap_attrs {
 	NLSMARTCAPWAP_ATTR_SNR,
 	NLSMARTCAPWAP_ATTR_RATE,
 
+	NLSMARTCAPWAP_ATTR_MAC,
+
 	/* Last attribute */
 	__NLSMARTCAPWAP_ATTR_AFTER_LAST,
 	NLSMARTCAPWAP_ATTR_MAX = __NLSMARTCAPWAP_ATTR_AFTER_LAST - 1
@@ -58,6 +60,9 @@ enum nlsmartcapwap_commands {
 	NLSMARTCAPWAP_CMD_JOIN_MAC80211_DEVICE,
 	NLSMARTCAPWAP_CMD_LEAVE_MAC80211_DEVICE,
 
+	NLSMARTCAPWAP_CMD_ADD_STATION,
+	NLSMARTCAPWAP_CMD_DEL_STATION,
+
 	/* Last command */
 	__NLSMARTCAPWAP_CMD_AFTER_LAST,
 	NLSMARTCAPWAP_CMD_MAX = __NLSMARTCAPWAP_CMD_AFTER_LAST - 1

src/wtp/wtp_kmod.c

@@ -464,6 +464,63 @@ int wtp_kmod_leave_mac80211_device(struct wifi_wlan* wlan) {
 	return result;
 }
 
+/* */
+int wtp_kmod_add_station(uint8_t radioid, const uint8_t *mac, uint8_t wlanid)
+{
+	int result;
+	struct nl_msg* msg;
+
+	/* */
+	if (!wtp_kmod_isconnected())
+		return -1;
+
+	/* */
+	msg = nlmsg_alloc();
+	if (!msg)
+		return -1;
+
+	/* */
+	genlmsg_put(msg, 0, 0, g_wtp.kmodhandle.nlsmartcapwap_id, 0, 0, NLSMARTCAPWAP_CMD_ADD_STATION, 0);
+	nla_put_u8(msg, NLSMARTCAPWAP_ATTR_RADIOID, radioid);
+	nla_put(msg, NLSMARTCAPWAP_ATTR_MAC, ETH_ALEN, mac);
+	nla_put_u8(msg, NLSMARTCAPWAP_ATTR_WLANID, wlanid);
+
+	/* */
+	result = wtp_kmod_send_and_recv_msg(msg, NULL, NULL);
+
+	/* */
+	nlmsg_free(msg);
+	return result;
+}
+
+/* */
+int wtp_kmod_del_station(uint8_t radioid, const uint8_t *mac)
+{
+	int result;
+	struct nl_msg* msg;
+
+	/* */
+	if (!wtp_kmod_isconnected())
+		return -1;
+
+	/* */
+	msg = nlmsg_alloc();
+	if (!msg)
+		return -1;
+
+	/* */
+	genlmsg_put(msg, 0, 0, g_wtp.kmodhandle.nlsmartcapwap_id, 0, 0, NLSMARTCAPWAP_CMD_DEL_STATION, 0);
+	nla_put_u8(msg, NLSMARTCAPWAP_ATTR_RADIOID, radioid);
+	nla_put(msg, NLSMARTCAPWAP_ATTR_MAC, ETH_ALEN, mac);
+
+	/* */
+	result = wtp_kmod_send_and_recv_msg(msg, NULL, NULL);
+
+	/* */
+	nlmsg_free(msg);
+	return result;
+}
+
 /* */
 int wtp_kmod_isconnected(void) {
 	return (g_wtp.kmodhandle.nlsmartcapwap_id ? 1 : 0);

src/wtp/wtp_kmod.h

@@ -62,4 +62,8 @@ int wtp_kmod_send_data(uint8_t radioid, const uint8_t* frame, int length, uint8_
 int wtp_kmod_join_mac80211_device(struct wifi_wlan* wlan, uint32_t flags);
 int wtp_kmod_leave_mac80211_device(struct wifi_wlan* wlan);
 
+/* */
+int wtp_kmod_add_station(uint8_t radioid, const uint8_t *mac, uint8_t wlanid);
+int wtp_kmod_del_station(uint8_t radioid, const uint8_t *mac);
+
 #endif /* __WTP_KMOD_HEADER__ */

src/wtp/wtp_radio.c

@@ -732,6 +732,7 @@ uint32_t wtp_radio_add_station(struct capwap_parsed_packet* packet) {
 	struct wtp_radio* radio;
 	struct wtp_radio_wlan* wlan;
 	struct station_add_params stationparams;
+	int err;
 
 	/* Get message elements */
 	addstation = (struct capwap_addstation_element*)capwap_get_message_element_data(packet, CAPWAP_ELEMENT_ADDSTATION);
@@ -759,7 +760,14 @@ uint32_t wtp_radio_add_station(struct capwap_parsed_packet* packet) {
 	memset(&stationparams, 0, sizeof(struct station_add_params));
 	stationparams.address = station80211->address;
 
+	err = wtp_kmod_add_station(addstation->radioid, station80211->address, station80211->wlanid);
+	if (err < 0) {
+		capwap_logging_debug("add_station: CAPWAP add_station failed with: %d", err);
+		return CAPWAP_RESULTCODE_FAILURE;
+	}
+
 	if (wifi_station_authorize(wlan->wlanhandle, &stationparams)) {
+		wtp_kmod_del_station(addstation->radioid, station80211->address);
 		capwap_logging_debug("add_station: station_authorize failed");
 		return CAPWAP_RESULTCODE_FAILURE;
 	}
@@ -784,6 +792,7 @@ uint32_t wtp_radio_delete_station(struct capwap_parsed_packet* packet) {
 
 	/* */
 	wifi_station_deauthorize(radio->devicehandle, deletestation->address);
+	wtp_kmod_del_station(deletestation->radioid, deletestation->address);
 
 	return CAPWAP_RESULTCODE_SUCCESS;
 }