ASoC: msm: qdsp6v2: Add support for AFE sidetone

Add support for AFE sidetone whenever USB device or
codec sidetone is not available or supported.

Change-Id: I325ba6448efb4c021a2974ad813be4f8192e9ad1
Signed-off-by: Vikram Panduranga <vpandura@codeaurora.org>
Signed-off-by: Siena Richard <sienar@codeaurora.org>

8 files changed, 399 insertions, 36 deletions

diff --git a/include/sound/apr_audio-v2.h b/include/sound/apr_audio-v2.h
index e098e2329ac6..e08f3670ec58 100644
--- a/include/sound/apr_audio-v2.h
+++ b/include/sound/apr_audio-v2.h
@@ -1,4 +1,4 @@
-/* Copyright (c) 2012-2016, The Linux Foundation. All rights reserved.
+/* Copyright (c) 2012-2017, The Linux Foundation. All rights reserved.
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 and
@@ -1206,6 +1206,8 @@ struct afe_mod_enable_param {
  * #AFE_MODULE_SIDETONE_IIR_FILTER module.
  */
 #define AFE_PARAM_ID_SIDETONE_IIR_FILTER_CONFIG	0x00010204
+#define MAX_SIDETONE_IIR_DATA_SIZE 224
+#define MAX_NO_IIR_FILTER_STAGE    10
 
 struct afe_sidetone_iir_filter_config_params {
 	u16                  num_biquad_stages;
@@ -1217,6 +1219,7 @@ struct afe_sidetone_iir_filter_config_params {
 /* Pregain for the compensating filter response.
  * Supported values: Any number in Q13 format
  */
+	uint8_t   iir_config[MAX_SIDETONE_IIR_DATA_SIZE];
 } __packed;
 
 #define AFE_MODULE_LOOPBACK	0x00010205
@@ -1368,6 +1371,55 @@ struct afe_loopback_cfg_v1 {
 
 } __packed;
 
+struct afe_loopback_sidetone_gain {
+	u16                  rx_port_id;
+	u16                  gain;
+} __packed;
+
+struct loopback_cfg_data {
+	u32                  loopback_cfg_minor_version;
+/* Minor version used for tracking the version of the RMC module
+ * configuration interface.
+ * Supported values: #AFE_API_VERSION_LOOPBACK_CONFIG
+ */
+	u16                  dst_port_id;
+	/* Destination Port Id. */
+	u16                  routing_mode;
+/* Specifies data path type from src to dest port.
+ * Supported values:
+ * #LB_MODE_DEFAULT
+ * #LB_MODE_SIDETONE
+ * #LB_MODE_EC_REF_VOICE_AUDIO
+ * #LB_MODE_EC_REF_VOICE_A
+ * #LB_MODE_EC_REF_VOICE
+ */
+
+	u16                  enable;
+/* Specifies whether to enable (1) or
+ * disable (0) an AFE loopback.
+ */
+	u16                  reserved;
+/* Reserved for 32-bit alignment. This field must be set to 0.
+ */
+} __packed;
+
+struct afe_st_loopback_cfg_v1 {
+	struct apr_hdr                    hdr;
+	struct afe_port_cmd_set_param_v2  param;
+	struct afe_port_param_data_v2     gain_pdata;
+	struct afe_loopback_sidetone_gain gain_data;
+	struct afe_port_param_data_v2     cfg_pdata;
+	struct loopback_cfg_data          cfg_data;
+} __packed;
+
+struct afe_loopback_iir_cfg_v2 {
+	struct apr_hdr                          hdr;
+	struct afe_port_cmd_set_param_v2        param;
+	struct afe_port_param_data_v2           st_iir_enable_pdata;
+	struct afe_mod_enable_param             st_iir_mode_enable_data;
+	struct afe_port_param_data_v2           st_iir_filter_config_pdata;
+	struct afe_sidetone_iir_filter_config_params st_iir_filter_config_data;
+} __packed;
 #define AFE_MODULE_SPEAKER_PROTECTION	0x00010209
 #define AFE_PARAM_ID_SPKR_PROT_CONFIG	0x0001020a
 #define AFE_API_VERSION_SPKR_PROT_CONFIG	0x1
diff --git a/include/sound/q6afe-v2.h b/include/sound/q6afe-v2.h
index e0da428fa52f..a47d2805b9c5 100644
--- a/include/sound/q6afe-v2.h
+++ b/include/sound/q6afe-v2.h
@@ -1,4 +1,4 @@
-/* Copyright (c) 2012-2016, The Linux Foundation. All rights reserved.
+/* Copyright (c) 2012-2017, The Linux Foundation. All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 and
@@ -265,7 +265,7 @@ struct aanc_data {
 int afe_open(u16 port_id, union afe_port_config *afe_config, int rate);
 int afe_close(int port_id);
 int afe_loopback(u16 enable, u16 rx_port, u16 tx_port);
-int afe_sidetone(u16 tx_port_id, u16 rx_port_id, u16 enable, uint16_t gain);
+int afe_sidetone_enable(u16 tx_port_id, u16 rx_port_id, bool enable);
 int afe_loopback_gain(u16 port_id, u16 volume);
 int afe_validate_port(u16 port_id);
 int afe_get_port_index(u16 port_id);
diff --git a/include/uapi/linux/msm_audio_calibration.h b/include/uapi/linux/msm_audio_calibration.h
index 3c6ab13ca470..f05f9aaddb70 100644
--- a/include/uapi/linux/msm_audio_calibration.h
+++ b/include/uapi/linux/msm_audio_calibration.h
@@ -98,12 +98,15 @@ enum {
 	ULP_LSM_TOPOLOGY_ID_CAL_TYPE,
 	AFE_FB_SPKR_PROT_TH_VI_CAL_TYPE,
 	AFE_FB_SPKR_PROT_EX_VI_CAL_TYPE,
+	AFE_SIDETONE_IIR_CAL_TYPE,
 	MAX_CAL_TYPES,
 };
 
 #define AFE_FB_SPKR_PROT_TH_VI_CAL_TYPE AFE_FB_SPKR_PROT_TH_VI_CAL_TYPE
 #define AFE_FB_SPKR_PROT_EX_VI_CAL_TYPE AFE_FB_SPKR_PROT_EX_VI_CAL_TYPE
 
+#define AFE_SIDETONE_IIR_CAL_TYPE AFE_SIDETONE_IIR_CAL_TYPE
+
 enum {
 	VERSION_0_0,
 };
@@ -346,6 +349,19 @@ struct audio_cal_info_sidetone {
 	int32_t		pid;
 };
 
+#define MAX_SIDETONE_IIR_DATA_SIZE   224
+#define MAX_NO_IIR_FILTER_STAGE      10
+
+struct audio_cal_info_sidetone_iir {
+	uint16_t	iir_enable;
+	uint16_t	num_biquad_stages;
+	uint16_t	pregain;
+	int32_t	        tx_acdb_id;
+	int32_t	        rx_acdb_id;
+	int32_t	        mid;
+	int32_t	        pid;
+	uint8_t	        iir_config[MAX_SIDETONE_IIR_DATA_SIZE];
+};
 struct audio_cal_info_lsm_top {
 	int32_t		topology;
 	int32_t		acdb_id;
@@ -580,6 +596,17 @@ struct audio_cal_sidetone {
 	struct audio_cal_type_sidetone		cal_type;
 };
 
+struct audio_cal_type_sidetone_iir {
+	struct audio_cal_type_header	   cal_hdr;
+	struct audio_cal_data		   cal_data;
+	struct audio_cal_info_sidetone_iir cal_info;
+};
+
+struct audio_cal_sidetone_iir {
+	struct audio_cal_header		   hdr;
+	struct audio_cal_type_sidetone_iir cal_type;
+};
+
 struct audio_cal_type_lsm_top {
 	struct audio_cal_type_header	cal_hdr;
 	struct audio_cal_data		cal_data;
diff --git a/sound/soc/msm/qdsp6v2/audio_cal_utils.c b/sound/soc/msm/qdsp6v2/audio_cal_utils.c
index 9b76b4892eb1..75af648baef3 100644
--- a/sound/soc/msm/qdsp6v2/audio_cal_utils.c
+++ b/sound/soc/msm/qdsp6v2/audio_cal_utils.c
@@ -1,4 +1,4 @@
-/* Copyright (c) 2014-2016, The Linux Foundation. All rights reserved.
+/* Copyright (c) 2014-2017, The Linux Foundation. All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 and
@@ -119,6 +119,9 @@ size_t get_cal_info_size(int32_t cal_type)
 	case AFE_SIDETONE_CAL_TYPE:
 		size = sizeof(struct audio_cal_info_sidetone);
 		break;
+	case AFE_SIDETONE_IIR_CAL_TYPE:
+		size = sizeof(struct audio_cal_info_sidetone_iir);
+		break;
 	case LSM_CUST_TOPOLOGY_CAL_TYPE:
 		size = 0;
 		break;
@@ -265,6 +268,9 @@ size_t get_user_cal_type_size(int32_t cal_type)
 	case AFE_SIDETONE_CAL_TYPE:
 		size = sizeof(struct audio_cal_type_sidetone);
 		break;
+	case AFE_SIDETONE_IIR_CAL_TYPE:
+		size = sizeof(struct audio_cal_type_sidetone_iir);
+		break;
 	case LSM_CUST_TOPOLOGY_CAL_TYPE:
 		size = sizeof(struct audio_cal_type_basic);
 		break;
diff --git a/sound/soc/msm/qdsp6v2/msm-pcm-voice-v2.c b/sound/soc/msm/qdsp6v2/msm-pcm-voice-v2.c
index 286fff1eb4a8..f3ec45b8f9b1 100644
--- a/sound/soc/msm/qdsp6v2/msm-pcm-voice-v2.c
+++ b/sound/soc/msm/qdsp6v2/msm-pcm-voice-v2.c
@@ -1,4 +1,4 @@
-/* Copyright (c) 2012-2015, The Linux Foundation. All rights reserved.
+/* Copyright (c) 2012-2017, The Linux Foundation. All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 and
@@ -390,6 +390,33 @@ static int msm_pcm_ioctl(struct snd_pcm_substream *substream,
 	return ret;
 }
 
+static int msm_voice_sidetone_put(struct snd_kcontrol *kcontrol,
+					struct snd_ctl_elem_value *ucontrol)
+{
+	int ret;
+	bool sidetone_enable = ucontrol->value.integer.value[0];
+	uint32_t session_id = ALL_SESSION_VSID;
+
+	if (sidetone_enable < 0) {
+		pr_err("%s: Invalid arguments sidetone enable %d\n",
+			 __func__, sidetone_enable);
+		ret = -EINVAL;
+		return ret;
+	}
+	ret = voc_set_afe_sidetone(session_id, sidetone_enable);
+	pr_debug("%s: AFE Sidetone enable=%d session_id=0x%x ret=%d\n",
+		 __func__, sidetone_enable, session_id, ret);
+	return ret;
+}
+
+static int msm_voice_sidetone_get(struct snd_kcontrol *kcontrol,
+					struct snd_ctl_elem_value *ucontrol)
+{
+
+	ucontrol->value.integer.value[0] = voc_get_afe_sidetone();
+	return 0;
+}
+
 static int msm_voice_gain_put(struct snd_kcontrol *kcontrol,
 			      struct snd_ctl_elem_value *ucontrol)
 {
@@ -632,6 +659,9 @@ static struct snd_kcontrol_new msm_voice_controls[] = {
 		.info	= msm_voice_cvd_version_info,
 		.get	= msm_voice_cvd_version_get,
 	},
+	SOC_SINGLE_MULTI_EXT("Voice Sidetone Enable", SND_SOC_NOPM, 0, 1, 0, 1,
+			     msm_voice_sidetone_get, msm_voice_sidetone_put),
+
 };
 
 static struct snd_pcm_ops msm_pcm_ops = {
diff --git a/sound/soc/msm/qdsp6v2/q6afe.c b/sound/soc/msm/qdsp6v2/q6afe.c
index b87dcd36958f..c46a0ff8744c 100644
--- a/sound/soc/msm/qdsp6v2/q6afe.c
+++ b/sound/soc/msm/qdsp6v2/q6afe.c
@@ -1,4 +1,4 @@
-/* Copyright (c) 2012-2016, The Linux Foundation. All rights reserved.
+/* Copyright (c) 2012-2017, The Linux Foundation. All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 and
@@ -36,6 +36,7 @@ enum {
 	AFE_FB_SPKR_PROT_CAL,
 	AFE_HW_DELAY_CAL,
 	AFE_SIDETONE_CAL,
+	AFE_SIDETONE_IIR_CAL,
 	AFE_TOPOLOGY_CAL,
 	AFE_CUST_TOPOLOGY_CAL,
 	AFE_FB_SPKR_PROT_TH_VI_CAL,
@@ -4807,58 +4808,249 @@ fail_cmd:
 	return ret;
 }
 
-int afe_sidetone(u16 tx_port_id, u16 rx_port_id, u16 enable, uint16_t gain)
+static int afe_sidetone_iir(u16 tx_port_id)
 {
-	struct afe_loopback_cfg_v1 cmd_sidetone;
-	int ret = 0;
+	struct afe_loopback_iir_cfg_v2 iir_sidetone;
+	int ret;
 	int index = 0;
+	uint16_t size = 0;
+	int cal_index = AFE_SIDETONE_IIR_CAL;
+	int iir_pregain = 0;
+	int iir_num_biquad_stages = 0;
+	int iir_enable;
+	struct cal_block_data *cal_block;
+	int mid;
+
+	memset(&iir_sidetone, 0, sizeof(iir_sidetone));
+	index = q6audio_get_port_index(tx_port_id);
+	iir_sidetone.hdr.hdr_field = APR_HDR_FIELD(APR_MSG_TYPE_SEQ_CMD,
+				     APR_HDR_LEN(APR_HDR_SIZE), APR_PKT_VER);
+	iir_sidetone.hdr.pkt_size = sizeof(iir_sidetone);
+	iir_sidetone.hdr.src_port = 0;
+	iir_sidetone.hdr.dest_port = 0;
+	iir_sidetone.hdr.token = index;
+	iir_sidetone.hdr.opcode = AFE_PORT_CMD_SET_PARAM_V2;
+	iir_sidetone.param.port_id = tx_port_id;
+	iir_sidetone.param.payload_address_lsw = 0x00;
+	iir_sidetone.param.payload_address_msw = 0x00;
+	iir_sidetone.param.mem_map_handle = 0x00;
 
-	pr_info("%s: tx_port_id: 0x%x rx_port_id: 0x%x enable:%d gain:%d\n",
-			__func__, tx_port_id, rx_port_id, enable, gain);
-	index = q6audio_get_port_index(rx_port_id);
-	if (index < 0 || index > AFE_MAX_PORTS) {
-		pr_err("%s: AFE port index[%d] invalid!\n",
-				__func__, index);
-		return -EINVAL;
+	if (this_afe.cal_data[cal_index] == NULL) {
+		pr_err("%s: cal data is NULL\n", __func__);
+		ret = -EINVAL;
+		goto done;
 	}
-	ret = q6audio_validate_port(rx_port_id);
-	if (ret < 0) {
-		pr_err("%s: Invalid port 0x%x %d", __func__, rx_port_id, ret);
-		return -EINVAL;
+	mutex_lock(&this_afe.cal_data[cal_index]->lock);
+	cal_block = cal_utils_get_only_cal_block(this_afe.cal_data[cal_index]);
+	if (cal_block == NULL) {
+		pr_err("%s: cal_block not found\n ", __func__);
+		mutex_unlock(&this_afe.cal_data[cal_index]->lock);
+		ret = -EINVAL;
+		goto done;
+	}
+
+	iir_pregain = ((struct audio_cal_info_sidetone_iir *)
+			cal_block->cal_info)->pregain;
+	iir_enable = ((struct audio_cal_info_sidetone_iir *)
+			cal_block->cal_info)->iir_enable;
+	iir_num_biquad_stages = ((struct audio_cal_info_sidetone_iir *)
+			cal_block->cal_info)->num_biquad_stages;
+	mid = ((struct audio_cal_info_sidetone_iir *)
+			cal_block->cal_info)->mid;
+
+	/*
+	 * calculate the actual size of payload based on no of stages
+	 * enabled in calibration
+	 */
+	size = (MAX_SIDETONE_IIR_DATA_SIZE / MAX_NO_IIR_FILTER_STAGE) *
+		iir_num_biquad_stages;
+	/*
+	 * For an odd number of stages, 2 bytes of padding are
+	 * required at the end of the payload.
+	 */
+	if (iir_num_biquad_stages % 2) {
+		pr_debug("%s: adding 2 to size:%d\n", __func__, size);
+		size = size + 2;
+	}
+	memcpy(&iir_sidetone.st_iir_filter_config_data.iir_config,
+		&((struct audio_cal_info_sidetone_iir *)
+		cal_block->cal_info)->iir_config,
+		sizeof(iir_sidetone.st_iir_filter_config_data.iir_config));
+	mutex_unlock(&this_afe.cal_data[cal_index]->lock);
+
+	/*
+	 * Calculate the payload size for setparams command
+	 */
+	iir_sidetone.param.payload_size = (sizeof(iir_sidetone) -
+				sizeof(struct apr_hdr) -
+				sizeof(struct afe_port_cmd_set_param_v2) -
+				(MAX_SIDETONE_IIR_DATA_SIZE - size));
+
+	pr_debug("%s: payload size :%d\n", __func__,
+		 iir_sidetone.param.payload_size);
+
+	/*
+	 * Set IIR enable params
+	 */
+	iir_sidetone.st_iir_enable_pdata.module_id = mid;
+	iir_sidetone.st_iir_enable_pdata.param_id =
+			AFE_PARAM_ID_ENABLE;
+	iir_sidetone.st_iir_enable_pdata.param_size =
+			sizeof(iir_sidetone.st_iir_mode_enable_data);
+	iir_sidetone.st_iir_mode_enable_data.enable = iir_enable;
+
+	/*
+	 * Set IIR filter config params
+	 */
+	iir_sidetone.st_iir_filter_config_pdata.module_id = mid;
+	iir_sidetone.st_iir_filter_config_pdata.param_id =
+			AFE_PARAM_ID_SIDETONE_IIR_FILTER_CONFIG;
+	iir_sidetone.st_iir_filter_config_pdata.param_size =
+		sizeof(iir_sidetone.st_iir_filter_config_data.num_biquad_stages)
+		+
+		sizeof(iir_sidetone.st_iir_filter_config_data.pregain) + size;
+	iir_sidetone.st_iir_filter_config_pdata.reserved = 0;
+	iir_sidetone.st_iir_filter_config_data.num_biquad_stages =
+			iir_num_biquad_stages;
+	iir_sidetone.st_iir_filter_config_data.pregain = iir_pregain;
+	pr_debug("%s: tx(0x%x)mid(0x%x)iir_en(%d)stg(%d)gain(0x%x)size(%d)\n",
+		  __func__, tx_port_id, mid,
+		  iir_sidetone.st_iir_mode_enable_data.enable,
+		  iir_sidetone.st_iir_filter_config_data.num_biquad_stages,
+		  iir_sidetone.st_iir_filter_config_data.pregain,
+		  iir_sidetone.st_iir_filter_config_pdata.param_size);
+	ret = afe_apr_send_pkt(&iir_sidetone, &this_afe.wait[index]);
+	if (ret)
+		pr_err("%s: AFE sidetone failed for tx_port(0x%x)\n",
+			 __func__, tx_port_id);
+
+done:
+	return ret;
+
+}
+
+static int afe_sidetone(u16 tx_port_id, u16 rx_port_id, bool enable)
+{
+	struct afe_st_loopback_cfg_v1 cmd_sidetone;
+	int ret;
+	int index;
+	int cal_index = AFE_SIDETONE_CAL;
+	int sidetone_gain;
+	int sidetone_enable;
+	struct cal_block_data *cal_block;
+	int mid = 0;
+
+	memset(&cmd_sidetone, 0, sizeof(cmd_sidetone));
+	if (this_afe.cal_data[cal_index] == NULL) {
+		pr_err("%s: cal data is NULL\n", __func__);
+		ret = -EINVAL;
+		goto done;
+	}
+	mutex_lock(&this_afe.cal_data[cal_index]->lock);
+	cal_block = cal_utils_get_only_cal_block(this_afe.cal_data[cal_index]);
+	if (cal_block == NULL) {
+		pr_err("%s: cal_block not found\n", __func__);
+		mutex_unlock(&this_afe.cal_data[cal_index]->lock);
+		ret = -EINVAL;
+		goto done;
 	}
+	sidetone_gain = ((struct audio_cal_info_sidetone *)
+			 cal_block->cal_info)->gain;
+	sidetone_enable = ((struct audio_cal_info_sidetone *)
+			 cal_block->cal_info)->enable;
+	mid = ((struct audio_cal_info_sidetone *)
+			 cal_block->cal_info)->mid;
+	mutex_unlock(&this_afe.cal_data[cal_index]->lock);
 
+	index = q6audio_get_port_index(tx_port_id);
 	cmd_sidetone.hdr.hdr_field = APR_HDR_FIELD(APR_MSG_TYPE_SEQ_CMD,
 				APR_HDR_LEN(APR_HDR_SIZE), APR_PKT_VER);
 	cmd_sidetone.hdr.pkt_size = sizeof(cmd_sidetone);
 	cmd_sidetone.hdr.src_port = 0;
 	cmd_sidetone.hdr.dest_port = 0;
-	cmd_sidetone.hdr.token = 0;
+	cmd_sidetone.hdr.token = index;
 	cmd_sidetone.hdr.opcode = AFE_PORT_CMD_SET_PARAM_V2;
-	/* should it be rx or tx port id ?? , bharath*/
 	cmd_sidetone.param.port_id = tx_port_id;
-	/* size of data param & payload */
 	cmd_sidetone.param.payload_size = (sizeof(cmd_sidetone) -
 			sizeof(struct apr_hdr) -
 			sizeof(struct afe_port_cmd_set_param_v2));
 	cmd_sidetone.param.payload_address_lsw = 0x00;
 	cmd_sidetone.param.payload_address_msw = 0x00;
 	cmd_sidetone.param.mem_map_handle = 0x00;
-	cmd_sidetone.pdata.module_id = AFE_MODULE_LOOPBACK;
-	cmd_sidetone.pdata.param_id = AFE_PARAM_ID_LOOPBACK_CONFIG;
-	/* size of actual payload only */
-	cmd_sidetone.pdata.param_size =  cmd_sidetone.param.payload_size -
-				sizeof(struct afe_port_param_data_v2);
+	cmd_sidetone.gain_pdata.module_id = AFE_MODULE_LOOPBACK;
+	cmd_sidetone.gain_pdata.param_id = AFE_PARAM_ID_LOOPBACK_GAIN_PER_PATH;
+	/*
+	 * size of actual payload only
+	 */
+	cmd_sidetone.gain_pdata.param_size = sizeof(
+					     struct afe_loopback_sidetone_gain);
+	cmd_sidetone.gain_data.rx_port_id = rx_port_id;
+	cmd_sidetone.gain_data.gain = sidetone_gain;
 
-	cmd_sidetone.loopback_cfg_minor_version =
+	cmd_sidetone.cfg_pdata.module_id = AFE_MODULE_LOOPBACK;
+	cmd_sidetone.cfg_pdata.param_id = AFE_PARAM_ID_LOOPBACK_CONFIG;
+	/*
+	 * size of actual payload only
+	 */
+	cmd_sidetone.cfg_pdata.param_size = sizeof(struct loopback_cfg_data);
+	cmd_sidetone.cfg_data.loopback_cfg_minor_version =
 					AFE_API_VERSION_LOOPBACK_CONFIG;
-	cmd_sidetone.dst_port_id = rx_port_id;
-	cmd_sidetone.routing_mode = LB_MODE_SIDETONE;
-	cmd_sidetone.enable = enable;
+	cmd_sidetone.cfg_data.dst_port_id = rx_port_id;
+	cmd_sidetone.cfg_data.routing_mode = LB_MODE_SIDETONE;
+	cmd_sidetone.cfg_data.enable = ((enable == 1) ? sidetone_enable : 0);
+
+	pr_debug("%s rx(0x%x) tx(0x%x) enable(%d) mid(0x%x) gain(%d) sidetone_enable(%d)\n",
+		  __func__, rx_port_id, tx_port_id,
+		  enable, mid, sidetone_gain, sidetone_enable);
 
 	ret = afe_apr_send_pkt(&cmd_sidetone, &this_afe.wait[index]);
 	if (ret)
-		pr_err("%s: sidetone failed tx_port:0x%x rx_port:0x%x ret%d\n",
-		__func__, tx_port_id, rx_port_id, ret);
+		pr_err("%s: AFE sidetone send failed for tx_port:%d rx_port:%d ret:%d\n",
+			__func__, tx_port_id, rx_port_id, ret);
+done:
+	return ret;
+}
+
+int afe_sidetone_enable(u16 tx_port_id, u16 rx_port_id, bool enable)
+{
+	int ret;
+	int index;
+
+	index = q6audio_get_port_index(rx_port_id);
+	if (index < 0 || index > AFE_MAX_PORTS) {
+		pr_err("%s: AFE port index[%d] invalid!\n",
+				__func__, index);
+		ret = -EINVAL;
+		goto done;
+	}
+	if (q6audio_validate_port(rx_port_id) < 0) {
+		pr_err("%s: Invalid port 0x%x\n",
+				__func__, rx_port_id);
+		ret = -EINVAL;
+		goto done;
+	}
+	index = q6audio_get_port_index(tx_port_id);
+	if (index < 0 || index > AFE_MAX_PORTS) {
+		pr_err("%s: AFE port index[%d] invalid!\n",
+				__func__, index);
+		ret = -EINVAL;
+		goto done;
+	}
+	if (q6audio_validate_port(tx_port_id) < 0) {
+		pr_err("%s: Invalid port 0x%x\n",
+				__func__, tx_port_id);
+		ret = -EINVAL;
+		goto done;
+	}
+	if (enable) {
+		ret = afe_sidetone_iir(tx_port_id);
+		if (ret)
+			goto done;
+	}
+
+	ret = afe_sidetone(tx_port_id, rx_port_id, enable);
+
+done:
 	return ret;
 }
 
