3 files changed, 299 insertions, 9 deletions

diff --git a/sound/soc/msm/qdsp6v2/msm-qti-pp-config.c b/sound/soc/msm/qdsp6v2/msm-qti-pp-config.c
index cac28f43e5ae..65f5167d9dee 100644
--- a/sound/soc/msm/qdsp6v2/msm-qti-pp-config.c
+++ b/sound/soc/msm/qdsp6v2/msm-qti-pp-config.c
@@ -298,11 +298,11 @@ int msm_qti_pp_send_stereo_to_custom_stereo_cmd(int port_id, int copp_idx,
 	*update_params_value16++ = op_FR_ip_FR_weight;
 	avail_length = avail_length - (4 * sizeof(uint16_t));
 	if (params_length) {
-		rc = adm_set_stereo_to_custom_stereo(port_id,
-						     copp_idx,
-						     session_id,
-						     params_value,
-						     params_length);
+		rc = adm_set_pspd_matrix_params(port_id,
+						copp_idx,
+						session_id,
+						params_value,
+						params_length);
 		if (rc) {
 			pr_err("%s: send params failed rc=%d\n", __func__, rc);
 			kfree(params_value);
diff --git a/sound/soc/msm/qdsp6v2/q6adm.c b/sound/soc/msm/qdsp6v2/q6adm.c
index 28aaf2172221..4aad16db3e0f 100644
--- a/sound/soc/msm/qdsp6v2/q6adm.c
+++ b/sound/soc/msm/qdsp6v2/q6adm.c
@@ -781,9 +781,9 @@ fail_cmd:
 	return ret;
 }
 
-int adm_set_stereo_to_custom_stereo(int port_id, int copp_idx,
-				    unsigned int session_id, char *params,
-				    uint32_t params_length)
+int adm_set_pspd_matrix_params(int port_id, int copp_idx,
+				unsigned int session_id, char *params,
+				uint32_t params_length)
 {
 	struct adm_cmd_set_pspd_mtmx_strtr_params_v5 *adm_params = NULL;
 	int sz, rc = 0, port_idx;
diff --git a/sound/soc/msm/qdsp6v2/q6asm.c b/sound/soc/msm/qdsp6v2/q6asm.c
index 44e5b01b1ccb..d3cff9fccf59 100644
--- a/sound/soc/msm/qdsp6v2/q6asm.c
+++ b/sound/soc/msm/qdsp6v2/q6asm.c
@@ -44,7 +44,7 @@
 #define TRUE        0x01
 #define FALSE       0x00
 #define SESSION_MAX 8
-
+#define ASM_MAX_CHANNELS 8
 enum {
 	ASM_TOPOLOGY_CAL = 0,
 	ASM_CUSTOM_TOP_CAL,
@@ -7419,6 +7419,296 @@ int q6asm_set_softvolume_v2(struct audio_client *ac,
 	return __q6asm_set_softvolume(ac, softvol_param, instance);
 }
 
+int q6asm_set_vol_ctrl_gain_pair(struct audio_client *ac,
+				 struct asm_stream_pan_ctrl_params *pan_param)
+{
+	int sz = 0;
+	int rc = 0;
+	int i = 0;
+	int32_t ch = 0;
+	struct apr_hdr hdr;
+	struct audproc_volume_ctrl_channel_type_gain_pair
+		gain_data[ASM_MAX_CHANNELS];
+	struct asm_stream_cmd_set_pp_params_v2 payload_params;
+	struct asm_stream_param_data_v2 data;
+	uint16_t *asm_params = NULL;
+
+	if (ac == NULL) {
+		pr_err("%s: ac is NULL\n", __func__);
+		rc = -EINVAL;
+		goto fail;
+	}
+	if (ac->apr == NULL) {
+		dev_err(ac->dev, "%s: ac apr handle NULL\n", __func__);
+		rc = -EINVAL;
+		goto fail;
+	}
+
+	sz = sizeof(struct apr_hdr) +
+	     sizeof(struct asm_stream_cmd_set_pp_params_v2) +
+	     sizeof(struct asm_stream_param_data_v2) +
+	     sizeof(uint32_t) +
+	     (sizeof(struct audproc_volume_ctrl_channel_type_gain_pair) *
+		     ASM_MAX_CHANNELS);
+	asm_params = kzalloc(sz, GFP_KERNEL);
+	if (!asm_params) {
+		rc = -ENOMEM;
+		goto fail;
+	}
+
+	q6asm_add_hdr_async(ac, &hdr, sz, TRUE);
+	atomic_set(&ac->cmd_state_pp, -1);
+
+	hdr.opcode = ASM_STREAM_CMD_SET_PP_PARAMS_V2;
+	memcpy(((u8 *)asm_params), &hdr, sizeof(struct apr_hdr));
+
+	payload_params.data_payload_addr_lsw = 0;
+	payload_params.data_payload_addr_msw = 0;
+	payload_params.mem_map_handle = 0;
+	payload_params.data_payload_size =
+		sizeof(struct asm_stream_param_data_v2) +
+		sizeof(uint32_t) +
+		(sizeof(struct audproc_volume_ctrl_channel_type_gain_pair) *
+		ASM_MAX_CHANNELS);
+	memcpy(((u8 *)asm_params + sizeof(struct apr_hdr)),
+		&payload_params,
+		sizeof(struct asm_stream_cmd_set_pp_params_v2));
+
+	data.module_id = AUDPROC_MODULE_ID_VOL_CTRL;
+	data.param_id = AUDPROC_PARAM_ID_MULTICHANNEL_GAIN;
+	data.param_size = sizeof(uint32_t) +
+		(sizeof(struct audproc_volume_ctrl_channel_type_gain_pair) *
+		ASM_MAX_CHANNELS);
+	data.reserved = 0;
+	memcpy(((u8 *)asm_params + sizeof(struct apr_hdr) +
+		sizeof(struct asm_stream_cmd_set_pp_params_v2)),
+		&data, sizeof(struct asm_stream_param_data_v2));
+
+	ch = pan_param->num_output_channels;
+	memcpy(((u8 *)asm_params + sizeof(struct apr_hdr) +
+		sizeof(struct asm_stream_cmd_set_pp_params_v2) +
+		sizeof(struct asm_stream_param_data_v2)),
+		&ch,
+		sizeof(uint32_t));
+
+	memset(gain_data, 0,
+		ASM_MAX_CHANNELS *
+		sizeof(struct audproc_volume_ctrl_channel_type_gain_pair));
+	for (i = 0; i < pan_param->num_output_channels; i++) {
+		gain_data[i].channel_type =
+		   pan_param->output_channel_map[i];
+		gain_data[i].gain = pan_param->gain[i];
+	}
+	memcpy(((u8 *)asm_params + sizeof(struct apr_hdr) +
+		sizeof(struct asm_stream_cmd_set_pp_params_v2) +
+		sizeof(struct asm_stream_param_data_v2) +
+		sizeof(uint32_t)),
+		gain_data,
+		ASM_MAX_CHANNELS *
+		sizeof(struct audproc_volume_ctrl_channel_type_gain_pair));
+
+	rc = apr_send_pkt(ac->apr, (uint32_t *) asm_params);
+	if (rc < 0) {
+		pr_err("%s: set-params send failed paramid[0x%x] rc %d\n",
+				__func__, data.param_id, rc);
+		goto done;
+	}
+
+	rc = wait_event_timeout(ac->cmd_wait,
+			(atomic_read(&ac->cmd_state_pp) >= 0), 5 * HZ);
+	if (!rc) {
+		pr_err("%s: timeout, set-params paramid[0x%x]\n", __func__,
+				data.param_id);
+		rc = -EINVAL;
+		goto done;
+	}
+	if (atomic_read(&ac->cmd_state_pp) > 0) {
+		pr_err("%s: DSP returned error[%d], set-params paramid[0x%x]\n",
+		       __func__, atomic_read(&ac->cmd_state_pp),
+		       data.param_id);
+		rc = -EINVAL;
+		goto done;
+	}
+	rc = 0;
+done:
+	kfree(asm_params);
+fail:
+	return rc;
+}
+
+int q6asm_set_mfc_panning_params(struct audio_client *ac,
+				 struct asm_stream_pan_ctrl_params *pan_param)
+{
+	int sz, rc, i;
+	struct audproc_mfc_output_media_fmt mfc_cfg;
+	struct apr_hdr hdr;
+	struct asm_stream_cmd_set_pp_params_v2 payload_params;
+	struct asm_stream_param_data_v2 data;
+	struct audproc_chmixer_param_coeff pan_cfg;
+	uint16_t variable_payload = 0;
+	uint16_t *asm_params = NULL;
+
+	sz = rc = i = 0;
+	if (ac == NULL) {
+		pr_err("%s: ac handle NULL\n", __func__);
+		rc = -EINVAL;
+		goto fail_cmd1;
+	}
+	if (ac->apr == NULL) {
+		pr_err("%s: ac apr handle NULL\n", __func__);
+		rc = -EINVAL;
+		goto fail_cmd1;
+	}
+
+	sz = sizeof(struct audproc_mfc_output_media_fmt);
+	q6asm_add_hdr_async(ac, &mfc_cfg.params.hdr, sz, TRUE);
+	atomic_set(&ac->cmd_state_pp, -1);
+	mfc_cfg.params.hdr.opcode = ASM_STREAM_CMD_SET_PP_PARAMS_V2;
+	mfc_cfg.params.payload_addr_lsw = 0;
+	mfc_cfg.params.payload_addr_msw = 0;
+	mfc_cfg.params.mem_map_handle = 0;
+	mfc_cfg.params.payload_size = sizeof(mfc_cfg) - sizeof(mfc_cfg.params);
+	mfc_cfg.data.module_id = AUDPROC_MODULE_ID_MFC;
+	mfc_cfg.data.param_id = AUDPROC_PARAM_ID_MFC_OUTPUT_MEDIA_FORMAT;
+	mfc_cfg.data.param_size = mfc_cfg.params.payload_size -
+		sizeof(mfc_cfg.data);
+	mfc_cfg.data.reserved = 0;
+	mfc_cfg.sampling_rate = 0;
+	mfc_cfg.bits_per_sample = 0;
+	mfc_cfg.num_channels = pan_param->num_output_channels;
+	for (i = 0; i < mfc_cfg.num_channels; i++)
+		mfc_cfg.channel_type[i] = pan_param->output_channel_map[i];
+
+	rc = apr_send_pkt(ac->apr, (uint32_t *) &mfc_cfg);
+	if (rc < 0) {
+		pr_err("%s: set-params send failed paramid[0x%x] rc %d\n",
+				__func__, mfc_cfg.data.param_id, rc);
+		rc = -EINVAL;
+		goto fail_cmd1;
+	}
+
+	rc = wait_event_timeout(ac->cmd_wait,
+			(atomic_read(&ac->cmd_state_pp) >= 0), 5*HZ);
+	if (!rc) {
+		pr_err("%s: timeout, set-params paramid[0x%x]\n", __func__,
+						mfc_cfg.data.param_id);
+		rc = -ETIMEDOUT;
+		goto fail_cmd1;
+	}
+	if (atomic_read(&ac->cmd_state_pp) > 0) {
+		pr_err("%s: DSP returned error[%s] set-params paramid[0x%x]\n",
+				__func__, adsp_err_get_err_str(
+				atomic_read(&ac->cmd_state_pp)),
+				mfc_cfg.data.param_id);
+		rc = adsp_err_get_lnx_err_code(
+				atomic_read(&ac->cmd_state_pp));
+		goto fail_cmd1;
+	}
+
+	variable_payload = pan_param->num_output_channels * sizeof(uint16_t)+
+			pan_param->num_input_channels * sizeof(uint16_t) +
+			pan_param->num_output_channels * sizeof(uint16_t) *
+			pan_param->num_input_channels * sizeof(uint16_t);
+	i = (variable_payload % sizeof(uint32_t));
+	variable_payload += (i == 0) ? 0 : sizeof(uint32_t) - i;
+	sz = sizeof(struct apr_hdr) +
+		sizeof(struct asm_stream_cmd_set_pp_params_v2) +
+		sizeof(struct asm_stream_param_data_v2) +
+		sizeof(struct audproc_chmixer_param_coeff) +
+		variable_payload;
+
+	asm_params = kzalloc(sz, GFP_KERNEL);
+	if (!asm_params) {
+		rc = -ENOMEM;
+		goto fail_cmd1;
+	}
+
+	q6asm_add_hdr_async(ac, &hdr, sz, TRUE);
+	atomic_set(&ac->cmd_state_pp, -1);
+	hdr.opcode = ASM_STREAM_CMD_SET_PP_PARAMS_V2;
+	memcpy(((u8 *)asm_params), &hdr, sizeof(struct apr_hdr));
+
+	payload_params.data_payload_addr_lsw = 0;
+	payload_params.data_payload_addr_msw = 0;
+	payload_params.mem_map_handle = 0;
+	payload_params.data_payload_size =
+		sizeof(struct audproc_chmixer_param_coeff) +
+		variable_payload + sizeof(struct asm_stream_param_data_v2);
+	memcpy(((u8 *)asm_params + sizeof(struct apr_hdr)),
+		&payload_params,
+		sizeof(struct asm_stream_cmd_set_pp_params_v2));
+
+	data.module_id = AUDPROC_MODULE_ID_MFC;
+	data.param_id = AUDPROC_CHMIXER_PARAM_ID_COEFF;
+	data.param_size = sizeof(struct audproc_chmixer_param_coeff) +
+				 variable_payload;
+	data.reserved = 0;
+	memcpy(((u8 *)asm_params + sizeof(struct apr_hdr) +
+		sizeof(struct asm_stream_cmd_set_pp_params_v2)),
+		&data, sizeof(struct asm_stream_param_data_v2));
+
+	pan_cfg.index = 0;
+	pan_cfg.num_output_channels = pan_param->num_output_channels;
+	pan_cfg.num_input_channels = pan_param->num_input_channels;
+	memcpy(((u8 *)asm_params + sizeof(struct apr_hdr) +
+		sizeof(struct asm_stream_cmd_set_pp_params_v2) +
+		sizeof(struct asm_stream_param_data_v2)),
+		&pan_cfg, sizeof(struct audproc_chmixer_param_coeff));
+	memcpy(((u8 *)asm_params + sizeof(struct apr_hdr) +
+		sizeof(struct asm_stream_cmd_set_pp_params_v2) +
+		sizeof(struct asm_stream_param_data_v2) +
+		sizeof(struct audproc_chmixer_param_coeff)),
+		pan_param->output_channel_map,
+		pan_param->num_output_channels * sizeof(uint16_t));
+	memcpy(((u8 *)asm_params + sizeof(struct apr_hdr) +
+		sizeof(struct asm_stream_cmd_set_pp_params_v2) +
+		sizeof(struct asm_stream_param_data_v2) +
+		sizeof(struct audproc_chmixer_param_coeff) +
+		pan_param->num_output_channels * sizeof(uint16_t)),
+		pan_param->input_channel_map,
+		pan_param->num_input_channels * sizeof(uint16_t));
+	memcpy(((u8 *)asm_params + sizeof(struct apr_hdr) +
+		sizeof(struct asm_stream_cmd_set_pp_params_v2) +
+		sizeof(struct asm_stream_param_data_v2) +
+		sizeof(struct audproc_chmixer_param_coeff) +
+		(pan_param->num_output_channels * sizeof(uint16_t)) +
+		(pan_param->num_input_channels * sizeof(uint16_t))),
+		pan_param->gain,
+		(pan_param->num_output_channels * sizeof(uint16_t)) *
+		(pan_param->num_input_channels * sizeof(uint16_t)));
+
+	rc = apr_send_pkt(ac->apr, (uint32_t *) asm_params);
+	if (rc < 0) {
+		pr_err("%s: set-params send failed paramid[0x%x] rc %d\n",
+				__func__, data.param_id, rc);
+		rc = -EINVAL;
+		goto fail_cmd2;
+	}
+
+	rc = wait_event_timeout(ac->cmd_wait,
+			(atomic_read(&ac->cmd_state_pp) >= 0), 5*HZ);
+	if (!rc) {
+		pr_err("%s: timeout, set-params paramid[0x%x]\n", __func__,
+						data.param_id);
+		rc = -ETIMEDOUT;
+		goto fail_cmd2;
+	}
+	if (atomic_read(&ac->cmd_state_pp) > 0) {
+		pr_err("%s: DSP returned error[%s] set-params paramid[0x%x]\n",
+				__func__, adsp_err_get_err_str(
+				atomic_read(&ac->cmd_state_pp)),
+				data.param_id);
+		rc = adsp_err_get_lnx_err_code(
+				atomic_read(&ac->cmd_state_pp));
+		goto fail_cmd2;
+	}
+	rc = 0;
+fail_cmd2:
+	kfree(asm_params);
+fail_cmd1:
+	return rc;
+}
+
 int q6asm_equalizer(struct audio_client *ac, void *eq_p)
 {
 	struct asm_eq_params eq;