wifi: ath11k: add P2P IE in beacon template

P2P Element is a necessary component of P2P protocol communication.
It contains the Vendor Specific Information Element which includes
the WFA OUI and an OUI Type indicating P2P.

Add P2P IE in beacon template, and implement WMI interface for it.

Tested-on: WCN6855 hw2.0 PCI WLAN.HSP.1.1-03125-QCAHSPSWPL_V1_V2_SILICONZ_LITE-3.6510.37
Tested-on: QCA2066 hw2.1 PCI WLAN.HSP.1.1-03926.13-QCAHSPSWPL_V2_SILICONZ_CE-2.52297.2

Signed-off-by: Kang Yang <quic_kangyang@quicinc.com>
Acked-by: Jeff Johnson <quic_jjohnson@quicinc.com>
Signed-off-by: Kalle Valo <quic_kvalo@quicinc.com>
Link: https://msgid.link/20240228093537.25052-3-quic_kangyang@quicinc.com

diff --git a/drivers/net/wireless/ath/ath11k/mac.c b/drivers/net/wireless/ath/ath11k/mac.c
index 8842e590df48847617ec6c2301a793d6db0948cf..e14da5e0f519e52284a58157018374ad02f4a49e 100644 (file)
--- a/drivers/net/wireless/ath/ath11k/mac.c
+++ b/drivers/net/wireless/ath/ath11k/mac.c
@@ -1423,10 +1423,67 @@ static bool ath11k_mac_set_nontx_vif_params(struct ath11k_vif *tx_arvif,
        return false;
 }
 
-static void ath11k_mac_set_vif_params(struct ath11k_vif *arvif,
-                                     struct sk_buff *bcn)
+static int ath11k_mac_setup_bcn_p2p_ie(struct ath11k_vif *arvif,
+                                      struct sk_buff *bcn)
 {
+       struct ath11k *ar = arvif->ar;
+       struct ieee80211_mgmt *mgmt;
+       const u8 *p2p_ie;
+       int ret;
+
+       mgmt = (void *)bcn->data;
+       p2p_ie = cfg80211_find_vendor_ie(WLAN_OUI_WFA, WLAN_OUI_TYPE_WFA_P2P,
+                                        mgmt->u.beacon.variable,
+                                        bcn->len - (mgmt->u.beacon.variable -
+                                                    bcn->data));
+       if (!p2p_ie)
+               return -ENOENT;
+
+       ret = ath11k_wmi_p2p_go_bcn_ie(ar, arvif->vdev_id, p2p_ie);
+       if (ret) {
+               ath11k_warn(ar->ab, "failed to submit P2P GO bcn ie for vdev %i: %d\n",
+                           arvif->vdev_id, ret);
+               return ret;
+       }
+
+       return ret;
+}
+
+static int ath11k_mac_remove_vendor_ie(struct sk_buff *skb, unsigned int oui,
+                                      u8 oui_type, size_t ie_offset)
+{
+       size_t len;
+       const u8 *next, *end;
+       u8 *ie;
+
+       if (WARN_ON(skb->len < ie_offset))
+               return -EINVAL;
+
+       ie = (u8 *)cfg80211_find_vendor_ie(oui, oui_type,
+                                          skb->data + ie_offset,
+                                          skb->len - ie_offset);
+       if (!ie)
+               return -ENOENT;
+
+       len = ie[1] + 2;
+       end = skb->data + skb->len;
+       next = ie + len;
+
+       if (WARN_ON(next > end))
+               return -EINVAL;
+
+       memmove(ie, next, end - next);
+       skb_trim(skb, skb->len - len);
+
+       return 0;
+}
+
+static int ath11k_mac_set_vif_params(struct ath11k_vif *arvif,
+                                    struct sk_buff *bcn)
+{
+       struct ath11k_base *ab = arvif->ar->ab;
        struct ieee80211_mgmt *mgmt;
+       int ret = 0;
        u8 *ies;
 
        ies = bcn->data + ieee80211_get_hdrlen_from_skb(bcn);
@@ -1444,6 +1501,32 @@ static void ath11k_mac_set_vif_params(struct ath11k_vif *arvif,
                arvif->wpaie_present = true;
        else
                arvif->wpaie_present = false;
+
+       if (arvif->vdev_subtype != WMI_VDEV_SUBTYPE_P2P_GO)
+               return ret;
+
+       ret = ath11k_mac_setup_bcn_p2p_ie(arvif, bcn);
+       if (ret) {
+               ath11k_warn(ab, "failed to setup P2P GO bcn ie: %d\n",
+                           ret);
+               return ret;
+       }
+
+       /* P2P IE is inserted by firmware automatically (as
+        * configured above) so remove it from the base beacon
+        * template to avoid duplicate P2P IEs in beacon frames.
+        */
+       ret = ath11k_mac_remove_vendor_ie(bcn, WLAN_OUI_WFA,
+                                         WLAN_OUI_TYPE_WFA_P2P,
+                                         offsetof(struct ieee80211_mgmt,
+                                                  u.beacon.variable));
+       if (ret) {
+               ath11k_warn(ab, "failed to remove P2P vendor ie: %d\n",
+                           ret);
+               return ret;
+       }
+
+       return ret;
 }
 
 static int ath11k_mac_setup_bcn_tmpl_ema(struct ath11k_vif *arvif)
@@ -1465,10 +1548,12 @@ static int ath11k_mac_setup_bcn_tmpl_ema(struct ath11k_vif *arvif)
                return -EPERM;
        }
 
