ASoC: msm: Fix pan control param gain to accommodate Q28 format

Fix pan control gain param to accommodate Q28 format.
Convert 32bit gain to be inlined with 16bit mixer coefficients
of ADSP.

Change-Id: I85ebd11c49b4af982f34091937acabb9e6580e8f
Signed-off-by: Varun Balaraj <varunb@codeaurora.org>

3 files changed, 25 insertions, 15 deletions

diff --git a/sound/soc/msm/qdsp6v2/msm-pcm-q6-v2.c b/sound/soc/msm/qdsp6v2/msm-pcm-q6-v2.c
index de769e8b806c..b94eb6fbfeea 100644
--- a/sound/soc/msm/qdsp6v2/msm-pcm-q6-v2.c
+++ b/sound/soc/msm/qdsp6v2/msm-pcm-q6-v2.c
@@ -1668,7 +1668,7 @@ static int msm_pcm_playback_pan_scale_ctl_put(struct snd_kcontrol *kcontrol,
 		for (i = 0; i < pan_param.num_output_channels *
 			pan_param.num_input_channels; i++) {
 			pan_param.gain[i] =
-			!(ucontrol->value.integer.value[len++] > 0) ?
+				!(ucontrol->value.integer.value[len++] > 0) ?
 				0 : 2 << 13;
 		}
 	}
@@ -1679,8 +1679,12 @@ static int msm_pcm_playback_pan_scale_ctl_put(struct snd_kcontrol *kcontrol,
 	       pan_param.num_input_channels;
 	for (i = 0; i < pan_param.num_output_channels *
 		pan_param.num_input_channels; i++) {
+		/*
+		 * The data userspace passes is already in Q14 format.
+		 * For volume gain is in Q28.
+		 */
 		pan_param.gain[i] =
-				ucontrol->value.integer.value[len++];
+				ucontrol->value.integer.value[len++] << 14;
 	}
 	ret = q6asm_set_vol_ctrl_gain_pair(prtd->audio_client,
 					   &pan_param);
@@ -1753,7 +1757,7 @@ static int msm_pcm_playback_dnmix_ctl_put(struct snd_kcontrol *kcontrol,
 	struct msm_audio *prtd;
 	struct asm_stream_pan_ctrl_params dnmix_param;
 
-	int be_id = ucontrol->value.integer.value[len];
+	int be_id = ucontrol->value.integer.value[len++];
 	int stream_id = 0;
 
 	if (!usr_info) {
@@ -1809,7 +1813,7 @@ static int msm_pcm_playback_dnmix_ctl_put(struct snd_kcontrol *kcontrol,
 		for (i = 0; i < dnmix_param.num_output_channels *
 				dnmix_param.num_input_channels; i++) {
 			dnmix_param.gain[i] =
-				ucontrol->value.integer.value[len++];
+					ucontrol->value.integer.value[len++];
 		}
 	}
 	msm_routing_set_downmix_control_data(be_id,
diff --git a/sound/soc/msm/qdsp6v2/msm-pcm-routing-v2.c b/sound/soc/msm/qdsp6v2/msm-pcm-routing-v2.c
index 38937612dcce..9e2e8b37ec10 100644
--- a/sound/soc/msm/qdsp6v2/msm-pcm-routing-v2.c
+++ b/sound/soc/msm/qdsp6v2/msm-pcm-routing-v2.c
@@ -16201,6 +16201,8 @@ int msm_routing_set_downmix_control_data(int be_id, int session_id,
 	char *adm_params = NULL;
 	int port_id, copp_idx = 0;
 	uint32_t params_length = 0;
+	uint16_t ii;
+	uint16_t *dst_gain_ptr = NULL;
 
 	if (be_id >= MSM_BACKEND_DAI_MAX) {
 		rc = -EINVAL;
@@ -16213,7 +16215,7 @@ int msm_routing_set_downmix_control_data(int be_id, int session_id,
 
 	variable_payload = dnmix_param->num_output_channels * sizeof(uint16_t)+
 			   dnmix_param->num_input_channels * sizeof(uint16_t) +
-			   dnmix_param->num_output_channels * sizeof(uint16_t) *
+			   dnmix_param->num_output_channels *
 			   dnmix_param->num_input_channels * sizeof(uint16_t);
 	i = (variable_payload % sizeof(uint32_t));
 	variable_payload += (i == 0) ? 0 : sizeof(uint32_t) - i;
@@ -16252,14 +16254,15 @@ int msm_routing_set_downmix_control_data(int be_id, int session_id,
 		dnmix_param->num_output_channels * sizeof(uint16_t)),
 		dnmix_param->input_channel_map,
 		dnmix_param->num_input_channels * sizeof(uint16_t));
-	memcpy(((u8 *)adm_params +
+
+	dst_gain_ptr = (uint16_t *) ((u8 *)adm_params +
 		sizeof(struct adm_pspd_param_data_t) +
 		sizeof(struct audproc_chmixer_param_coeff) +
 		(dnmix_param->num_output_channels * sizeof(uint16_t)) +
-		(dnmix_param->num_input_channels * sizeof(uint16_t))),
-		dnmix_param->gain,
-		(dnmix_param->num_output_channels * sizeof(uint16_t)) *
 		(dnmix_param->num_input_channels * sizeof(uint16_t)));
+	for (ii = 0; ii < dnmix_param->num_output_channels *
+			dnmix_param->num_input_channels; ii++)
+		dst_gain_ptr[ii] = (uint16_t) dnmix_param->gain[ii];
 
 	if (params_length) {
 		rc = adm_set_pspd_matrix_params(port_id,
diff --git a/sound/soc/msm/qdsp6v2/q6asm.c b/sound/soc/msm/qdsp6v2/q6asm.c
index d297acd62665..609ae8cd82ac 100644
--- a/sound/soc/msm/qdsp6v2/q6asm.c
+++ b/sound/soc/msm/qdsp6v2/q6asm.c
@@ -7543,7 +7543,9 @@ int q6asm_set_mfc_panning_params(struct audio_client *ac,
 	struct asm_stream_param_data_v2 data;
 	struct audproc_chmixer_param_coeff pan_cfg;
 	uint16_t variable_payload = 0;
-	uint16_t *asm_params = NULL;
+	char *asm_params = NULL;
+	uint16_t ii;
+	uint16_t *dst_gain_ptr = NULL;
 
 	sz = rc = i = 0;
 	if (ac == NULL) {
@@ -7604,7 +7606,7 @@ int q6asm_set_mfc_panning_params(struct audio_client *ac,
 
 	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_output_channels *
 			pan_param->num_input_channels * sizeof(uint16_t);
 	i = (variable_payload % sizeof(uint32_t));
 	variable_payload += (i == 0) ? 0 : sizeof(uint32_t) - i;
@@ -7664,15 +7666,16 @@ int q6asm_set_mfc_panning_params(struct audio_client *ac,
 		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) +
+
+	dst_gain_ptr = (uint16_t *) ((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)));
+	for (ii = 0; ii < pan_param->num_output_channels *
+			pan_param->num_input_channels; ii++)
+		dst_gain_ptr[ii] = (uint16_t) pan_param->gain[ii];
 
 	rc = apr_send_pkt(ac->apr, (uint32_t *) asm_params);
 	if (rc < 0) {