mac80211: add support for radiotap timestamp field

[cascardo/linux.git] / net / mac80211 / rx.c

diff --git a/net/mac80211/rx.c b/net/mac80211/rx.c
index 708c3b1..284f0f2 100644 (file)
--- a/net/mac80211/rx.c
+++ b/net/mac80211/rx.c
@@ -180,6 +180,11 @@ ieee80211_rx_radiotap_hdrlen(struct ieee80211_local *local,
                len += 12;
        }
 
+       if (local->hw.radiotap_timestamp.units_pos >= 0) {
+               len = ALIGN(len, 8);
+               len += 12;
+       }
+
        if (status->chains) {
                /* antenna and antenna signal fields */
                len += 2 * hweight8(status->chains);
@@ -447,6 +452,31 @@ ieee80211_add_rx_radiotap_header(struct ieee80211_local *local,
                pos += 2;
        }
 
+       if (local->hw.radiotap_timestamp.units_pos >= 0) {
+               u16 accuracy = 0;
+               u8 flags = IEEE80211_RADIOTAP_TIMESTAMP_FLAG_32BIT;
+
+               rthdr->it_present |=
+                       cpu_to_le32(1 << IEEE80211_RADIOTAP_TIMESTAMP);
+
+               /* ensure 8 byte alignment */
+               while ((pos - (u8 *)rthdr) & 7)
+                       pos++;
+
+               put_unaligned_le64(status->device_timestamp, pos);
+               pos += sizeof(u64);
+
+               if (local->hw.radiotap_timestamp.accuracy >= 0) {
+                       accuracy = local->hw.radiotap_timestamp.accuracy;
+                       flags |= IEEE80211_RADIOTAP_TIMESTAMP_FLAG_ACCURACY;
+               }
+               put_unaligned_le16(accuracy, pos);
+               pos += sizeof(u16);
+
+               *pos++ = local->hw.radiotap_timestamp.units_pos;
+               *pos++ = flags;
+       }
+
        for_each_set_bit(chain, &chains, IEEE80211_MAX_CHAINS) {
                *pos++ = status->chain_signal[chain];
                *pos++ = chain;
@@ -485,6 +515,9 @@ ieee80211_rx_monitor(struct ieee80211_local *local, struct sk_buff *origskb,
        struct net_device *prev_dev = NULL;
        int present_fcs_len = 0;
        unsigned int rtap_vendor_space = 0;
+       struct ieee80211_mgmt *mgmt;
+       struct ieee80211_sub_if_data *monitor_sdata =
+               rcu_dereference(local->monitor_sdata);
 
        if (unlikely(status->flag & RX_FLAG_RADIOTAP_VENDOR_DATA)) {
                struct ieee80211_vendor_radiotap *rtap = (void *)origskb->data;
@@ -585,6 +618,23 @@ ieee80211_rx_monitor(struct ieee80211_local *local, struct sk_buff *origskb,
                ieee80211_rx_stats(sdata->dev, skb->len);
        }
 
+       mgmt = (void *)skb->data;
+       if (monitor_sdata &&
+           skb->len >= IEEE80211_MIN_ACTION_SIZE + 1 + VHT_MUMIMO_GROUPS_DATA_LEN &&
+           ieee80211_is_action(mgmt->frame_control) &&
+           mgmt->u.action.category == WLAN_CATEGORY_VHT &&
+           mgmt->u.action.u.vht_group_notif.action_code == WLAN_VHT_ACTION_GROUPID_MGMT &&
+           is_valid_ether_addr(monitor_sdata->u.mntr.mu_follow_addr) &&
+           ether_addr_equal(mgmt->da, monitor_sdata->u.mntr.mu_follow_addr)) {
+               struct sk_buff *mu_skb = skb_copy(skb, GFP_ATOMIC);
+
+               if (mu_skb) {
+                       mu_skb->pkt_type = IEEE80211_SDATA_QUEUE_TYPE_FRAME;
+                       skb_queue_tail(&monitor_sdata->skb_queue, mu_skb);
+                       ieee80211_queue_work(&local->hw, &monitor_sdata->work);
+               }
+       }
+
        if (prev_dev) {
                skb->dev = prev_dev;
                netif_receive_skb(skb);