ASoC: msm: qdsp6v2: Add dynamic chmix config support

Add support to dynamically configure channel mixing weightage
coefficients for both ASM and LSM sessions.
Currently the code is executed only for LSM session and can be
extended to ASM as well.

Change-Id: I727ea6fa2d3a99bbf6811005c5733535f680891b
Signed-off-by: Chaithanya Krishna Bacharaju <[email protected]>
Signed-off-by: Bhalchandra Gajare <[email protected]>

asoc/msm-pcm-routing-v2.c

@@ -802,6 +802,69 @@ static bool is_mm_lsm_fe_id(int fe_id)
 	return rc;
 }
 
+/*
+ * msm_pcm_routing_send_chmix_cfg:
+ *	send the channel mixer command to mix the input channels
+ *	into output channels.
+ *
+ * @fe_id: front end id
+ * @ip_channel_cnt: input channel count
+ * @op_channel_cnt: output channel count
+ * @ch_wght_coeff: channel weight co-efficients for channel mixing
+ * @session_type: indicates session is of type TX or RX
+ * @stream_type: indicates either Audio or Listen stream type
+ */
+int msm_pcm_routing_send_chmix_cfg(int fe_id, int ip_channel_cnt,
+				int op_channel_cnt, int *ch_wght_coeff,
+				int session_type, int stream_type)
+{
+
+	int rc = 0, idx = 0;
+	int be_index = 0, port_id;
+	unsigned int session_id = 0;
+
+	pr_debug("%s:fe_id[%d] ip_ch[%d] op_ch[%d] sess_type [%d], stream_type[%d]",
+		 __func__, fe_id, ip_channel_cnt, op_channel_cnt, session_type,
+		 stream_type);
+	if (!is_mm_lsm_fe_id(fe_id)) {
+		/* bad ID assigned in machine driver */
+		pr_err("%s: bad MM ID %d\n", __func__, fe_id);
+		return -EINVAL;
+	}
+
+	if (ch_wght_coeff == NULL) {
+		pr_err("%s: Null channel weightage coefficients passed\n",
+			__func__);
+		return -EINVAL;
+	}
+
+	for (be_index = 0; be_index < MSM_BACKEND_DAI_MAX; be_index++) {
+		port_id = msm_bedais[be_index].port_id;
+		if (!msm_bedais[be_index].active ||
+		    !test_bit(fe_id, &msm_bedais[be_index].fe_sessions[0]))
+			continue;
+
+		session_id = fe_dai_map[fe_id][session_type].strm_id;
+
+		for (idx = 0; idx < MAX_COPPS_PER_PORT; idx++) {
+			unsigned long copp =
+				session_copp_map[fe_id][session_type][be_index];
+			if (!test_bit(idx, &copp))
+				continue;
+			msm_qti_pp_send_chmix_cfg_cmd(port_id, idx,
+						session_id, ip_channel_cnt,
+						op_channel_cnt, ch_wght_coeff,
+						session_type, stream_type);
+			if (rc < 0)
+				pr_err("%s: err setting channel mix config\n",
+					__func__);
+		}
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL(msm_pcm_routing_send_chmix_cfg);
+
 int msm_pcm_routing_reg_stream_app_type_cfg(
 	int fedai_id, int session_type, int be_id,
 	struct msm_pcm_stream_app_type_cfg *cfg_data)

asoc/msm-pcm-routing-v2.h

@@ -454,6 +454,9 @@ enum {
 #define BE_DAI_PORT_SESSIONS_IDX_MAX		4
 #define BE_DAI_FE_SESSIONS_IDX_MAX		2
 
+#define STREAM_TYPE_ASM 0
+#define STREAM_TYPE_LSM 1
+
 enum {
 	ADM_TOPOLOGY_CAL_TYPE_IDX = 0,
 	ADM_LSM_TOPOLOGY_CAL_TYPE_IDX,
@@ -543,4 +546,7 @@ int msm_pcm_routing_reg_stream_app_type_cfg(
 int msm_pcm_routing_get_stream_app_type_cfg(
 	int fedai_id, int session_type, int *be_id,
 	struct msm_pcm_stream_app_type_cfg *cfg_data);
+int msm_pcm_routing_send_chmix_cfg(int fe_id, int ip_channel_cnt,
+	int op_channel_cnt, int *ch_wght_coeff,
+	int session_type, int stream_type);
 #endif /*_MSM_PCM_H*/

asoc/msm-qti-pp-config.c

@@ -29,6 +29,7 @@
 /* EQUALIZER */
 /* Equal to Frontend after last of the MULTIMEDIA SESSIONS */
 #define MAX_EQ_SESSIONS		(MSM_FRONTEND_DAI_MULTIMEDIA20 + 1)
+#define CHMIX_CFG_CONST_PARAM_SIZE 4
 
 enum {
 	EQ_BAND1 = 0,
@@ -325,6 +326,161 @@ skip_send_cmd:
 		kfree(params_value);
 		return -ENOMEM;
 }
+
+static int msm_qti_pp_arrange_mch_map(int16_t *update_params_value16,
+			 int channel_count)
+{
+	int i;
+	int16_t ch_map[PCM_FORMAT_MAX_CHANNELS_9] = {
+			PCM_CHANNEL_FL, PCM_CHANNEL_FR, PCM_CHANNEL_FC,
+			PCM_CHANNEL_LS, PCM_CHANNEL_RS, PCM_CHANNEL_LFE,
+			PCM_CHANNEL_LB, PCM_CHANNEL_RB, PCM_CHANNEL_CS };
+
+	if (channel_count < 1 ||
+	    channel_count > PCM_FORMAT_MAX_CHANNELS_9) {
+		pr_err("%s: invalid ch_cnt %d\n",
+			__func__, channel_count);
+		return -EINVAL;
+	}
+
+	switch (channel_count) {
+	/* Add special cases here */
+	case 1:
+		*update_params_value16++ = PCM_CHANNEL_FC;
+		break;
+	case 4:
+		*update_params_value16++ = PCM_CHANNEL_FL;
+		*update_params_value16++ = PCM_CHANNEL_FR;
+		*update_params_value16++ = PCM_CHANNEL_LS;
+		*update_params_value16++ = PCM_CHANNEL_RS;
+		break;
+
+	/* Add standard cases here */
+	default:
+		for (i = 0; i < channel_count; i++)
+			*update_params_value16++ = ch_map[i];
+		break;
+	}
+
+	return 0;
+}
+
+static uint32_t msm_qti_pp_get_chmix_param_size(int ip_ch_cnt, int op_ch_cnt)
+{
+	uint32_t param_size;
+	/* Assign constant part of param length initially -
+	 * Index, Num out channels, Num in channels.
+	 */
+	param_size = CHMIX_CFG_CONST_PARAM_SIZE * sizeof(uint16_t);
+
+	/* Calculate variable part of param length using ip and op channels */
+
+	/* channel map for input and output channels */
+	param_size += op_ch_cnt * sizeof(uint16_t);
+	param_size += ip_ch_cnt * sizeof(uint16_t);
+
+	/* weightage coeff for each op ch corresponding to each ip ch */
+	param_size += (ip_ch_cnt * op_ch_cnt) * sizeof(uint16_t);
+
+	/* Params length should be multiple of 4 bytes i.e 32bit aligned*/
+	param_size = (param_size + 3) & 0xFFFFFFFC;
+
+	return param_size;
+}
+
+/*
+ * msm_qti_pp_send_chmix_cfg_cmd:
+ *	Send the custom channel mixer configuration command.
+ *
+ * @port_id: Backend port id
+ * @copp_idx: ADM copp index
+ * @session_id: id for the session requesting channel mixer
+ * @ip_channel_cnt: Input channel count
+ * @op_channel_cnt: Output channel count
+ * @ch_wght_coeff: Channel weight co-efficients for mixing
+ * @session_type: Indicates TX or RX session
+ * @stream_type: Indicates Audio or Listen stream type
+ */
+int msm_qti_pp_send_chmix_cfg_cmd(int port_id, int copp_idx,
+				unsigned int session_id, int ip_channel_cnt,
+				int op_channel_cnt, int *ch_wght_coeff,
+				int session_type, int stream_type)
+{
+	char *params_value;
+	int rc = 0, i, direction;
+	u8 *param_ptr;
+	int16_t *update_params_value16 = 0;
+	uint32_t param_size = msm_qti_pp_get_chmix_param_size(ip_channel_cnt,
+				op_channel_cnt);
+	struct param_hdr_v3 *param_hdr;
+
+	/* constant payload data size represents module_id, param_id,
+	 * param size, reserved field.
+	 */
+	uint32_t params_length = param_size + sizeof(*param_hdr);
+
+	pr_debug("%s: port_id - %d, session id - %d\n", __func__, port_id,
+		 session_id);
+
+	params_value = kzalloc(params_length, GFP_KERNEL);
+	if (!params_value)
+		return -ENOMEM;
+
+	param_ptr = params_value;
+
+	param_hdr = (struct param_hdr_v3 *) param_ptr;
+	param_hdr->module_id = MTMX_MODULE_ID_DEFAULT_CHMIXER;
+	param_hdr->instance_id = INSTANCE_ID_0;
+	param_hdr->param_id = DEFAULT_CHMIXER_PARAM_ID_COEFF;
+	param_hdr->param_size = param_size;
+
+	param_ptr += sizeof(*param_hdr);
+
+	update_params_value16 = (int16_t *) param_ptr;
+	/*for alignment only*/
+	*update_params_value16++ = 0;
+	/*index is 32-bit param in little endian*/
+	*update_params_value16++ = CUSTOM_STEREO_INDEX_PARAM;
+	*update_params_value16++ = 0;
+	/*number of out ch*/
+	*update_params_value16++ = op_channel_cnt;
+	/*number of in ch*/
+	*update_params_value16++ = ip_channel_cnt;
+
+	/* Out ch map FL/FR*/
+	msm_qti_pp_arrange_mch_map(update_params_value16, op_channel_cnt);
+	update_params_value16 += op_channel_cnt;
+
+	/* In ch map FL/FR*/
+	msm_qti_pp_arrange_mch_map(update_params_value16, ip_channel_cnt);
+	update_params_value16 += ip_channel_cnt;
+
+	/* weighting coefficients as name suggests,
+	 * mixing will be done according to these coefficients.
+	 */
+	for (i = 0; i < ip_channel_cnt * op_channel_cnt; i++)
+		*update_params_value16++ =
+					ch_wght_coeff[i] ? Q14_GAIN_UNITY : 0;
+	if (params_length) {
+		direction = (session_type == SESSION_TYPE_RX) ?
+			ADM_MATRIX_ID_AUDIO_RX : ADM_MATRIX_ID_AUDIO_TX;
+		rc = adm_set_custom_chmix_cfg(port_id,
+					      copp_idx,
+					      session_id,
+					      params_value,
+					      params_length,
+					      direction,
+					      stream_type);
+		if (rc) {
+			pr_err("%s: send params failed rc=%d\n", __func__, rc);
+			kfree(params_value);
+			return -EINVAL;
+		}
+	}
+	kfree(params_value);
+	return 0;
+}
+EXPORT_SYMBOL(msm_qti_pp_send_chmix_cfg_cmd);
 #endif /* CONFIG_QTI_PP */
 
 /* RMS */

asoc/msm-qti-pp-config.h

@@ -34,6 +34,10 @@ int msm_qti_pp_send_stereo_to_custom_stereo_cmd(int port_id, int copp_idx,
 						uint16_t op_FR_ip_FL_weight,
 						uint16_t op_FR_ip_FR_weight);
 void msm_qti_pp_add_controls(struct snd_soc_platform *platform);
+int msm_qti_pp_send_chmix_cfg_cmd(int port_id, int copp_idx,
+				  unsigned int session_id, int ip_channel_count,
+				  int out_channel_cnt, int *ch_wght_coeff,
+				  int session_type, int stream_type);
 #else /* CONFIG_QTI_PP */
 static inline int msm_adsp_inform_mixer_ctl(struct snd_soc_pcm_runtime *rtd,
 			uint32_t *payload)
@@ -71,6 +75,13 @@ static inline int msm_adsp_stream_callback_info(struct snd_kcontrol *kcontrol,
 	return 0;
 }
 
+int msm_qti_pp_send_chmix_cfg_cmd(int port_id, int copp_idx,
+				  unsigned int session_id, int ip_channel_count,
+				  int out_channel_cnt, int *ch_wght_coeff,
+				  int session_type, int stream_type)
+{
+	return 0;
+}
 #define msm_qti_pp_send_eq_values(fedai_id) do {} while (0)
 #define msm_qti_pp_send_stereo_to_custom_stereo_cmd(port_id, copp_idx, \
 			session_id, op_FL_ip_FL_weight, op_FL_ip_FR_weight, \

dsp/q6adm.c

@@ -789,6 +789,106 @@ set_stereo_to_custom_stereo_return:
 }
 EXPORT_SYMBOL(adm_set_stereo_to_custom_stereo);
 
+/*
+ * adm_set_custom_chmix_cfg:
+ *	Set the custom channel mixer configuration for ADM
+ *
+ * @port_id: Backend port id
+ * @copp_idx: ADM copp index
+ * @session_id: ID of the requesting session
+ * @params: Expected packaged params for channel mixer
+ * @params_length: Length of the params to be set
+ * @direction: RX or TX direction
+ * @stream_type: Audio or Listen stream type
+ */
+int adm_set_custom_chmix_cfg(int port_id, int copp_idx,
+			     unsigned int session_id, char *params,
+			     uint32_t params_length, int direction,
+			     int stream_type)
+{
+	struct adm_cmd_set_pspd_mtmx_strtr_params_v6 *adm_params = NULL;
+	int sz, rc = 0, port_idx;
+
+	port_id = afe_convert_virtual_to_portid(port_id);
+	port_idx = adm_validate_and_get_port_index(port_id);
+	if (port_idx < 0) {
+		pr_err("%s: Invalid port_id 0x%x\n", __func__, port_id);
+		return -EINVAL;
+	}
+
+	sz = sizeof(struct adm_cmd_set_pspd_mtmx_strtr_params_v6) +
+		params_length;
+	adm_params = kzalloc(sz, GFP_KERNEL);
+	if (!adm_params) {
+		pr_err("%s, adm params memory alloc failed\n", __func__);
+		return -ENOMEM;
+	}
+
+	memcpy(((u8 *)adm_params +
+		sizeof(struct adm_cmd_set_pspd_mtmx_strtr_params_v6)),
+		params, params_length);
+	adm_params->hdr.hdr_field = APR_HDR_FIELD(APR_MSG_TYPE_SEQ_CMD,
+					APR_HDR_LEN(APR_HDR_SIZE), APR_PKT_VER);
+	adm_params->hdr.pkt_size = sz;
+	adm_params->hdr.src_svc = APR_SVC_ADM;
+	adm_params->hdr.src_domain = APR_DOMAIN_APPS;
+	adm_params->hdr.src_port = port_id;
+	adm_params->hdr.dest_svc = APR_SVC_ADM;
+	adm_params->hdr.dest_domain = APR_DOMAIN_ADSP;
+	adm_params->hdr.dest_port = 0; /* Ignored */;
+	adm_params->hdr.token = port_idx << 16 | copp_idx;
+	adm_params->hdr.opcode = ADM_CMD_SET_PSPD_MTMX_STRTR_PARAMS_V6;
+	adm_params->payload_addr_lsw = 0;
+	adm_params->payload_addr_msw = 0;
+	adm_params->mem_map_handle = 0;
+	adm_params->payload_size = params_length;
+	adm_params->direction = direction;
+	/* session id for this cmd to be applied on */
+	adm_params->sessionid = session_id;
+	adm_params->deviceid =
+			atomic_read(&this_adm.copp.id[port_idx][copp_idx]);
+	/* connecting stream type i.e. lsm or asm */
+	adm_params->stream_type = stream_type;
+	pr_debug("%s: deviceid %d, session_id %d, src_port %d, dest_port %d\n",
+		__func__, adm_params->deviceid, adm_params->sessionid,
+		adm_params->hdr.src_port, adm_params->hdr.dest_port);
+	atomic_set(&this_adm.copp.stat[port_idx][copp_idx], -1);
+	rc = apr_send_pkt(this_adm.apr, (uint32_t *)adm_params);
+	if (rc < 0) {
+		pr_err("%s: Set params failed port = 0x%x rc %d\n",
+			__func__, port_id, rc);
+		rc = -EINVAL;
+		goto exit;
+	}
+	/* Wait for the callback */
+	rc = wait_event_timeout(this_adm.copp.wait[port_idx][copp_idx],
+				atomic_read(&this_adm.copp.stat
+				[port_idx][copp_idx]),
+				msecs_to_jiffies(TIMEOUT_MS));
+	if (!rc) {
+		pr_err("%s: Set params timed out port = 0x%x\n", __func__,
+			port_id);
+		rc = -EINVAL;
+		goto exit;
+	} else if (atomic_read(&this_adm.copp.stat
+				[port_idx][copp_idx]) > 0) {
+		pr_err("%s: DSP returned error[%s]\n", __func__,
+			adsp_err_get_err_str(atomic_read(
+				&this_adm.copp.stat
+				[port_idx][copp_idx])));
+		rc = adsp_err_get_lnx_err_code(
+				atomic_read(&this_adm.copp.stat
+					[port_idx][copp_idx]));
+		goto exit;
+	}
+
+	rc = 0;
+exit:
+	kfree(adm_params);
+	return rc;
+}
+EXPORT_SYMBOL(adm_set_custom_chmix_cfg);
+
 /*
  * With pre-packed data, only the opcode differes from V5 and V6.
  * Use q6common_pack_pp_params to pack the data correctly.
@@ -1544,7 +1644,8 @@ static int32_t adm_callback(struct apr_client_data *data, void *priv)
 				}
 				break;
 			case ADM_CMD_SET_PSPD_MTMX_STRTR_PARAMS_V5:
-				pr_debug("%s: ADM_CMD_SET_PSPD_MTMX_STRTR_PARAMS_V5\n",
+			case ADM_CMD_SET_PSPD_MTMX_STRTR_PARAMS_V6:
+				pr_debug("%s:callback received PSPD MTMX, wake up\n",
 					__func__);
 				atomic_set(&this_adm.copp.stat[port_idx]
 						[copp_idx], payload[1]);
@@ -2280,6 +2381,8 @@ int adm_arrange_mch_map(struct adm_cmd_device_open_v5 *open, int path,
 {
 	int rc = 0, idx;
 
+	pr_debug("%s: channel mode %d", __func__, channel_mode);
+
 	memset(open->dev_channel_mapping, 0, PCM_FORMAT_MAX_NUM_CHANNEL);
 	switch (path) {
 	case ADM_PATH_PLAYBACK:

include/dsp/apr_audio-v2.h

@@ -575,6 +575,29 @@ struct adm_cmd_set_pspd_mtmx_strtr_params_v5 {
 	u16		reserved;
 } __packed;
 
+/* set customized mixing on matrix mixer.
+ * Updated to account for both LSM as well as ASM path.
+ */
+#define ADM_CMD_SET_PSPD_MTMX_STRTR_PARAMS_V6                        0x00010364
+struct adm_cmd_set_pspd_mtmx_strtr_params_v6 {
+	struct apr_hdr hdr;
+	/* LSW of parameter data payload address.*/
+	u32		payload_addr_lsw;
+	/* MSW of parameter data payload address.*/
+	u32		payload_addr_msw;
+	/* Memory map handle returned by ADM_CMD_SHARED_MEM_MAP_REGIONS */
+	/* command. If mem_map_handle is zero implies the message is in */
+	/* the payload */
+	u32		mem_map_handle;
+	/* Size in bytes of the variable payload accompanying this */
+	/* message or in shared memory. This is used for parsing the */
+	/* parameter payload. */
+	u32		payload_size;
+	u16		direction;
+	u16		sessionid;
+	u16		deviceid;
+	u16		stream_type;
+} __packed;
 /* Returns the status and COPP ID to an #ADM_CMD_DEVICE_OPEN_V5 command.
  */
 #define ADM_CMDRSP_DEVICE_OPEN_V5                      0x00010329
@@ -4683,6 +4706,7 @@ struct asm_softvolume_params {
 #define PCM_CHANNEL_RRC  16
 
 #define PCM_FORMAT_MAX_NUM_CHANNEL  8
+#define PCM_FORMAT_MAX_CHANNELS_9   9
 
 #define ASM_MEDIA_FMT_MULTI_CHANNEL_PCM_V2 0x00010DA5
 

include/dsp/q6adm-v2.h

@@ -196,6 +196,10 @@ int adm_get_sound_focus(int port_id, int copp_idx,
 			struct sound_focus_param *soundFocusData);
 int adm_get_source_tracking(int port_id, int copp_idx,
 			    struct source_tracking_param *sourceTrackingData);
+int adm_set_custom_chmix_cfg(int port_id, int copp_idx,
+			     unsigned int session_id, char *params,
+			     uint32_t params_length, int direction,
+				 int stream_type);
 int adm_swap_speaker_channels(int port_id, int copp_idx, int sample_rate,
 				bool spk_swap);
 int adm_programable_channel_mixer(int port_id, int copp_idx, int session_id,