[PATCH v2 4/6] ASoC: qcom: audioreach: Add support for shared memory push/pull modules

[Srinivas Kandagatla]

Push-pull graphs use MODULE_ID_SH_MEM_PULL_MODE for playback and
MODULE_ID_SH_MEM_PUSH_MODE for capture instead of the legacy WR/RD shared
memory endpoints. Detect these modules when opening the graph, cache their
instance ID in graph->shm_iid, and use them for media format setup.

Also add support for mapping the position buffer required by push-pull mode
and configuring the DSP with circular buffer and position buffer addresses.

Signed-off-by: Srinivas Kandagatla <srinivas.kandagatla@xxxxxxxxxxxxxxxx>
---
 sound/soc/qcom/qdsp6/audioreach.c |  39 ++++++++
 sound/soc/qcom/qdsp6/audioreach.h |  51 ++++++++++
 sound/soc/qcom/qdsp6/q6apm.c      | 158 +++++++++++++++++++++++++-----
 sound/soc/qcom/qdsp6/q6apm.h      |   9 ++
 4 files changed, 231 insertions(+), 26 deletions(-)

diff --git a/sound/soc/qcom/qdsp6/audioreach.c b/sound/soc/qcom/qdsp6/audioreach.c
index 5b73f1d81c9b..c984b12409dd 100644
--- a/sound/soc/qcom/qdsp6/audioreach.c
+++ b/sound/soc/qcom/qdsp6/audioreach.c
@@ -1342,6 +1342,7 @@ int audioreach_set_media_format(struct q6apm_graph *graph,
 		rc = audioreach_i2s_set_media_format(graph, module, cfg);
 		break;
 	case MODULE_ID_WR_SHARED_MEM_EP:
+	case MODULE_ID_SH_MEM_PULL_MODE:
 		rc = audioreach_shmem_set_media_format(graph, module, cfg);
 		break;
 	case MODULE_ID_GAIN:
@@ -1401,6 +1402,44 @@ void audioreach_graph_free_buf(struct q6apm_graph *graph)
 }
 EXPORT_SYMBOL_GPL(audioreach_graph_free_buf);
 
