On 2/25/2024 10:01 PM, Kang Yang wrote:
> 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@xxxxxxxxxxx>
> ---
> drivers/net/wireless/ath/ath11k/mac.c | 103 ++++++++++++++++++++++++--
> drivers/net/wireless/ath/ath11k/wmi.c | 39 ++++++++++
> drivers/net/wireless/ath/ath11k/wmi.h | 9 +++
> 3 files changed, 143 insertions(+), 8 deletions(-)
>
> diff --git a/drivers/net/wireless/ath/ath11k/mac.c b/drivers/net/wireless/ath/ath11k/mac.c
> index 9240dedf3217..f52dd52dabbb 100644
> --- a/drivers/net/wireless/ath/ath11k/mac.c
> +++ b/drivers/net/wireless/ath/ath11k/mac.c
> @@ -1430,10 +1430,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 = 0;
unnecessary initializer?
> +
> + 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);
> @@ -1451,6 +1508,32 @@ static void ath11k_mac_set_vif_params(struct ath11k_vif *arvif,
> arvif->wpaie_present = true;
> else
> arvif->wpaie_present = false;
> +
> + if (arvif->vif->type != NL80211_IFTYPE_AP || !arvif->vif->p2p)
> + 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)
> @@ -1472,10 +1555,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)
> @@ -1534,10 +1619,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 34ab9631ff36..d86fcdd374c6 100644
> --- 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 bb419e3abb00..4c20202947c7 100644
> --- a/drivers/net/wireless/ath/ath11k/wmi.h
> +++ b/drivers/net/wireless/ath/ath11k/wmi.h
> @@ -3653,6 +3653,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;
> @@ -6349,6 +6356,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);
