From c26411bf0d00a6ceed42e85d66812b591b7c4c83 Mon Sep 17 00:00:00 2001 From: Andreas Schultz Date: Thu, 4 Feb 2016 15:57:11 +0100 Subject: [PATCH] support patch for smartcapwap Allows for kernel side interception and injection of IEEE 802.11 frames. --- include/net/mac80211.h | 25 ++++ net/mac80211/ieee80211_i.h | 6 + net/mac80211/iface.c | 56 ++++++++ net/mac80211/rx.c | 81 ++++++++++-- net/mac80211/tx.c | 312 +++++++++++++++++++++++++++++++++++++++++++++ 5 files changed, 468 insertions(+), 12 deletions(-) diff --git a/include/net/mac80211.h b/include/net/mac80211.h index 760bc4d..6722da6 100644 --- a/include/net/mac80211.h +++ b/include/net/mac80211.h @@ -2200,6 +2200,31 @@ ieee80211_get_alt_retry_rate(const struct ieee80211_hw *hw, void ieee80211_free_txskb(struct ieee80211_hw *hw, struct sk_buff *skb); /** + * + */ +struct ieee80211_pcktunnel { + u16 subtype_mask[3]; /* 0: MGMT, 1: CTLR, 2: DATA */ + + int (*handler)(u32 ifindex, struct sk_buff *skb, int sig_dbm, unsigned char rate, void *data); + void *data; +}; + +/** + * + */ +int ieee80211_pcktunnel_register(struct net_device *dev, struct ieee80211_pcktunnel *handler); + +/** + * + */ +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); + +/** * DOC: Hardware crypto acceleration * * mac80211 is capable of taking advantage of many hardware diff --git a/net/mac80211/ieee80211_i.h b/net/mac80211/ieee80211_i.h index 5322b4c..667c9ff 100644 --- a/net/mac80211/ieee80211_i.h +++ b/net/mac80211/ieee80211_i.h @@ -180,6 +180,7 @@ typedef unsigned __bitwise__ ieee80211_rx_result; #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 @@ -835,6 +836,9 @@ struct ieee80211_sub_if_data { char name[IFNAMSIZ]; + /* Packet tunnel handlers */ + struct ieee80211_pcktunnel __rcu *pcktunnel_handlers; + /* Fragment table for host-based reassembly */ struct ieee80211_fragment_entry fragments[IEEE80211_FRAGMENT_MAX]; unsigned int fragment_next; @@ -1632,6 +1636,8 @@ netdev_tx_t ieee80211_subif_start_xmit(struct sk_buff *skb, void __ieee80211_subif_start_xmit(struct sk_buff *skb, struct net_device *dev, u32 info_flags); +netdev_tx_t ieee80211_capwap_subif_start_xmit(struct sk_buff *skb, + struct net_device *dev); void ieee80211_purge_tx_queue(struct ieee80211_hw *hw, struct sk_buff_head *skbs); struct sk_buff * diff --git a/net/mac80211/iface.c b/net/mac80211/iface.c index c9e325d..15ce6e3 100644 --- a/net/mac80211/iface.c +++ b/net/mac80211/iface.c @@ -1952,3 +1952,59 @@ void ieee80211_iface_exit(void) { unregister_netdevice_notifier(&mac80211_netdev_notifier); } + +static const struct net_device_ops ieee80211_capwapif_ops = { + .ndo_open = ieee80211_open, + .ndo_stop = ieee80211_stop, + .ndo_uninit = ieee80211_uninit, + .ndo_start_xmit = ieee80211_capwap_subif_start_xmit, + .ndo_set_rx_mode = ieee80211_set_multicast_list, + .ndo_change_mtu = ieee80211_change_mtu, + .ndo_set_mac_address = ieee80211_change_mac, + .ndo_select_queue = ieee80211_netdev_select_queue, + .ndo_get_stats64 = ieee80211_get_stats64, +}; + +int ieee80211_pcktunnel_register(struct net_device *dev, struct ieee80211_pcktunnel *handler) +{ + int ret = 0; + struct ieee80211_sub_if_data *sdata = IEEE80211_DEV_TO_SUB_IF(dev); + + dev->netdev_ops = &ieee80211_capwapif_ops; + + mutex_lock(&sdata->local->iflist_mtx); + + if (rcu_dereference_protected(sdata->pcktunnel_handlers, lockdep_is_held(&sdata->local->iflist_mtx))) { + ret = -EBUSY; + } else { + rcu_assign_pointer(sdata->pcktunnel_handlers, handler); + } + + mutex_unlock(&sdata->local->iflist_mtx); + synchronize_net(); + + return ret; +} +EXPORT_SYMBOL(ieee80211_pcktunnel_register); + +int ieee80211_pcktunnel_deregister(struct net_device *dev, struct ieee80211_pcktunnel *handler) +{ + int ret = -ENODEV; + struct ieee80211_sub_if_data *sdata = IEEE80211_DEV_TO_SUB_IF(dev); + struct ieee80211_pcktunnel *h; + + mutex_lock(&sdata->local->iflist_mtx); + + h = rcu_dereference_protected(sdata->pcktunnel_handlers, lockdep_is_held(&sdata->local->iflist_mtx)); + if (h == handler) { + ret = 0; + rcu_assign_pointer(sdata->pcktunnel_handlers, NULL); + } + + mutex_unlock(&sdata->local->iflist_mtx); + synchronize_net(); + + return ret; +} +EXPORT_SYMBOL(ieee80211_pcktunnel_deregister); + diff --git a/net/mac80211/rx.c b/net/mac80211/rx.c index 82af407..29cc59b 100644 --- a/net/mac80211/rx.c +++ b/net/mac80211/rx.c @@ -3039,6 +3039,51 @@ ieee80211_rx_h_mgmt(struct ieee80211_rx_data *rx) return RX_QUEUED; } +static ieee80211_rx_result debug_noinline +ieee80211_rx_h_pcktunnel(struct ieee80211_rx_data *rx, struct ieee80211_rate *rate) +{ + struct ieee80211_pcktunnel *handler; + + handler = rcu_dereference(rx->sdata->pcktunnel_handlers); + if (handler) { + u16 fc; + u16 fc_type; + int sig_dbm = 0; + unsigned char pckrate = 0; + struct ieee80211_rx_status *status = IEEE80211_SKB_RXCB(rx->skb); + struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)rx->skb->data; + + if (ieee80211_hw_check(&rx->local->hw, SIGNAL_DBM)) + sig_dbm = status->signal; + + if (rate && !(status->flag & (RX_FLAG_HT | RX_FLAG_VHT))) { + int shift = 0; + if (status->flag & RX_FLAG_10MHZ) + shift = 1; + else if (status->flag & RX_FLAG_5MHZ) + shift = 2; + pckrate = DIV_ROUND_UP(rate->bitrate, 5 * (1 << shift)); + } + + /* Retrieve type and subtype packet */ + fc = le16_to_cpu(hdr->frame_control); + fc_type = ((fc & IEEE80211_FCTL_FTYPE) >> 2); + if (fc_type < 3) { + u16 bitmask = 1 << ((fc & IEEE80211_FCTL_STYPE) >> 4); + + /* Delegate packet to external handler */ + if (handler->subtype_mask[fc_type] & bitmask) { + if (handler->handler(rx->sdata->dev->ifindex, rx->skb, sig_dbm, pckrate, handler->data)) { + return RX_IGNORE_MONITOR; + } + } + } + } + + return RX_CONTINUE; +} + +/* TODO: use IEEE80211_RX_FRAGMENTED */ static void ieee80211_rx_cooked_monitor(struct ieee80211_rx_data *rx, struct ieee80211_rate *rate) { @@ -3118,6 +3163,7 @@ static void ieee80211_rx_handlers_result(struct ieee80211_rx_data *rx, if (rx->sta) rx->sta->rx_stats.dropped++; /* fall through */ + case RX_IGNORE_MONITOR: case RX_CONTINUE: { struct ieee80211_rate *rate = NULL; struct ieee80211_supported_band *sband; @@ -3146,7 +3192,9 @@ static void ieee80211_rx_handlers_result(struct ieee80211_rx_data *rx, } static void ieee80211_rx_handlers(struct ieee80211_rx_data *rx, - struct sk_buff_head *frames) + struct sk_buff_head *frames, + struct ieee80211_rate *rate) + { ieee80211_rx_result res = RX_DROP_MONITOR; struct sk_buff *skb; @@ -3185,6 +3233,11 @@ static void ieee80211_rx_handlers(struct ieee80211_rx_data *rx, if (ieee80211_vif_is_mesh(&rx->sdata->vif)) CALL_RXH(ieee80211_rx_h_mesh_fwding); #endif + /* special treatment */ + res = ieee80211_rx_h_pcktunnel(rx, rate); + if (res != RX_CONTINUE) + goto rxh_next; + CALL_RXH(ieee80211_rx_h_amsdu) CALL_RXH(ieee80211_rx_h_data) @@ -3208,7 +3261,8 @@ static void ieee80211_rx_handlers(struct ieee80211_rx_data *rx, spin_unlock_bh(&rx->local->rx_path_lock); } -static void ieee80211_invoke_rx_handlers(struct ieee80211_rx_data *rx) +static void ieee80211_invoke_rx_handlers(struct ieee80211_rx_data *rx, + struct ieee80211_rate *rate) { struct sk_buff_head reorder_release; ieee80211_rx_result res = RX_DROP_MONITOR; @@ -3227,7 +3281,7 @@ static void ieee80211_invoke_rx_handlers(struct ieee80211_rx_data *rx) ieee80211_rx_reorder_ampdu(rx, &reorder_release); - ieee80211_rx_handlers(rx, &reorder_release); + ieee80211_rx_handlers(rx, &reorder_release, rate); return; rxh_next: @@ -3273,7 +3327,7 @@ void ieee80211_release_reorder_timeout(struct sta_info *sta, int tid) drv_event_callback(rx.local, rx.sdata, &event); } - ieee80211_rx_handlers(&rx, &frames); + ieee80211_rx_handlers(&rx, &frames, NULL); } /* main receive path */ @@ -3395,7 +3449,9 @@ static bool ieee80211_accept_frame(struct ieee80211_rx_data *rx) * or not the skb was consumed. */ static bool ieee80211_prepare_and_rx_handle(struct ieee80211_rx_data *rx, - struct sk_buff *skb, bool consume) + struct sk_buff *skb, + struct ieee80211_rate *rate, + bool consume) { struct ieee80211_local *local = rx->local; struct ieee80211_sub_if_data *sdata = rx->sdata; @@ -3418,7 +3474,7 @@ static bool ieee80211_prepare_and_rx_handle(struct ieee80211_rx_data *rx, rx->skb = skb; } - ieee80211_invoke_rx_handlers(rx); + ieee80211_invoke_rx_handlers(rx, rate); return true; } @@ -3428,7 +3484,8 @@ static bool ieee80211_prepare_and_rx_handle(struct ieee80211_rx_data *rx, */ static void __ieee80211_rx_handle_packet(struct ieee80211_hw *hw, struct sk_buff *skb, - struct napi_struct *napi) + struct napi_struct *napi, + struct ieee80211_rate *rate) { struct ieee80211_local *local = hw_to_local(hw); struct ieee80211_sub_if_data *sdata; @@ -3487,7 +3544,7 @@ static void __ieee80211_rx_handle_packet(struct ieee80211_hw *hw, rx.sta = prev_sta; rx.sdata = prev_sta->sdata; - ieee80211_prepare_and_rx_handle(&rx, skb, false); + ieee80211_prepare_and_rx_handle(&rx, skb, rate, false); prev_sta = sta; } @@ -3496,7 +3553,7 @@ static void __ieee80211_rx_handle_packet(struct ieee80211_hw *hw, rx.sta = prev_sta; rx.sdata = prev_sta->sdata; - if (ieee80211_prepare_and_rx_handle(&rx, skb, true)) + if (ieee80211_prepare_and_rx_handle(&rx, skb, rate, true)) return; goto out; } @@ -3525,7 +3582,7 @@ static void __ieee80211_rx_handle_packet(struct ieee80211_hw *hw, rx.sta = sta_info_get_bss(prev, hdr->addr2); rx.sdata = prev; - ieee80211_prepare_and_rx_handle(&rx, skb, false); + ieee80211_prepare_and_rx_handle(&rx, skb, rate, false); prev = sdata; } @@ -3534,7 +3591,7 @@ static void __ieee80211_rx_handle_packet(struct ieee80211_hw *hw, rx.sta = sta_info_get_bss(prev, hdr->addr2); rx.sdata = prev; - if (ieee80211_prepare_and_rx_handle(&rx, skb, true)) + if (ieee80211_prepare_and_rx_handle(&rx, skb, rate, true)) return; } @@ -3646,7 +3703,7 @@ void ieee80211_rx_napi(struct ieee80211_hw *hw, struct sk_buff *skb, ieee80211_tpt_led_trig_rx(local, ((struct ieee80211_hdr *)skb->data)->frame_control, skb->len); - __ieee80211_rx_handle_packet(hw, skb, napi); + __ieee80211_rx_handle_packet(hw, skb, napi, rate); rcu_read_unlock(); diff --git a/net/mac80211/tx.c b/net/mac80211/tx.c index bdc224d..fd48ccf 100644 --- a/net/mac80211/tx.c +++ b/net/mac80211/tx.c @@ -2939,6 +2939,144 @@ netdev_tx_t ieee80211_subif_start_xmit(struct sk_buff *skb, return NETDEV_TX_OK; } +/* + * inject raw 802.11 frame, processing is mostly identical + * to ieee80211_monitor_start_xmit, except for the different + * headers + */ +static void __ieee80211_capwap_inject_start_xmit(struct sk_buff *skb, + struct net_device *dev) +{ + struct ieee80211_local *local = wdev_priv(dev->ieee80211_ptr); + struct ieee80211_chanctx_conf *chanctx_conf; + struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb); + struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data; + struct ieee80211_sub_if_data *tmp_sdata, *sdata; + struct cfg80211_chan_def *chandef; + int hdrlen; + + /* check for not even having the fixed 802.11 header */ + if (unlikely(skb->len < sizeof(struct ieee80211_hdr))) + goto fail; /* too short to be possibly valid */ + + hdrlen = ieee80211_hdrlen(hdr->frame_control); + + /* does the skb contain enough to deliver on the alleged length? */ + if (unlikely(skb->len < hdrlen)) + goto fail; /* skb too short for claimed header length */ + + skb_reset_mac_header(skb); + /* + * these are just fixed to the end of the rt area since we + * don't have any better information and at this point, nobody cares + */ + skb_reset_network_header(skb); + skb_reset_transport_header(skb); + + /* + * Initialize skb->protocol if the injected frame is a data frame + * carrying a rfc1042 header + */ + if (ieee80211_is_data(hdr->frame_control) && + skb->len >= hdrlen + sizeof(rfc1042_header) + 2) { + u8 *payload = (u8 *)hdr + hdrlen; + + if (ether_addr_equal(payload, rfc1042_header)) + skb->protocol = cpu_to_be16((payload[6] << 8) | + payload[7]); + } + + if (ieee80211_is_data_qos(hdr->frame_control)) { + u8 *p = ieee80211_get_qos_ctl(hdr); + + skb->priority = *p & IEEE80211_QOS_CTL_TAG1D_MASK; + } + + memset(info, 0, sizeof(*info)); + + info->flags = IEEE80211_TX_CTL_REQ_TX_STATUS | + IEEE80211_TX_CTL_INJECTED; + + + /* + * we might have set these flags later..... + * info->flags &= ~IEEE80211_TX_INTFL_DONT_ENCRYPT; + * info->flags &= ~IEEE80211_TX_CTL_DONTFRAG; + */ + + rcu_read_lock(); + + sdata = IEEE80211_DEV_TO_SUB_IF(dev); + + chanctx_conf = rcu_dereference(sdata->vif.chanctx_conf); + if (!chanctx_conf) { + tmp_sdata = rcu_dereference(local->monitor_sdata); + if (tmp_sdata) + chanctx_conf = + rcu_dereference(tmp_sdata->vif.chanctx_conf); + } + + if (chanctx_conf) + chandef = &chanctx_conf->def; + else if (!local->use_chanctx) + chandef = &local->_oper_chandef; + else + goto fail_rcu; + + /* + * Frame injection is not allowed if beaconing is not allowed + * or if we need radar detection. Beaconing is usually not allowed when + * the mode or operation (Adhoc, AP, Mesh) does not support DFS. + * Passive scan is also used in world regulatory domains where + * your country is not known and as such it should be treated as + * NO TX unless the channel is explicitly allowed in which case + * your current regulatory domain would not have the passive scan + * flag. + * + * Since AP mode uses monitor interfaces to inject/TX management + * frames we can make AP mode the exception to this rule once it + * supports radar detection as its implementation can deal with + * radar detection by itself. We can do that later by adding a + * monitor flag interfaces used for AP support. + */ + if (!cfg80211_reg_can_beacon(local->hw.wiphy, chandef, + sdata->vif.type)) + goto fail_rcu; + + info->band = chandef->chan->band; + ieee80211_xmit(sdata, NULL, skb); + rcu_read_unlock(); + + return; + +fail_rcu: + rcu_read_unlock(); +fail: + dev_kfree_skb(skb); +} + +/** + * ieee80211_capwap_subif_start_xmit - netif start_xmit function for 802.3 vifs + * @skb: packet to be sent + * @dev: incoming interface + * + * On failure skb will be freed. + */ +netdev_tx_t ieee80211_capwap_subif_start_xmit(struct sk_buff *skb, + struct net_device *dev) +{ + if (skb->protocol != htons(ETH_P_IP)) + printk(KERN_DEBUG "capwap xmit %s: protocol: %04X, data: %p, MAC: %p\n", + dev->name, ntohs(skb->protocol), skb->data, skb_mac_header(skb)); + + if (skb->protocol == htons(ETH_P_CONTROL)) { + __ieee80211_capwap_inject_start_xmit(skb, dev); + } else + __ieee80211_subif_start_xmit(skb, dev, 0); + + return NETDEV_TX_OK; +} + struct sk_buff * ieee80211_build_data_template(struct ieee80211_sub_if_data *sdata, struct sk_buff *skb, u32 info_flags) @@ -3914,3 +4052,177 @@ void __ieee80211_tx_skb_tid_band(struct ieee80211_sub_if_data *sdata, ieee80211_xmit(sdata, NULL, skb); local_bh_enable(); } + +netdev_tx_t ieee80211_inject_xmit(struct sk_buff* skb, struct net_device* dev) { + int multicast; + struct ieee80211_local *local = wdev_priv(dev->ieee80211_ptr); + struct ieee80211_chanctx_conf *chanctx_conf; + struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb); + struct ieee80211_sub_if_data *sdata; + struct cfg80211_chan_def *chandef; + struct ieee80211_hdr *hdr; + int hdrlen; + int queue_index; + + /* */ + if (skb->len < hdrlen) { + goto error; + } + + /* */ + skb->dev = dev; + skb_reset_mac_header(skb); + skb_reset_network_header(skb); + skb_reset_transport_header(skb); + + hdr = (struct ieee80211_hdr *)skb->data; + hdrlen = ieee80211_hdrlen(hdr->frame_control); + + if (skb->len < hdrlen) { + printk(KERN_WARNING, "droping packet for TO SMALL"); + goto error; + } + + /* + * Initialize skb->protocol if the injected frame is a data frame + * carrying a rfc1042 header + */ + if (ieee80211_is_data(hdr->frame_control) && + skb->len >= hdrlen + sizeof(rfc1042_header) + 2) { + u8 *payload = (u8 *)hdr + hdrlen; + + if (ether_addr_equal(payload, rfc1042_header)) + skb->protocol = cpu_to_be16((payload[6] << 8) | + payload[7]); + } + + memset(info, 0, sizeof(struct ieee80211_tx_info)); + + rcu_read_lock(); + + sdata = IEEE80211_DEV_TO_SUB_IF(dev); + + if (sdata->vif.type != NL80211_IFTYPE_AP) { + printk(KERN_WARNING, "droping packet for NOT AP"); + goto error_rcu; + } + + chanctx_conf = rcu_dereference(sdata->vif.chanctx_conf); + if (!chanctx_conf) { + printk(KERN_WARNING, "droping packet for NO CHANTX"); + goto error_rcu; + } + + chandef = &chanctx_conf->def; + + queue_index = ieee80211_select_queue_80211(sdata, skb, hdr); + skb_set_queue_mapping(skb, queue_index); + + info->band = chandef->chan->band; + info->hw_queue = + sdata->vif.hw_queue[queue_index]; + + /* + * Frame injection is not allowed if beaconing is not allowed + * or if we need radar detection. Beaconing is usually not allowed when + * the mode or operation (Adhoc, AP, Mesh) does not support DFS. + * Passive scan is also used in world regulatory domains where + * your country is not known and as such it should be treated as + * NO TX unless the channel is explicitly allowed in which case + * your current regulatory domain would not have the passive scan + * flag. + * + * Since AP mode uses monitor interfaces to inject/TX management + * frames we can make AP mode the exception to this rule once it + * supports radar detection as its implementation can deal with + * radar detection by itself. We can do that later by adding a + * monitor flag interfaces used for AP support. + */ + if (!cfg80211_reg_can_beacon(local->hw.wiphy, chandef, + sdata->vif.type)) { + printk(KERN_WARNING, "droping packet for CAN NOT BEACON"); + goto error_rcu; + } + /* */ + 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)) { + skb->pkt_type = PACKET_OTHERHOST; + } else { + printk(KERN_WARNING, "droping packet for STA not AUTHORIZED"); + goto error_rcu; + } + } else { + if (ether_addr_equal_64bits(hdr->addr1, dev->broadcast)) { + skb->pkt_type = PACKET_BROADCAST; + } else { + skb->pkt_type = PACKET_MULTICAST; + } + } + + /* */ + if (unlikely(!multicast && skb->sk && + skb_shinfo(skb)->tx_flags & SKBTX_WIFI_STATUS)) { + struct sk_buff *ack_skb = skb_clone_sk(skb); + + if (ack_skb) { + unsigned long flags; + int id; + struct ieee80211_local* local = sdata->local; + + spin_lock_irqsave(&local->ack_status_lock, flags); + id = idr_alloc(&local->ack_status_frames, ack_skb, + 1, 0x10000, GFP_ATOMIC); + spin_unlock_irqrestore(&local->ack_status_lock, flags); + + if (id >= 0) { + info->ack_frame_id = id; + info->flags |= IEEE80211_TX_CTL_REQ_TX_STATUS; + } else { + kfree_skb(ack_skb); + } + } + } + +#if 0 + /* 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->ack_frame_id); + + skb = skb_clone(skb, GFP_ATOMIC); + kfree_skb(tmp_skb); + + if (!skb) + goto error_rcu; + } +#endif + + /* */ + hdr->duration_id = 0; + hdr->seq_ctrl = 0; + + /* */ + ieee80211_tx_stats(dev, skb->len); + + /* */ +/* dev->trans_start = jiffies; */ + + /* */ + ieee80211_xmit(sdata, NULL, skb); + rcu_read_unlock(); + + return NETDEV_TX_OK; + +error_rcu: + rcu_read_unlock(); + +error: + dev_kfree_skb(skb); + return NETDEV_TX_OK; +} +EXPORT_SYMBOL(ieee80211_inject_xmit); + -- 2.8.1