ASoC: Add TWS+ mono mode support

Add support to enable mono mode when one TWS+ earbud is
connected. Change the encoder config when device switches from two
earbuds to single earbud.

CRs-Fixed: 2341876
Change-Id: I880aa6a2767d88e99be2d80afff0e5fbec94b8a4
Signed-off-by: Manisha Agarwal <maniagar@codeaurora.org>

asoc/msm-dai-q6-v2.c

@@ -26,6 +26,8 @@
 #define MSM_DAI_QUAT_AUXPCM_DT_DEV_ID 4
 #define MSM_DAI_QUIN_AUXPCM_DT_DEV_ID 5
 
+#define MSM_DAI_TWS_CHANNEL_MODE_ONE 1
+#define MSM_DAI_TWS_CHANNEL_MODE_TWO 2
 
 #define spdif_clock_value(rate) (2*rate*32*2)
 #define CHANNEL_STATUS_SIZE 24
@@ -2928,6 +2930,12 @@ static const struct soc_enum afe_bit_format_enum[] = {
 	SOC_ENUM_SINGLE_EXT(3, afe_bit_format_text),
 };
 
+static const char *const tws_chs_mode_text[] = {"Zero", "One", "Two"};
+
+static const struct soc_enum tws_chs_mode_enum[] = {
+	SOC_ENUM_SINGLE_EXT(ARRAY_SIZE(tws_chs_mode_text), tws_chs_mode_text),
+};
+
 static int msm_dai_q6_afe_input_channel_get(struct snd_kcontrol *kcontrol,
 			struct snd_ctl_elem_value *ucontrol)
 {
@@ -2956,6 +2964,61 @@ static int msm_dai_q6_afe_input_channel_put(struct snd_kcontrol *kcontrol,
 	return 0;
 }
 
+static int msm_dai_q6_tws_channel_mode_get(struct snd_kcontrol *kcontrol,
+			struct snd_ctl_elem_value *ucontrol)
+{
+	struct snd_soc_dai *dai = kcontrol->private_data;
+	struct msm_dai_q6_dai_data *dai_data = NULL;
+
+	if (dai)
+		dai_data = dev_get_drvdata(dai->dev);
+
+	if (dai_data) {
+		ucontrol->value.integer.value[0] =
+				dai_data->enc_config.mono_mode;
+		pr_debug("%s:tws channel mode = %d\n",
+			 __func__, dai_data->enc_config.mono_mode);
+	}
+
+	return 0;
+}
+
+static int msm_dai_q6_tws_channel_mode_put(struct snd_kcontrol *kcontrol,
+			struct snd_ctl_elem_value *ucontrol)
+{
+	struct snd_soc_dai *dai = kcontrol->private_data;
+	struct msm_dai_q6_dai_data *dai_data = NULL;
+	int ret = 0;
+
+	if (dai)
+		dai_data = dev_get_drvdata(dai->dev);
+
+	if (dai_data && (dai_data->enc_config.format == ENC_FMT_APTX)) {
+		if (test_bit(STATUS_PORT_STARTED, dai_data->status_mask)) {
+			ret = afe_set_tws_channel_mode(dai->id,
+					ucontrol->value.integer.value[0]);
+			if (ret < 0) {
+				pr_err("%s: channel mode setting failed for TWS\n",
+				__func__);
+				goto exit;
+			} else {
+				pr_debug("%s: updating tws channel mode : %d\n",
+				__func__, dai_data->enc_config.mono_mode);
+			}
+		}
+		if (ucontrol->value.integer.value[0] ==
+			MSM_DAI_TWS_CHANNEL_MODE_ONE ||
+			ucontrol->value.integer.value[0] ==
+			MSM_DAI_TWS_CHANNEL_MODE_TWO)
+			dai_data->enc_config.mono_mode =
+				ucontrol->value.integer.value[0];
+		else
+			return -EINVAL;
+	}
+exit:
+	return ret;
+}
+
 static int msm_dai_q6_afe_input_bit_format_get(
 			struct snd_kcontrol *kcontrol,
 			struct snd_ctl_elem_value *ucontrol)
@@ -3149,6 +3212,9 @@ static const struct snd_kcontrol_new afe_enc_config_controls[] = {
 		       0, 0, 1, 0,
 		       msm_dai_q6_afe_scrambler_mode_get,
 		       msm_dai_q6_afe_scrambler_mode_put),
+	SOC_ENUM_EXT("TWS Channel Mode", tws_chs_mode_enum[0],
+		       msm_dai_q6_tws_channel_mode_get,
+		       msm_dai_q6_tws_channel_mode_put)
 };
 
 static int  msm_dai_q6_afe_dec_cfg_info(struct snd_kcontrol *kcontrol,
@@ -3432,6 +3498,9 @@ static int msm_dai_q6_dai_probe(struct snd_soc_dai *dai)
 		rc = snd_ctl_add(dai->component->card->snd_card,
 				 snd_ctl_new1(&afe_enc_config_controls[3],
 				 dai_data));
+		rc = snd_ctl_add(dai->component->card->snd_card,
+				 snd_ctl_new1(&afe_enc_config_controls[4],
+				 dai));
 		rc = snd_ctl_add(dai->component->card->snd_card,
 				snd_ctl_new1(&avd_drift_config_controls[2],
 					dai));

dsp/q6afe.c

@@ -3615,7 +3615,7 @@ static int q6afe_send_enc_config(u16 port_id,
 				 union afe_enc_config_data *cfg, u32 format,
 				 union afe_port_config afe_config,
 				 u16 afe_in_channels, u16 afe_in_bit_width,
-				 u32 scrambler_mode)
+				 u32 scrambler_mode, u32 mono_mode)
 {
 	u32 enc_fmt;
 	struct afe_enc_cfg_blk_param_t enc_blk_param;
@@ -3626,6 +3626,7 @@ static int q6afe_send_enc_config(u16 port_id,
 	struct afe_enc_level_to_bitrate_map_param_t map_param;
 	struct afe_enc_dec_imc_info_param_t imc_info_param;
 	struct afe_port_media_type_t media_type;
+	struct aptx_channel_mode_param_t channel_mode_param;
 	struct param_hdr_v3 param_hdr;
 	int ret;
 
@@ -3760,6 +3761,23 @@ static int q6afe_send_enc_config(u16 port_id,
 		goto exit;
 	}
 
+	if (format == ASM_MEDIA_FMT_APTX) {
+		pr_debug("%s:sending CAPI_V2_PARAM_ID_APTX_ENC_SWITCH_TO_MONO mode= %d to DSP payload\n",
+			__func__, mono_mode);
+		param_hdr.param_id = CAPI_V2_PARAM_ID_APTX_ENC_SWITCH_TO_MONO;
+		param_hdr.param_size = sizeof(channel_mode_param);
+		channel_mode_param.channel_mode = mono_mode;
+		ret = q6afe_pack_and_set_param_in_band(port_id,
+				q6audio_get_port_index(port_id),
+							param_hdr,
+					(u8 *) &channel_mode_param);
+
+		if (ret) {
+			pr_err("%s: CAPI_V2_PARAM_ID_APTX_ENC_SWITCH_TO_MONO for port 0x%x failed %d\n",
+				__func__, port_id, ret);
+		}
+	}
+
 	if ((format == ASM_MEDIA_FMT_LDAC &&
 	     cfg->ldac_config.abr_config.is_abr_enabled) ||
 	     format == ASM_MEDIA_FMT_APTX_ADAPTIVE) {
@@ -3843,10 +3861,38 @@ exit:
 	return ret;
 }
 
+int afe_set_tws_channel_mode(u16 port_id, u32 channel_mode)
+{
+	struct aptx_channel_mode_param_t channel_mode_param;
+	struct param_hdr_v3 param_info;
+	int ret = 0;
+
+	memset(&param_info, 0, sizeof(param_info));
+	memset(&channel_mode_param, 0, sizeof(channel_mode_param));
+
+	param_info.module_id = AFE_MODULE_ID_ENCODER;
+	param_info.instance_id = INSTANCE_ID_0;
+	param_info.param_id = CAPI_V2_PARAM_ID_APTX_ENC_SWITCH_TO_MONO;
+	param_info.param_size = sizeof(channel_mode_param);
+
+	channel_mode_param.channel_mode = channel_mode;
+
+	ret = q6afe_pack_and_set_param_in_band(port_id,
+			q6audio_get_port_index(port_id),
+						param_info,
+				(u8 *) &channel_mode_param);
+	if (ret)
+		pr_err("%s: AFE set channel mode cfg for port 0x%x failed %d\n",
+			 __func__, port_id, ret);
+
+	return ret;
+}
+EXPORT_SYMBOL(afe_set_tws_channel_mode);
+
 static int __afe_port_start(u16 port_id, union afe_port_config *afe_config,
 			    u32 rate, u16 afe_in_channels, u16 afe_in_bit_width,
 			    union afe_enc_config_data *enc_cfg,
-			    u32 codec_format, u32 scrambler_mode,
+			    u32 codec_format, u32 scrambler_mode, u32 mono_mode,
 			    struct afe_dec_config *dec_cfg)
 {
 	union afe_port_config port_cfg;
@@ -4149,7 +4195,7 @@ static int __afe_port_start(u16 port_id, union afe_port_config *afe_config,
 						    codec_format, *afe_config,
 						    afe_in_channels,
 						    afe_in_bit_width,
-						    scrambler_mode);
+						    scrambler_mode, mono_mode);
 			if (ret) {
 				pr_err("%s: AFE encoder config for port 0x%x failed %d\n",
 					__func__, port_id, ret);
@@ -4216,7 +4262,7 @@ int afe_port_start(u16 port_id, union afe_port_config *afe_config,
 		   u32 rate)
 {
 	return __afe_port_start(port_id, afe_config, rate,
-				0, 0, NULL, ASM_MEDIA_FMT_NONE, 0, NULL);
+				0, 0, NULL, ASM_MEDIA_FMT_NONE, 0, 0, NULL);
 }
 EXPORT_SYMBOL(afe_port_start);
 
@@ -4245,11 +4291,12 @@ int afe_port_start_v2(u16 port_id, union afe_port_config *afe_config,
 		ret = __afe_port_start(port_id, afe_config, rate,
 					afe_in_channels, afe_in_bit_width,
 					&enc_cfg->data, enc_cfg->format,
-					enc_cfg->scrambler_mode, dec_cfg);
+					enc_cfg->scrambler_mode,
+					enc_cfg->mono_mode, dec_cfg);
 	else if (dec_cfg != NULL)
 		ret = __afe_port_start(port_id, afe_config, rate,
 					afe_in_channels, afe_in_bit_width,
-					NULL, dec_cfg->format, 0, dec_cfg);
+					NULL, dec_cfg->format, 0, 0, dec_cfg);
 
 	return ret;
 }

