From 912fd67d6cf3d88b2a60fb90514541a5586f13f5 Mon Sep 17 00:00:00 2001
From: Vignesh Kulothungan <vigneshk@codeaurora.org>
Date: Wed, 5 Aug 2020 23:26:06 -0700
Subject: [PATCH 1/3] ASoC: wsa883x: add function to get swr device number

Add a function in wsa883x which returns the sound wire
device number for a given wsa883x codec slave instance.

Change-Id: Ie3d0e309870f488b1643a853bcddcf93314d94b5
Signed-off-by: Vignesh Kulothungan <vigneshk@codeaurora.org>
---
 asoc/codecs/wsa883x/wsa883x.c | 24 ++++++++++++++++++++++++
 asoc/codecs/wsa883x/wsa883x.h |  5 +++++
 2 files changed, 29 insertions(+)

diff --git a/asoc/codecs/wsa883x/wsa883x.c b/asoc/codecs/wsa883x/wsa883x.c
index a228ab5e137f..752d4b7f5a7a 100644
--- a/asoc/codecs/wsa883x/wsa883x.c
+++ b/asoc/codecs/wsa883x/wsa883x.c
@@ -801,6 +801,30 @@ int wsa883x_codec_info_create_codec_entry(struct snd_info_entry *codec_root,
 }
 EXPORT_SYMBOL(wsa883x_codec_info_create_codec_entry);
 
