diff options
| -rw-r--r-- | drivers/net/wireless/ath/ath12k/mac.c | 84 | ||||
| -rw-r--r-- | drivers/net/wireless/ath/ath12k/wmi.c | 42 | ||||
| -rw-r--r-- | drivers/net/wireless/ath/ath12k/wmi.h | 8 |
3 files changed, 132 insertions, 2 deletions
diff --git a/drivers/net/wireless/ath/ath12k/mac.c b/drivers/net/wireless/ath/ath12k/mac.c index f2920a3401c6..7bfc6ee6567c 100644 --- a/drivers/net/wireless/ath/ath12k/mac.c +++ b/drivers/net/wireless/ath/ath12k/mac.c @@ -1139,6 +1139,63 @@ static int ath12k_mac_op_config(struct ieee80211_hw *hw, u32 changed) return ret; } +static int ath12k_mac_setup_bcn_p2p_ie(struct ath12k_vif *arvif, + struct sk_buff *bcn) +{ + struct ath12k *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) { + ath12k_warn(ar->ab, "no P2P ie found in beacon\n"); + return -ENOENT; + } + + ret = ath12k_wmi_p2p_go_bcn_ie(ar, arvif->vdev_id, p2p_ie); + if (ret) { + ath12k_warn(ar->ab, "failed to submit P2P GO bcn ie for vdev %i: %d\n", + arvif->vdev_id, ret); + return ret; + } + + return 0; +} + +static int ath12k_mac_remove_vendor_ie(struct sk_buff *skb, unsigned int oui, + u8 oui_type, size_t ie_offset) +{ + const u8 *next, *end; + size_t len; + 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 ath12k_mac_setup_bcn_tmpl(struct ath12k_vif *arvif) { struct ath12k *ar = arvif->ar; @@ -1171,14 +1228,37 @@ static int ath12k_mac_setup_bcn_tmpl(struct ath12k_vif *arvif) ies, (skb_tail_pointer(bcn) - ies))) arvif->wpaie_present = true; - ret = ath12k_wmi_bcn_tmpl(ar, arvif->vdev_id, &offs, bcn); + if (arvif->vif->type == NL80211_IFTYPE_AP && arvif->vif->p2p) { + ret = ath12k_mac_setup_bcn_p2p_ie(arvif, bcn); + if (ret) { + ath12k_warn(ab, "failed to setup P2P GO bcn ie: %d\n", + ret); + goto free_bcn_skb; + } - kfree_skb(bcn); + /* 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 = ath12k_mac_remove_vendor_ie(bcn, WLAN_OUI_WFA, + WLAN_OUI_TYPE_WFA_P2P, + offsetof(struct ieee80211_mgmt, + u.beacon.variable)); + if (ret) { + ath12k_warn(ab, "failed to remove P2P vendor ie: %d\n", + ret); + goto free_bcn_skb; + } + } + + ret = ath12k_wmi_bcn_tmpl(ar, arvif->vdev_id, &offs, bcn); if (ret) ath12k_warn(ab, "failed to submit beacon template command: %d\n", ret); +free_bcn_skb: + kfree_skb(bcn); return ret; } diff --git a/drivers/net/wireless/ath/ath12k/wmi.c b/drivers/net/wireless/ath/ath12k/wmi.c index 49ac8d4ccbef..61fc7ec86985 100644 --- a/drivers/net/wireless/ath/ath12k/wmi.c +++ b/drivers/net/wireless/ath/ath12k/wmi.c @@ -1712,6 +1712,48 @@ int ath12k_wmi_send_bcn_offload_control_cmd(struct ath12k *ar, return ret; } +int ath12k_wmi_p2p_go_bcn_ie(struct ath12k *ar, u32 vdev_id, + const u8 *p2p_ie) +{ + struct ath12k_wmi_pdev *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; + void *ptr; + int ret, len; + + p2p_ie_len = p2p_ie[1] + 2; + aligned_len = roundup(p2p_ie_len, sizeof(u32)); + + len = sizeof(*cmd) + TLV_HDR_SIZE + aligned_len; + + skb = ath12k_wmi_alloc_skb(wmi->wmi_ab, len); + if (!skb) + return -ENOMEM; + + ptr = skb->data; + cmd = ptr; + cmd->tlv_header = ath12k_wmi_tlv_cmd_hdr(WMI_TAG_P2P_GO_SET_BEACON_IE, + sizeof(*cmd)); + cmd->vdev_id = cpu_to_le32(vdev_id); + cmd->ie_buf_len = cpu_to_le32(p2p_ie_len); + + ptr += sizeof(*cmd); + tlv = ptr; + tlv->header = ath12k_wmi_tlv_cmd_hdr(WMI_TAG_ARRAY_BYTE, + aligned_len); + memcpy(tlv->value, p2p_ie, p2p_ie_len); + + ret = ath12k_wmi_cmd_send(wmi, skb, WMI_P2P_GO_SET_BEACON_IE); + if (ret) { + ath12k_warn(ar->ab, "failed to send WMI_P2P_GO_SET_BEACON_IE\n"); + dev_kfree_skb(skb); + } + + return ret; +} + int ath12k_wmi_bcn_tmpl(struct ath12k *ar, u32 vdev_id, struct ieee80211_mutable_offsets *offs, struct sk_buff *bcn) diff --git a/drivers/net/wireless/ath/ath12k/wmi.h b/drivers/net/wireless/ath/ath12k/wmi.h index 61b2dd1d32cd..c5002468b1de 100644 --- a/drivers/net/wireless/ath/ath12k/wmi.h +++ b/drivers/net/wireless/ath/ath12k/wmi.h @@ -3518,6 +3518,12 @@ struct wmi_bcn_tmpl_cmd { __le32 esp_ie_offset; } __packed; +struct wmi_p2p_go_set_beacon_ie_cmd { + __le32 tlv_header; + __le32 vdev_id; + __le32 ie_buf_len; +} __packed; + struct wmi_vdev_install_key_cmd { __le32 tlv_header; __le32 vdev_id; @@ -4831,6 +4837,8 @@ int ath12k_wmi_cmd_send(struct ath12k_wmi_pdev *wmi, struct sk_buff *skb, struct sk_buff *ath12k_wmi_alloc_skb(struct ath12k_wmi_base *wmi_sc, u32 len); int ath12k_wmi_mgmt_send(struct ath12k *ar, u32 vdev_id, u32 buf_id, struct sk_buff *frame); +int ath12k_wmi_p2p_go_bcn_ie(struct ath12k *ar, u32 vdev_id, + const u8 *p2p_ie); int ath12k_wmi_bcn_tmpl(struct ath12k *ar, u32 vdev_id, struct ieee80211_mutable_offsets *offs, struct sk_buff *bcn); |