From 67daa8dcbb0ca4de5714c5b64ffe102ea982b993 Mon Sep 17 00:00:00 2001 From: vemax78 Date: Sat, 7 Jun 2014 22:37:19 +0200 Subject: [PATCH] Permit to forward IEEE802.11 Data Packet from kernel space to user space bypass kernel network stack. --- .../920-mac80211_packet_tunnel.patch | 58 +++++--- src/common/capwap_error.h | 3 +- src/common/capwap_network.c | 13 +- src/kmod/netlinkapp.c | 101 +++++++++---- src/kmod/nlsmartcapwap.h | 13 ++ src/wtp/wtp.c | 49 +++---- src/wtp/wtp.h | 16 ++- src/wtp/wtp_dfa.c | 135 ++++++++++++++++-- src/wtp/wtp_dfa.h | 3 + src/wtp/wtp_kmod.c | 98 ++++++++----- src/wtp/wtp_kmod.h | 25 ++++ src/wtp/wtp_radio.c | 44 +----- src/wtp/wtp_radio.h | 1 - 13 files changed, 385 insertions(+), 174 deletions(-) diff --git a/openwrt/mac80211_patchs/920-mac80211_packet_tunnel.patch b/openwrt/mac80211_patchs/920-mac80211_packet_tunnel.patch index 933a67a..325aaa8 100644 --- a/openwrt/mac80211_patchs/920-mac80211_packet_tunnel.patch +++ b/openwrt/mac80211_patchs/920-mac80211_packet_tunnel.patch @@ -1,5 +1,5 @@ --- a/include/net/mac80211.h 2014-06-02 11:48:37.000000000 +0200 -+++ b/include/net/mac80211.h 2014-06-04 21:02:25.000000000 +0200 ++++ b/include/net/mac80211.h 2014-06-07 17:23:03.000000000 +0200 @@ -4699,4 +4699,24 @@ int ieee80211_parse_p2p_noa(const struct */ void ieee80211_update_p2p_noa(struct ieee80211_noa_data *data, u32 tsf); @@ -10,7 +10,7 @@ +struct ieee80211_pcktunnel { + u16 subtype_mask[3]; /* 0: MGMT, 1: CTLR, 2: DATA */ + -+ int (*handler)(struct sk_buff *skb, int sig_dbm, unsigned char rate, void *data); ++ int (*handler)(u32 ifindex, struct sk_buff *skb, int sig_dbm, unsigned char rate, void *data); + void *data; +}; + @@ -26,8 +26,16 @@ + #endif /* MAC80211_H */ --- a/net/mac80211/ieee80211_i.h 2014-06-02 11:48:37.000000000 +0200 -+++ b/net/mac80211/ieee80211_i.h 2014-06-02 14:48:23.000000000 +0200 -@@ -727,6 +727,9 @@ struct ieee80211_sub_if_data { ++++ b/net/mac80211/ieee80211_i.h 2014-06-07 17:14:25.000000000 +0200 +@@ -165,6 +165,7 @@ typedef unsigned __bitwise__ ieee80211_r + #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) ++#define RX_IGNORE_MONITOR ((__force ieee80211_rx_result) 4u) + + /** + * enum ieee80211_packet_rx_flags - packet RX flags +@@ -727,6 +728,9 @@ struct ieee80211_sub_if_data { char name[IFNAMSIZ]; @@ -38,7 +46,7 @@ struct ieee80211_fragment_entry fragments[IEEE80211_FRAGMENT_MAX]; unsigned int fragment_next; --- a/net/mac80211/iface.c 2014-06-02 11:48:37.000000000 +0200 -+++ b/net/mac80211/iface.c 2014-06-04 22:12:23.000000000 +0200 ++++ b/net/mac80211/iface.c 2014-06-04 22:52:06.000000000 +0200 @@ -1850,3 +1850,80 @@ void ieee80211_iface_exit(void) { unregister_netdevice_notifier(&mac80211_netdev_notifier); @@ -121,7 +129,7 @@ +EXPORT_SYMBOL(ieee80211_pcktunnel_deregister); + --- a/net/mac80211/rx.c 2014-06-02 11:48:37.000000000 +0200 -+++ b/net/mac80211/rx.c 2014-06-04 22:47:51.707004555 +0200 ++++ b/net/mac80211/rx.c 2014-06-07 17:22:05.000000000 +0200 @@ -2828,6 +2828,51 @@ ieee80211_rx_h_mgmt(struct ieee80211_rx_ return RX_QUEUED; } @@ -160,8 +168,8 @@ + + /* Delegate packet to external handler */ + if (handler->subtype_mask[fc_type] & bitmask) { -+ if (handler->handler(rx->skb, sig_dbm, pckrate, handler->data)) { -+ return RX_DROP_MONITOR; ++ if (handler->handler(rx->sdata->dev->ifindex, rx->skb, sig_dbm, pckrate, handler->data)) { ++ return RX_IGNORE_MONITOR; + } + } + } @@ -174,7 +182,15 @@ /* TODO: use IEEE80211_RX_FRAGMENTED */ static void ieee80211_rx_cooked_monitor(struct ieee80211_rx_data *rx, struct ieee80211_rate *rate) -@@ -2935,7 +2980,8 @@ static void ieee80211_rx_handlers_result +@@ -2907,6 +2952,7 @@ static void ieee80211_rx_handlers_result + if (rx->sta) + rx->sta->rx_dropped++; + /* fall through */ ++ case RX_IGNORE_MONITOR: + case RX_CONTINUE: { + struct ieee80211_rate *rate = NULL; + struct ieee80211_supported_band *sband; +@@ -2935,7 +2981,8 @@ static void ieee80211_rx_handlers_result } static void ieee80211_rx_handlers(struct ieee80211_rx_data *rx, @@ -184,7 +200,7 @@ { ieee80211_rx_result res = RX_DROP_MONITOR; struct sk_buff *skb; -@@ -2968,6 +3014,11 @@ static void ieee80211_rx_handlers(struct +@@ -2968,6 +3015,11 @@ static void ieee80211_rx_handlers(struct if (ieee80211_vif_is_mesh(&rx->sdata->vif)) CALL_RXH(ieee80211_rx_h_mesh_fwding); #endif @@ -196,7 +212,7 @@ CALL_RXH(ieee80211_rx_h_amsdu) CALL_RXH(ieee80211_rx_h_data) -@@ -2991,7 +3042,8 @@ static void ieee80211_rx_handlers(struct +@@ -2991,7 +3043,8 @@ static void ieee80211_rx_handlers(struct spin_unlock_bh(&rx->local->rx_path_lock); } @@ -206,7 +222,7 @@ { struct sk_buff_head reorder_release; ieee80211_rx_result res = RX_DROP_MONITOR; -@@ -3009,7 +3061,7 @@ static void ieee80211_invoke_rx_handlers +@@ -3009,7 +3062,7 @@ static void ieee80211_invoke_rx_handlers ieee80211_rx_reorder_ampdu(rx, &reorder_release); @@ -215,7 +231,7 @@ return; rxh_next: -@@ -3046,7 +3098,7 @@ void ieee80211_release_reorder_timeout(s +@@ -3046,7 +3099,7 @@ void ieee80211_release_reorder_timeout(s ieee80211_sta_reorder_release(sta->sdata, tid_agg_rx, &frames); spin_unlock(&tid_agg_rx->reorder_lock); @@ -224,7 +240,7 @@ } /* main receive path */ -@@ -3160,7 +3212,9 @@ static bool prepare_for_handlers(struct +@@ -3160,7 +3213,9 @@ static bool prepare_for_handlers(struct * or not the skb was consumed. */ static bool ieee80211_prepare_and_rx_handle(struct ieee80211_rx_data *rx, @@ -235,7 +251,7 @@ { struct ieee80211_local *local = rx->local; struct ieee80211_sub_if_data *sdata = rx->sdata; -@@ -3186,7 +3240,7 @@ static bool ieee80211_prepare_and_rx_han +@@ -3186,7 +3241,7 @@ static bool ieee80211_prepare_and_rx_han rx->skb = skb; } @@ -244,7 +260,7 @@ return true; } -@@ -3195,7 +3249,8 @@ static bool ieee80211_prepare_and_rx_han +@@ -3195,7 +3250,8 @@ static bool ieee80211_prepare_and_rx_han * be called with rcu_read_lock protection. */ static void __ieee80211_rx_handle_packet(struct ieee80211_hw *hw, @@ -254,7 +270,7 @@ { struct ieee80211_local *local = hw_to_local(hw); struct ieee80211_sub_if_data *sdata; -@@ -3248,7 +3303,7 @@ static void __ieee80211_rx_handle_packet +@@ -3248,7 +3304,7 @@ static void __ieee80211_rx_handle_packet rx.sta = prev_sta; rx.sdata = prev_sta->sdata; @@ -263,7 +279,7 @@ prev_sta = sta; } -@@ -3257,7 +3312,7 @@ static void __ieee80211_rx_handle_packet +@@ -3257,7 +3313,7 @@ static void __ieee80211_rx_handle_packet rx.sta = prev_sta; rx.sdata = prev_sta->sdata; @@ -272,7 +288,7 @@ return; goto out; } -@@ -3286,7 +3341,7 @@ static void __ieee80211_rx_handle_packet +@@ -3286,7 +3342,7 @@ static void __ieee80211_rx_handle_packet rx.sta = sta_info_get_bss(prev, hdr->addr2); rx.sdata = prev; @@ -281,7 +297,7 @@ prev = sdata; } -@@ -3295,7 +3350,7 @@ static void __ieee80211_rx_handle_packet +@@ -3295,7 +3351,7 @@ static void __ieee80211_rx_handle_packet rx.sta = sta_info_get_bss(prev, hdr->addr2); rx.sdata = prev; @@ -290,7 +306,7 @@ return; } -@@ -3406,7 +3461,7 @@ void ieee80211_rx(struct ieee80211_hw *h +@@ -3406,7 +3462,7 @@ void ieee80211_rx(struct ieee80211_hw *h ieee80211_tpt_led_trig_rx(local, ((struct ieee80211_hdr *)skb->data)->frame_control, skb->len); diff --git a/src/common/capwap_error.h b/src/common/capwap_error.h index 2c4c2ba..d6aa05d 100644 --- a/src/common/capwap_error.h +++ b/src/common/capwap_error.h @@ -2,7 +2,8 @@ #define __CAPWAP_ERROR_HEADER__ #define CAPWAP_SUCCESSFUL 0 -#define CAPWAP_ASSERT_CONDITION -1 +#define CAPWAP_GENERIC_ERROR -1 +#define CAPWAP_ASSERT_CONDITION -2 #define CAPWAP_OUT_OF_MEMORY -2 #define CAPWAP_REQUEST_ROOT -3 #define CAPWAP_CRYPT_ERROR -4 diff --git a/src/common/capwap_network.c b/src/common/capwap_network.c index 4e85468..77795a8 100644 --- a/src/common/capwap_network.c +++ b/src/common/capwap_network.c @@ -495,8 +495,12 @@ int capwap_network_set_pollfd(struct capwap_network* net, struct pollfd* fds, in int count = 0; ASSERT(net != NULL); - ASSERT(fds != NULL); - ASSERT(fdscount > 0); + ASSERT(fdscount >= 0); + + /* */ + if (!fds && fdscount) { + return -1; + } /* Count the socket */ for (i = 0; i < CAPWAP_MAX_SOCKETS; i++) { @@ -506,6 +510,11 @@ int capwap_network_set_pollfd(struct capwap_network* net, struct pollfd* fds, in } } + /* */ + if (!fds && !fdscount) { + return (count * 2); + } + /* Check size of fds array */ if (fdscount < (count * 2)) { return -1; diff --git a/src/kmod/netlinkapp.c b/src/kmod/netlinkapp.c index 9b34305..b34b248 100644 --- a/src/kmod/netlinkapp.c +++ b/src/kmod/netlinkapp.c @@ -14,6 +14,7 @@ struct nlsmartcapwap_device { struct ieee80211_pcktunnel pcktunnel_handler; u32 ifindex; + u32 flags; }; /* */ @@ -21,8 +22,72 @@ static u32 nlsmartcapwap_usermodeid = 0; static LIST_HEAD(nlsmartcapwap_dev_list); /* */ -static int nlsmartcapwap_handler(struct sk_buff *skb, int sig_dbm, unsigned char rate, void *data) { - printk("Receive packet\n"); +static int nlsmartcapwap_pre_doit(__genl_const struct genl_ops* ops, struct sk_buff* skb, struct genl_info* info) { + rtnl_lock(); + return 0; +} + +/* */ +static void nlsmartcapwap_post_doit(__genl_const struct genl_ops* ops, struct sk_buff* skb, struct genl_info* info) { + rtnl_unlock(); +} + +/* Netlink Family */ +static struct genl_family nlsmartcapwap_family = { + .id = GENL_ID_GENERATE, + .name = SMARTCAPWAP_GENL_NAME, + .hdrsize = 0, + .version = 1, + .maxattr = NLSMARTCAPWAP_ATTR_MAX, + .netnsok = true, + .pre_doit = nlsmartcapwap_pre_doit, + .post_doit = nlsmartcapwap_post_doit, +}; + +/* */ +static int nlsmartcapwap_handler(u32 ifindex, struct sk_buff* skb, int sig_dbm, unsigned char rate, void* data) { + struct nlsmartcapwap_device* nldev = (struct nlsmartcapwap_device*)data; + struct ieee80211_hdr* hdr = (struct ieee80211_hdr*)skb->data; + + /* Check source network */ + if (ifindex == nldev->ifindex) { + if (nldev->flags & SMARTCAPWAP_FLAGS_SEND_USERSPACE) { + void* msg; + struct sk_buff* sk_msg; + + /* Alloc message */ + sk_msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_ATOMIC); + if (sk_msg) { + /* Set command */ + msg = genlmsg_put(sk_msg, 0, 0, &nlsmartcapwap_family, 0, NLSMARTCAPWAP_CMD_FRAME); + if (msg) { + /* Set params */ + if (nla_put_u32(sk_msg, NLSMARTCAPWAP_ATTR_IFINDEX, nldev->ifindex) || + nla_put(sk_msg, NLSMARTCAPWAP_ATTR_FRAME, skb->len, skb->data) || + (sig_dbm && nla_put_u32(sk_msg, NLSMARTCAPWAP_ATTR_RX_SIGNAL_DBM, (u32)sig_dbm)) || + (rate && nla_put_u8(sk_msg, NLSMARTCAPWAP_ATTR_RX_RATE, (u8)rate))) { + + /* Abort message */ + genlmsg_cancel(sk_msg, msg); + nlmsg_free(sk_msg); + } else { + /* Send message */ + genlmsg_end(sk_msg, msg); + genlmsg_unicast(&init_net, sk_msg, nlsmartcapwap_usermodeid); + } + } else { + nlmsg_free(sk_msg); + } + } + } + } + + /* Check if block all IEEE802.11 Data Packet */ + if ((nldev->flags & SMARTCAPWAP_FLAGS_BLOCK_DATA_FRAME) && + ((le16_to_cpu(hdr->frame_control) & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_DATA)) { + return -1; + } + return 0; } @@ -110,16 +175,6 @@ static void nlsmartcapwap_close(void) { } } -/* */ -static int nlsmartcapwap_pre_doit(__genl_const struct genl_ops* ops, struct sk_buff* skb, struct genl_info* info) { - rtnl_lock(); - return 0; -} - -/* */ -static void nlsmartcapwap_post_doit(__genl_const struct genl_ops* ops, struct sk_buff* skb, struct genl_info* info) { - rtnl_unlock(); -} /* */ static int nlsmartcapwap_link(struct sk_buff* skb, struct genl_info* info) { @@ -181,6 +236,11 @@ static int nlsmartcapwap_join_mac80211_device(struct sk_buff* skb, struct genl_i return -EINVAL; } + /* */ + if (info->attrs[NLSMARTCAPWAP_ATTR_FLAGS]) { + nldev->flags = nla_get_u32(info->attrs[NLSMARTCAPWAP_ATTR_FLAGS]); + } + /* Set subtype masking */ if (info->attrs[NLSMARTCAPWAP_ATTR_MGMT_SUBTYPE_MASK]) { nldev->pcktunnel_handler.subtype_mask[0] = nla_get_u16(info->attrs[NLSMARTCAPWAP_ATTR_MGMT_SUBTYPE_MASK]); @@ -221,23 +281,16 @@ static int nlsmartcapwap_leave_mac80211_device(struct sk_buff* skb, struct genl_ return nlsmartcapwap_unregister_device(ifindex); } -/* Netlink Family */ -static struct genl_family nlsmartcapwap_family = { - .id = GENL_ID_GENERATE, - .name = SMARTCAPWAP_GENL_NAME, - .hdrsize = 0, - .version = 1, - .maxattr = NLSMARTCAPWAP_ATTR_MAX, - .netnsok = true, - .pre_doit = nlsmartcapwap_pre_doit, - .post_doit = nlsmartcapwap_post_doit, -}; - +/* */ static const struct nla_policy nlsmartcapwap_policy[NLSMARTCAPWAP_ATTR_MAX + 1] = { [NLSMARTCAPWAP_ATTR_IFINDEX] = { .type = NLA_U32 }, + [NLSMARTCAPWAP_ATTR_FLAGS] = { .type = NLA_U32 }, [NLSMARTCAPWAP_ATTR_MGMT_SUBTYPE_MASK] = { .type = NLA_U16 }, [NLSMARTCAPWAP_ATTR_CTRL_SUBTYPE_MASK] = { .type = NLA_U16 }, [NLSMARTCAPWAP_ATTR_DATA_SUBTYPE_MASK] = { .type = NLA_U16 }, + [NLSMARTCAPWAP_ATTR_FRAME] = { .type = NLA_BINARY, .len = IEEE80211_MAX_DATA_LEN }, + [NLSMARTCAPWAP_ATTR_RX_SIGNAL_DBM] = { .type = NLA_U32 }, + [NLSMARTCAPWAP_ATTR_RX_RATE] = { .type = NLA_U8 }, }; /* Netlink Ops */ diff --git a/src/kmod/nlsmartcapwap.h b/src/kmod/nlsmartcapwap.h index 3e73eb6..701e804 100644 --- a/src/kmod/nlsmartcapwap.h +++ b/src/kmod/nlsmartcapwap.h @@ -4,15 +4,26 @@ /* */ #define SMARTCAPWAP_GENL_NAME "smartcapwap" +/* */ +#define SMARTCAPWAP_FLAGS_SEND_USERSPACE 0x00000001 +#define SMARTCAPWAP_FLAGS_BLOCK_DATA_FRAME 0x00000002 + /* */ enum nlsmartcapwap_attrs { NLSMARTCAPWAP_ATTR_UNSPEC, NLSMARTCAPWAP_ATTR_IFINDEX, + + NLSMARTCAPWAP_ATTR_FLAGS, + NLSMARTCAPWAP_ATTR_MGMT_SUBTYPE_MASK, NLSMARTCAPWAP_ATTR_CTRL_SUBTYPE_MASK, NLSMARTCAPWAP_ATTR_DATA_SUBTYPE_MASK, + NLSMARTCAPWAP_ATTR_FRAME, + NLSMARTCAPWAP_ATTR_RX_SIGNAL_DBM, + NLSMARTCAPWAP_ATTR_RX_RATE, + /* Last attribute */ __NLSMARTCAPWAP_ATTR_AFTER_LAST, NLSMARTCAPWAP_ATTR_MAX = __NLSMARTCAPWAP_ATTR_AFTER_LAST - 1 @@ -31,6 +42,8 @@ enum nlsmartcapwap_commands { NLSMARTCAPWAP_CMD_SEND_KEEPALIVE, NLSMARTCAPWAP_CMD_RECV_KEEPALIVE, + NLSMARTCAPWAP_CMD_FRAME, + NLSMARTCAPWAP_CMD_JOIN_MAC80211_DEVICE, NLSMARTCAPWAP_CMD_LEAVE_MAC80211_DEVICE, diff --git a/src/wtp/wtp.c b/src/wtp/wtp.c index 9736f65..ae5b8a8 100644 --- a/src/wtp/wtp.c +++ b/src/wtp/wtp.c @@ -8,7 +8,6 @@ #include "capwap_dtls.h" #include "wtp_dfa.h" #include "wtp_radio.h" -#include "wtp_kmod.h" #include #include @@ -1305,24 +1304,28 @@ static void wtp_wait_radio_ready(void) { /* Get only radio file descriptor */ memset(&fds, 0, sizeof(struct wtp_fds)); - wtp_radio_update_fdevent(&fds); + wtp_dfa_update_fdspool(&fds); + if (fds.wifieventscount > 0) { + ASSERT(fds.fdsnetworkcount == 0); + ASSERT(fds.kmodeventscount == 0); - for (;;) { - capwap_timeout_set(g_wtp.timeout, g_wtp.idtimercontrol, WTP_RADIO_INITIALIZATION_INTERVAL, NULL, NULL, NULL); + for (;;) { + capwap_timeout_set(g_wtp.timeout, g_wtp.idtimercontrol, WTP_RADIO_INITIALIZATION_INTERVAL, NULL, NULL, NULL); - /* Wait packet */ - index = capwap_wait_recvready(fds.fdspoll, fds.fdstotalcount, g_wtp.timeout); - if (index < 0) { - break; - } else if (!fds.events[index].event_handler) { - break; + /* Wait packet */ + index = capwap_wait_recvready(fds.fdspoll, fds.fdstotalcount, g_wtp.timeout); + if (index < 0) { + break; + } else if (!fds.wifievents[index].event_handler) { + break; + } + + fds.wifievents[index].event_handler(fds.fdspoll[index].fd, fds.wifievents[index].params, fds.wifievents[index].paramscount); } - - fds.events[index].event_handler(fds.fdspoll[index].fd, fds.events[index].params, fds.events[index].paramscount); } /* */ - wtp_free_fds(&fds); + wtp_dfa_free_fdspool(&fds); capwap_timeout_unset(g_wtp.timeout, g_wtp.idtimercontrol); } @@ -1332,19 +1335,6 @@ int wtp_update_radio_in_use() { return g_wtp.radios->count; } -/* */ -void wtp_free_fds(struct wtp_fds* fds) { - ASSERT(fds != NULL); - - if (fds->fdspoll) { - capwap_free(fds->fdspoll); - } - - if (fds->events) { - capwap_free(fds->events); - } -} - /* Main*/ int main(int argc, char** argv) { int value; @@ -1394,8 +1384,7 @@ int main(int argc, char** argv) { /* Connect WTP with kernel module */ value = wtp_kmod_init(); if (!value || !g_wtp.kmodrequest) { - if (!value) { - g_wtp.kmodconnect = 1; + if (wtp_kmod_isconnected()) { capwap_logging_info("SmartCAPWAP kernel module connected"); } @@ -1413,9 +1402,7 @@ int main(int argc, char** argv) { } /* Disconnect kernel module */ - if (g_wtp.kmodconnect) { - wtp_kmod_free(); - } + wtp_kmod_free(); /* */ capwap_logging_info("Terminate WTP"); diff --git a/src/wtp/wtp.h b/src/wtp/wtp.h index cfd2455..857a051 100644 --- a/src/wtp/wtp.h +++ b/src/wtp/wtp.h @@ -6,6 +6,7 @@ #include "capwap_dtls.h" #include "capwap_network.h" #include "capwap_protocol.h" +#include "wtp_kmod.h" #include "wifi_drivers.h" /* WTP Configuration */ @@ -43,12 +44,18 @@ /* */ struct wtp_fds { - struct pollfd* fdspoll; int fdstotalcount; + struct pollfd* fdspoll; + int fdsnetworkcount; - struct wifi_event* events; - int eventscount; + struct wtp_kmod_event* kmodevents; + int kmodeventscount; + int kmodeventsstartpos; + + struct wifi_event* wifievents; + int wifieventscount; + int wifieventsstartpos; }; /* WTP */ @@ -58,7 +65,7 @@ struct wtp_t { /* */ int kmodrequest; - int kmodconnect; + struct wtp_kmod_handle kmodhandle; /* */ char wlanprefix[IFNAMSIZ]; @@ -151,7 +158,6 @@ extern struct wtp_t g_wtp; /* */ int wtp_update_radio_in_use(); -void wtp_free_fds(struct wtp_fds* fds); /* Build capwap element helper */ void wtp_create_radioadmstate_element(struct capwap_packet_txmng* txmngpacket); diff --git a/src/wtp/wtp_dfa.c b/src/wtp/wtp_dfa.c index 78edeca..21c9f54 100644 --- a/src/wtp/wtp_dfa.c +++ b/src/wtp/wtp_dfa.c @@ -158,15 +158,27 @@ static int wtp_recvfrom(struct wtp_fds* fds, void* buffer, int* size, struct soc index = capwap_wait_recvready(fds->fdspoll, fds->fdstotalcount, g_wtp.timeout); if (index < 0) { return index; - } else if (index >= fds->fdsnetworkcount) { - int pos = index - fds->fdsnetworkcount; + } else if ((fds->wifieventsstartpos >= 0) && (index >= fds->wifieventsstartpos)) { + int pos = index - fds->wifieventsstartpos; - if (pos < fds->eventscount) { - if (!fds->events[pos].event_handler) { + if (pos < fds->wifieventscount) { + if (!fds->wifievents[pos].event_handler) { return CAPWAP_RECV_ERROR_SOCKET; } - fds->events[pos].event_handler(fds->fdspoll[index].fd, fds->events[pos].params, fds->events[pos].paramscount); + fds->wifievents[pos].event_handler(fds->fdspoll[index].fd, fds->wifievents[pos].params, fds->wifievents[pos].paramscount); + } + + return WTP_RECV_NOERROR_RADIO; + } else if ((fds->kmodeventsstartpos >= 0) && (index >= fds->kmodeventsstartpos)) { + int pos = index - fds->kmodeventsstartpos; + + if (pos < fds->kmodeventscount) { + if (!fds->kmodevents[pos].event_handler) { + return CAPWAP_RECV_ERROR_SOCKET; + } + + fds->kmodevents[pos].event_handler(fds->fdspoll[index].fd, fds->kmodevents[pos].params, fds->kmodevents[pos].paramscount); } return WTP_RECV_NOERROR_RADIO; @@ -181,21 +193,115 @@ static int wtp_recvfrom(struct wtp_fds* fds, void* buffer, int* size, struct soc } /* */ -static void wtp_dfa_init_fdspool(struct wtp_fds* fds, struct capwap_network* net) { +static int wtp_dfa_init_fdspool(struct wtp_fds* fds, struct capwap_network* net) { ASSERT(fds != NULL); ASSERT(net != NULL); /* */ memset(fds, 0, sizeof(struct wtp_fds)); - fds->fdstotalcount = CAPWAP_MAX_SOCKETS * 2; - fds->fdspoll = (struct pollfd*)capwap_alloc(sizeof(struct pollfd) * fds->fdstotalcount); + fds->fdsnetworkcount = capwap_network_set_pollfd(net, NULL, 0); + fds->fdspoll = (struct pollfd*)capwap_alloc(sizeof(struct pollfd) * fds->fdsnetworkcount); /* Retrive all socket for polling */ - fds->fdsnetworkcount = capwap_network_set_pollfd(net, fds->fdspoll, fds->fdstotalcount); - fds->fdstotalcount = fds->fdsnetworkcount; + fds->fdstotalcount = capwap_network_set_pollfd(net, fds->fdspoll, fds->fdsnetworkcount); + if (fds->fdsnetworkcount != fds->fdstotalcount) { + capwap_free(fds->fdspoll); + return -1; + } /* Update Event File Descriptor */ - wtp_radio_update_fdevent(fds); + wtp_dfa_update_fdspool(fds); + return 0; +} + +/* */ +int wtp_dfa_update_fdspool(struct wtp_fds* fds) { + int totalcount; + int kmodcount; + int wificount; + struct pollfd* fdsbuffer; + + ASSERT(fds != NULL); + + /* Retrieve number of Dynamic File Descriptor Event */ + kmodcount = wtp_kmod_getfd(NULL, NULL, 0); + wificount = wifi_event_getfd(NULL, NULL, 0); + if ((kmodcount < 0) || (wificount < 0)) { + return -1; + } + + /* Kernel Module Events Callback */ + fds->kmodeventsstartpos = -1; + if (kmodcount != fds->kmodeventscount) { + if (fds->kmodevents) { + capwap_free(fds->kmodevents); + } + + /* */ + fds->kmodeventscount = kmodcount; + fds->kmodevents = (struct wtp_kmod_event*)((wificount > 0) ? capwap_alloc(sizeof(struct wtp_kmod_event) * kmodcount) : NULL); + } + + /* Wifi Events Callback */ + fds->wifieventsstartpos = -1; + if (wificount != fds->wifieventscount) { + if (fds->wifievents) { + capwap_free(fds->wifievents); + } + + /* */ + fds->wifieventscount = wificount; + fds->wifievents = (struct wifi_event*)((wificount > 0) ? capwap_alloc(sizeof(struct wifi_event) * wificount) : NULL); + } + + /* Resize poll */ + totalcount = fds->fdsnetworkcount + fds->kmodeventscount + fds->wifieventscount; + if (fds->fdstotalcount != totalcount) { + fdsbuffer = (struct pollfd*)capwap_alloc(sizeof(struct pollfd) * totalcount); + if (fds->fdspoll) { + if (fds->fdsnetworkcount > 0) { + memcpy(fdsbuffer, fds->fdspoll, sizeof(struct pollfd) * fds->fdsnetworkcount); + } + + capwap_free(fds->fdspoll); + } + + /* */ + fds->fdspoll = fdsbuffer; + fds->fdstotalcount = totalcount; + } + + + /* Retrieve File Descriptor Kernel Module Event */ + if (fds->kmodeventscount > 0) { + fds->kmodeventsstartpos = fds->fdsnetworkcount; + wtp_kmod_getfd(&fds->fdspoll[fds->kmodeventsstartpos], fds->kmodevents, fds->kmodeventscount); + } + + /* Retrieve File Descriptor Wifi Event */ + if (fds->wifieventscount > 0) { + fds->wifieventsstartpos = fds->fdsnetworkcount + fds->kmodeventscount; + wifi_event_getfd(&fds->fdspoll[fds->wifieventsstartpos], fds->wifievents, fds->wifieventscount); + } + + return fds->fdstotalcount; +} + +/* */ +void wtp_dfa_free_fdspool(struct wtp_fds* fds) { + ASSERT(fds != NULL); + + if (fds->fdspoll) { + capwap_free(fds->fdspoll); + } + + if (fds->kmodevents) { + capwap_free(fds->kmodevents); + } + + if (fds->wifievents) { + capwap_free(fds->wifievents); + } } /* */ @@ -242,7 +348,9 @@ int wtp_dfa_running(void) { memset(&packet, 0, sizeof(struct capwap_parsed_packet)); /* Configure poll struct */ - wtp_dfa_init_fdspool(&g_wtp.fds, &g_wtp.net); + if (wtp_dfa_init_fdspool(&g_wtp.fds, &g_wtp.net)) { + return CAPWAP_GENERIC_ERROR; + } /* Handler signal */ g_wtp.running = 1; @@ -453,8 +561,7 @@ int wtp_dfa_running(void) { } /* Free memory */ - wtp_free_fds(&g_wtp.fds); - + wtp_dfa_free_fdspool(&g_wtp.fds); return result; } diff --git a/src/wtp/wtp_dfa.h b/src/wtp/wtp_dfa.h index 7cb5ce2..f299098 100644 --- a/src/wtp/wtp_dfa.h +++ b/src/wtp/wtp_dfa.h @@ -40,6 +40,9 @@ void wtp_send_datacheck(void); /* */ void wtp_dfa_retransmition_timeout(struct capwap_timeout* timeout, unsigned long index, void* context, void* param); +int wtp_dfa_update_fdspool(struct wtp_fds* fds); +void wtp_dfa_free_fdspool(struct wtp_fds* fds); + /* */ void wtp_dfa_state_idle(void); diff --git a/src/wtp/wtp_kmod.c b/src/wtp/wtp_kmod.c index d7fb7d8..5fc07d6 100644 --- a/src/wtp/wtp_kmod.c +++ b/src/wtp/wtp_kmod.c @@ -1,5 +1,4 @@ #include "wtp.h" -#include "wtp_kmod.h" #include #include #include @@ -7,7 +6,6 @@ /* Compatibility functions */ #ifdef HAVE_LIBNL_10 -#define nl_sock nl_handle static uint32_t g_portbitmap[32] = { 0 }; static struct nl_sock* nl_socket_alloc_cb(void* cb) { @@ -40,19 +38,9 @@ static void nl_socket_free(struct nl_sock* handle) { } #endif -/* */ -struct wtp_kmod_handle { - struct nl_sock* nl; - struct nl_cb* nl_cb; - int nlsmartcapwap_id; -}; - /* */ typedef int (*wtp_kmod_valid_cb)(struct nl_msg* msg, void* data); -/* */ -static struct wtp_kmod_handle g_kmodhandle; - /* */ static struct nl_sock* nl_create_handle(struct nl_cb* cb) { struct nl_sock* handle; @@ -141,7 +129,7 @@ static int wtp_kmod_send_and_recv(struct nl_sock* nl, struct nl_cb* nl_cb, struc /* */ static int wtp_kmod_send_and_recv_msg(struct nl_msg* msg, wtp_kmod_valid_cb valid_cb, void* data) { - return wtp_kmod_send_and_recv(g_kmodhandle.nl, g_kmodhandle.nl_cb, msg, valid_cb, data); + return wtp_kmod_send_and_recv(g_wtp.kmodhandle.nl, g_wtp.kmodhandle.nl_cb, msg, valid_cb, data); } /* */ @@ -156,7 +144,7 @@ static int wtp_kmod_link(void) { } /* */ - genlmsg_put(msg, 0, 0, g_kmodhandle.nlsmartcapwap_id, 0, 0, NLSMARTCAPWAP_CMD_LINK, 0); + genlmsg_put(msg, 0, 0, g_wtp.kmodhandle.nlsmartcapwap_id, 0, 0, NLSMARTCAPWAP_CMD_LINK, 0); /* */ result = wtp_kmod_send_and_recv_msg(msg, NULL, NULL); @@ -173,13 +161,28 @@ static int wtp_kmod_link(void) { return result; } +/* */ +static void wtp_kmod_event_receive(int fd, void** params, int paramscount) { + int res; + + ASSERT(fd >= 0); + ASSERT(params != NULL); + ASSERT(paramscount == 2); + + /* */ + res = nl_recvmsgs((struct nl_sock*)params[0], (struct nl_cb*)params[1]); + if (res) { + capwap_logging_warning("Receive kernel module message failed: %d", res); + } +} + /* */ int wtp_kmod_join_mac80211_device(uint32_t ifindex) { int result; struct nl_msg* msg; /* */ - if (!g_kmodhandle.nlsmartcapwap_id) { + if (!g_wtp.kmodhandle.nlsmartcapwap_id) { return -1; } @@ -190,8 +193,9 @@ int wtp_kmod_join_mac80211_device(uint32_t ifindex) { } /* */ - genlmsg_put(msg, 0, 0, g_kmodhandle.nlsmartcapwap_id, 0, 0, NLSMARTCAPWAP_CMD_JOIN_MAC80211_DEVICE, 0); + genlmsg_put(msg, 0, 0, g_wtp.kmodhandle.nlsmartcapwap_id, 0, 0, NLSMARTCAPWAP_CMD_JOIN_MAC80211_DEVICE, 0); nla_put_u32(msg, NLSMARTCAPWAP_ATTR_IFINDEX, ifindex); + nla_put_u32(msg, NLSMARTCAPWAP_ATTR_FLAGS, SMARTCAPWAP_FLAGS_SEND_USERSPACE | SMARTCAPWAP_FLAGS_BLOCK_DATA_FRAME); nla_put_u16(msg, NLSMARTCAPWAP_ATTR_DATA_SUBTYPE_MASK, 0xffff); /* */ @@ -205,38 +209,68 @@ int wtp_kmod_join_mac80211_device(uint32_t ifindex) { return result; } +/* */ +int wtp_kmod_isconnected(void) { + return (g_wtp.kmodhandle.nlsmartcapwap_id ? 1 : 0); +} + +/* */ +int wtp_kmod_getfd(struct pollfd* fds, struct wtp_kmod_event* events, int count) { + int kmodcount = (wtp_kmod_isconnected() ? 1 : 0); + + /* */ + if (!fds && !events && !count) { + return kmodcount; + } else if ((count > 0) && (!fds || !events)) { + return -1; + } else if (count < kmodcount) { + return -1; + } + + /* */ + fds[0].fd = g_wtp.kmodhandle.nl_fd; + fds[0].events = POLLIN | POLLERR | POLLHUP; + + /* */ + events[0].event_handler = wtp_kmod_event_receive; + events[0].params[0] = (void*)g_wtp.kmodhandle.nl; + events[0].params[1] = (void*)g_wtp.kmodhandle.nl_cb; + events[0].paramscount = 2; + + return kmodcount; +} + /* */ int wtp_kmod_init(void) { int result; - /* */ - memset(&g_kmodhandle, 0, sizeof(struct wtp_kmod_handle)); - /* Configure netlink callback */ - g_kmodhandle.nl_cb = nl_cb_alloc(NL_CB_DEFAULT); - if (!g_kmodhandle.nl_cb) { + g_wtp.kmodhandle.nl_cb = nl_cb_alloc(NL_CB_DEFAULT); + if (!g_wtp.kmodhandle.nl_cb) { wtp_kmod_free(); return -1; } /* Create netlink socket */ - g_kmodhandle.nl = nl_create_handle(g_kmodhandle.nl_cb); - if (!g_kmodhandle.nl) { + g_wtp.kmodhandle.nl = nl_create_handle(g_wtp.kmodhandle.nl_cb); + if (!g_wtp.kmodhandle.nl) { wtp_kmod_free(); return -1; } + g_wtp.kmodhandle.nl_fd = nl_socket_get_fd(g_wtp.kmodhandle.nl); + /* Get nlsmartcapwap netlink family */ - g_kmodhandle.nlsmartcapwap_id = genl_ctrl_resolve(g_kmodhandle.nl, SMARTCAPWAP_GENL_NAME); - if (g_kmodhandle.nlsmartcapwap_id < 0) { + g_wtp.kmodhandle.nlsmartcapwap_id = genl_ctrl_resolve(g_wtp.kmodhandle.nl, SMARTCAPWAP_GENL_NAME); + if (g_wtp.kmodhandle.nlsmartcapwap_id < 0) { capwap_logging_warning("Unable to found kernel module"); wtp_kmod_free(); return -1; } /* Configure callback function */ - nl_cb_set(g_kmodhandle.nl_cb, NL_CB_SEQ_CHECK, NL_CB_CUSTOM, wtp_kmod_no_seq_check, NULL); - nl_cb_set(g_kmodhandle.nl_cb, NL_CB_VALID, NL_CB_CUSTOM, wtp_kmod_valid_handler, NULL); + nl_cb_set(g_wtp.kmodhandle.nl_cb, NL_CB_SEQ_CHECK, NL_CB_CUSTOM, wtp_kmod_no_seq_check, NULL); + nl_cb_set(g_wtp.kmodhandle.nl_cb, NL_CB_VALID, NL_CB_CUSTOM, wtp_kmod_valid_handler, NULL); /* Link to kernel module */ result = wtp_kmod_link(); @@ -250,14 +284,14 @@ int wtp_kmod_init(void) { /* */ void wtp_kmod_free(void) { - if (g_kmodhandle.nl) { - nl_socket_free(g_kmodhandle.nl); + if (g_wtp.kmodhandle.nl) { + nl_socket_free(g_wtp.kmodhandle.nl); } - if (g_kmodhandle.nl_cb) { - nl_cb_put(g_kmodhandle.nl_cb); + if (g_wtp.kmodhandle.nl_cb) { + nl_cb_put(g_wtp.kmodhandle.nl_cb); } /* */ - memset(&g_kmodhandle, 0, sizeof(struct wtp_kmod_handle)); + memset(&g_wtp.kmodhandle, 0, sizeof(struct wtp_kmod_handle)); } diff --git a/src/wtp/wtp_kmod.h b/src/wtp/wtp_kmod.h index eae9667..20d1b6a 100644 --- a/src/wtp/wtp_kmod.h +++ b/src/wtp/wtp_kmod.h @@ -1,10 +1,35 @@ #ifndef __WTP_KMOD_HEADER__ #define __WTP_KMOD_HEADER__ +/* */ +#ifdef HAVE_LIBNL_10 +#define nl_sock nl_handle +#endif + +/* */ +struct wtp_kmod_handle { + struct nl_sock* nl; + int nl_fd; + struct nl_cb* nl_cb; + int nlsmartcapwap_id; +}; + +/* */ +#define WTP_KMOD_EVENT_MAX_ITEMS 2 +struct wtp_kmod_event { + void (*event_handler)(int fd, void** params, int paramscount); + int paramscount; + void* params[WTP_KMOD_EVENT_MAX_ITEMS]; +}; + /* */ int wtp_kmod_init(void); void wtp_kmod_free(void); +/* */ +int wtp_kmod_isconnected(void); +int wtp_kmod_getfd(struct pollfd* fds, struct wtp_kmod_event* events, int count); + /* */ int wtp_kmod_join_mac80211_device(uint32_t ifindex); diff --git a/src/wtp/wtp_radio.c b/src/wtp/wtp_radio.c index b9f1c58..a7e83fd 100644 --- a/src/wtp/wtp_radio.c +++ b/src/wtp/wtp_radio.c @@ -555,48 +555,6 @@ struct wtp_radio_wlan* wtp_radio_search_wlan(struct wtp_radio* radio, const uint return NULL; } -/* */ -void wtp_radio_update_fdevent(struct wtp_fds* fds) { - int count; - struct pollfd* fdsbuffer; - - ASSERT(fds != NULL); - - /* Retrieve number of File Descriptor Event */ - count = wifi_event_getfd(NULL, NULL, 0); - if (count < 0) { - return; - } - - /* Resize poll */ - if (fds->eventscount != count) { - fdsbuffer = (struct pollfd*)capwap_alloc(sizeof(struct pollfd) * (fds->fdsnetworkcount + count)); - if (fds->fdspoll && (fds->fdsnetworkcount > 0)) { - memcpy(fdsbuffer, fds->fdspoll, sizeof(struct pollfd) * fds->fdsnetworkcount); - capwap_free(fds->fdspoll); - } - - fds->fdspoll = fdsbuffer; - - /* Events Callback */ - if (fds->events) { - capwap_free(fds->events); - } - - fds->events = (struct wifi_event*)((count > 0) ? capwap_alloc(sizeof(struct wifi_event) * count) : NULL); - - /* */ - fds->eventscount = count; - fds->fdstotalcount = fds->fdsnetworkcount + count; - } - - /* Retrieve File Descriptor Event */ - if (count > 0) { - ASSERT(fds->fdspoll != NULL); - wifi_event_getfd(&fds->fdspoll[fds->fdsnetworkcount], fds->events, fds->eventscount); - } -} - /* */ void wtp_radio_receive_data_packet(uint8_t radioid, unsigned short binding, const uint8_t* frame, int length) { struct wtp_radio* radio; @@ -704,7 +662,7 @@ uint32_t wtp_radio_create_wlan(struct capwap_parsed_packet* packet, struct capwa capwap_itemlist_insert_after(radio->wlan, NULL, itemwlan); /* Update Event File Descriptor */ - wtp_radio_update_fdevent(&g_wtp.fds); + wtp_dfa_update_fdspool(&g_wtp.fds); /* Retrieve macaddress of new device */ bssid->radioid = addwlan->radioid; diff --git a/src/wtp/wtp_radio.h b/src/wtp/wtp_radio.h index d2a28c8..bf15841 100644 --- a/src/wtp/wtp_radio.h +++ b/src/wtp/wtp_radio.h @@ -73,7 +73,6 @@ void wtp_radio_receive_data_packet(uint8_t radioid, unsigned short binding, cons /* */ int wtp_radio_setconfiguration(struct capwap_parsed_packet* packet); -void wtp_radio_update_fdevent(struct wtp_fds* fds); /* */ uint32_t wtp_radio_create_wlan(struct capwap_parsed_packet* packet, struct capwap_80211_assignbssid_element* bssid);