-       if (tx_arvif == arvif)
-               ath11k_mac_set_vif_params(tx_arvif, beacons->bcn[0].skb);
-       else
+       if (tx_arvif == arvif) {
+               if (ath11k_mac_set_vif_params(tx_arvif, beacons->bcn[0].skb))
+                       return -EINVAL;
+       } else {
                arvif->wpaie_present = tx_arvif->wpaie_present;
+       }
 
        for (i = 0; i < beacons->cnt; i++) {
                if (tx_arvif != arvif && !nontx_vif_params_set)
@@ -1527,10 +1612,12 @@ static int ath11k_mac_setup_bcn_tmpl_mbssid(struct ath11k_vif *arvif)
                return -EPERM;
        }
 
-       if (tx_arvif == arvif)
-               ath11k_mac_set_vif_params(tx_arvif, bcn);
-       else if (!ath11k_mac_set_nontx_vif_params(tx_arvif, arvif, bcn))
+       if (tx_arvif == arvif) {
+               if (ath11k_mac_set_vif_params(tx_arvif, bcn))
+                       return -EINVAL;
+       } else if (!ath11k_mac_set_nontx_vif_params(tx_arvif, arvif, bcn)) {
                return -EINVAL;
+       }
 
        ret = ath11k_wmi_bcn_tmpl(ar, arvif->vdev_id, &offs, bcn, 0);
        kfree_skb(bcn);

diff --git a/drivers/net/wireless/ath/ath11k/wmi.c b/drivers/net/wireless/ath/ath11k/wmi.c
index 34ab9631ff365c55cb3314ea30df58a68ce4d71b..d86fcdd374c6b5ecdbfae323a297e76bf6e6532d 100644 (file)
--- a/drivers/net/wireless/ath/ath11k/wmi.c
+++ b/drivers/net/wireless/ath/ath11k/wmi.c
@@ -1704,6 +1704,45 @@ int ath11k_wmi_send_bcn_offload_control_cmd(struct ath11k *ar,
        return ret;
 }
 
+int ath11k_wmi_p2p_go_bcn_ie(struct ath11k *ar, u32 vdev_id,
+                            const u8 *p2p_ie)
+{
+       struct ath11k_pdev_wmi *wmi = ar->wmi;
+       struct wmi_p2p_go_set_beacon_ie_cmd *cmd;
+       size_t p2p_ie_len, aligned_len;
+       struct wmi_tlv *tlv;
+       struct sk_buff *skb;
+       int ret, len;
+
+       p2p_ie_len = p2p_ie[1] + 2;
+       aligned_len = roundup(p2p_ie_len, 4);
+
+       len = sizeof(*cmd) + TLV_HDR_SIZE + aligned_len;
+
+       skb = ath11k_wmi_alloc_skb(wmi->wmi_ab, len);
+       if (!skb)
+               return -ENOMEM;
+
+       cmd = (struct wmi_p2p_go_set_beacon_ie_cmd *)skb->data;
+       cmd->tlv_header = FIELD_PREP(WMI_TLV_TAG, WMI_TAG_P2P_GO_SET_BEACON_IE) |
+                         FIELD_PREP(WMI_TLV_LEN, sizeof(*cmd) - TLV_HDR_SIZE);
+       cmd->vdev_id = vdev_id;
+       cmd->ie_buf_len = p2p_ie_len;
+
+       tlv = (struct wmi_tlv *)cmd->tlv;
+       tlv->header = FIELD_PREP(WMI_TLV_TAG, WMI_TAG_ARRAY_BYTE) |
+                     FIELD_PREP(WMI_TLV_LEN, aligned_len);
+       memcpy(tlv->value, p2p_ie, p2p_ie_len);
+
+       ret = ath11k_wmi_cmd_send(wmi, skb, WMI_P2P_GO_SET_BEACON_IE);
+       if (ret) {
+               ath11k_warn(ar->ab, "failed to send WMI_P2P_GO_SET_BEACON_IE\n");
+               dev_kfree_skb(skb);
+       }
+
+       return ret;
+}
+
 int ath11k_wmi_bcn_tmpl(struct ath11k *ar, u32 vdev_id,
                        struct ieee80211_mutable_offsets *offs,
                        struct sk_buff *bcn, u32 ema_params)

diff --git a/drivers/net/wireless/ath/ath11k/wmi.h b/drivers/net/wireless/ath/ath11k/wmi.h
index 1c980f56bc729283ae1e17419d7db080933f3c23..9b8344a74a6f0ac30292727ddcb0e64e3026e5f1 100644 (file)
--- a/drivers/net/wireless/ath/ath11k/wmi.h
+++ b/drivers/net/wireless/ath/ath11k/wmi.h
@@ -3625,6 +3625,13 @@ struct wmi_bcn_tmpl_cmd {
        u32 ema_params;
 } __packed;
 
+struct wmi_p2p_go_set_beacon_ie_cmd {
+       u32 tlv_header;
+       u32 vdev_id;
+       u32 ie_buf_len;
+       u8 tlv[];
+} __packed;
+
 struct wmi_key_seq_counter {
        u32 key_seq_counter_l;
        u32 key_seq_counter_h;
@@ -6321,6 +6328,8 @@ int ath11k_wmi_cmd_send(struct ath11k_pdev_wmi *wmi, struct sk_buff *skb,
 struct sk_buff *ath11k_wmi_alloc_skb(struct ath11k_wmi_base *wmi_sc, u32 len);
 int ath11k_wmi_mgmt_send(struct ath11k *ar, u32 vdev_id, u32 buf_id,
                         struct sk_buff *frame);
+int ath11k_wmi_p2p_go_bcn_ie(struct ath11k *ar, u32 vdev_id,
+                            const u8 *p2p_ie);
 int ath11k_wmi_bcn_tmpl(struct ath11k *ar, u32 vdev_id,
                        struct ieee80211_mutable_offsets *offs,
                        struct sk_buff *bcn, u32 ema_param);