ASoC: Add q6voice ready for FP5 (on v7.2-kernel)

arch/arm64/boot/dts/qcom/kodiak.dtsi

@@ -4557,6 +4557,35 @@
 							#sound-dai-cells = <0>;
 						};
 					};
+
+					q6mvm: apr-service@9 {
+						compatible = "qcom,q6mvm";
+						reg = <APR_SVC_ADSP_MVM>;
+						qcom,protection-domain = "avs/audio", "msm/adsp/audio_pd";
+						status = "disabled";
+
+						q6voicedai: dais {
+							compatible = "qcom,q6voice-dais";
+							#address-cells = <1>;
+							#size-cells = <0>;
+							#sound-dai-cells = <1>;
+						};
+					};
+
+					q6cvs: apr-service@a {
+						compatible = "qcom,q6cvs";
+						reg = <APR_SVC_ADSP_CVS>;
+						qcom,protection-domain = "avs/audio", "msm/adsp/audio_pd";
+						status = "disabled";
+					};
+
+					q6cvp: apr-service@b {
+						compatible = "qcom,q6cvp";
+						reg = <APR_SVC_ADSP_CVP>;
+						qcom,protection-domain = "avs/audio", "msm/adsp/audio_pd";
+						status = "disabled";
+					};
+
 				};
 
 				fastrpc {

arch/arm64/boot/dts/qcom/qcm6490-fairphone-fp5.dts

@@ -17,6 +17,8 @@
 #include <dt-bindings/regulator/qcom,rpmh-regulator.h>
 #include <dt-bindings/sound/qcom,q6asm.h>
 #include <dt-bindings/sound/qcom,q6dsp-lpass-ports.h>
+#include <dt-bindings/sound/qcom,q6voice.h>
+
 #include "kodiak.dtsi"
 #include "pm7250b.dtsi" /* SID-8 & SID-9 */
 #include "pm7325.dtsi"  /* SID-1 */
@@ -1389,7 +1391,38 @@
 	status = "okay";
 };
 
+&q6cvp {
+	status = "okay";
+};
+
+&q6cvs {
+	status = "okay";
+};
+
+&q6mvm {
+	status = "okay";
+};
+
+
+&q6voicedai {
+	dai@1 {
+		reg = <VOICEMMODE1>;
+	};
+};
+
 &q6afedai {
+	dai@104 {
+		reg = <DISPLAY_PORT_RX>;
+	};
+
+	dai@113 {
+		reg = <RX_CODEC_DMA_RX_0>;
+	};
+
+	dai@120 {
+		reg = <TX_CODEC_DMA_TX_3>;
+	};
+
 	dai@127 {
 		reg = <QUINARY_MI2S_RX>;
 		qcom,sd-lines = <0>;
@@ -1574,6 +1607,14 @@
 			sound-dai = <&q6routing>;
 		};
 	};
+
+	voicemmode1-dai-link {
+		link-name = "VoiceMMode1";
+		cpu {
+			sound-dai = <&q6voicedai VOICEMMODE1>;
+		};
+	};
+
 };
 
 &spi13 {

arch/arm64/configs/fp5_defconfig

@@ -420,6 +420,7 @@ CONFIG_SND_SOC=y
 CONFIG_SND_SOC_USB=y
 CONFIG_SND_SOC_QCOM=y
 CONFIG_SND_SOC_QDSP6_USB=y
+CONFIG_SND_SOC_QDSP6_Q6VOICE=y
 CONFIG_SND_SOC_SM8250=y
 CONFIG_SND_SOC_AW88261=y
 CONFIG_SND_SOC_WCD938X_SDW=y

include/dt-bindings/sound/qcom,q6voice.h

@@ -0,0 +1,8 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+#ifndef __DT_BINDINGS_Q6_VOICE_H__
+#define __DT_BINDINGS_Q6_VOICE_H__
+
+#define	CS_VOICE        0
+#define	VOICEMMODE1     1
+
+#endif /* __DT_BINDINGS_Q6_VOICE_H__ */

sound/soc/codecs/aw88261.c

@@ -417,17 +417,7 @@ static int aw88261_dev_reg_update(struct aw88261 *aw88261,
 			break;
 	}
 
-	ret = aw88261_dev_set_vcalb(aw_dev);
-	if (ret)
-		return ret;
-
-	if (aw_dev->prof_cur != aw_dev->prof_index)
-		vol_desc->ctl_volume = 0;
-
-	/* keep min volume */
-	aw88261_dev_set_volume(aw_dev, vol_desc->mute_volume);
-
-	return ret;
+	return aw88261_dev_set_vcalb(aw_dev);
 }
 
 static int aw88261_dev_get_prof_name(struct aw_device *aw_dev, int index, char **prof_name)

sound/soc/qcom/Kconfig

@@ -156,6 +156,14 @@ config SND_SOC_QDSP6
 	 audio drivers. This includes q6asm, q6adm,
 	 q6afe interfaces to DSP using apr.
 
