ath10k: implement beacon template command

New firmware revisions may support setting beacon
template. 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, 103 insertions, 0 deletions

diff --git a/drivers/net/wireless/ath/ath10k/wmi-ops.h b/drivers/net/wireless/ath/ath10k/wmi-ops.h
index 3a3d15e65e0a..546970259de2 100644
--- a/drivers/net/wireless/ath/ath10k/wmi-ops.h
+++ b/drivers/net/wireless/ath/ath10k/wmi-ops.h
@@ -129,6 +129,10 @@ struct wmi_ops {
         struct sk_buff *(*gen_delba_send)(struct ath10k *ar, u32 vdev_id,
                                           const u8 *mac, u32 tid, u32 initiator,
                                           u32 reason);
+        struct sk_buff *(*gen_bcn_tmpl)(struct ath10k *ar, u32 vdev_id,
+                                        u32 tim_ie_offset, struct sk_buff *bcn,
+                                        u32 prb_caps, u32 prb_erp,
+                                        void *prb_ies, size_t prb_ies_len);
 };
 
 int ath10k_wmi_cmd_send(struct ath10k *ar, struct sk_buff *skb, u32 cmd_id);
@@ -935,4 +939,23 @@ ath10k_wmi_delba_send(struct ath10k *ar, u32 vdev_id, const u8 *mac,
                                    ar->wmi.cmd->delba_send_cmdid);
 }
 
+static inline int
+ath10k_wmi_bcn_tmpl(struct ath10k *ar, u32 vdev_id, u32 tim_ie_offset,
+                    struct sk_buff *bcn, u32 prb_caps, u32 prb_erp,
+                    void *prb_ies, size_t prb_ies_len)
+{
+        struct sk_buff *skb;
+
+        if (!ar->wmi.ops->gen_bcn_tmpl)
+                return -EOPNOTSUPP;
+
+        skb = ar->wmi.ops->gen_bcn_tmpl(ar, vdev_id, tim_ie_offset, bcn,
+                                        prb_caps, prb_erp, prb_ies,
+                                        prb_ies_len);
+        if (IS_ERR(skb))
+                return PTR_ERR(skb);
+
+        return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->bcn_tmpl_cmdid);
+}
+
 #endif
diff --git a/drivers/net/wireless/ath/ath10k/wmi-tlv.c b/drivers/net/wireless/ath/ath10k/wmi-tlv.c
index 67138a69bb42..08a9b2b35d06 100644
--- a/drivers/net/wireless/ath/ath10k/wmi-tlv.c
+++ b/drivers/net/wireless/ath/ath10k/wmi-tlv.c
@@ -1972,6 +1972,70 @@ ath10k_wmi_tlv_op_gen_pktlog_disable(struct ath10k *ar)
         return skb;
 }
 
+static struct sk_buff *
+ath10k_wmi_tlv_op_gen_bcn_tmpl(struct ath10k *ar, u32 vdev_id,
+                               u32 tim_ie_offset, struct sk_buff *bcn,
+                               u32 prb_caps, u32 prb_erp, void *prb_ies,
+                               size_t prb_ies_len)
+{
+        struct wmi_tlv_bcn_tmpl_cmd *cmd;
+        struct wmi_tlv_bcn_prb_info *info;
+        struct wmi_tlv *tlv;
+        struct sk_buff *skb;
+        void *ptr;
+        size_t len;
+
+        if (WARN_ON(prb_ies_len > 0 && !prb_ies))
+                return ERR_PTR(-EINVAL);
+
+        len = sizeof(*tlv) + sizeof(*cmd) +
+              sizeof(*tlv) + sizeof(*info) + prb_ies_len +
+              sizeof(*tlv) + roundup(bcn->len, 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_BCN_TMPL_CMD);
+        tlv->len = __cpu_to_le16(sizeof(*cmd));
+        cmd = (void *)tlv->value;
+        cmd->vdev_id = __cpu_to_le32(vdev_id);
+        cmd->tim_ie_offset = __cpu_to_le32(tim_ie_offset);
+        cmd->buf_len = __cpu_to_le32(bcn->len);
+
+        ptr += sizeof(*tlv);
+        ptr += sizeof(*cmd);
+
+        /* FIXME: prb_ies_len should be probably aligned to 4byte boundary but
+         * then it is then impossible to pass original ie len.
+         * This chunk is not used yet so if setting probe resp template yields
+         * problems with beaconing or crashes firmware look here.
+         */
+        tlv = ptr;
+        tlv->tag = __cpu_to_le16(WMI_TLV_TAG_STRUCT_BCN_PRB_INFO);
+        tlv->len = __cpu_to_le16(sizeof(*info) + prb_ies_len);
+        info = (void *)tlv->value;
+        info->caps = __cpu_to_le32(prb_caps);
+        info->erp = __cpu_to_le32(prb_erp);
+        memcpy(info->ies, prb_ies, prb_ies_len);
+
+        ptr += sizeof(*tlv);
+        ptr += sizeof(*info);
+        ptr += prb_ies_len;
+
+        tlv = ptr;
+        tlv->tag = __cpu_to_le16(WMI_TLV_TAG_ARRAY_BYTE);
+        tlv->len = __cpu_to_le16(roundup(bcn->len, 4));
+        memcpy(tlv->value, bcn->data, bcn->len);
+
+        /* FIXME: Adjust TSF? */
+
+        ath10k_dbg(ar, ATH10K_DBG_WMI, "wmi tlv bcn tmpl vdev_id %i\n",
+                   vdev_id);
+        return skb;
+}
+
 /****************/
 /* TLV mappings */
 /****************/
