wl12xx: remove P2P ie from probe response

wl12xx uses a single probe response template, regardless of
the probe request.
However, the P2P spec forbids including the p2p ie in some
cases (e.g. the probe request didn't include the p2p ie).

The fw responds only to probe requests that don't
include the p2p ie, and passes up probe requests that
include them (the supplicant will answer them).

Thus, strip the p2p ie from the probe response template.

Signed-off-by: Eliad Peller <eliad@wizery.com>
Signed-off-by: Luciano Coelho <coelho@ti.com>

drivers/net/wireless/wl12xx/main.c

@@ -3083,6 +3083,23 @@ static void wl12xx_remove_ie(struct sk_buff *skb, u8 eid, int ieoffset)
 	skb_trim(skb, skb->len - len);
 }
 
+static void wl12xx_remove_vendor_ie(struct sk_buff *skb,
+					    unsigned int oui, u8 oui_type,
+					    int ieoffset)
+{
+	int len;
+	const u8 *next, *end = skb->data + skb->len;
+	u8 *ie = (u8 *)cfg80211_find_vendor_ie(oui, oui_type,
+					       skb->data + ieoffset,
+					       skb->len - ieoffset);
+	if (!ie)
+		return;
+	len = ie[1] + 2;
+	next = ie + len;
+	memmove(ie, next, end - next);
+	skb_trim(skb, skb->len - len);
+}
+
 static int wl1271_bss_erp_info_changed(struct wl1271 *wl,
 				       struct ieee80211_bss_conf *bss_conf,
 				       u32 changed)
@@ -3168,6 +3185,17 @@ static int wl1271_bss_beacon_info_changed(struct wl1271 *wl,
 		/* remove TIM ie from probe response */
 		wl12xx_remove_ie(beacon, WLAN_EID_TIM, ieoffset);
 
+		/*
+		 * remove p2p ie from probe response.
+		 * the fw reponds to probe requests that don't include
+		 * the p2p ie. probe requests with p2p ie will be passed,
+		 * and will be responded by the supplicant (the spec
+		 * forbids including the p2p ie when responding to probe
+		 * requests that didn't include it).
+		 */
+		wl12xx_remove_vendor_ie(beacon, WLAN_OUI_WFA,
+					WLAN_OUI_TYPE_WFA_P2P, ieoffset);
+
 		hdr = (struct ieee80211_hdr *) beacon->data;
 		hdr->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT |
 						 IEEE80211_STYPE_PROBE_RESP);