Merge "dsp: afe: send cps config after sending topology"

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);