ath6kl: Include P2P IE(s) in GO Probe Response depending on request
authorJouni Malinen <jouni@qca.qualcomm.com>
Tue, 30 Aug 2011 18:58:09 +0000 (21:58 +0300)
committerKalle Valo <kvalo@qca.qualcomm.com>
Wed, 31 Aug 2011 07:13:02 +0000 (10:13 +0300)
P2P has special rules on when to include P2P IE(s) in Probe Response
frame based on the Probe Request frame. Handle P2P IE(s) separately
to follow these rules.

Signed-off-by: Jouni Malinen <jouni@qca.qualcomm.com>
Signed-off-by: Kalle Valo <kvalo@qca.qualcomm.com>
drivers/net/wireless/ath/ath6kl/cfg80211.c

index a3aa1505894734543c9f2714ae8f1210c35263ea..2136899561df83adfe0d1b53116d2f2bab37830a 100644 (file)
@@ -1518,6 +1518,48 @@ static int ath6kl_set_channel(struct wiphy *wiphy, struct net_device *dev,
        return 0;
 }
 
+static bool ath6kl_is_p2p_ie(const u8 *pos)
+{
+       return pos[0] == WLAN_EID_VENDOR_SPECIFIC && pos[1] >= 4 &&
+               pos[2] == 0x50 && pos[3] == 0x6f &&
+               pos[4] == 0x9a && pos[5] == 0x09;
+}
+
+static int ath6kl_set_ap_probe_resp_ies(struct ath6kl *ar, const u8 *ies,
+                                       size_t ies_len)
+{
+       const u8 *pos;
+       u8 *buf = NULL;
+       size_t len = 0;
+       int ret;
+
+       /*
+        * Filter out P2P IE(s) since they will be included depending on
+        * the Probe Request frame in ath6kl_send_go_probe_resp().
+        */
+
+       if (ies && ies_len) {
+               buf = kmalloc(ies_len, GFP_KERNEL);
+               if (buf == NULL)
+                       return -ENOMEM;
+               pos = ies;
+               while (pos + 1 < ies + ies_len) {
+                       if (pos + 2 + pos[1] > ies + ies_len)
+                               break;
+                       if (!ath6kl_is_p2p_ie(pos)) {
+                               memcpy(buf + len, pos, 2 + pos[1]);
+                               len += 2 + pos[1];
+                       }
+                       pos += 2 + pos[1];
+               }
+       }
+
+       ret = ath6kl_wmi_set_appie_cmd(ar->wmi, WMI_FRAME_PROBE_RESP,
+                                      buf, len);
+       kfree(buf);
+       return ret;
+}
+
 static int ath6kl_ap_beacon(struct wiphy *wiphy, struct net_device *dev,
                            struct beacon_parameters *info, bool add)
 {
@@ -1545,9 +1587,8 @@ static int ath6kl_ap_beacon(struct wiphy *wiphy, struct net_device *dev,
                        return res;
        }
        if (info->proberesp_ies) {
-               res = ath6kl_wmi_set_appie_cmd(ar->wmi, WMI_FRAME_PROBE_RESP,
-                                              info->proberesp_ies,
-                                              info->proberesp_ies_len);
+               res = ath6kl_set_ap_probe_resp_ies(ar, info->proberesp_ies,
+                                                  info->proberesp_ies_len);
                if (res)
                        return res;
        }
@@ -1735,6 +1776,41 @@ static int ath6kl_cancel_remain_on_channel(struct wiphy *wiphy,
        return ath6kl_wmi_cancel_remain_on_chnl_cmd(ar->wmi);
 }
 
+static int ath6kl_send_go_probe_resp(struct ath6kl *ar, const u8 *buf,
+                                    size_t len, unsigned int freq)
+{
+       const u8 *pos;
+       u8 *p2p;
+       int p2p_len;
+       int ret;
+       const struct ieee80211_mgmt *mgmt;
+
+       mgmt = (const struct ieee80211_mgmt *) buf;
+
+       /* Include P2P IE(s) from the frame generated in user space. */
+
+       p2p = kmalloc(len, GFP_KERNEL);
+       if (p2p == NULL)
+               return -ENOMEM;
+       p2p_len = 0;
+
+       pos = mgmt->u.probe_resp.variable;
+       while (pos + 1 < buf + len) {
+               if (pos + 2 + pos[1] > buf + len)
+                       break;
+               if (ath6kl_is_p2p_ie(pos)) {
+                       memcpy(p2p + p2p_len, pos, 2 + pos[1]);
+                       p2p_len += 2 + pos[1];
+               }
+               pos += 2 + pos[1];
+       }
+
+       ret = ath6kl_wmi_send_probe_response_cmd(ar->wmi, freq, mgmt->da,
+                                                p2p, p2p_len);
+       kfree(p2p);
+       return ret;
+}
+
 static int ath6kl_mgmt_tx(struct wiphy *wiphy, struct net_device *dev,
                          struct ieee80211_channel *chan, bool offchan,
                          enum nl80211_channel_type channel_type,
@@ -1743,6 +1819,20 @@ static int ath6kl_mgmt_tx(struct wiphy *wiphy, struct net_device *dev,
 {
        struct ath6kl *ar = ath6kl_priv(dev);
        u32 id;
+       const struct ieee80211_mgmt *mgmt;
+
+       mgmt = (const struct ieee80211_mgmt *) buf;
+       if (buf + len >= mgmt->u.probe_resp.variable &&
+           ar->nw_type == AP_NETWORK && test_bit(CONNECTED, &ar->flag) &&
+           ieee80211_is_probe_resp(mgmt->frame_control)) {
+               /*
+                * Send Probe Response frame in AP mode using a separate WMI
+                * command to allow the target to fill in the generic IEs.
+                */
+               *cookie = 0; /* TX status not supported */
+               return ath6kl_send_go_probe_resp(ar, buf, len,
+                                                chan->center_freq);
+       }
 
        id = ar->send_action_id++;
        if (id == 0) {