dsp: fix issue with MDF shared memory access in QCS403

MDF shared memory uses different SID from other use cases.
Existing memory map function hard codes SID from audio ion.
Use separate map function for MDF to make use of proper SID
for MDF shared memory in 32bit build.

Change-Id: I0e854054ad6f58344644f3386618986eab238549
Signed-off-by: Mangesh Kunchamwar <[email protected]>

dsp/msm_mdf.c

@@ -112,7 +112,7 @@ struct msm_mdf_smmu {
 	uint8_t device_status;
 	uint64_t sid;
 	struct dma_iommu_mapping *mapping;
-	dma_addr_t pa;
+	u64 pa;
 	size_t pa_len;
 };
 
@@ -183,6 +183,9 @@ static int msm_mdf_dma_buf_map(struct msm_mdf_mem *mem,
 			       struct msm_mdf_smmu *smmu)
 {
 	int rc = 0;
+	dma_addr_t pa = 0;
+
+	smmu->pa = 0;
 
 	if (!smmu)
 		return -EINVAL;
@@ -207,7 +210,7 @@ static int msm_mdf_dma_buf_map(struct msm_mdf_mem *mem,
 				return -ENODEV;
 		}
 
-		smmu->pa = dma_map_single_attrs(smmu->cb_dev, mem->va,
+		pa = dma_map_single_attrs(smmu->cb_dev, mem->va,
 			mem->size, DMA_BIDIRECTIONAL, DMA_ATTR_SKIP_CPU_SYNC);
 		if (dma_mapping_error(smmu->cb_dev, smmu->pa)) {
 			rc = -ENOMEM;
@@ -215,13 +218,14 @@ static int msm_mdf_dma_buf_map(struct msm_mdf_mem *mem,
 				__func__, rc);
 			goto err;
 		}
+		smmu->pa |= pa;
 		smmu->pa_len = mem->size;
 
 		/* Append the SMMU SID information to the IOVA address */
 		if (smmu->sid)
 			smmu->pa |= smmu->sid;
 	} else {
-		smmu->pa = mem->dma_addr;
+		smmu->pa |= mem->dma_addr;
 		smmu->pa_len = mem->size;
 	}
 	pr_err("%s: pa=%pa, pa_len=%zd\n", __func__,
@@ -312,7 +316,7 @@ static int msm_mdf_map_memory_to_subsys(struct msm_mdf_mem *mem,
 
 	/* Map mdf shared memory to ADSP */
 	if (!strcmp("adsp", smmu->subsys)) {
-		rc = q6core_map_memory_regions((phys_addr_t *)&smmu->pa,
+		rc = q6core_map_mdf_memory_regions((uint64_t *)&smmu->pa,
 				ADSP_MEMORY_MAP_MDF_SHMEM_4K_POOL,
 				(uint32_t *)&smmu->pa_len, 1, &mem->map_handle);
 		if (rc)  {
@@ -323,7 +327,7 @@ static int msm_mdf_map_memory_to_subsys(struct msm_mdf_mem *mem,
 		if (mem->map_handle) {
 			/* Map mdf shared memory to remote DSPs */
 			rc = q6core_map_mdf_shared_memory(mem->map_handle,
-					(phys_addr_t *)&smmu->pa, smmu->proc_id,
+					(uint64_t *)&smmu->pa, smmu->proc_id,
 					(uint32_t *)&smmu->pa_len, 1);
 			if (rc)  {
 				pr_err("%s: q6core_map_mdf_shared_memory failed, rc = %d\n",
@@ -601,7 +605,7 @@ static int msm_mdf_cb_probe(struct device *dev)
 			dev_err(dev,
 				"%s: qcom,smmu-sid-mask missing in DT node, using default\n",
 				__func__);
-			smmu_sid_mask = 0xFFFFFFFFFFFFFFFF;
+			smmu_sid_mask = 0xF;
 		}
 
 		rc = of_parse_phandle_with_args(dev->of_node, "iommus",

dsp/q6core.c

@@ -41,6 +41,8 @@
 #define ADSP_STATE_READY_TIMEOUT_MS 3000
 
 #define APR_ENOTREADY 10
+#define MEMPOOL_ID_MASK 0xFF
+#define MDF_MAP_TOKEN 0xF000
 
 enum {
 	META_CAL,
@@ -62,9 +64,11 @@ struct q6core_avcs_ver_info {
 struct q6core_str {
 	struct apr_svc *core_handle_q;
 	wait_queue_head_t bus_bw_req_wait;
+	wait_queue_head_t mdf_map_resp_wait;
 	wait_queue_head_t cmd_req_wait;
 	wait_queue_head_t avcs_fwk_ver_req_wait;
 	u32 bus_bw_resp_received;
+	u32 mdf_map_resp_received;
 	enum cmd_flags {
 		FLAG_NONE,
 		FLAG_CMDRSP_LICENSE_RESULT
@@ -79,6 +83,7 @@ struct q6core_str {
 	u32 param;
 	struct cal_type_data *cal_data[CORE_MAX_CAL];
 	uint32_t mem_map_cal_handle;
+	uint32_t mdf_mem_map_cal_handle;
 	int32_t adsp_status;
 	int32_t avs_state;
 	struct q6core_avcs_ver_info q6core_avcs_ver_info;
@@ -340,9 +345,15 @@ static int32_t aprv2_core_fn_q(struct apr_client_data *data, void *priv)
 		payload1 = data->payload;
 		pr_debug("%s: AVCS_CMDRSP_SHARED_MEM_MAP_REGIONS handle %d\n",
 			__func__, payload1[0]);
-		q6core_lcl.mem_map_cal_handle = payload1[0];
-		q6core_lcl.bus_bw_resp_received = 1;
-		wake_up(&q6core_lcl.bus_bw_req_wait);
+		if (data->token == MDF_MAP_TOKEN) {
+			q6core_lcl.mdf_mem_map_cal_handle = payload1[0];
+			q6core_lcl.mdf_map_resp_received = 1;
+			wake_up(&q6core_lcl.mdf_map_resp_wait);
+		} else {
+			q6core_lcl.mem_map_cal_handle = payload1[0];
+			q6core_lcl.bus_bw_resp_received = 1;
+			wake_up(&q6core_lcl.bus_bw_req_wait);
+		}
 		break;
 	case AVCS_CMDRSP_ADSP_EVENT_GET_STATE:
 		payload1 = data->payload;
@@ -1006,6 +1017,103 @@ done:
 	return ret;
 }
 
+/**
+ * q6core_map_mdf_memory_regions - for sending MDF shared memory map information
+ * to ADSP.
+ *
+ * @buf_add: array of buffers.
+ * @mempool_id: memory pool ID
+ * @bufsz: size of the buffer
+ * @bufcnt: buffers count
+ * @map_handle: map handle received from ADSP
+ */
+int q6core_map_mdf_memory_regions(uint64_t *buf_add, uint32_t mempool_id,
+			uint32_t *bufsz, uint32_t bufcnt, uint32_t *map_handle)
+{
+	struct avs_cmd_shared_mem_map_regions *mmap_regions = NULL;
+	struct avs_shared_map_region_payload *mregions = NULL;
+	void *mmap_region_cmd = NULL;
+	void *payload = NULL;
+	int ret = 0;
+	int i = 0;
+	int cmd_size = 0;
+
+	mutex_lock(&q6core_lcl.cmd_lock);
+
+	cmd_size = sizeof(struct avs_cmd_shared_mem_map_regions)
+			+ sizeof(struct avs_shared_map_region_payload)
+			* bufcnt;
+
+	mmap_region_cmd = kzalloc(cmd_size, GFP_KERNEL);
+	if (mmap_region_cmd == NULL)
+		return -ENOMEM;
+
+	mmap_regions = (struct avs_cmd_shared_mem_map_regions *)mmap_region_cmd;
+	mmap_regions->hdr.hdr_field = APR_HDR_FIELD(APR_MSG_TYPE_SEQ_CMD,
+						APR_HDR_LEN(APR_HDR_SIZE),
+						APR_PKT_VER);
+	mmap_regions->hdr.pkt_size = cmd_size;
+	mmap_regions->hdr.src_port = 0;
+	mmap_regions->hdr.dest_port = 0;
+	mmap_regions->hdr.token = MDF_MAP_TOKEN;
+	mmap_regions->hdr.opcode = AVCS_CMD_SHARED_MEM_MAP_REGIONS;
+	mmap_regions->mem_pool_id = mempool_id & MEMPOOL_ID_MASK;
+	mmap_regions->num_regions = bufcnt & 0x00ff;
+	mmap_regions->property_flag = 0x00;
+
+	payload = ((u8 *) mmap_region_cmd +
+				sizeof(struct avs_cmd_shared_mem_map_regions));
+	mregions = (struct avs_shared_map_region_payload *)payload;
+
+	for (i = 0; i < bufcnt; i++) {
+		mregions->shm_addr_lsw = lower_32_bits(buf_add[i]);
+		mregions->shm_addr_msw = upper_32_bits(buf_add[i]);
+		mregions->mem_size_bytes = bufsz[i];
+		++mregions;
+	}
+
+	pr_debug("%s: sending MDF memory map, addr %pK, size %d, bufcnt = %d\n",
+		__func__, buf_add, bufsz[0], mmap_regions->num_regions);
+
+	*map_handle = 0;
+	q6core_lcl.adsp_status = 0;
+	q6core_lcl.mdf_map_resp_received = 0;
+	ret = apr_send_pkt(q6core_lcl.core_handle_q, (uint32_t *)
+		mmap_regions);
+	if (ret < 0) {
+		pr_err("%s: mmap regions failed %d\n",
+			__func__, ret);
+		ret = -EINVAL;
+		goto done;
+	}
+
+	ret = wait_event_timeout(q6core_lcl.mdf_map_resp_wait,
+				(q6core_lcl.mdf_map_resp_received == 1),
+				msecs_to_jiffies(TIMEOUT_MS));
+	if (!ret) {
+		pr_err("%s: timeout. waited for memory map\n", __func__);
+		ret = -ETIMEDOUT;
+		goto done;
+	} else {
+		/* set ret to 0 as no timeout happened */
+		ret = 0;
+	}
+
+	if (q6core_lcl.adsp_status < 0) {
+		pr_err("%s: DSP returned error %d\n",
+			__func__, q6core_lcl.adsp_status);
+		ret = q6core_lcl.adsp_status;
+		goto done;
+	}
+
+	*map_handle = q6core_lcl.mdf_mem_map_cal_handle;
+done:
+	kfree(mmap_region_cmd);
+	mutex_unlock(&q6core_lcl.cmd_lock);
+	return ret;
+}
+EXPORT_SYMBOL(q6core_map_mdf_memory_regions);
+
 int q6core_memory_unmap_regions(uint32_t mem_map_handle)
 {
 	struct avs_cmd_shared_mem_unmap_regions unmap_regions;
@@ -1063,7 +1171,7 @@ done:
 }
 
 
-int q6core_map_mdf_shared_memory(uint32_t map_handle, phys_addr_t *buf_add,
+int q6core_map_mdf_shared_memory(uint32_t map_handle, uint64_t *buf_add,
 			uint32_t proc_id, uint32_t *bufsz, uint32_t bufcnt)
 {
 	struct avs_cmd_map_mdf_shared_memory *mmap_regions = NULL;
@@ -1563,6 +1671,7 @@ int __init core_init(void)
 	init_waitqueue_head(&q6core_lcl.bus_bw_req_wait);
 	init_waitqueue_head(&q6core_lcl.cmd_req_wait);
 	init_waitqueue_head(&q6core_lcl.avcs_fwk_ver_req_wait);
+	init_waitqueue_head(&q6core_lcl.mdf_map_resp_wait);
 	q6core_lcl.cmd_resp_received_flag = FLAG_NONE;
 	mutex_init(&q6core_lcl.cmd_lock);
 	mutex_init(&q6core_lcl.ver_lock);

include/dsp/q6core.h

@@ -199,7 +199,11 @@ struct avcs_cmd_load_unload_topo_modules {
 int q6core_map_memory_regions(phys_addr_t *buf_add, uint32_t mempool_id,
 			uint32_t *bufsz, uint32_t bufcnt, uint32_t *map_handle);
 int q6core_memory_unmap_regions(uint32_t mem_map_handle);
-int q6core_map_mdf_shared_memory(uint32_t map_handle, phys_addr_t *buf_add,
+
+int q6core_map_mdf_memory_regions(uint64_t *buf_add, uint32_t mempool_id,
+			uint32_t *bufsz, uint32_t bufcnt, uint32_t *map_handle);
+
+int q6core_map_mdf_shared_memory(uint32_t map_handle, uint64_t *buf_add,
 			uint32_t proc_id, uint32_t *bufsz, uint32_t bufcnt);
 
 int32_t core_set_license(uint32_t key, uint32_t module_id);