+/*
+ * wsa883x_codec_get_dev_num - returns swr device number
+ * @component: Codec instance
+ *
+ * Return: swr device number on success or negative error
+ * code on failure.
+ */
+int wsa883x_codec_get_dev_num(struct snd_soc_component *component)
+{
+	struct wsa883x_priv *wsa883x;
+
+	if (!component)
+		return -EINVAL;
+
+	wsa883x = snd_soc_component_get_drvdata(component);
+	if (!wsa883x) {
+		pr_err("%s: wsa883x component is NULL\n", __func__);
+		return -EINVAL;
+	}
+
+	return wsa883x->swr_slave->dev_num;
+}
+EXPORT_SYMBOL(wsa883x_codec_get_dev_num);
+
 static int wsa883x_get_compander(struct snd_kcontrol *kcontrol,
 			       struct snd_ctl_elem_value *ucontrol)
 {
diff --git a/asoc/codecs/wsa883x/wsa883x.h b/asoc/codecs/wsa883x/wsa883x.h
index e080134eb016..bb719831ed61 100644
--- a/asoc/codecs/wsa883x/wsa883x.h
+++ b/asoc/codecs/wsa883x/wsa883x.h
@@ -21,6 +21,7 @@ int wsa883x_set_channel_map(struct snd_soc_component *component,
 int wsa883x_codec_info_create_codec_entry(
 					struct snd_info_entry *codec_root,
 					struct snd_soc_component *component);
+int wsa883x_codec_get_dev_num(struct snd_soc_component *component);
 #else
 static int wsa883x_set_channel_map(struct snd_soc_component *component,
 				   u8 *port, u8 num_port, unsigned int *ch_mask,
@@ -36,6 +37,10 @@ static int wsa883x_codec_info_create_codec_entry(
 	return 0;
 }
 
+static int wsa883x_codec_get_dev_num(struct snd_soc_component *component)
+{
+	return 0;
+}
 #endif
 
 #endif /* _WSA883X_H */

From 3fa8c9411fcdc709437480a2a198734b3e3219c0 Mon Sep 17 00:00:00 2001
From: Vignesh Kulothungan <vigneshk@codeaurora.org>
Date: Wed, 5 Aug 2020 23:39:37 -0700
Subject: [PATCH 2/3] dsp: add afe function to send cps configuration

Define cps hw interface configuration structures.
Add AFE_PARAM_ID_CPS_LPASS_HW_INTF_CFG parameter id.
Add a function to send afe cps speaker protection
configuration.

Change-Id: I865e9981b6dd1da4b9ef1a3e18be82cea2996309
Signed-off-by: Vignesh Kulothungan <vigneshk@codeaurora.org>
---
 dsp/q6afe.c                | 60 ++++++++++++++++++++++++++++++++++++++
 include/dsp/apr_audio-v2.h | 26 +++++++++++++++++
 include/dsp/q6afe-v2.h     |  3 ++
 3 files changed, 89 insertions(+)

diff --git a/dsp/q6afe.c b/dsp/q6afe.c
index 8fbbfd9b873e..1f8c102ae328 100644
--- a/dsp/q6afe.c
+++ b/dsp/q6afe.c
@@ -2179,6 +2179,66 @@ fail_cmd:
 	return ret;
 }
 
+/**
+ * afe_send_cps_config -
+ *         to send cps speaker protection configuration
+ *
+ * @src_port: source port to send configuration to
+ * @cps_config: cps speaker protection v4 configuration
+ * @param_size: size of payload
+ *
+ * Returns 0 on success or error value on failure.
+ */
+int afe_send_cps_config(int src_port,
+			struct afe_cps_hw_intf_cfg *cps_config,
+			int param_size)
+{
+	struct param_hdr_v3 param_info;
+	int ret = -EINVAL;
+	u8 *packed_payload = NULL;
+	int cpy_size = 0;
+
+	ret = q6audio_validate_port(src_port);
+	if (ret < 0) {
+		pr_err("%s: Invalid src port 0x%x ret %d", __func__,
+		       src_port, ret);
+		return -EINVAL;
+	}
+
+	packed_payload = kzalloc(param_size, GFP_KERNEL);
+	if (packed_payload == NULL)
+		return -ENOMEM;
+
+	cpy_size = sizeof(struct afe_cps_hw_intf_cfg) -
+			sizeof(cps_config->spkr_dep_cfg);
+	memcpy(packed_payload, cps_config, cpy_size);
+	memcpy(packed_payload + cpy_size, cps_config->spkr_dep_cfg,
+		sizeof(struct lpass_swr_spkr_dep_cfg_t)
+			* cps_config->hw_reg_cfg.num_spkr);
+
+	memset(&param_info, 0, sizeof(param_info));
+
+	mutex_lock(&this_afe.afe_cmd_lock);
+	param_info.module_id = AFE_MODULE_SPEAKER_PROTECTION_V4_RX;
+	param_info.instance_id = INSTANCE_ID_0;
+	param_info.param_id = AFE_PARAM_ID_CPS_LPASS_HW_INTF_CFG;
+	param_info.param_size = param_size;
+
+	ret = q6afe_pack_and_set_param_in_band(src_port,
+					       q6audio_get_port_index(src_port),
+					       param_info, packed_payload);
+	if (ret)
+		pr_err("%s: port = 0x%x param = 0x%x failed %d\n", __func__,
+		       src_port, param_info.param_id, ret);
+
+	mutex_unlock(&this_afe.afe_cmd_lock);
+	pr_debug("%s: config.pdata.param_id 0x%x status %d 0x%x\n", __func__,
+		 param_info.param_id, ret, src_port);
+	kfree(packed_payload);
+	return ret;
+}
+EXPORT_SYMBOL(afe_send_cps_config);
+
 static int afe_spk_prot_prepare(int src_port, int dst_port, int param_id,
 		union afe_spkr_prot_config *prot_config, uint32_t param_size)
 {
diff --git a/include/dsp/apr_audio-v2.h b/include/dsp/apr_audio-v2.h
index 090bceb4d5f9..4cd34ddbf362 100644
--- a/include/dsp/apr_audio-v2.h
+++ b/include/dsp/apr_audio-v2.h
@@ -10,6 +10,9 @@
 #include <ipc/apr.h>
 #include <audio/linux/msm_audio.h>
 
+/* number of threshold levels in speaker protection module */
+#define MAX_CPS_LEVELS 3
+
 /* size of header needed for passing data out of band */
 #define APR_CMD_OB_HDR_SZ  12
 
@@ -2364,6 +2367,28 @@ int16_t        excursionf[AFE_SPKR_PROT_EXCURSIONF_LEN];
  */
 } __packed;
 
+struct lpass_swr_spkr_dep_cfg_t {
+	uint32_t vbatt_pkd_reg_addr;
+	uint32_t temp_pkd_reg_addr;
+	uint32_t value_normal_thrsd[MAX_CPS_LEVELS];
+	uint32_t value_low1_thrsd[MAX_CPS_LEVELS];
+	uint32_t value_low2_thrsd[MAX_CPS_LEVELS];
+} __packed;
+
+struct lpass_swr_hw_reg_cfg_t {
+	uint32_t lpass_wr_cmd_reg_phy_addr;
+	uint32_t lpass_rd_cmd_reg_phy_addr;
+	uint32_t lpass_rd_fifo_reg_phy_addr;
+	uint32_t vbatt_lower1_threshold;
+	uint32_t vbatt_lower2_threshold;
+	uint32_t num_spkr;
+} __packed;
+
+struct afe_cps_hw_intf_cfg {
+	uint32_t lpass_hw_intf_cfg_mode;
+	struct lpass_swr_hw_reg_cfg_t hw_reg_cfg;
+	struct lpass_swr_spkr_dep_cfg_t *spkr_dep_cfg;
+} __packed;
 
 #define AFE_SERVICE_CMD_REGISTER_RT_PORT_DRIVER	0x000100E0
 
@@ -10807,6 +10832,7 @@ struct cmd_set_topologies {
 #define AFE_PARAM_ID_FBSP_MODE_RX_CFG 0x0001021D
 #define AFE_PARAM_ID_FBSP_PTONE_RAMP_CFG 0x00010260
 #define AFE_PARAM_ID_SP_RX_TMAX_XMAX_LOGGING 0x000102BC
+#define AFE_PARAM_ID_CPS_LPASS_HW_INTF_CFG 0x000102EF
 
 struct asm_fbsp_mode_rx_cfg {
 	uint32_t minor_version;
diff --git a/include/dsp/q6afe-v2.h b/include/dsp/q6afe-v2.h
index bd048f85791c..45946e95b4bf 100644
--- a/include/dsp/q6afe-v2.h
+++ b/include/dsp/q6afe-v2.h
@@ -438,6 +438,9 @@ int afe_pseudo_port_start_nowait(u16 port_id);
 int afe_pseudo_port_stop_nowait(u16 port_id);
 int afe_set_lpass_clock(u16 port_id, struct afe_clk_cfg *cfg);
 int afe_set_lpass_clock_v2(u16 port_id, struct afe_clk_set *cfg);
+int afe_send_cps_config(int src_port,
+			struct afe_cps_hw_intf_cfg *cps_config,
+			int param_size);
 int afe_set_lpass_clk_cfg(int index, struct afe_clk_set *cfg);
 int afe_set_digital_codec_core_clock(u16 port_id,
 			struct afe_digital_clk_cfg *cfg);

From 7902b497b5cced89693f8814da3ef84511202a07 Mon Sep 17 00:00:00 2001
From: Vignesh Kulothungan <vigneshk@codeaurora.org>
Date: Thu, 6 Aug 2020 14:23:27 -0700
Subject: [PATCH 3/3] ASoC: lahaina: add support for CPS speaker protection

Add support to parse static cps configuration from dt.
Add support to send cps configuration for speaker protection
usecases.

Change-Id: I0166a378f24cedef07393bdbb8618df944c62984
Signed-off-by: Vignesh Kulothungan <vigneshk@codeaurora.org>
---
 asoc/lahaina.c | 222 ++++++++++++++++++++++++++++++++++++++++++++++++-
 1 file changed, 221 insertions(+), 1 deletion(-)

diff --git a/asoc/lahaina.c b/asoc/lahaina.c
index 4a8a8088714b..40835c6100ab 100644
--- a/asoc/lahaina.c
+++ b/asoc/lahaina.c
@@ -73,7 +73,6 @@
 #define WCD_MBHC_HS_V_MAX           1600
 
 #define TDM_CHANNEL_MAX		8
-#define DEV_NAME_STR_LEN	32
 
 #define MSM_LL_QOS_VALUE	300 /* time in us to ensure LPM doesn't go in C3/C4 */
 
@@ -83,6 +82,8 @@
 #define WCN_CDC_SLIM_TX_CH_MAX 2
 #define WCN_CDC_SLIM_TX_CH_MAX_LITO 3
 
+#define SWR_MAX_SLAVE_DEVICES 6
+
 enum {
 	RX_PATH = 0,
 	TX_PATH,
@@ -195,6 +196,8 @@ struct msm_asoc_mach_data {
 	u32 wsa_max_devs;
 	u32 tdm_max_slots; /* Max TDM slots used */
 	int wcd_disabled;
+	int (*get_wsa_dev_num)(struct snd_soc_component*);
+	struct afe_cps_hw_intf_cfg cps_config;
 };
 
 struct tdm_port {
@@ -5017,6 +5020,106 @@ static int msm_snd_cdc_dma_startup(struct snd_pcm_substream *substream)
 	return ret;
 }
 
+static int send_cps_config(struct snd_soc_pcm_runtime *rtd,
+				u32 num_ch, u32 ch_mask)
+{
+	int i = 0;
+	int ret = 0;
+	int val = 0;
+	u8 dev_num = 0;
+	int param_size = 0;
+	int ch_configured = 0;
+	char wsa_cdc_name[DEV_NAME_STR_LEN];
+	struct snd_soc_component *component = NULL;
+	struct snd_soc_dai_link *dai_link = rtd->dai_link;
+        struct msm_asoc_mach_data *pdata =
+			snd_soc_card_get_drvdata(rtd->card);
+
+	if (!pdata) {
+		pr_err("%s: pdata is NULL\n", __func__);
+		return -EINVAL;
+	}
+
+	if (!num_ch) {
+		pr_err("%s: channel count is 0\n", __func__);
+		return -EINVAL;
+	}
+
+	if (!pdata->get_wsa_dev_num) {
+		pr_err("%s: get_wsa_dev_num is NULL\n", __func__);
+		return -EINVAL;
+	}
+
+	if (!pdata->cps_config.spkr_dep_cfg) {
+		pr_err("%s: spkr_dep_cfg is NULL\n", __func__);
+		return -EINVAL;
+	}
+
+	if (!pdata->cps_config.hw_reg_cfg.lpass_wr_cmd_reg_phy_addr ||
+	   !pdata->cps_config.hw_reg_cfg.lpass_rd_cmd_reg_phy_addr ||
+	   !pdata->cps_config.hw_reg_cfg.lpass_rd_fifo_reg_phy_addr) {
+		pr_err("%s: cps static configuration is not set\n", __func__);
+		return -EINVAL;
+	}
+
+	pdata->cps_config.lpass_hw_intf_cfg_mode = 1;
+
+	while (ch_configured < num_ch) {
+		if (!(ch_mask & (1 << i))) {
+			i++;
+			continue;
+		}
+
+		snprintf(wsa_cdc_name, sizeof(wsa_cdc_name), "wsa-codec.%d",
+			 i+1);
+
+		component = snd_soc_rtdcom_lookup(rtd, wsa_cdc_name);
+		if (!component) {
+			pr_err("%s: %s component is NULL\n", __func__,
+				wsa_cdc_name);
+			return -EINVAL;
+		}
+
+		dev_num = pdata->get_wsa_dev_num(component);
+		if (dev_num < 0 || dev_num > SWR_MAX_SLAVE_DEVICES) {
+			pr_err("%s: invalid slave dev num : %d\n", __func__,
+				dev_num);
+			return -EINVAL;
+		}
+
+		/* Clear stale dev num info */
+		pdata->cps_config.spkr_dep_cfg[i].vbatt_pkd_reg_addr &= 0xFFFF;
+		pdata->cps_config.spkr_dep_cfg[i].temp_pkd_reg_addr &= 0xFFFF;
+
+		val = 0;
+
+		/* bits 20:23 carry swr device number */
+		val |= dev_num << 20;
+
+		/* bits 24:27 carry read length in bytes */
+		val |= 1 << 24;
+
+		/* Update dev num in packed reg addr */
+		pdata->cps_config.spkr_dep_cfg[i].vbatt_pkd_reg_addr |= val;
+		pdata->cps_config.spkr_dep_cfg[i].temp_pkd_reg_addr |= val;
+		i++;
+		ch_configured++;
+	}
+
+	param_size = sizeof(struct afe_cps_hw_intf_cfg) -
+			sizeof(pdata->cps_config.spkr_dep_cfg) +
+			(sizeof(struct lpass_swr_spkr_dep_cfg_t)
+				* pdata->cps_config.hw_reg_cfg.num_spkr);
+
+	ret = afe_send_cps_config(msm_get_port_id(dai_link->id),
+					&pdata->cps_config, param_size);
+	if (ret) {
+		pr_err("%s: afe_send_cps_cfg failed\n", __func__);
+	}
+
+	return ret;
+}
+
 static int msm_snd_cdc_dma_hw_params(struct snd_pcm_substream *substream,
 			     struct snd_pcm_hw_params *params)
 {
@@ -5065,6 +5168,11 @@ static int msm_snd_cdc_dma_hw_params(struct snd_pcm_substream *substream,
 				goto err;
 			}
 
+			if (dai_link->id == MSM_BACKEND_DAI_WSA_CDC_DMA_RX_0 ||
+			    dai_link->id == MSM_BACKEND_DAI_WSA_CDC_DMA_RX_1) {
+				send_cps_config(rtd, user_set_rx_ch,
+						rx_ch_cdc_dma);
+			}
 		}
 		break;
 		}
@@ -7831,6 +7939,115 @@ static int msm_audio_ssr_register(struct device *dev)
 	return ret;
 }
 
+static void parse_cps_configuration(struct platform_device *pdev,
+			struct msm_asoc_mach_data *pdata)
+{
+	int ret = 0;
+	int i = 0, j = 0;
+	u32 dt_values[MAX_CPS_LEVELS];
+
+	if (!pdev || !pdata || !pdata->wsa_max_devs)
+		return;
+
+	pdata->get_wsa_dev_num = wsa883x_codec_get_dev_num;
+	pdata->cps_config.hw_reg_cfg.num_spkr = pdata->wsa_max_devs;
+
+	ret = of_property_read_u32_array(pdev->dev.of_node,
+				"qcom,cps_reg_phy_addr", dt_values,
+				sizeof(dt_values)/sizeof(dt_values[0]));
+	if (ret) {
+		dev_dbg(&pdev->dev, "%s: could not find %s entry in dt\n",
+			__func__, "qcom,cps_reg_phy_addr");
+	} else {
+		pdata->cps_config.hw_reg_cfg.lpass_wr_cmd_reg_phy_addr =
+								dt_values[0];
+		pdata->cps_config.hw_reg_cfg.lpass_rd_cmd_reg_phy_addr =
+								dt_values[1];
+		pdata->cps_config.hw_reg_cfg.lpass_rd_fifo_reg_phy_addr =
+								dt_values[2];
+	}
+
+	ret = of_property_read_u32_array(pdev->dev.of_node,
+				"qcom,cps_threshold_levels", dt_values,
+				sizeof(dt_values)/sizeof(dt_values[0]) - 1);
+	if (ret) {
+		dev_dbg(&pdev->dev, "%s: could not find %s entry in dt\n",
+			__func__, "qcom,cps_threshold_levels");
+	} else {
+		pdata->cps_config.hw_reg_cfg.vbatt_lower2_threshold =
+								dt_values[0];
+		pdata->cps_config.hw_reg_cfg.vbatt_lower1_threshold =
+								dt_values[1];
+	}
+
+	pdata->cps_config.spkr_dep_cfg = devm_kzalloc(&pdev->dev,
+				    sizeof(struct lpass_swr_spkr_dep_cfg_t)
+				    * pdata->wsa_max_devs, GFP_KERNEL);
+	if (!pdata->cps_config.spkr_dep_cfg) {
+		dev_err(&pdev->dev, "%s: spkr dep cfg alloc failed\n", __func__);
+		return;
+	}
+	ret = of_property_read_u32_array(pdev->dev.of_node,
+				"qcom,cps_wsa_vbatt_temp_reg_addr", dt_values,
+				sizeof(dt_values)/sizeof(dt_values[0]) - 1);
+	if (ret) {
+		dev_dbg(&pdev->dev, "%s: could not find %s entry in dt\n",
+			__func__, "qcom,cps_wsa_vbatt_temp_reg_addr");
+	} else {
+		for (i = 0; i < pdata->wsa_max_devs; i++) {
+			pdata->cps_config.spkr_dep_cfg[i].vbatt_pkd_reg_addr =
+								dt_values[0];
+			pdata->cps_config.spkr_dep_cfg[i].temp_pkd_reg_addr =
+								dt_values[1];
+		}
+	}
+
+	ret = of_property_read_u32_array(pdev->dev.of_node,
+				"qcom,cps_normal_values", dt_values,
+				sizeof(dt_values)/sizeof(dt_values[0]));
+	if (ret) {
+		dev_dbg(&pdev->dev, "%s: could not find %s entry in dt\n",
+			__func__, "qcom,cps_normal_values");
+	} else {
+		for (i = 0; i < pdata->wsa_max_devs; i++) {
+			for (j = 0; j < MAX_CPS_LEVELS; j++) {
+				pdata->cps_config.spkr_dep_cfg[i].
+					value_normal_thrsd[j] = dt_values[j];
+			}
+		}
+	}
+
+	ret = of_property_read_u32_array(pdev->dev.of_node,
+				"qcom,cps_lower1_values", dt_values,
+				sizeof(dt_values)/sizeof(dt_values[0]));
+	if (ret) {
+		dev_dbg(&pdev->dev, "%s: could not find %s entry in dt\n",
+			__func__, "qcom,cps_lower1_values");
+	} else {
+		for (i = 0; i < pdata->wsa_max_devs; i++) {
+			for (j = 0; j < MAX_CPS_LEVELS; j++) {
+				pdata->cps_config.spkr_dep_cfg[i].
+					value_low1_thrsd[j] = dt_values[j];
+			}
+		}
+	}
+
+	ret = of_property_read_u32_array(pdev->dev.of_node,
+				"qcom,cps_lower2_values", dt_values,
+				sizeof(dt_values)/sizeof(dt_values[0]));
+	if (ret) {
+		dev_dbg(&pdev->dev, "%s: could not find %s entry in dt\n",
+			__func__, "qcom,cps_lower2_values");
+	} else {
+		for (i = 0; i < pdata->wsa_max_devs; i++) {
+			for (j = 0; j < MAX_CPS_LEVELS; j++) {
+				pdata->cps_config.spkr_dep_cfg[i].
+					value_low2_thrsd[j] = dt_values[j];
+			}
+		}
+	}
+}
+
 static int msm_asoc_machine_probe(struct platform_device *pdev)
 {
 	struct snd_soc_card *card = NULL;
@@ -8020,6 +8237,9 @@ static int msm_asoc_machine_probe(struct platform_device *pdev)
 	for (index = PRIM_MI2S; index < MI2S_MAX; index++)
 		atomic_set(&(pdata->mi2s_gpio_ref_count[index]), 0);
 
+	/* parse cps configuration from dt */
+	parse_cps_configuration(pdev, pdata);
+
 	/* Register LPASS audio hw vote */
 	lpass_audio_hw_vote = devm_clk_get(&pdev->dev, "lpass_audio_hw_vote");
 	if (IS_ERR(lpass_audio_hw_vote)) {