dsp: afe: send cps config after sending topology

Set params for a module should be done after sending
the topology information in which the module resides in.
Send cps configuration after sending afe topology.
Send cps configuration only for speaker protected usecases.

Change-Id: I89c7df9f0d70862813aa7a2bd1ea1afc2ddcc882
Signed-off-by: Vignesh Kulothungan <[email protected]>

asoc/lahaina.c

@@ -5020,14 +5020,12 @@ 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,
+static void set_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;
@@ -5037,29 +5035,29 @@ static int send_cps_config(struct snd_soc_pcm_runtime *rtd,
 
 	if (!pdata) {
 		pr_err("%s: pdata is NULL\n", __func__);
-		return -EINVAL;
+		return;
 	}
 
 	if (!num_ch) {
 		pr_err("%s: channel count is 0\n", __func__);
-		return -EINVAL;
+		return;
 	}
 
 	if (!pdata->get_wsa_dev_num) {
 		pr_err("%s: get_wsa_dev_num is NULL\n", __func__);
-		return -EINVAL;
+		return;
 	}
 
 	if (!pdata->cps_config.spkr_dep_cfg) {
 		pr_err("%s: spkr_dep_cfg is NULL\n", __func__);
-		return -EINVAL;
+		return;
 	}
 
 	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;
+		return;
 	}
 
 	pdata->cps_config.lpass_hw_intf_cfg_mode = 1;
@@ -5077,14 +5075,14 @@ static int send_cps_config(struct snd_soc_pcm_runtime *rtd,
 		if (!component) {
 			pr_err("%s: %s component is NULL\n", __func__,
 				wsa_cdc_name);
-			return -EINVAL;
+			return;
 		}
 
 		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;
+			return;
 		}
 
 		/* Clear stale dev num info */
@@ -5106,18 +5104,8 @@ static int send_cps_config(struct snd_soc_pcm_runtime *rtd,
 		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;
+	afe_set_cps_config(msm_get_port_id(dai_link->id),
+					&pdata->cps_config, ch_mask);
 }
 
 static int msm_snd_cdc_dma_hw_params(struct snd_pcm_substream *substream,
@@ -5170,7 +5158,7 @@ static int msm_snd_cdc_dma_hw_params(struct snd_pcm_substream *substream,
 
 			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,
+				set_cps_config(rtd, user_set_rx_ch,
 						rx_ch_cdc_dma);
 			}
 		}

dsp/q6afe.c

@@ -247,6 +247,8 @@ struct afe_ctl {
 	uint32_t initial_cal;
 	uint32_t v_vali_flag;
 	uint32_t num_spkrs;
+	uint32_t cps_ch_mask;
+	struct afe_cps_hw_intf_cfg *cps_config;
 };
 
 struct afe_clkinfo_per_port {
@@ -2240,46 +2242,53 @@ 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)
+static int afe_send_cps_config(int src_port)
 {
+	int i = 0;
 	struct param_hdr_v3 param_info;
 	int ret = -EINVAL;
 	u8 *packed_payload = NULL;
 	int cpy_size = 0;
+	int ch_copied = 0;
+	size_t param_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);
+	if ((-1 == this_afe.vi_tx_port) || (!this_afe.cps_ch_mask) ||
+	    (!this_afe.cps_config)) {
+		pr_err("%s: speaker prot not configured for 0x%x\n", __func__,
+		       src_port);
 		return -EINVAL;
 	}
 
+	param_size = sizeof(struct afe_cps_hw_intf_cfg) -
+			sizeof(this_afe.cps_config->spkr_dep_cfg) +
+			(sizeof(struct lpass_swr_spkr_dep_cfg_t)
+				* this_afe.num_spkrs);
+
+	this_afe.cps_config->hw_reg_cfg.num_spkr = this_afe.num_spkrs;
 	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);
+			sizeof(this_afe.cps_config->spkr_dep_cfg);
+	memcpy(packed_payload, this_afe.cps_config, cpy_size);
+
+	while (ch_copied < this_afe.num_spkrs) {
+		if (!(this_afe.cps_ch_mask & (1 << i))) {
+			i++;
+			continue;
+		}
+
+		memcpy(packed_payload + cpy_size,
+			&this_afe.cps_config->spkr_dep_cfg[i],
+			sizeof(struct lpass_swr_spkr_dep_cfg_t));
+		cpy_size += sizeof(struct lpass_swr_spkr_dep_cfg_t);
+		ch_copied++;
+		i++;
+	}
 
 	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;
@@ -2292,13 +2301,11 @@ int afe_send_cps_config(int src_port,
 		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)
@@ -5648,6 +5655,11 @@ static int __afe_port_start(u16 port_id, union afe_port_config *afe_config,
 		afe_send_hw_delay(port_id, rate);
 	}
 
+	if ((this_afe.cps_config) &&
+	    (this_afe.vi_rx_port == port_id)) {
+		afe_send_cps_config(port_id);
+	}
+
 	/* Start SW MAD module */
 	mad_type = afe_port_get_mad_type(port_id);
 	pr_debug("%s: port_id 0x%x, mad_type %d\n", __func__, port_id,
@@ -11366,3 +11378,34 @@ done:
 	return ret;
 }
 EXPORT_SYMBOL(afe_unvote_lpass_core_hw);
+
+/**
+ * afe_set_cps_config -
+ *         to set cps speaker protection configuration
+ *
+ * @src_port: source port to send configuration to
+ * @cps_config: cps speaker protection v4 configuration
+ * @ch_mask: channel mask
+ *
+ */
+void afe_set_cps_config(int src_port,
+			struct afe_cps_hw_intf_cfg *cps_config,
+			u32 ch_mask)
+{
+	this_afe.cps_config = NULL;
+	this_afe.cps_ch_mask = 0;
+
+	if (!cps_config) {
+		pr_err("%s: cps config is NULL\n", __func__);
+		return;
+	}
+
+	if (q6audio_validate_port(src_port) < 0) {
+		pr_err("%s: Invalid src port 0x%x\n", __func__, src_port);
+		return;
+	}
+
+	this_afe.cps_ch_mask = ch_mask;
+	this_afe.cps_config = cps_config;
+}
+EXPORT_SYMBOL(afe_set_cps_config);

include/dsp/q6afe-v2.h

@@ -459,9 +459,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,
+void afe_set_cps_config(int src_port,
 			struct afe_cps_hw_intf_cfg *cps_config,
-			int param_size);
+			u32 ch_mask);
 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);