+config SND_SOC_QDSP6_Q6VOICE_DAI
+	tristate
+
+config SND_SOC_QDSP6_Q6VOICE
+	tristate "QDSP6 Q6Voice audio driver"
+	depends on SND_SOC_QDSP6
+	select SND_SOC_QDSP6_Q6VOICE_DAI
+
 config SND_SOC_MSM8996
 	tristate "SoC Machine driver for MSM8996 and APQ8096 boards"
 	depends on QCOM_APR

sound/soc/qcom/qdsp6/Makefile

@@ -11,6 +11,9 @@ obj-$(CONFIG_SND_SOC_QDSP6_ADM) += q6adm.o
 obj-$(CONFIG_SND_SOC_QDSP6_ROUTING) += q6routing.o
 obj-$(CONFIG_SND_SOC_QDSP6_ASM) += q6asm.o
 obj-$(CONFIG_SND_SOC_QDSP6_ASM_DAI) += q6asm-dai.o
+obj-$(CONFIG_SND_SOC_QDSP6_Q6VOICE) += q6cvp.o q6cvs.o q6mvm.o q6voice-common.o
+obj-$(CONFIG_SND_SOC_QDSP6_Q6VOICE) += q6voice.o
+obj-$(CONFIG_SND_SOC_QDSP6_Q6VOICE_DAI) += q6voice-dai.o
 
 obj-$(CONFIG_SND_SOC_QDSP6_APM) += snd-q6apm.o
 obj-$(CONFIG_SND_SOC_QDSP6_APM_DAI) += q6apm-dai.o

sound/soc/qcom/qdsp6/q6afe-dai.c

@@ -278,6 +278,7 @@ static int q6tdm_hw_params(struct snd_pcm_substream *substream,
 	tdm->data_align_type = dai_data->priv[dai->id].data_align;
 	tdm->sync_src = dai_data->priv[dai->id].sync_src;
 	tdm->sync_mode = dai_data->priv[dai->id].sync_mode;
+	tdm->data_delay = dai_data->priv[dai->id].data_delay;
 
 	return 0;
 }

sound/soc/qcom/qdsp6/q6afe.c