include/dsp/apr_audio-v2.h

@@ -3914,6 +3914,11 @@ struct afe_id_aptx_adaptive_enc_init
  */
 #define AFE_DECODER_PARAM_ID_DEPACKETIZER_ID        0x00013235
 
+#define CAPI_V2_PARAM_ID_APTX_ENC_SWITCH_TO_MONO    0x0001332A
+
+struct aptx_channel_mode_param_t {
+	u32 channel_mode;
+} __packed;
 /*
  * Decoder buffer ID parameter for the #AVS_MODULE_ID_DECODER module.
  * This parameter cannot be set runtime.
@@ -4418,6 +4423,7 @@ union afe_enc_config_data {
 struct afe_enc_config {
 	u32 format;
 	u32 scrambler_mode;
+	u32 mono_mode;
 	union afe_enc_config_data data;
 };
 

include/dsp/q6afe-v2.h

@@ -366,6 +366,7 @@ void afe_set_island_mode_cfg(u16 port_id, u32 enable_flag);
 void afe_get_island_mode_cfg(u16 port_id, u32 *enable_flag);
 int afe_port_start(u16 port_id, union afe_port_config *afe_config,
 	u32 rate);
+int afe_set_tws_channel_mode(u16 port_id, u32 channel_mode);
 int afe_port_start_v2(u16 port_id, union afe_port_config *afe_config,
 		      u32 rate, u16 afe_in_channels, u16 afe_in_bit_width,
 		      struct afe_enc_config *enc_config,