From 4876ce33bfde35dd070e93737c8709b352f4f5bf Mon Sep 17 00:00:00 2001
From: Shwetha G K <skempa@codeaurora.org>
Date: Wed, 22 Jul 2020 20:10:36 +0530
Subject: [PATCH] qcacmn: Add support for wmi_ext2 service bitmap

Add support for wmi_ext2 service bitmap

Change-Id: I2c147b91e11c2487027a7af88ff1752c7b48730b
---
 wmi/inc/wmi_unified_priv.h |  1 +
 wmi/src/wmi_unified.c      |  5 +++++
 wmi/src/wmi_unified_tlv.c  | 43 ++++++++++++++++++++++++++++++++++++++
 3 files changed, 49 insertions(+)

diff --git a/wmi/inc/wmi_unified_priv.h b/wmi/inc/wmi_unified_priv.h
index dc977a54a491..3db47b27851d 100644
--- a/wmi/inc/wmi_unified_priv.h
+++ b/wmi/inc/wmi_unified_priv.h
@@ -2473,6 +2473,7 @@ struct wmi_soc {
 	/* WMI service bitmap received from target */
 	uint32_t *wmi_service_bitmap;
 	uint32_t *wmi_ext_service_bitmap;
+	uint32_t *wmi_ext2_service_bitmap;
 	uint32_t services[wmi_services_max];
 	uint16_t wmi_max_cmds;
 	uint32_t soc_idx;
diff --git a/wmi/src/wmi_unified.c b/wmi/src/wmi_unified.c
index a3225a2547e3..c045d2cc4257 100644
--- a/wmi/src/wmi_unified.c
+++ b/wmi/src/wmi_unified.c
@@ -2877,6 +2877,11 @@ void wmi_unified_detach(struct wmi_unified *wmi_handle)
 		soc->wmi_ext_service_bitmap = NULL;
 	}
 
+	if (soc->wmi_ext2_service_bitmap) {
+		qdf_mem_free(soc->wmi_ext2_service_bitmap);
+		soc->wmi_ext2_service_bitmap = NULL;
+	}
+
 	/* Decrease the ref count once refcount infra is present */
 	soc->wmi_psoc = NULL;
 	qdf_mem_free(soc);
diff --git a/wmi/src/wmi_unified_tlv.c b/wmi/src/wmi_unified_tlv.c
index 6513b10c3c99..b2ae93e70ff5 100644
--- a/wmi/src/wmi_unified_tlv.c
+++ b/wmi/src/wmi_unified_tlv.c
@@ -8428,6 +8428,25 @@ static bool is_service_enabled_tlv(wmi_unified_t wmi_handle,
 		return false;
 	}
 
+	if (!soc->wmi_ext_service_bitmap) {
+		WMI_LOGE("WMI service ext bit map is not saved yet");
+		return false;
+	}
+
+	/* if wmi_service_enabled was received with extended2 bitmap,
+	 * use WMI_SERVICE_EXT2_IS_ENABLED to check the services.
+	 */
+	if (soc->wmi_ext2_service_bitmap)
+		return WMI_SERVICE_EXT2_IS_ENABLED(soc->wmi_service_bitmap,
+				soc->wmi_ext_service_bitmap,
+				soc->wmi_ext2_service_bitmap,
+				service_id);
+
+	if (service_id >= WMI_MAX_EXT_SERVICE) {
+		WMI_LOGE("Service id %d but WMI ext2 service bitmap is NULL",
+			 service_id);
+		return false;
+	}
 	/* if wmi_service_enabled was received with extended bitmap,
 	 * use WMI_SERVICE_EXT_IS_ENABLED to check the services.
 	 */
@@ -9252,6 +9271,7 @@ QDF_STATUS save_ext_service_bitmap_tlv(wmi_unified_t wmi_handle, void *evt_buf,
 	WMI_SERVICE_AVAILABLE_EVENTID_param_tlvs *param_buf;
 	wmi_service_available_event_fixed_param *ev;
 	struct wmi_soc *soc = wmi_handle->soc;
+	uint32_t i = 0;
 
 	param_buf = (WMI_SERVICE_AVAILABLE_EVENTID_param_tlvs *) evt_buf;
 
@@ -9280,6 +9300,29 @@ QDF_STATUS save_ext_service_bitmap_tlv(wmi_unified_t wmi_handle, void *evt_buf,
 			soc->wmi_ext_service_bitmap,
 			(WMI_SERVICE_SEGMENT_BM_SIZE32 * sizeof(uint32_t)));
 
+	if (!param_buf->wmi_service_ext_bitmap) {
+		WMI_LOGD("wmi_service_ext_bitmap not available");
+		return QDF_STATUS_SUCCESS;
+	}
+
+	if (!soc->wmi_ext2_service_bitmap) {
+		soc->wmi_ext2_service_bitmap =
+			qdf_mem_malloc(param_buf->num_wmi_service_ext_bitmap *
+				       sizeof(uint32_t));
+		if (!soc->wmi_ext2_service_bitmap)
+			return QDF_STATUS_E_NOMEM;
+	}
+
+	qdf_mem_copy(soc->wmi_ext2_service_bitmap,
+		     param_buf->wmi_service_ext_bitmap,
+		     (param_buf->num_wmi_service_ext_bitmap *
+		      sizeof(uint32_t)));
+
+	for (i = 0; i < param_buf->num_wmi_service_ext_bitmap; i++) {
+		WMI_LOGD("wmi_ext2_service_bitmap %u:0x%x",
+			 i, soc->wmi_ext2_service_bitmap[i]);
+	}
+
 	return QDF_STATUS_SUCCESS;
 }