diff --git a/asoc/lahaina.c b/asoc/lahaina.c index d0c659969c..3912a15f74 100644 --- a/asoc/lahaina.c +++ b/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); } } diff --git a/dsp/q6afe.c b/dsp/q6afe.c index 527d355672..b4d51c01c6 100644 --- a/dsp/q6afe.c +++ b/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(¶m_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); diff --git a/include/dsp/q6afe-v2.h b/include/dsp/q6afe-v2.h index 0d608b6d61..09afc2e2aa 100644 --- a/include/dsp/q6afe-v2.h +++ b/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);