ath10k: implement p2p bcn ie command

Along beacon template host is expected to setup
p2p information elements as well. Implement wmi
interface for it.

Signed-off-by: Michal Kazior <michal.kazior@tieto.com>
Signed-off-by: Kalle Valo <kvalo@qca.qualcomm.com>

4 files changed, 66 insertions, 0 deletions

diff --git a/drivers/net/wireless/ath/ath10k/wmi-ops.h b/drivers/net/wireless/ath/ath10k/wmi-ops.h
index 7084096c0f62..0dd49a7a89f0 100644
--- a/drivers/net/wireless/ath/ath10k/wmi-ops.h
+++ b/drivers/net/wireless/ath/ath10k/wmi-ops.h
@@ -135,6 +135,8 @@ struct wmi_ops {
                                         void *prb_ies, size_t prb_ies_len);
         struct sk_buff *(*gen_prb_tmpl)(struct ath10k *ar, u32 vdev_id,
                                         struct sk_buff *bcn);
+        struct sk_buff *(*gen_p2p_go_bcn_ie)(struct ath10k *ar, u32 vdev_id,
+                                             const u8 *p2p_ie);
 };
 
 int ath10k_wmi_cmd_send(struct ath10k *ar, struct sk_buff *skb, u32 cmd_id);
@@ -975,4 +977,19 @@ ath10k_wmi_prb_tmpl(struct ath10k *ar, u32 vdev_id, struct sk_buff *prb)
         return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->prb_tmpl_cmdid);
 }
 
+static inline int
+ath10k_wmi_p2p_go_bcn_ie(struct ath10k *ar, u32 vdev_id, const u8 *p2p_ie)
+{
+        struct sk_buff *skb;
+
+        if (!ar->wmi.ops->gen_p2p_go_bcn_ie)
+                return -EOPNOTSUPP;
+
+        skb = ar->wmi.ops->gen_p2p_go_bcn_ie(ar, vdev_id, p2p_ie);
+        if (IS_ERR(skb))
+                return PTR_ERR(skb);
+
+        return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->p2p_go_set_beacon_ie);
+}
+
 #endif
diff --git a/drivers/net/wireless/ath/ath10k/wmi-tlv.c b/drivers/net/wireless/ath/ath10k/wmi-tlv.c
index 29dc0941cf08..6f34fc7d663e 100644
--- a/drivers/net/wireless/ath/ath10k/wmi-tlv.c
+++ b/drivers/net/wireless/ath/ath10k/wmi-tlv.c
@@ -2085,6 +2085,46 @@ ath10k_wmi_tlv_op_gen_prb_tmpl(struct ath10k *ar, u32 vdev_id,
         return skb;
 }
 
+static struct sk_buff *
+ath10k_wmi_tlv_op_gen_p2p_go_bcn_ie(struct ath10k *ar, u32 vdev_id,
+                                    const u8 *p2p_ie)
+{
+        struct wmi_tlv_p2p_go_bcn_ie *cmd;
+        struct wmi_tlv *tlv;
+        struct sk_buff *skb;
+        void *ptr;
+        size_t len;
+
+        len = sizeof(*tlv) + sizeof(*cmd) +
+              sizeof(*tlv) + roundup(p2p_ie[1] + 2, 4);
+        skb = ath10k_wmi_alloc_skb(ar, len);
+        if (!skb)
+                return ERR_PTR(-ENOMEM);
+
+        ptr = (void *)skb->data;
+        tlv = ptr;
+        tlv->tag = __cpu_to_le16(WMI_TLV_TAG_STRUCT_P2P_GO_SET_BEACON_IE);
+        tlv->len = __cpu_to_le16(sizeof(*cmd));
+        cmd = (void *)tlv->value;
+        cmd->vdev_id = __cpu_to_le32(vdev_id);
+        cmd->ie_len = __cpu_to_le32(p2p_ie[1] + 2);
+
+        ptr += sizeof(*tlv);
+        ptr += sizeof(*cmd);
+
+        tlv = ptr;
+        tlv->tag = __cpu_to_le16(WMI_TLV_TAG_ARRAY_BYTE);
+        tlv->len = __cpu_to_le16(roundup(p2p_ie[1] + 2, 4));
+        memcpy(tlv->value, p2p_ie, p2p_ie[1] + 2);
+
+        ptr += sizeof(*tlv);
+        ptr += roundup(p2p_ie[1] + 2, 4);
+
+        ath10k_dbg(ar, ATH10K_DBG_WMI, "wmi tlv p2p go bcn ie for vdev %i\n",
+                   vdev_id);
+        return skb;
+}
+
 /****************/
 /* TLV mappings */
 /****************/
@@ -2376,6 +2416,7 @@ static const struct wmi_ops wmi_tlv_ops = {
         /* .gen_delba_send not implemented */
         .gen_bcn_tmpl = ath10k_wmi_tlv_op_gen_bcn_tmpl,
         .gen_prb_tmpl = ath10k_wmi_tlv_op_gen_prb_tmpl,
+        .gen_p2p_go_bcn_ie = ath10k_wmi_tlv_op_gen_p2p_go_bcn_ie,
 };
 
 /************/
diff --git a/drivers/net/wireless/ath/ath10k/wmi-tlv.h b/drivers/net/wireless/ath/ath10k/wmi-tlv.h
index 577251955c04..eb02290075a7 100644
--- a/drivers/net/wireless/ath/ath10k/wmi-tlv.h
+++ b/drivers/net/wireless/ath/ath10k/wmi-tlv.h
@@ -1404,6 +1404,11 @@ struct wmi_tlv_prb_tmpl_cmd {
         __le32 buf_len;
 } __packed;
 
+struct wmi_tlv_p2p_go_bcn_ie {
+        __le32 vdev_id;
+        __le32 ie_len;
+} __packed;
+
 void ath10k_wmi_tlv_attach(struct ath10k *ar);
 
 #endif
diff --git a/drivers/net/wireless/ath/ath10k/wmi.c b/drivers/net/wireless/ath/ath10k/wmi.c
index bfa38628cdd0..5fe17e80bc6b 100644
--- a/drivers/net/wireless/ath/ath10k/wmi.c
+++ b/drivers/net/wireless/ath/ath10k/wmi.c
@@ -5040,6 +5040,7 @@ static const struct wmi_ops wmi_ops = {
         .gen_delba_send = ath10k_wmi_op_gen_delba_send,
         /* .gen_bcn_tmpl not implemented */
         /* .gen_prb_tmpl not implemented */
+        /* .gen_p2p_go_bcn_ie not implemented */
 };
 
 static const struct wmi_ops wmi_10_1_ops = {
@@ -5100,6 +5101,7 @@ static const struct wmi_ops wmi_10_1_ops = {
         .gen_delba_send = ath10k_wmi_op_gen_delba_send,
         /* .gen_bcn_tmpl not implemented */
         /* .gen_prb_tmpl not implemented */
+        /* .gen_p2p_go_bcn_ie not implemented */
 };
 
 static const struct wmi_ops wmi_10_2_ops = {
@@ -5220,6 +5222,7 @@ static const struct wmi_ops wmi_10_2_4_ops = {
         .gen_delba_send = ath10k_wmi_op_gen_delba_send,
         /* .gen_bcn_tmpl not implemented */
         /* .gen_prb_tmpl not implemented */
+        /* .gen_p2p_go_bcn_ie not implemented */
 };
 
 int ath10k_wmi_attach(struct ath10k *ar)