1 files changed, 305 insertions, 0 deletions

diff --git a/sound/soc/qcom/qdsp6/audioreach.c b/sound/soc/qcom/qdsp6/audioreach.c
index 198d962c81f8..63a0e6c9ac14 100644
--- a/sound/soc/qcom/qdsp6/audioreach.c
+++ b/sound/soc/qcom/qdsp6/audioreach.c
@@ -5,6 +5,7 @@
 #include <linux/slab.h>
 #include <linux/soc/qcom/apr.h>
 #include <dt-bindings/soc/qcom,gpr.h>
+#include "q6apm.h"
 #include "audioreach.h"
 
 /* SubGraph Config */
@@ -253,3 +254,307 @@ void *audioreach_alloc_apm_cmd_pkt(int pkt_size, uint32_t opcode, uint32_t token
 				       APM_MODULE_INSTANCE_ID, true);
 }
 EXPORT_SYMBOL_GPL(audioreach_alloc_apm_cmd_pkt);
+
+static void apm_populate_container_config(struct apm_container_obj *cfg,
+					  struct audioreach_container *cont)
+{
+
+	/* Container Config */
+	cfg->container_cfg.container_id = cont->container_id;
+	cfg->container_cfg.num_prop = 4;
+
+	/* Capability list */
+	cfg->cap_data.prop_id = APM_CONTAINER_PROP_ID_CAPABILITY_LIST;
+	cfg->cap_data.prop_size = APM_CONTAINER_PROP_ID_CAPABILITY_SIZE;
+	cfg->num_capability_id = 1;
+	cfg->capability_id = cont->capability_id;
+
+	/* Graph Position */
+	cfg->pos_data.prop_id = APM_CONTAINER_PROP_ID_GRAPH_POS;
+	cfg->pos_data.prop_size = sizeof(struct apm_cont_prop_id_graph_pos);
+	cfg->pos.graph_pos = cont->graph_pos;
+
+	/* Stack size */
+	cfg->stack_data.prop_id = APM_CONTAINER_PROP_ID_STACK_SIZE;
+	cfg->stack_data.prop_size = sizeof(struct apm_cont_prop_id_stack_size);
+	cfg->stack.stack_size = cont->stack_size;
+
+	/* Proc domain */
+	cfg->domain_data.prop_id = APM_CONTAINER_PROP_ID_PROC_DOMAIN;
+	cfg->domain_data.prop_size = sizeof(struct apm_cont_prop_id_domain);
+	cfg->domain.proc_domain = cont->proc_domain;
+}
+
+static void apm_populate_sub_graph_config(struct apm_sub_graph_data *cfg,
+					  struct audioreach_sub_graph *sg)
+{
+	cfg->sub_graph_cfg.sub_graph_id = sg->sub_graph_id;
+	cfg->sub_graph_cfg.num_sub_graph_prop = APM_SUB_GRAPH_CFG_NPROP;
+
+	/* Perf Mode */
+	cfg->perf_data.prop_id = APM_SUB_GRAPH_PROP_ID_PERF_MODE;
+	cfg->perf_data.prop_size = APM_SG_PROP_ID_PERF_MODE_SIZE;
+	cfg->perf.perf_mode = sg->perf_mode;
+
+	/* Direction */
+	cfg->dir_data.prop_id = APM_SUB_GRAPH_PROP_ID_DIRECTION;
+	cfg->dir_data.prop_size = APM_SG_PROP_ID_DIR_SIZE;
+	cfg->dir.direction = sg->direction;
+
+	/* Scenario ID */
+	cfg->sid_data.prop_id = APM_SUB_GRAPH_PROP_ID_SCENARIO_ID;
+	cfg->sid_data.prop_size = APM_SG_PROP_ID_SID_SIZE;
+	cfg->sid.scenario_id = sg->scenario_id;
+}
+
+static void apm_populate_connection_obj(struct apm_module_conn_obj *obj,
+					struct audioreach_module *module)
+{
+	obj->src_mod_inst_id = module->src_mod_inst_id;
+	obj->src_mod_op_port_id = module->src_mod_op_port_id;
+	obj->dst_mod_inst_id = module->instance_id;
+	obj->dst_mod_ip_port_id = module->in_port;
+}
+
+static void apm_populate_module_prop_obj(struct apm_mod_prop_obj *obj,
+					 struct audioreach_module *module)
+{
+
+	obj->instance_id = module->instance_id;
+	obj->num_props = 1;
+	obj->prop_data_1.prop_id = APM_MODULE_PROP_ID_PORT_INFO;
+	obj->prop_data_1.prop_size = APM_MODULE_PROP_ID_PORT_INFO_SZ;
+	obj->prop_id_port.max_ip_port = module->max_ip_port;
+	obj->prop_id_port.max_op_port = module->max_op_port;
+}
+
+struct audioreach_module *audioreach_get_container_last_module(
+							struct audioreach_container *container)
+{
+	struct audioreach_module *module;
+
+	list_for_each_entry(module, &container->modules_list, node) {
+		if (module->dst_mod_inst_id == 0)
+			return module;
+	}
+
+	return NULL;
+}
+EXPORT_SYMBOL_GPL(audioreach_get_container_last_module);
+
+static bool is_module_in_container(struct audioreach_container *container, int module_iid)
+{
+	struct audioreach_module *module;
+
+	list_for_each_entry(module, &container->modules_list, node) {
+		if (module->instance_id == module_iid)
+			return true;
+	}
+
+	return false;
+}
+
+struct audioreach_module *audioreach_get_container_first_module(
+							struct audioreach_container *container)
+{
+	struct audioreach_module *module;
+
+	/* get the first module from both connected or un-connected containers */
+	list_for_each_entry(module, &container->modules_list, node) {
+		if (module->src_mod_inst_id == 0 ||
+		    !is_module_in_container(container, module->src_mod_inst_id))
+			return module;
+	}
+	return NULL;
+}
+EXPORT_SYMBOL_GPL(audioreach_get_container_first_module);
+
+struct audioreach_module *audioreach_get_container_next_module(
+						struct audioreach_container *container,
+						struct audioreach_module *module)
+{
+	int nmodule_iid = module->dst_mod_inst_id;
+	struct audioreach_module *nmodule;
+
+	list_for_each_entry(nmodule, &container->modules_list, node) {
+		if (nmodule->instance_id == nmodule_iid)
+			return nmodule;
+	}
+
+	return NULL;
+}
+EXPORT_SYMBOL_GPL(audioreach_get_container_next_module);
+
+static void apm_populate_module_list_obj(struct apm_mod_list_obj *obj,
+					 struct audioreach_container *container,
+					 int sub_graph_id)
+{
+	struct audioreach_module *module;
+	int i;
+
+	obj->sub_graph_id = sub_graph_id;
+	obj->container_id = container->container_id;
+	obj->num_modules = container->num_modules;
+	i = 0;
+	list_for_each_container_module(module, container) {
+		obj->mod_cfg[i].module_id = module->module_id;
+		obj->mod_cfg[i].instance_id = module->instance_id;
+		i++;
+	}
+}
+
+static void audioreach_populate_graph(struct apm_graph_open_params *open,
+				      struct list_head *sg_list,
+				      int num_sub_graphs)
+{
+	struct apm_mod_conn_list_params *mc_data = open->mod_conn_list_data;
+	struct apm_module_list_params *ml_data = open->mod_list_data;
+	struct apm_prop_list_params *mp_data = open->mod_prop_data;
+	struct apm_container_params *c_data = open->cont_data;
+	struct apm_sub_graph_params *sg_data = open->sg_data;
+	int ncontainer = 0, nmodule = 0, nconn = 0;
+	struct apm_mod_prop_obj *module_prop_obj;
+	struct audioreach_container *container;
+	struct apm_module_conn_obj *conn_obj;
+	struct audioreach_module *module;
+	struct audioreach_sub_graph *sg;
+	struct apm_container_obj *cobj;
+	struct apm_mod_list_obj *mlobj;
+	int i = 0;
+
+	mlobj = &ml_data->mod_list_obj[0];
+
+	list_for_each_entry(sg, sg_list, node) {
+		struct apm_sub_graph_data *sg_cfg = &sg_data->sg_cfg[i++];
+
+		apm_populate_sub_graph_config(sg_cfg, sg);
+
+		list_for_each_entry(container, &sg->container_list, node) {
+			cobj = &c_data->cont_obj[ncontainer];
+
+			apm_populate_container_config(cobj, container);
+			apm_populate_module_list_obj(mlobj, container, sg->sub_graph_id);
+
+			list_for_each_container_module(module, container) {
+				uint32_t src_mod_inst_id;
+
+				src_mod_inst_id = module->src_mod_inst_id;
+
+				module_prop_obj = &mp_data->mod_prop_obj[nmodule];
+				apm_populate_module_prop_obj(module_prop_obj, module);
+
+				if (src_mod_inst_id) {
+					conn_obj = &mc_data->conn_obj[nconn];
+					apm_populate_connection_obj(conn_obj, module);
+					nconn++;
+				}
+
+				nmodule++;
+			}
+			mlobj = (void *) mlobj + APM_MOD_LIST_OBJ_PSIZE(mlobj, container->num_modules);
+
+			ncontainer++;
+		}
+	}
+}
+
+void *audioreach_alloc_graph_pkt(struct q6apm *apm, struct list_head *sg_list, int graph_id)
+{
+	int payload_size, sg_sz, cont_sz, ml_sz, mp_sz, mc_sz;
+	struct apm_module_param_data  *param_data;
+	struct apm_container_params *cont_params;
+	struct audioreach_container *container;
+	struct apm_sub_graph_params *sg_params;
+	struct apm_mod_conn_list_params *mcon;
+	struct apm_graph_open_params params;
+	struct apm_prop_list_params *mprop;
+	struct audioreach_module *module;
+	struct audioreach_sub_graph *sgs;
+	struct apm_mod_list_obj *mlobj;
+	int num_modules_per_list;
+	int num_connections = 0;
+	int num_containers = 0;
+	int num_sub_graphs = 0;
+	int num_modules = 0;
+	int num_modules_list;
+	struct gpr_pkt *pkt;
+	void *p;
+
+	list_for_each_entry(sgs, sg_list, node) {
+		num_sub_graphs++;
+		list_for_each_entry(container, &sgs->container_list, node) {
+			num_containers++;
+			num_modules += container->num_modules;
+			list_for_each_container_module(module, container) {
+				if (module->src_mod_inst_id)
+					num_connections++;
+			}
+		}
+	}
+
+	num_modules_list = num_containers;
+	num_modules_per_list = num_modules/num_containers;
+	sg_sz = APM_SUB_GRAPH_PSIZE(sg_params, num_sub_graphs);
+	cont_sz = APM_CONTAINER_PSIZE(cont_params, num_containers);
+	ml_sz =	ALIGN(sizeof(struct apm_module_list_params) +
+		num_modules_list * APM_MOD_LIST_OBJ_PSIZE(mlobj,  num_modules_per_list), 8);
+	mp_sz = APM_MOD_PROP_PSIZE(mprop, num_modules);
+	mc_sz =	APM_MOD_CONN_PSIZE(mcon, num_connections);
+
+	payload_size = sg_sz + cont_sz + ml_sz + mp_sz + mc_sz;
+	pkt = audioreach_alloc_apm_cmd_pkt(payload_size, APM_CMD_GRAPH_OPEN, 0);
+	if (IS_ERR(pkt))
+		return pkt;
+
+	p = (void *)pkt + GPR_HDR_SIZE + APM_CMD_HDR_SIZE;
+
+	/* SubGraph */
+	params.sg_data = p;
+	param_data = &params.sg_data->param_data;
+	param_data->module_instance_id = APM_MODULE_INSTANCE_ID;
+	param_data->param_id = APM_PARAM_ID_SUB_GRAPH_CONFIG;
+	param_data->param_size = sg_sz - APM_MODULE_PARAM_DATA_SIZE;
+	params.sg_data->num_sub_graphs = num_sub_graphs;
+	p += sg_sz;
+
+	/* Container */
+	params.cont_data = p;
+	param_data = &params.cont_data->param_data;
+	param_data->module_instance_id = APM_MODULE_INSTANCE_ID;
+	param_data->param_id = APM_PARAM_ID_CONTAINER_CONFIG;
+	param_data->param_size = cont_sz - APM_MODULE_PARAM_DATA_SIZE;
+	params.cont_data->num_containers = num_containers;
+	p += cont_sz;
+
+	/* Module List*/
+	params.mod_list_data = p;
+	param_data = &params.mod_list_data->param_data;
+	param_data->module_instance_id = APM_MODULE_INSTANCE_ID;
+	param_data->param_id = APM_PARAM_ID_MODULE_LIST;
+	param_data->param_size = ml_sz - APM_MODULE_PARAM_DATA_SIZE;
+	params.mod_list_data->num_modules_list = num_sub_graphs;
+	p += ml_sz;
+
+	/* Module Properties */
+	params.mod_prop_data = p;
+	param_data = &params.mod_prop_data->param_data;
+	param_data->module_instance_id = APM_MODULE_INSTANCE_ID;
+	param_data->param_id = APM_PARAM_ID_MODULE_PROP;
+	param_data->param_size = mp_sz - APM_MODULE_PARAM_DATA_SIZE;
+	params.mod_prop_data->num_modules_prop_cfg = num_modules;
+	p += mp_sz;
+
+	/* Module Connections */
+	params.mod_conn_list_data = p;
+	param_data = &params.mod_conn_list_data->param_data;
+	param_data->module_instance_id = APM_MODULE_INSTANCE_ID;
+	param_data->param_id = APM_PARAM_ID_MODULE_CONN;
+	param_data->param_size = mc_sz - APM_MODULE_PARAM_DATA_SIZE;
+	params.mod_conn_list_data->num_connections = num_connections;
+	p += mc_sz;
+
+	audioreach_populate_graph(&params, sg_list, num_sub_graphs);
+
+	return pkt;
+}
+EXPORT_SYMBOL_GPL(audioreach_alloc_graph_pkt);