@@ -1366,6 +1366,7 @@ void q6afe_tdm_port_prepare(struct q6afe_port *port,
 	pcfg->tdm_cfg.sync_mode = cfg->sync_mode;
 	pcfg->tdm_cfg.sync_src = cfg->sync_src;
 	pcfg->tdm_cfg.nslots_per_frame = cfg->nslots_per_frame;
+	pcfg->tdm_cfg.ctrl_sync_data_delay = cfg->data_delay;
 
 	pcfg->tdm_cfg.slot_width = cfg->slot_width;
 	pcfg->tdm_cfg.slot_mask = cfg->slot_mask;

sound/soc/qcom/qdsp6/q6afe.h

@@ -190,6 +190,7 @@ struct q6afe_tdm_cfg {
 	u16	sync_mode;
 	u16	sync_src;
 	u16	nslots_per_frame;
+	u16	data_delay;
 	u16	slot_width;
 	u16	slot_mask;
 	u32	data_align_type;

sound/soc/qcom/qdsp6/q6core.c

@@ -10,6 +10,7 @@
 #include <linux/of.h>
 #include <linux/of_platform.h>
 #include <linux/jiffies.h>
+#include <linux/firmware.h>
 #include <linux/soc/qcom/apr.h>
 #include "q6core.h"
 #include "q6dsp-errno.h"
@@ -20,9 +21,17 @@
 #define AVCS_CMDRSP_ADSP_EVENT_GET_STATE	0x0001290D
 #define AVCS_GET_VERSIONS       0x00012905
 #define AVCS_GET_VERSIONS_RSP   0x00012906
+#define AVCS_CMD_REGISTER_TOPOLOGIES	0x00012923
+#define AVCS_CMD_DEREGISTER_TOPOLOGIES	0x0001292a
 #define AVCS_CMD_GET_FWK_VERSION	0x001292c
 #define AVCS_CMDRSP_GET_FWK_VERSION	0x001292d
 
+#define AVCS_MODE_DEREGISTER_ALL_CUSTOM_TOPOLOGIES	2
+
+#define ACDB_SECTION_GPROPLUT	0x54554c504f525047
+#define ACDB_SECTION_DATAPOOL	0x4c4f4f5041544144
+#define ACDB_TOPOLOGIES_BLOB	0x000131a7
+
 struct avcs_svc_info {
 	uint32_t service_id;
 	uint32_t version;
@@ -41,6 +50,13 @@ struct avcs_svc_api_info {
 	uint32_t api_branch_version;
 } __packed;
 
+struct avcs_cmd_register_topologies {
+	uint32_t payload_addr_lsw;
+	uint32_t payload_addr_msw;
+	uint32_t mem_map_handle;
+	uint32_t payload_size;
+} __packed;
+
 struct avcs_cmdrsp_get_fwk_version {
 	uint32_t build_major_version;
 	uint32_t build_minor_version;
@@ -236,6 +252,38 @@ static bool __q6core_is_adsp_ready(struct q6core *core)
 	return false;
 }
 
+static int q6core_register_topologies(struct device *dev, u32 size, const void *data)
+{
+	struct q6core *core = dev_get_drvdata(dev);
+	struct apr_device *adev = core->adev;
+	struct avcs_cmd_register_topologies *cmd;
+	struct apr_pkt *pkt;
+	void *p, *dest;
+	int pkt_size;
+
+	pkt_size = APR_HDR_SIZE + sizeof(*cmd) + size;
+
+	p = kzalloc(pkt_size, GFP_KERNEL);
+	if (!p)
+		return -ENOMEM;
+
+	pkt = p;
+	cmd = p + APR_HDR_SIZE;
+	dest = p + APR_HDR_SIZE + sizeof(*cmd);
+	if (!p)
+		return -ENOMEM;
+
+	pkt->hdr.hdr_field = APR_HDR_FIELD(APR_MSG_TYPE_SEQ_CMD,
+				      APR_HDR_LEN(APR_HDR_SIZE), APR_PKT_VER);
+	pkt->hdr.pkt_size = APR_HDR_SIZE + sizeof(*cmd) + size;
+	pkt->hdr.opcode = AVCS_CMD_REGISTER_TOPOLOGIES;
+	cmd->payload_size = size;
+
+	memcpy(dest, data, size);
+
+	return apr_send_pkt(adev, pkt);
+}
+
 /**
  * q6core_get_svc_api_info() - Get version number of a service.
  *
@@ -325,6 +373,103 @@ bool q6core_is_adsp_ready(void)
 }
 EXPORT_SYMBOL_GPL(q6core_is_adsp_ready);
 
+static int acdb_section_offset(const struct firmware *fw,
+			       u64 section, u32 *off_out)
+{
+	const u8 *data = fw->data + 32;
+	u32 off = 32;
+	u32 sect_size;
+	u64 sect_id;
+
+	while (off + 12 <= fw->size) {
+		sect_id = *(const u64 *) data;
+		sect_size = *(const u32 *) (data + 8);
+
+		if (off + 12 + sect_size > fw->size)
+			return -EINVAL;
+
+		if (sect_id == section) {
+			*off_out = off;
+			return 0;
+		}
+
+		off += 12 + sect_size;
+		data += 12 + sect_size;
+	}
+
+	return -ENOENT;
+}
+
+static int acdb_get_global_property(const struct firmware *fw,
+				    u32 prop_id, u32 *offset)
+{
+	u32 gproplut_off, datapool_off;
+	u32 lut_size, lut_nelts, i;
+	const u32 *data;
+	int ret;
+
+	ret = acdb_section_offset(fw, ACDB_SECTION_GPROPLUT, &gproplut_off);
+	if (ret)
+		return ret;
+
+	ret = acdb_section_offset(fw, ACDB_SECTION_DATAPOOL, &datapool_off);
+	if (ret)
+		return ret;
+
+	lut_size = *(const u32 *) (fw->data + gproplut_off + 8);
+	if (lut_size < 4)
+		return -EINVAL;
+
+	lut_nelts = *(const u32 *) (fw->data + gproplut_off + 12);
+	if (lut_size < 4 + 8 * lut_nelts)
+		return -EINVAL;
+
+	data = (const u32 *) (fw->data + gproplut_off + 16);
+	for (i = 0; i < lut_nelts; i++) {
+		if (datapool_off + 12 + data[i * 2 + 1] + 4 > fw->size)
+			return -EINVAL;
+
+		if (data[i * 2] == prop_id) {
+			*offset = datapool_off + 12 + data[i * 2 + 1];
+			return 0;
+		}
+	}
+
+	return -ENOENT;
+}
+
+static int q6core_load_and_register_topologies(struct device *dev)
+{
+	const struct device_node *np = dev_of_node(dev);
+	const char *fw_name;
+	const struct firmware *fw;
+	u32 topo_off, size;
+	int ret;
+
+	ret = of_property_read_string(np, "qcom,acdb-name", &fw_name);
+	if (ret)
+		return 0;
+
+	ret = request_firmware(&fw, fw_name, dev);
+	if (ret)
+		return dev_err_probe(dev, ret, "failed to load Global_cal.acdb");
+
+	ret = acdb_get_global_property(fw, ACDB_TOPOLOGIES_BLOB, &topo_off);
+	if (ret) {
+		dev_err(dev, "ACDB parse error: %d\n", ret);
+		goto release_fw;
+	}
+
+	size = *(const u32 *) (fw->data + topo_off);
+	ret = q6core_register_topologies(dev, size, fw->data + topo_off + 4);
+	if (ret < 0)
+		goto release_fw;
+
+release_fw:
+	release_firmware(fw);
+	return 0;
+}
+
 static int q6core_probe(struct apr_device *adev)
 {
 	g_core = kzalloc_obj(*g_core);
@@ -336,7 +481,8 @@ static int q6core_probe(struct apr_device *adev)
 	mutex_init(&g_core->lock);
 	g_core->adev = adev;
 	init_waitqueue_head(&g_core->wait);
-	return 0;
+
+	return q6core_load_and_register_topologies(&adev->dev);
 }
 
 static void q6core_exit(struct apr_device *adev)