[PATCH 2/3] Add the feature capability exchange function

[dreamfly281 at gmail.com]

From: Yanbo Li <yanbol at qti.qualcomm.com>

The function can be used to exchange the capability between
Host and Firmware.

Signed-off-by: Yanbo Li <yanbol at qti.qualcomm.com>
---
 hal.h  |    2 +-
 main.c |    5 +++++
 smd.c  |   72 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 smd.h  |    2 +-
 4 files changed, 79 insertions(+), 2 deletions(-)

diff --git a/hal.h b/hal.h
index 69b3702..bdfeb46 100644
--- a/hal.h
+++ b/hal.h
@@ -4263,7 +4263,7 @@ struct wlan_feat_caps_msg {
 	struct wcn36xx_hal_msg_header header;
 
 	u32 feat_caps[4];
-};
+} __packed;
 
 /* status codes to help debug rekey failures */
 enum gtk_rekey_status {
diff --git a/main.c b/main.c
index 9699356..b59a885 100644
--- a/main.c
+++ b/main.c
@@ -243,6 +243,11 @@ static int wcn36xx_start(struct ieee80211_hw *hw)
 	}
 	wcn36xx_pmc_init(wcn);
 	wcn36xx_debugfs_init(wcn);
+	ret = wcn36xx_smd_feature_caps_exchange_req(wcn);
+	if (ret) {
+		wcn36xx_warn("Exchange feature caps failed");
+	}
+
 	return 0;
 
 out_smd_stop:
diff --git a/smd.c b/smd.c
index 9fa2f0f..a594ffd 100644
--- a/smd.c
+++ b/smd.c
@@ -1110,6 +1110,75 @@ int wcn36xx_smd_dump_cmd_req(struct wcn36xx *wcn, u32 arg1, u32 arg2,
 	return wcn36xx_smd_send_and_wait(wcn, msg_body.header.len);
 }
 
+static inline void set_feat_caps(u32 *bitmap,
+				 enum place_holder_in_cap_bitmap cap)
+{
+	int arr_idx, bit_idx;
+	if (cap < 0 || cap > 127) {
+		wcn36xx_warn("error cap idx %d", cap);
+	} else {
+		arr_idx = cap / 32;
+		bit_idx = cap % 32;
+		bitmap[arr_idx] |= (1 << bit_idx);
+	}
+}
+
+static inline int get_feat_caps(u32 *bitmap,
+				enum place_holder_in_cap_bitmap cap)
+{
+	int arr_idx, bit_idx;
+	int ret = 0;
+
+	if (cap < 0 || cap > 127) {
+		wcn36xx_warn("error cap idx %d", cap);
+		return -1;
+	} else {
+		arr_idx = cap / 32;
+		bit_idx = cap % 32;
+		ret = (bitmap[arr_idx] & (1 << bit_idx)) ? 1 : 0;
+		return ret;
+	}
+}
+
+static inline void clear_feat_caps(u32 *bitmap,
+				enum place_holder_in_cap_bitmap cap)
+{
+	int arr_idx, bit_idx;
+
+	if (cap < 0 || cap > 127) {
+		wcn36xx_warn("error cap idx %d", cap);
+	} else {
+		arr_idx = cap / 32;
+		bit_idx = cap % 32;
+		bitmap[arr_idx] &= ~(1 << bit_idx);
+	}
+}
+
+int wcn36xx_smd_feature_caps_exchange_req(struct wcn36xx *wcn)
+{
+	struct wlan_feat_caps_msg msg_body;
+
+	INIT_HAL_MSG(msg_body, WCN36XX_HAL_FEATURE_CAPS_EXCHANGE_REQ);
+
+	/* Fixme theres features should be get from config */
+	memset(msg_body.feat_caps, 0, sizeof(msg_body.feat_caps));
+	set_feat_caps(msg_body.feat_caps, STA_POWERSAVE);
+
+	PREPARE_HAL_BUF(wcn->smd_buf, msg_body);
+
+	return wcn36xx_smd_send_and_wait(wcn, msg_body.header.len);
+}
+
+/* FW sends its capability bitmap as a response */
+int wcn36xx_smd_feature_caps_exchange_rsp(void *buf, size_t len)
+{
+	/* TODO: print the caps of rsp for comapre */
+	if (wcn36xx_smd_rsp_status_check(buf, len)) {
+		wcn36xx_warn("error response for caps exchange");
+	}
+	return 0;
+}
+
 static void wcn36xx_smd_notify(void *data, unsigned event)
 {
 	struct wcn36xx *wcn = (struct wcn36xx *)data;
@@ -1230,6 +1299,9 @@ static void wcn36xx_smd_rsp_process(struct wcn36xx *wcn, void *buf, size_t len)
 	case WCN36XX_HAL_MISSED_BEACON_IND:
 		wcn36xx_smd_missed_beacon_ind(wcn, buf, len);
 		break;
+	case WCN36XX_HAL_FEATURE_CAPS_EXCHANGE_RSP:
+		wcn36xx_smd_feature_caps_exchange_rsp(buf, len);
+		break;
 	default:
 		wcn36xx_error("SMD_EVENT (%d) not supported", msg_header->msg_type);
 	}
diff --git a/smd.h b/smd.h
index 964e3c2..089c69a 100644
--- a/smd.h
+++ b/smd.h
@@ -92,7 +92,7 @@ int wcn36xx_smd_exit_bmps(struct wcn36xx *wcn);
 int wcn36xx_smd_keep_alive_req(struct wcn36xx *wcn, int packet_type);
 int wcn36xx_smd_dump_cmd_req(struct wcn36xx *wcn, u32 arg1, u32 arg2,
 			     u32 arg3, u32 arg4, u32 arg5);
-
+int wcn36xx_smd_feature_caps_exchange_req(struct wcn36xx *wcn);
 /* WCN36XX configuration parameters */
 struct wcn36xx_fw_cfg {
 	u16		id;
-- 
1.7.9.5