diff -ur a/include/net/mac80211.h b/include/net/mac80211.h --- a/include/net/mac80211.h 2014-12-23 18:25:24.000000000 +0100 +++ 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); +/** + * + */ +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); + #endif /* MAC80211_H */ diff -ur a/net/mac80211/ieee80211_i.h b/net/mac80211/ieee80211_i.h --- a/net/mac80211/ieee80211_i.h 2014-12-23 18:25:24.000000000 +0100 +++ 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) +#define RX_IGNORE_MONITOR ((__force ieee80211_rx_result) 4u) /** * enum ieee80211_packet_rx_flags - packet RX flags @@ -743,6 +744,9 @@ 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; diff -ur a/net/mac80211/iface.c b/net/mac80211/iface.c --- a/net/mac80211/iface.c 2014-12-23 18:25:24.000000000 +0100 +++ b/net/mac80211/iface.c 2014-07-23 21:22:30.000000000 +0200 @@ -1844,3 +1844,45 @@ { unregister_netdevice_notifier(&mac80211_netdev_notifier); } + +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); + + 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 -ur a/net/mac80211/rx.c b/net/mac80211/rx.c --- a/net/mac80211/rx.c 2014-12-23 18:25:24.000000000 +0100 +++ b/net/mac80211/rx.c 2014-07-23 21:22:30.000000000 +0200 @@ -2831,6 +2831,51 @@ 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 (rx->local->hw.flags & IEEE80211_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) @@ -2910,6 +2955,7 @@ 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; @@ -2938,7 +2984,9 @@ } 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; @@ -2971,6 +3019,11 @@ 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) @@ -2994,7 +3047,8 @@ 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; @@ -3012,7 +3066,7 @@ ieee80211_rx_reorder_ampdu(rx, &reorder_release); - ieee80211_rx_handlers(rx, &reorder_release); + ieee80211_rx_handlers(rx, &reorder_release, rate); return; rxh_next: @@ -3049,7 +3103,7 @@ ieee80211_sta_reorder_release(sta->sdata, tid_agg_rx, &frames); spin_unlock(&tid_agg_rx->reorder_lock); - ieee80211_rx_handlers(&rx, &frames); + ieee80211_rx_handlers(&rx, &frames, NULL); } /* main receive path */ @@ -3163,7 +3217,9 @@ * 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; @@ -3189,7 +3245,7 @@ rx->skb = skb; } - ieee80211_invoke_rx_handlers(rx); + ieee80211_invoke_rx_handlers(rx, rate); return true; } @@ -3198,7 +3254,8 @@ * be called with rcu_read_lock protection. */ static void __ieee80211_rx_handle_packet(struct ieee80211_hw *hw, - struct sk_buff *skb) + struct sk_buff *skb, + struct ieee80211_rate *rate) { struct ieee80211_local *local = hw_to_local(hw); struct ieee80211_sub_if_data *sdata; @@ -3251,7 +3308,7 @@ 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; } @@ -3260,7 +3317,7 @@ 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; } @@ -3289,7 +3346,7 @@ 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; } @@ -3298,7 +3355,7 @@ 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; } @@ -3409,7 +3466,7 @@ ieee80211_tpt_led_trig_rx(local, ((struct ieee80211_hdr *)skb->data)->frame_control, skb->len); - __ieee80211_rx_handle_packet(hw, skb); + __ieee80211_rx_handle_packet(hw, skb, rate); 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); +