4 files changed, 68 insertions, 0 deletions

diff --git a/drivers/net/wireless/iwlwifi/iwl-agn-rx.c b/drivers/net/wireless/iwlwifi/iwl-agn-rx.c
index 5af9e6258a16..f0d6d9429be7 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn-rx.c
+++ b/drivers/net/wireless/iwlwifi/iwl-agn-rx.c
@@ -1032,6 +1032,50 @@ static int iwlagn_rx_reply_rx(struct iwl_priv *priv,
         return 0;
 }
 
+static int iwlagn_rx_noa_notification(struct iwl_priv *priv,
+                                      struct iwl_rx_mem_buffer *rxb,
+                                      struct iwl_device_cmd *cmd)
+{
+        struct iwl_wipan_noa_data *new_data, *old_data;
+        struct iwl_rx_packet *pkt = rxb_addr(rxb);
+        struct iwl_wipan_noa_notification *noa_notif = (void *)pkt->u.raw;
+
+        /* no condition -- we're in softirq */
+        old_data = rcu_dereference_protected(priv->noa_data, true);
+
+        if (noa_notif->noa_active) {
+                u32 len = le16_to_cpu(noa_notif->noa_attribute.length);
+                u32 copylen = len;
+
+                /* EID, len, OUI, subtype */
+                len += 1 + 1 + 3 + 1;
+                /* P2P id, P2P length */
+                len += 1 + 2;
+                copylen += 1 + 2;
+
+                new_data = kmalloc(sizeof(*new_data) + len, GFP_ATOMIC);
+                if (new_data) {
+                        new_data->length = len;
+                        new_data->data[0] = WLAN_EID_VENDOR_SPECIFIC;
+                        new_data->data[1] = len - 2; /* not counting EID, len */
+                        new_data->data[2] = (WLAN_OUI_WFA >> 16) & 0xff;
+                        new_data->data[3] = (WLAN_OUI_WFA >> 8) & 0xff;
+                        new_data->data[4] = (WLAN_OUI_WFA >> 0) & 0xff;
+                        new_data->data[5] = WLAN_OUI_TYPE_WFA_P2P;
+                        memcpy(&new_data->data[6], &noa_notif->noa_attribute,
+                               copylen);
+                }
+        } else
+                new_data = NULL;
+
+        rcu_assign_pointer(priv->noa_data, new_data);
+
+        if (old_data)
+                kfree_rcu(old_data, rcu_head);
+
+        return 0;
+}
+
 /**
  * iwl_setup_rx_handlers - Initialize Rx handler callbacks
  *
@@ -1055,6 +1099,8 @@ void iwl_setup_rx_handlers(struct iwl_priv *priv)
         handlers[BEACON_NOTIFICATION]           = iwlagn_rx_beacon_notif;
         handlers[REPLY_ADD_STA]                 = iwl_add_sta_callback;
 
+        handlers[REPLY_WIPAN_NOA_NOTIFICATION]  = iwlagn_rx_noa_notification;
+
         /*
          * The same handler is used for both the REPLY to a discrete
          * statistics request from the host as well as for the periodic
diff --git a/drivers/net/wireless/iwlwifi/iwl-agn-tx.c b/drivers/net/wireless/iwlwifi/iwl-agn-tx.c
index 35a6b71f358c..014b98ab6816 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn-tx.c
+++ b/drivers/net/wireless/iwlwifi/iwl-agn-tx.c
@@ -283,6 +283,19 @@ int iwlagn_tx_skb(struct iwl_priv *priv, struct sk_buff *skb)
                 IWL_DEBUG_TX(priv, "Sending REASSOC frame\n");
 #endif
 
+        if (unlikely(ieee80211_is_probe_resp(fc))) {
+                struct iwl_wipan_noa_data *noa_data =
+                        rcu_dereference(priv->noa_data);
+
+                if (noa_data &&
+                    pskb_expand_head(skb, 0, noa_data->length,
+                                     GFP_ATOMIC) == 0) {
+                        memcpy(skb_put(skb, noa_data->length),
+                               noa_data->data, noa_data->length);
+                        hdr = (struct ieee80211_hdr *)skb->data;
+                }
+        }
+
         hdr_len = ieee80211_hdrlen(fc);
 
         /* For management frames use broadcast id to do not break aggregation */
diff --git a/drivers/net/wireless/iwlwifi/iwl-agn.c b/drivers/net/wireless/iwlwifi/iwl-agn.c
index 9d463cf40380..3c0f4e6c9357 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn.c
+++ b/drivers/net/wireless/iwlwifi/iwl-agn.c
@@ -3069,6 +3069,7 @@ static void iwl_uninit_drv(struct iwl_priv *priv)
                 kmem_cache_destroy(priv->tx_cmd_pool);
         kfree(priv->scan_cmd);
         kfree(priv->beacon_cmd);
+        kfree(rcu_dereference_raw(priv->noa_data));
 #ifdef CONFIG_IWLWIFI_DEBUGFS
         kfree(priv->wowlan_sram);
 #endif
diff --git a/drivers/net/wireless/iwlwifi/iwl-dev.h b/drivers/net/wireless/iwlwifi/iwl-dev.h
index 6c00a447963d..ef8620b10bbc 100644
--- a/drivers/net/wireless/iwlwifi/iwl-dev.h
+++ b/drivers/net/wireless/iwlwifi/iwl-dev.h
@@ -824,6 +824,12 @@ struct iwl_testmode_trace {
 };
 #endif
 
+struct iwl_wipan_noa_data {
+        struct rcu_head rcu_head;
+        u32 length;
+        u8 data[];
+};
+
 struct iwl_priv {
 
         /*data shared among all the driver's layers */
@@ -883,6 +889,8 @@ struct iwl_priv {
         /* init calibration results */
         struct iwl_calib_result calib_results[IWL_CALIB_MAX];
 
+        struct iwl_wipan_noa_data __rcu *noa_data;
+
         /* Scan related variables */
         unsigned long scan_start;
         unsigned long scan_start_tsf;