qtnfmac: extend "IE set" TLV to include frame type info

Specifying frame type for "IE set" TLV will allow to use several
TLVs in a single message.
Modify users accordingly.

Signed-off-by: Igor Mitsyanko <igor.mitsyanko.os@quantenna.com>
Signed-off-by: Kalle Valo <kvalo@codeaurora.org>

3 files changed, 80 insertions, 30 deletions

diff --git a/drivers/net/wireless/quantenna/qtnfmac/commands.c b/drivers/net/wireless/quantenna/qtnfmac/commands.c
index 503187a371ae..8d3cdba99390 100644
--- a/drivers/net/wireless/quantenna/qtnfmac/commands.c
+++ b/drivers/net/wireless/quantenna/qtnfmac/commands.c
@@ -147,6 +147,21 @@ static struct sk_buff *qtnf_cmd_alloc_new_cmdskb(u8 macid, u8 vifid, u16 cmd_no,
         return cmd_skb;
 }
 
+static void qtnf_cmd_tlv_ie_set_add(struct sk_buff *cmd_skb, u8 frame_type,
+                                    const u8 *buf, size_t len)
+{
+        struct qlink_tlv_ie_set *tlv;
+
+        tlv = (struct qlink_tlv_ie_set *)skb_put(cmd_skb, sizeof(*tlv) + len);
+        tlv->hdr.type = cpu_to_le16(QTN_TLV_ID_IE_SET);
+        tlv->hdr.len = cpu_to_le16(len + sizeof(*tlv) - sizeof(tlv->hdr));
+        tlv->type = frame_type;
+        tlv->flags = 0;
+
+        if (len && buf)
+                memcpy(tlv->ie_data, buf, len);
+}
+
 int qtnf_cmd_send_start_ap(struct qtnf_vif *vif)
 {
         struct sk_buff *cmd_skb;
@@ -2028,9 +2043,8 @@ int qtnf_cmd_send_scan(struct qtnf_wmac *mac)
         }
 
         if (scan_req->ie_len != 0)
