diff --git a/src/wtp/kmod/capwap.c b/src/wtp/kmod/capwap.c index 19ff817..22a5d8c 100644 --- a/src/wtp/kmod/capwap.c +++ b/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; diff --git a/src/wtp/kmod/capwap.h b/src/wtp/kmod/capwap.h index a2ebbb1..ee68acb 100644 --- a/src/wtp/kmod/capwap.h +++ b/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 */ diff --git a/src/wtp/kmod/capwap_private.c b/src/wtp/kmod/capwap_private.c index b1a567a..2552c4c 100644 --- a/src/wtp/kmod/capwap_private.c +++ b/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 */ - if (is80211 || (radioaddr && (radioaddr->length == MACADDRESS_EUI48_LENGTH))){ - if (!is80211) { - if (sc_capwap_8023_to_80211(skb, radioaddr->addr)) { - goto error; - } - } + uint32_t hash; + struct hlist_head *sta_head; + struct sc_station *sta; - /* */ - dev = sc_netlink_getdev_from_bssid(session->net, GET_RID_HEADER(header), ((struct ieee80211_hdr*)skb->data)->addr2); - if (!dev) { - goto error; - } + hash = jhash(dstaddress, ETH_ALEN, GET_RID_HEADER(header)) % STA_HASH_SIZE; + sta_head = &session->station_list[hash]; - TRACEKMOD("** Send packet to interface: %d\n", dev->ifindex); + rcu_read_lock(); - /* Send packet */ - local_bh_disable(); - ieee80211_inject_xmit(skb, dev); - local_bh_enable(); - } else { + sta = sc_find_station(sta_head, GET_RID_HEADER(header), dstaddress); + if (!sta) { + rcu_read_unlock(); + TRACEKMOD("*** Radio Id for STA invalid: %d, %pM\n", + 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; diff --git a/src/wtp/kmod/capwap_private.h b/src/wtp/kmod/capwap_private.h index a42d64c..08ace4e 100644 --- a/src/wtp/kmod/capwap_private.h +++ b/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); diff --git a/src/wtp/kmod/netlinkapp.c b/src/wtp/kmod/netlinkapp.c index 1d4637e..9a75ad3 100644 --- a/src/wtp/kmod/netlinkapp.c +++ b/src/wtp/kmod/netlinkapp.c @@ -7,6 +7,7 @@ #include #include #include +#include #include #include @@ -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); diff --git a/src/wtp/kmod/netlinkapp.h b/src/wtp/kmod/netlinkapp.h index 8de341f..013ce0e 100644 --- a/src/wtp/kmod/netlinkapp.h +++ b/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, diff --git a/src/wtp/kmod/nlsmartcapwap.h b/src/wtp/kmod/nlsmartcapwap.h index decd22f..5b13e48 100644 --- a/src/wtp/kmod/nlsmartcapwap.h +++ b/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 diff --git a/src/wtp/wtp_kmod.c b/src/wtp/wtp_kmod.c index c59d88f..e5f18fa 100644 --- a/src/wtp/wtp_kmod.c +++ b/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); diff --git a/src/wtp/wtp_kmod.h b/src/wtp/wtp_kmod.h index 17d6ef3..c279d65 100644 --- a/src/wtp/wtp_kmod.h +++ b/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__ */ diff --git a/src/wtp/wtp_radio.c b/src/wtp/wtp_radio.c index 74026b5..92f5d48 100644 --- a/src/wtp/wtp_radio.c +++ b/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; }