qtnfmac: implement cfg80211 dump_survey handler

This patch implements cfg80211 dump_survey handler enabling
per-channel survey data reports.

Signed-off-by: Igor Mitsyanko <igor.mitsyanko.os@quantenna.com>
Signed-off-by: Sergey Matyukevich <sergey.matyukevich.os@quantenna.com>
Signed-off-by: Avinash Patil <avinashp@quantenna.com>
Signed-off-by: Kalle Valo <kvalo@codeaurora.org>

drivers/net/wireless/quantenna/qtnfmac/commands.c

@@ -1333,6 +1333,62 @@ static int qtnf_cmd_resp_proc_phy_params(struct qtnf_wmac *mac,
 	return 0;
 }
 
+static int
+qtnf_cmd_resp_proc_chan_stat_info(struct qtnf_chan_stats *stats,
+				  const u8 *payload, size_t payload_len)
+{
+	struct qlink_chan_stats *qlink_stats;
+	const struct qlink_tlv_hdr *tlv;
+	size_t tlv_full_len;
+	u16 tlv_value_len;
+	u16 tlv_type;
+
+	tlv = (struct qlink_tlv_hdr *)payload;
+	while (payload_len >= sizeof(struct qlink_tlv_hdr)) {
+		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("malformed TLV 0x%.2X; LEN: %u\n",
+				tlv_type, tlv_value_len);
+			return -EINVAL;
+		}
+		switch (tlv_type) {
+		case QTN_TLV_ID_CHANNEL_STATS:
+			if (unlikely(tlv_value_len != sizeof(*qlink_stats))) {
+				pr_err("invalid CHANNEL_STATS entry size\n");
+				return -EINVAL;
+			}
+
+			qlink_stats = (void *)tlv->val;
+
+			stats->chan_num = le32_to_cpu(qlink_stats->chan_num);
+			stats->cca_tx = le32_to_cpu(qlink_stats->cca_tx);
+			stats->cca_rx = le32_to_cpu(qlink_stats->cca_rx);
+			stats->cca_busy = le32_to_cpu(qlink_stats->cca_busy);
+			stats->cca_try = le32_to_cpu(qlink_stats->cca_try);
+			stats->chan_noise = qlink_stats->chan_noise;
+
+			pr_debug("chan(%u) try(%u) busy(%u) noise(%d)\n",
+				 stats->chan_num, stats->cca_try,
+				 stats->cca_busy, stats->chan_noise);
+			break;
+		default:
+			pr_warn("Unknown TLV type: %#x\n",
+				le16_to_cpu(tlv->type));
+		}
+		payload_len -= tlv_full_len;
+		tlv = (struct qlink_tlv_hdr *)(tlv->val + tlv_value_len);
+	}
+
+	if (payload_len) {
+		pr_warn("malformed TLV buf; bytes left: %zu\n", payload_len);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
 int qtnf_cmd_get_mac_info(struct qtnf_wmac *mac)
 {
 	struct sk_buff *cmd_skb, *resp_skb = NULL;
@@ -2176,3 +2232,54 @@ out:
 
 	return ret;
 }
+
+int qtnf_cmd_get_chan_stats(struct qtnf_wmac *mac, u16 channel,
+			    struct qtnf_chan_stats *stats)
+{
+	struct sk_buff *cmd_skb, *resp_skb = NULL;
+	struct qlink_cmd_get_chan_stats *cmd;
+	struct qlink_resp_get_chan_stats *resp;
+	size_t var_data_len;
+	u16 res_code = QLINK_CMD_RESULT_OK;
+	int ret = 0;
+
+	cmd_skb = qtnf_cmd_alloc_new_cmdskb(mac->macid, QLINK_VIFID_RSVD,
+					    QLINK_CMD_CHAN_STATS,
+					    sizeof(*cmd));
+	if (!cmd_skb)
+		return -ENOMEM;
+
+	qtnf_bus_lock(mac->bus);
+
+	cmd = (struct qlink_cmd_get_chan_stats *)cmd_skb->data;
+	cmd->channel = cpu_to_le16(channel);
+
+	ret = qtnf_cmd_send_with_reply(mac->bus, cmd_skb, &resp_skb, &res_code,
+				       sizeof(*resp), &var_data_len);
+	if (unlikely(ret)) {
+		qtnf_bus_unlock(mac->bus);
+		return ret;
+	}
+
+	if (unlikely(res_code != QLINK_CMD_RESULT_OK)) {
+		switch (res_code) {
+		case QLINK_CMD_RESULT_ENOTFOUND:
+			ret = -ENOENT;
+			break;
+		default:
+			pr_err("cmd exec failed: 0x%.4X\n", res_code);
+			ret = -EFAULT;
+			break;
+		}
+		goto out;
+	}
+
+	resp = (struct qlink_resp_get_chan_stats *)resp_skb->data;
+	ret = qtnf_cmd_resp_proc_chan_stat_info(stats, resp->info,
+						var_data_len);
+
+out:
+	qtnf_bus_unlock(mac->bus);
+	consume_skb(resp_skb);
+	return ret;
+}