-                qtnf_cmd_skb_put_tlv_arr(cmd_skb, QTN_TLV_ID_IE_SET,
-                                         scan_req->ie,
-                                         scan_req->ie_len);
+                qtnf_cmd_tlv_ie_set_add(cmd_skb, QLINK_IE_SET_PROBE_REQ,
+                                        scan_req->ie, scan_req->ie_len);
 
         if (scan_req->n_channels) {
                 n_channels = scan_req->n_channels;
@@ -2154,9 +2168,8 @@ int qtnf_cmd_send_connect(struct qtnf_vif *vif,
                                  sme->ssid_len);
 
         if (sme->ie_len != 0)
-                qtnf_cmd_skb_put_tlv_arr(cmd_skb, QTN_TLV_ID_IE_SET,
-                                         sme->ie,
-                                         sme->ie_len);
+                qtnf_cmd_tlv_ie_set_add(cmd_skb, QLINK_IE_SET_ASSOC_REQ,
+                                        sme->ie, sme->ie_len);
 
         qtnf_bus_lock(vif->mac->bus);
 
diff --git a/drivers/net/wireless/quantenna/qtnfmac/event.c b/drivers/net/wireless/quantenna/qtnfmac/event.c
index 4b29f9fb9c3c..b67a082eed69 100644
--- a/drivers/net/wireless/quantenna/qtnfmac/event.c
+++ b/drivers/net/wireless/quantenna/qtnfmac/event.c
@@ -65,34 +65,39 @@ qtnf_event_handle_sta_assoc(struct qtnf_wmac *mac, struct qtnf_vif *vif,
         sinfo.assoc_req_ies_len = 0;
 
         payload_len = len - sizeof(*sta_assoc);
-        tlv = (struct qlink_tlv_hdr *)sta_assoc->ies;
+        tlv = (const struct qlink_tlv_hdr *)sta_assoc->ies;
 
-        while (payload_len >= sizeof(struct qlink_tlv_hdr)) {
+        while (payload_len >= sizeof(*tlv)) {
                 tlv_type = le16_to_cpu(tlv->type);
                 tlv_value_len = le16_to_cpu(tlv->len);
                 tlv_full_len = tlv_value_len + sizeof(struct qlink_tlv_hdr);
 
-                if (tlv_full_len > payload_len) {
-                        pr_warn("VIF%u.%u: malformed TLV 0x%.2X; LEN: %u\n",
-                                mac->macid, vif->vifid, tlv_type,
-                                tlv_value_len);
+                if (tlv_full_len > payload_len)
                         return -EINVAL;
-                }
 
                 if (tlv_type == QTN_TLV_ID_IE_SET) {
-                        sinfo.assoc_req_ies = tlv->val;
-                        sinfo.assoc_req_ies_len = tlv_value_len;
+                        const struct qlink_tlv_ie_set *ie_set;
+                        unsigned int ie_len;
+
+                        if (payload_len < sizeof(*ie_set))
+                                return -EINVAL;
+
+                        ie_set = (const struct qlink_tlv_ie_set *)tlv;
+                        ie_len = tlv_value_len -
+                                (sizeof(*ie_set) - sizeof(ie_set->hdr));
+
+                        if (ie_set->type == QLINK_IE_SET_ASSOC_REQ && ie_len) {
+                                sinfo.assoc_req_ies = ie_set->ie_data;
+                                sinfo.assoc_req_ies_len = ie_len;
+                        }
                 }
 
                 payload_len -= tlv_full_len;
                 tlv = (struct qlink_tlv_hdr *)(tlv->val + tlv_value_len);
         }
 
-        if (payload_len) {
-                pr_warn("VIF%u.%u: malformed TLV buf; bytes left: %zu\n",
-                        mac->macid, vif->vifid, payload_len);
+        if (payload_len)
                 return -EINVAL;
-        }
 
         cfg80211_new_sta(vif->netdev, sta_assoc->sta_addr, &sinfo,
                          GFP_KERNEL);
@@ -289,27 +294,32 @@ qtnf_event_handle_scan_results(struct qtnf_vif *vif,
                 tlv_value_len = le16_to_cpu(tlv->len);
                 tlv_full_len = tlv_value_len + sizeof(struct qlink_tlv_hdr);
 
-                if (tlv_full_len > payload_len) {
-                        pr_warn("VIF%u.%u: malformed TLV 0x%.2X; LEN: %u\n",
-                                vif->mac->macid, vif->vifid, tlv_type,
-                                tlv_value_len);
+                if (tlv_full_len > payload_len)
                         return -EINVAL;
-                }
 
                 if (tlv_type == QTN_TLV_ID_IE_SET) {
-                        ies = tlv->val;
-                        ies_len = tlv_value_len;
+                        const struct qlink_tlv_ie_set *ie_set;
+                        unsigned int ie_len;
+
+                        if (payload_len < sizeof(*ie_set))
+                                return -EINVAL;
+
+                        ie_set = (const struct qlink_tlv_ie_set *)tlv;
+                        ie_len = tlv_value_len -
+                                (sizeof(*ie_set) - sizeof(ie_set->hdr));
+
+                        if (ie_len) {
+                                ies = ie_set->ie_data;
+                                ies_len = ie_len;
+                        }
                 }
 
                 payload_len -= tlv_full_len;
                 tlv = (struct qlink_tlv_hdr *)(tlv->val + tlv_value_len);
         }
 
-        if (payload_len) {
-                pr_warn("VIF%u.%u: malformed TLV buf; bytes left: %zu\n",
-                        vif->mac->macid, vif->vifid, payload_len);
+        if (payload_len)
                 return -EINVAL;
-        }
 
         bss = cfg80211_inform_bss(wiphy, channel, frame_type,
                                   sr->bssid, get_unaligned_le64(&sr->tsf),
diff --git a/drivers/net/wireless/quantenna/qtnfmac/qlink.h b/drivers/net/wireless/quantenna/qtnfmac/qlink.h
index c558d819a966..f491942eb6d0 100644
--- a/drivers/net/wireless/quantenna/qtnfmac/qlink.h
+++ b/drivers/net/wireless/quantenna/qtnfmac/qlink.h
@@ -1147,6 +1147,33 @@ struct qlink_tlv_chandef {
         struct qlink_chandef chan;
 } __packed;
 
+enum qlink_ie_set_type {
+        QLINK_IE_SET_UNKNOWN,
+        QLINK_IE_SET_ASSOC_REQ,
+        QLINK_IE_SET_ASSOC_RESP,
+        QLINK_IE_SET_PROBE_REQ,
+        QLINK_IE_SET_SCAN,
+        QLINK_IE_SET_BEACON_HEAD,
+        QLINK_IE_SET_BEACON_TAIL,
+        QLINK_IE_SET_BEACON_IES,
+        QLINK_IE_SET_PROBE_RESP,
+        QLINK_IE_SET_PROBE_RESP_IES,
+};
+
+/**
+ * struct qlink_tlv_ie_set - data for QTN_TLV_ID_IE_SET
+ *
+ * @type: type of MGMT frame IEs belong to, one of &enum qlink_ie_set_type.
+ * @flags: for future use.
+ * @ie_data: IEs data.
+ */
+struct qlink_tlv_ie_set {
+        struct qlink_tlv_hdr hdr;
+        u8 type;
+        u8 flags;
+        u8 ie_data[0];
+} __packed;
+
 struct qlink_chan_stats {
         __le32 chan_num;
         __le32 cca_tx;