+int audioreach_setup_push_pull(struct q6apm_graph *graph, phys_addr_t bphys,
+				phys_addr_t pphys, uint32_t mem_map_handle,
+				uint32_t pos_buf_mem_map_handle, uint32_t size)
+{
+	struct param_id_sh_mem_pull_push_mode_cfg *cfg;
+	struct apm_module_param_data *param_data;
+	int payload_size;
+	struct gpr_pkt *pkt __free(kfree) = NULL;
+	void *p;
+
+	payload_size = sizeof(*cfg) + APM_MODULE_PARAM_DATA_SIZE;
+	pkt = audioreach_alloc_apm_cmd_pkt(payload_size, APM_CMD_SET_CFG, 0);
+	if (IS_ERR(pkt))
+		return PTR_ERR(pkt);
+
+	p = (void *)pkt + GPR_HDR_SIZE + APM_CMD_HDR_SIZE;
+
+	param_data = p;
+	param_data->module_instance_id = graph->shm_iid;
+	param_data->error_code = 0;
+	param_data->param_id = PARAM_ID_SH_MEM_PULL_PUSH_MODE_CFG;
+	param_data->param_size = payload_size - APM_MODULE_PARAM_DATA_SIZE;
+
+	p = p + APM_MODULE_PARAM_DATA_SIZE;
+	cfg = p;
+
+	cfg->shared_circ_buf_addr_lsw = lower_32_bits(bphys);
+	cfg->shared_circ_buf_addr_msw = upper_32_bits(bphys);
+	cfg->shared_circ_buf_size = size;
+	cfg->circ_buf_mem_map_handle = mem_map_handle;
+	cfg->shared_pos_buf_addr_lsw = lower_32_bits(pphys);
+	cfg->shared_pos_buf_addr_msw = upper_32_bits(pphys);
+	cfg->pos_buf_mem_map_handle = pos_buf_mem_map_handle;
+
+	return q6apm_send_cmd_sync(graph->apm, pkt, 0);
+}
+EXPORT_SYMBOL_GPL(audioreach_setup_push_pull);
+
 int audioreach_shared_memory_send_eos(struct q6apm_graph *graph)
 {
 	struct data_cmd_wr_sh_mem_ep_eos *eos;
diff --git a/sound/soc/qcom/qdsp6/audioreach.h b/sound/soc/qcom/qdsp6/audioreach.h
index 6859770b38a6..b85c7e5b085e 100644
--- a/sound/soc/qcom/qdsp6/audioreach.h
+++ b/sound/soc/qcom/qdsp6/audioreach.h
@@ -16,6 +16,8 @@ struct q6apm_graph;
 #define MODULE_ID_PCM_CNV		0x07001003
 #define MODULE_ID_PCM_ENC		0x07001004
 #define MODULE_ID_PCM_DEC		0x07001005
+#define MODULE_ID_SH_MEM_PULL_MODE	0x07001006
+#define MODULE_ID_SH_MEM_PUSH_MODE	0x07001007
 #define MODULE_ID_PLACEHOLDER_ENCODER	0x07001008
 #define MODULE_ID_PLACEHOLDER_DECODER	0x07001009
 #define MODULE_ID_I2S_SINK		0x0700100A
@@ -61,6 +63,9 @@ struct q6apm_graph;
 #define APM_CMD_SHARED_MEM_MAP_REGIONS		0x0100100C
 #define APM_CMD_SHARED_MEM_UNMAP_REGIONS	0x0100100D
 #define APM_CMD_RSP_SHARED_MEM_MAP_REGIONS	0x02001001
+#define APM_MMAP_TOKEN_GID_MASK			GENMASK(15, 0)
+#define APM_MMAP_TOKEN_MAP_TYPE_POS_BUF		BIT(16)
+#define APM_MMAP_TOKEN_MAP_TYPE_SHIFT		16
 #define APM_CMD_RSP_GET_CFG			0x02001000
 #define APM_CMD_CLOSE_ALL			0x01001013
 #define APM_CMD_REGISTER_SHARED_CFG		0x0100100A
@@ -710,6 +715,46 @@ struct param_id_placeholder_real_module_id {
 	uint32_t real_module_id;
 } __packed;
 
+
+#define PARAM_ID_SH_MEM_PULL_PUSH_MODE_CFG	0x0800100A
+
+/**
+ * struct param_id_sh_mem_pull_push_mode_cfg - Shared memory push/pull config
+ * @shared_circ_buf_addr_lsw: Lower 32 bits of the circular buffer address.
+ * @shared_circ_buf_addr_msw: Upper 32 bits of the circular buffer address.
+ * @shared_circ_buf_size: Circular buffer size in bytes.
+ * @circ_buf_mem_map_handle: Circular buffer memory map handle.
+ * @shared_pos_buf_addr_lsw: Lower 32 bits of the position buffer address.
+ * @shared_pos_buf_addr_msw: Upper 32 bits of the position buffer address.
+ * @pos_buf_mem_map_handle: Position buffer memory map handle.
+ */
+struct param_id_sh_mem_pull_push_mode_cfg {
+	uint32_t shared_circ_buf_addr_lsw;
+	uint32_t shared_circ_buf_addr_msw;
+	uint32_t shared_circ_buf_size;
+	uint32_t circ_buf_mem_map_handle;
+	uint32_t shared_pos_buf_addr_lsw;
+	uint32_t shared_pos_buf_addr_msw;
+	uint32_t pos_buf_mem_map_handle;
+} __packed;
+
+/**
+ * struct sh_mem_pull_push_mode_position_buffer - Shared position buffer
+ * @frame_counter: Synchronization counter.
+ * @index: Current read/write index in bytes.
+ * @timestamp_us_lsw: Lower 32 bits of the timestamp in microseconds.
+ * @timestamp_us_msw: Upper 32 bits of the timestamp in microseconds.
+ *
+ * The frame counter should be read before and after the other fields to
+ * ensure the DSP did not update them while they were being read.
+ */
+struct sh_mem_pull_push_mode_position_buffer {
+	uint32_t frame_counter;
+	uint32_t index;
+	uint32_t timestamp_us_lsw;
+	uint32_t timestamp_us_msw;
+} __packed;
+
 /* Graph */
 struct audioreach_connection {
 	/* Connections */
@@ -723,8 +768,10 @@ struct audioreach_connection {
 struct audioreach_graph_info {
 	int id;
 	uint32_t mem_map_handle;
+	uint32_t pos_buf_mem_map_handle;
 	uint32_t num_sub_graphs;
 	struct list_head sg_list;
+	bool is_push_pull_mode;
 	/* DPCM connection from FE Graph to BE graph */
 	uint32_t src_mod_inst_id;
 	uint32_t src_mod_op_port_id;
@@ -855,5 +902,9 @@ int audioreach_send_u32_param(struct q6apm_graph *graph,
 			      uint32_t param_id, uint32_t param_val);
 int audioreach_compr_set_param(struct q6apm_graph *graph,
 			       const struct audioreach_module_config *mcfg);
+int audioreach_setup_push_pull(struct q6apm_graph *graph, phys_addr_t bphys,
+				phys_addr_t pphys, uint32_t mem_map_handle,
+				uint32_t pos_buf_mem_map_handle, uint32_t size);
+int audioreach_map_memory_position_buffer(struct q6apm_graph *graph, unsigned int dir);
 
 #endif /* __AUDIOREACH_H__ */
diff --git a/sound/soc/qcom/qdsp6/q6apm.c b/sound/soc/qcom/qdsp6/q6apm.c
index 6ae7d1645dce..9235089c1b46 100644
--- a/sound/soc/qcom/qdsp6/q6apm.c
+++ b/sound/soc/qcom/qdsp6/q6apm.c
@@ -186,20 +186,27 @@ int q6apm_graph_media_format_shmem(struct q6apm_graph *graph,
 {
 	struct audioreach_module *module;
 
-	if (cfg->direction == SNDRV_PCM_STREAM_CAPTURE)
-		module = q6apm_find_module_by_mid(graph, MODULE_ID_RD_SHARED_MEM_EP);
-	else
-		module = q6apm_find_module_by_mid(graph, MODULE_ID_WR_SHARED_MEM_EP);
+	if (cfg->direction == SNDRV_PCM_STREAM_CAPTURE) {
+		module = q6apm_find_module_by_mid(graph, MODULE_ID_SH_MEM_PUSH_MODE);
+		if (!module)
+			module = q6apm_find_module_by_mid(graph, MODULE_ID_RD_SHARED_MEM_EP);
+	} else {
+		module = q6apm_find_module_by_mid(graph, MODULE_ID_SH_MEM_PULL_MODE);
+		if (!module)
+			module = q6apm_find_module_by_mid(graph, MODULE_ID_WR_SHARED_MEM_EP);
+	}
 
-	if (!module)
+	if (!module) {
+		dev_err(graph->dev, "No SHMEM module found in graph\n");
 		return -ENODEV;
+	}
 
 	return audioreach_set_media_format(graph, module, cfg);
 }
 EXPORT_SYMBOL_GPL(q6apm_graph_media_format_shmem);
 
-int q6apm_map_memory_fixed_region(struct device *dev, unsigned int graph_id, phys_addr_t phys,
-				  size_t sz)
+static int __q6apm_map_memory_fixed_region(struct device *dev, unsigned int graph_id,
+					   phys_addr_t phys, size_t sz, bool is_pos_buf)
 {
 	struct audioreach_graph_info *info;
 	struct q6apm *apm = dev_get_drvdata(dev->parent);
@@ -208,8 +215,10 @@ int q6apm_map_memory_fixed_region(struct device *dev, unsigned int graph_id, phy
 	int payload_size = sizeof(*cmd) + (sizeof(*mregions));
 	uint32_t buf_sz;
 	void *p;
+	uint32_t pos_mask = is_pos_buf ? APM_MMAP_TOKEN_MAP_TYPE_POS_BUF : 0;
 	struct gpr_pkt *pkt __free(kfree) = audioreach_alloc_apm_cmd_pkt(payload_size,
-						APM_CMD_SHARED_MEM_MAP_REGIONS, graph_id);
+					APM_CMD_SHARED_MEM_MAP_REGIONS, (graph_id | pos_mask));
+
 	if (IS_ERR(pkt))
 		return PTR_ERR(pkt);
 
@@ -217,8 +226,13 @@ int q6apm_map_memory_fixed_region(struct device *dev, unsigned int graph_id, phy
 	if (!info)
 		return -ENODEV;
 
-	if (info->mem_map_handle)
-		return 0;
+	if (is_pos_buf) {
+		if (info->pos_buf_mem_map_handle)
+			return 0;
+	} else {
+		if (info->mem_map_handle)
+			return 0;
+	}
 
 	/* DSP expects size should be aligned to 4K */
 	buf_sz = ALIGN(sz, 4096);
@@ -227,7 +241,10 @@ int q6apm_map_memory_fixed_region(struct device *dev, unsigned int graph_id, phy
 	cmd = p;
 	cmd->mem_pool_id = APM_MEMORY_MAP_SHMEM8_4K_POOL;
 	cmd->num_regions = 1;
-	cmd->property_flag = 0x0;
+	if (is_pos_buf)
+		cmd->property_flag = 0x2;
+	else
+		cmd->property_flag = 0x0;
 
 	mregions = p + sizeof(*cmd);
 
@@ -237,6 +254,18 @@ int q6apm_map_memory_fixed_region(struct device *dev, unsigned int graph_id, phy
 
 	return q6apm_send_cmd_sync(apm, pkt, APM_CMD_RSP_SHARED_MEM_MAP_REGIONS);
 }
+
+int q6apm_map_pos_buffer(struct device *dev, unsigned int graph_id, phys_addr_t phys, size_t sz)
+{
+	return __q6apm_map_memory_fixed_region(dev, graph_id, phys, sz, true);
+}
+EXPORT_SYMBOL_GPL(q6apm_map_pos_buffer);
+
+int q6apm_map_memory_fixed_region(struct device *dev, unsigned int graph_id,
+				  phys_addr_t phys, size_t sz)
+{
+	return __q6apm_map_memory_fixed_region(dev, graph_id, phys, sz, false);
+}
 EXPORT_SYMBOL_GPL(q6apm_map_memory_fixed_region);
 
 int q6apm_alloc_fragments(struct q6apm_graph *graph, unsigned int dir, phys_addr_t phys,
@@ -290,11 +319,13 @@ int q6apm_alloc_fragments(struct q6apm_graph *graph, unsigned int dir, phys_addr
 }
 EXPORT_SYMBOL_GPL(q6apm_alloc_fragments);
 
-int q6apm_unmap_memory_fixed_region(struct device *dev, unsigned int graph_id)
+static int __q6apm_unmap_memory_fixed_region(struct device *dev, unsigned int graph_id,
+					     bool is_pos_buf)
 {
 	struct apm_cmd_shared_mem_unmap_regions *cmd;
 	struct q6apm *apm = dev_get_drvdata(dev->parent);
 	struct audioreach_graph_info *info;
+	uint32_t mem_map_handle;
 	struct gpr_pkt *pkt __free(kfree) = audioreach_alloc_apm_cmd_pkt(sizeof(*cmd),
 						APM_CMD_SHARED_MEM_UNMAP_REGIONS, graph_id);
 	if (IS_ERR(pkt))
@@ -304,16 +335,35 @@ int q6apm_unmap_memory_fixed_region(struct device *dev, unsigned int graph_id)
 	if (!info)
 		return -ENODEV;
 
-	if (!info->mem_map_handle)
-		return 0;
+	if (is_pos_buf) {
+		if (!info->pos_buf_mem_map_handle)
+			return 0;
+		mem_map_handle = info->pos_buf_mem_map_handle;
+	} else {
+
+		if (!info->mem_map_handle)
+			return 0;
+		mem_map_handle = info->mem_map_handle;
+	}
 
 	cmd = (void *)pkt + GPR_HDR_SIZE;
-	cmd->mem_map_handle = info->mem_map_handle;
+	cmd->mem_map_handle = mem_map_handle;
 
 	return q6apm_send_cmd_sync(apm, pkt, APM_CMD_SHARED_MEM_UNMAP_REGIONS);
 }
+
+int q6apm_unmap_memory_fixed_region(struct device *dev, unsigned int graph_id)
+{
+	return __q6apm_unmap_memory_fixed_region(dev, graph_id, false);
+}
 EXPORT_SYMBOL_GPL(q6apm_unmap_memory_fixed_region);
 
+int q6apm_unmap_pos_buffer(struct device *dev, unsigned int graph_id)
+{
+	return __q6apm_unmap_memory_fixed_region(dev, graph_id, true);
+}
+EXPORT_SYMBOL_GPL(q6apm_unmap_pos_buffer);
+
 int q6apm_free_fragments(struct q6apm_graph *graph, unsigned int dir)
 {
 	audioreach_graph_free_buf(graph);
@@ -402,7 +452,9 @@ int q6apm_graph_media_format_pcm(struct q6apm_graph *graph, struct audioreach_mo
 		list_for_each_entry(container, &sgs->container_list, node) {
 			list_for_each_entry(module, &container->modules_list, node) {
 				if ((module->module_id == MODULE_ID_WR_SHARED_MEM_EP) ||
-					(module->module_id == MODULE_ID_RD_SHARED_MEM_EP))
+					(module->module_id == MODULE_ID_RD_SHARED_MEM_EP) ||
+					(module->module_id == MODULE_ID_SH_MEM_PULL_MODE) ||
+					(module->module_id == MODULE_ID_SH_MEM_PUSH_MODE))
 					continue;
 
 				ret = audioreach_set_media_format(graph, module, cfg);
@@ -589,6 +641,42 @@ static int graph_callback(const struct gpr_resp_pkt *data, void *priv, int op)
 	return 0;
 }
 
+int q6apm_push_pull_config(struct q6apm_graph *graph, phys_addr_t bphys,
+			   phys_addr_t pphys, uint32_t size)
+{
+	struct audioreach_graph_info *info = graph->info;
+
+	return audioreach_setup_push_pull(graph, bphys, pphys, info->mem_map_handle,
+					  info->pos_buf_mem_map_handle, size);
+}
+EXPORT_SYMBOL_GPL(q6apm_push_pull_config);
+
+bool q6apm_is_graph_in_push_pull_mode_from_id(struct device *dev, unsigned int graph_id, int dir)
+{
+	struct audioreach_graph_info *info;
+	struct q6apm *apm = dev_get_drvdata(dev->parent);
+	struct audioreach_module *module;
+
+	info = idr_find(&apm->graph_info_idr, graph_id);
+	if (!info)
+		return false;
+
+	if (dir == SNDRV_PCM_STREAM_PLAYBACK)
+		module = __q6apm_find_module_by_mid(apm, info, MODULE_ID_SH_MEM_PULL_MODE);
+	else
+		module = __q6apm_find_module_by_mid(apm, info, MODULE_ID_SH_MEM_PUSH_MODE);
+
+	return !!module;
+
+}
+EXPORT_SYMBOL_GPL(q6apm_is_graph_in_push_pull_mode_from_id);
+
+bool q6apm_is_graph_in_push_pull_mode(struct q6apm_graph *graph)
+{
+	return graph->info->is_push_pull_mode;
+}
+EXPORT_SYMBOL_GPL(q6apm_is_graph_in_push_pull_mode);
+
 static int q6apm_graph_get_module_iid(struct q6apm_graph *graph, uint32_t mid)
 {
 	struct audioreach_module *module;
@@ -598,7 +686,6 @@ static int q6apm_graph_get_module_iid(struct q6apm_graph *graph, uint32_t mid)
 		return -ENODEV;
 
 	return module->instance_id;
-
 }
 
 struct q6apm_graph *q6apm_graph_open(struct device *dev, q6apm_cb cb,
@@ -607,7 +694,7 @@ struct q6apm_graph *q6apm_graph_open(struct device *dev, q6apm_cb cb,
 	struct q6apm *apm = dev_get_drvdata(dev->parent);
 	struct audioreach_graph *ar_graph;
 	struct q6apm_graph *graph;
-	int ret;
+	int ret, iid = 0;
 
 	ar_graph = q6apm_get_audioreach_graph(apm, graph_id);
 	if (IS_ERR(ar_graph)) {
@@ -629,11 +716,23 @@ struct q6apm_graph *q6apm_graph_open(struct device *dev, q6apm_cb cb,
 	graph->id = ar_graph->id;
 	graph->dev = dev;
 
-	if (dir == SNDRV_PCM_STREAM_PLAYBACK)
-		graph->shm_iid = q6apm_graph_get_module_iid(graph, MODULE_ID_WR_SHARED_MEM_EP);
-	else
-		graph->shm_iid = q6apm_graph_get_module_iid(graph, MODULE_ID_RD_SHARED_MEM_EP);
+	if (dir == SNDRV_PCM_STREAM_PLAYBACK) {
+		iid = q6apm_graph_get_module_iid(graph, MODULE_ID_SH_MEM_PULL_MODE);
+		if (iid < 0)
+			iid = q6apm_graph_get_module_iid(graph, MODULE_ID_WR_SHARED_MEM_EP);
+		else
+			graph->info->is_push_pull_mode = true;
 
+	} else {
+		iid = q6apm_graph_get_module_iid(graph, MODULE_ID_SH_MEM_PUSH_MODE);
+		if (iid < 0)
+			iid = q6apm_graph_get_module_iid(graph, MODULE_ID_RD_SHARED_MEM_EP);
+		else
+			graph->info->is_push_pull_mode = true;
+	}
+
+	if (iid > 0)
+		graph->shm_iid = iid;
 
 	mutex_init(&graph->lock);
 	init_waitqueue_head(&graph->cmd_wait);
@@ -790,6 +889,7 @@ static int apm_callback(const struct gpr_resp_pkt *data, void *priv, int op)
 	struct device *dev = &gdev->dev;
 	struct gpr_ibasic_rsp_result_t *result;
 	const struct gpr_hdr *hdr = &data->hdr;
+	int graph_id, is_pos_buf;
 
 	result = data->payload;
 
@@ -840,13 +940,19 @@ static int apm_callback(const struct gpr_resp_pkt *data, void *priv, int op)
 		apm->result.opcode = hdr->opcode;
 		apm->result.status = 0;
 		rsp = data->payload;
+		graph_id = hdr->token & APM_MMAP_TOKEN_GID_MASK;
+		is_pos_buf = hdr->token & APM_MMAP_TOKEN_MAP_TYPE_POS_BUF;
 
-		info = idr_find(&apm->graph_info_idr, hdr->token);
-		if (info)
-			info->mem_map_handle = rsp->mem_map_handle;
-		else
+		info = idr_find(&apm->graph_info_idr, graph_id);
+		if (info) {
+			if (is_pos_buf)
+				info->pos_buf_mem_map_handle = rsp->mem_map_handle;
+			else
+				info->mem_map_handle = rsp->mem_map_handle;
+		} else {
 			dev_err(dev, "Error (%d) Processing 0x%08x cmd\n", result->status,
 				result->opcode);
+		}
 
 		wake_up(&apm->wait);
 		break;
diff --git a/sound/soc/qcom/qdsp6/q6apm.h b/sound/soc/qcom/qdsp6/q6apm.h
index 8ea64085860f..780933ff17e9 100644
--- a/sound/soc/qcom/qdsp6/q6apm.h
+++ b/sound/soc/qcom/qdsp6/q6apm.h
@@ -136,6 +136,10 @@ int q6apm_write_async(struct q6apm_graph *graph, uint32_t len, uint32_t msw_ts,
 int q6apm_map_memory_fixed_region(struct device *dev,
 			     unsigned int graph_id, phys_addr_t phys,
 			     size_t sz);
+int q6apm_map_pos_buffer(struct device *dev,
+			     unsigned int graph_id, phys_addr_t phys,
+			     size_t sz);
+int q6apm_unmap_pos_buffer(struct device *dev, unsigned int graph_id);
 int q6apm_alloc_fragments(struct q6apm_graph *graph,
 			unsigned int dir, phys_addr_t phys,
 			size_t period_sz, unsigned int periods);
@@ -155,4 +159,9 @@ int q6apm_remove_initial_silence(struct device *dev, struct q6apm_graph *graph,
 int q6apm_remove_trailing_silence(struct device *dev, struct q6apm_graph *graph, uint32_t samples);
 int q6apm_set_real_module_id(struct device *dev, struct q6apm_graph *graph, uint32_t codec_id);
 int q6apm_get_hw_pointer(struct q6apm_graph *graph, int dir);
+bool q6apm_is_graph_in_push_pull_mode(struct q6apm_graph *graph);
+bool q6apm_is_graph_in_push_pull_mode_from_id(struct device *dev, unsigned int graph_id, int dir);
+int q6apm_push_pull_config(struct q6apm_graph *graph, phys_addr_t bphys,
+			   phys_addr_t pphys, uint32_t size);
+
 #endif /* __APM_GRAPH_ */
-- 
2.53.0