@@ -2261,6 +2325,7 @@ static const struct wmi_ops wmi_tlv_ops = {
         /* .gen_addba_send not implemented */
         /* .gen_addba_set_resp not implemented */
         /* .gen_delba_send not implemented */
+        .gen_bcn_tmpl = ath10k_wmi_tlv_op_gen_bcn_tmpl,
 };
 
 /************/
diff --git a/drivers/net/wireless/ath/ath10k/wmi-tlv.h b/drivers/net/wireless/ath/ath10k/wmi-tlv.h
index ee19353ce65c..c4773652d380 100644
--- a/drivers/net/wireless/ath/ath10k/wmi-tlv.h
+++ b/drivers/net/wireless/ath/ath10k/wmi-tlv.h
@@ -1387,6 +1387,18 @@ struct wmi_tlv_bcn_tx_status_ev {
         __le32 tx_status;
 } __packed;
 
+struct wmi_tlv_bcn_prb_info {
+        __le32 caps;
+        __le32 erp;
+        u8 ies[0];
+} __packed;
+
+struct wmi_tlv_bcn_tmpl_cmd {
+        __le32 vdev_id;
+        __le32 tim_ie_offset;
+        __le32 buf_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 2d3c700e2cbb..d37a4859fc2a 100644
--- a/drivers/net/wireless/ath/ath10k/wmi.c
+++ b/drivers/net/wireless/ath/ath10k/wmi.c
@@ -5038,6 +5038,7 @@ static const struct wmi_ops wmi_ops = {
         .gen_addba_send = ath10k_wmi_op_gen_addba_send,
         .gen_addba_set_resp = ath10k_wmi_op_gen_addba_set_resp,
         .gen_delba_send = ath10k_wmi_op_gen_delba_send,
+        /* .gen_bcn_tmpl not implemented */
 };
 
 static const struct wmi_ops wmi_10_1_ops = {
@@ -5096,6 +5097,7 @@ static const struct wmi_ops wmi_10_1_ops = {
         .gen_addba_send = ath10k_wmi_op_gen_addba_send,
         .gen_addba_set_resp = ath10k_wmi_op_gen_addba_set_resp,
         .gen_delba_send = ath10k_wmi_op_gen_delba_send,
+        /* .gen_bcn_tmpl not implemented */
 };
 
 static const struct wmi_ops wmi_10_2_ops = {
@@ -5214,6 +5216,7 @@ static const struct wmi_ops wmi_10_2_4_ops = {
         .gen_addba_send = ath10k_wmi_op_gen_addba_send,
         .gen_addba_set_resp = ath10k_wmi_op_gen_addba_set_resp,
         .gen_delba_send = ath10k_wmi_op_gen_delba_send,
+        /* .gen_bcn_tmpl not implemented */
 };
 
 int ath10k_wmi_attach(struct ath10k *ar)