@@ -5965,6 +6157,9 @@ static int get_cal_type_index(int32_t cal_type)
 	case AFE_SIDETONE_CAL_TYPE:
 		ret = AFE_SIDETONE_CAL;
 		break;
+	case AFE_SIDETONE_IIR_CAL_TYPE:
+		ret = AFE_SIDETONE_IIR_CAL;
+		break;
 	case AFE_TOPOLOGY_CAL_TYPE:
 		ret = AFE_TOPOLOGY_CAL;
 		break;
@@ -6490,6 +6685,11 @@ static int afe_init_cal_data(void)
 		afe_set_cal, NULL, NULL} },
 		{NULL, NULL, cal_utils_match_buf_num} },
 
+		{{AFE_SIDETONE_IIR_CAL_TYPE,
+		{NULL, NULL, NULL,
+		afe_set_cal, NULL, NULL} },
+		{NULL, NULL, cal_utils_match_buf_num} },
+
 		{{AFE_TOPOLOGY_CAL_TYPE,
 		{NULL, NULL, NULL,
 		afe_set_cal, NULL, NULL} },
diff --git a/sound/soc/msm/qdsp6v2/q6voice.c b/sound/soc/msm/qdsp6v2/q6voice.c
index 5cff3dd1ed4f..e7619c0ca0dd 100644
--- a/sound/soc/msm/qdsp6v2/q6voice.c
+++ b/sound/soc/msm/qdsp6v2/q6voice.c
@@ -1,4 +1,4 @@
-/*  Copyright (c) 2012-2016, The Linux Foundation. All rights reserved.
+/*  Copyright (c) 2012-2017, The Linux Foundation. All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 and
@@ -5847,6 +5847,48 @@ int voc_set_hd_enable(uint32_t session_id, uint32_t enable)
 	return ret;
 }
 
+int voc_set_afe_sidetone(uint32_t session_id, bool sidetone_enable)
+{
+	struct voice_data *v = NULL;
+	int ret = -EINVAL;
+	struct voice_session_itr itr;
+	u16 rx_port, tx_port;
+
+	common.sidetone_enable = sidetone_enable;
+	voice_itr_init(&itr, session_id);
+	while (voice_itr_get_next_session(&itr, &v)) {
+		if (v == NULL) {
+			pr_err("%s: invalid session_id 0x%x\n", __func__,
+				  session_id);
+			ret = -EINVAL;
+			break;
+		}
+		mutex_lock(&v->lock);
+		if (v->voc_state != VOC_RUN) {
+			mutex_unlock(&v->lock);
+			continue;
+		}
+		rx_port = v->dev_rx.port_id;
+		tx_port = v->dev_tx.port_id;
+		ret = afe_sidetone_enable(tx_port, rx_port,
+					  sidetone_enable);
+		if (!ret) {
+			mutex_unlock(&v->lock);
+			break;
+		}
+		mutex_unlock(&v->lock);
+	}
+	return ret;
+}
+
+bool voc_get_afe_sidetone(void)
+{
+	bool ret;
+
+	ret = common.sidetone_enable;
+	return ret;
+}
+
 int voc_get_pp_enable(uint32_t session_id, uint32_t module_id)
 {
 	struct voice_data *v = voice_get_session(session_id);
@@ -8559,6 +8601,9 @@ static int __init voice_init(void)
 	memset(&common.ec_media_fmt_info.channel_mapping, 0,
 	       VSS_CHANNEL_MAPPING_SIZE);
 
+	/* Initialize AFE Sidetone Enable */
+	common.sidetone_enable = false;
+
 	/* Initialize MVS info. */
 	common.mvs_info.network_type = VSS_NETWORK_ID_DEFAULT;
 
diff --git a/sound/soc/msm/qdsp6v2/q6voice.h b/sound/soc/msm/qdsp6v2/q6voice.h
index 9c3ec62a980d..f7ea650dfda9 100644
--- a/sound/soc/msm/qdsp6v2/q6voice.h
+++ b/sound/soc/msm/qdsp6v2/q6voice.h
@@ -1,4 +1,4 @@
-/* Copyright (c) 2012-2016, The Linux Foundation. All rights reserved.
+/* Copyright (c) 2012-2017, The Linux Foundation. All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 and
@@ -1766,6 +1766,7 @@ struct common_data {
 	struct vss_isoundfocus_rsp_get_sectors_t soundFocusResponse;
 	struct shared_mem_info source_tracking_sh_mem;
 	struct vss_isourcetrack_activity_data_t sourceTrackingResponse;
+	bool sidetone_enable;
 };
 
 struct voice_session_itr {
@@ -1899,4 +1900,6 @@ uint32_t voice_get_topology(uint32_t topology_idx);
 int voc_set_sound_focus(struct sound_focus_param sound_focus_param);
 int voc_get_sound_focus(struct sound_focus_param *soundFocusData);
 int voc_get_source_tracking(struct source_tracking_param *sourceTrackingData);
+int voc_set_afe_sidetone(uint32_t session_id, bool sidetone_enable);
+bool voc_get_afe_sidetone(void);
 #endif