C++ CMDQ_ERR函数代码示例

// TODO: when do release secure command buffer
int32_t cmdq_sec_fill_iwc_resource_msg_unlocked(int32_t iwcCommand, void *_pTask, int32_t thread, void *_pIwc)
{
	iwcCmdqMessage_t *pIwc;
	cmdqSecSharedMemoryHandle pSharedMem;

	pSharedMem = cmdq_core_get_secure_shared_memory();
	if (NULL == pSharedMem) {
		CMDQ_ERR("FILL:RES, NULL shared memory\n");
		return -EFAULT;
	}

	if(pSharedMem && NULL == pSharedMem->pVABase) {
		CMDQ_ERR("FILL:RES, %p shared memory has not init\n", pSharedMem);
		return -EFAULT;
	}

	pIwc = (iwcCmdqMessage_t *)_pIwc;
	memset(pIwc, 0x0, sizeof(iwcCmdqMessage_t));

	pIwc->cmd = iwcCommand;
	pIwc->pathResource.shareMemoyPA = 0LL | (pSharedMem->MVABase);
	pIwc->pathResource.size = pSharedMem->size;

	CMDQ_MSG("FILL:RES, shared memory:%pa(0x%llx), size:%d\n",
		&(pSharedMem->MVABase), pIwc->pathResource.shareMemoyPA, pSharedMem->size);

	/* medatada: debug config */
	pIwc->debug.logLevel = (cmdq_core_should_print_msg()) ? (1) : (0);
	pIwc->debug.enableProfile = cmdq_core_profile_enabled();

	return 0;
}

int32_t cmdq_sec_allocate_wsm_impl(uint32_t deviceId, uint8_t **ppWsm, uint32_t wsmSize)
{
	int32_t status = 0;
	enum mc_result mcRet = MC_DRV_OK;

	do {
		if ((*ppWsm) != NULL) {
			status = -1;
			CMDQ_ERR("[SEC]_WSM_ALLOC: err[pWsm is not NULL]");
			break;
		}

		/* because world shared mem(WSM) will ba managed by mobicore device, not linux kernel */
		/* instead of vmalloc/kmalloc, call mc_malloc_wasm to alloc WSM to prvent error such as */
		/* "can not resolve tci physicall address" etc */
		mcRet = mc_malloc_wsm(deviceId, 0, wsmSize, ppWsm, 0);
		if (MC_DRV_OK != mcRet) {
			CMDQ_ERR("[SEC]_WSM_ALLOC: err[0x%x]\n", mcRet);
			status = -1;
			break;
		}

		CMDQ_MSG("[SEC]_WSM_ALLOC: status[%d], *ppWsm: 0x%p\n", status, (*ppWsm));
	} while (0);

	return status;
}

int32_t cmdq_sec_open_session_impl(uint32_t deviceId,
				const struct mc_uuid_t *uuid,
				uint8_t *pWsm,
				uint32_t wsmSize,
				struct mc_session_handle *pSessionHandle)
{
	int32_t status = 0;
	enum mc_result mcRet = MC_DRV_OK;
	do {
		if (NULL == pWsm || NULL == pSessionHandle) {
			status = -1;
			CMDQ_ERR("[SEC]_SESSION_OPEN: invalid param, pWsm[0x%p], pSessionHandle[0x%p]\n",
			     pWsm, pSessionHandle);
			break;
		}

		memset(pSessionHandle, 0, sizeof(*pSessionHandle));
		pSessionHandle->device_id = deviceId;
		mcRet = mc_open_session(pSessionHandle, uuid, pWsm, wsmSize);
		if (MC_DRV_OK != mcRet) {
			CMDQ_ERR("[SEC]_SESSION_OPEN: err[0x%x]\n", mcRet);
			status = -1;
			break;
		}

		CMDQ_MSG("[SEC]_SESSION_OPEN: status[%d], mcRet[0x%x]\n", status, mcRet);
	} while (0);

	return status;
}

int32_t cmdq_sec_allocate_path_resource_unlocked(void)
{
#ifdef CMDQ_SECURE_PATH_SUPPORT
	int32_t status = 0;

	if(1 == atomic_read(&gCmdqSecPathResource)) {
		/* has allocated successfully */
		return status;
	}

	status = cmdq_sec_submit_to_secure_world_async_unlocked(
				CMD_CMDQ_TL_PATH_RES_ALLOCATE, NULL, CMDQ_INVALID_THREAD, NULL, NULL);
	if (0 > status) {
		CMDQ_ERR("%s[%d]\n", __func__, status);
	} else {
		atomic_set(&gCmdqSecPathResource, 1);
	}

	return status;

#else
	CMDQ_ERR("secure path not support\n");
	return -EFAULT;
#endif
}

static long cmdq_ioctl_compat(struct file *pFile, unsigned int code, unsigned long param)
{
	switch (code) {
	case CMDQ_IOCTL_QUERY_USAGE:
	case CMDQ_IOCTL_EXEC_COMMAND:
	case CMDQ_IOCTL_ASYNC_JOB_EXEC:
	case CMDQ_IOCTL_ASYNC_JOB_WAIT_AND_CLOSE:
	case CMDQ_IOCTL_ALLOC_WRITE_ADDRESS:
	case CMDQ_IOCTL_FREE_WRITE_ADDRESS:
	case CMDQ_IOCTL_READ_ADDRESS_VALUE:
	case CMDQ_IOCTL_QUERY_CAP_BITS:
	case CMDQ_IOCTL_QUERY_DTS:
	case CMDQ_IOCTL_NOTIFY_ENGINE:
		/* All ioctl structures should be the same size in 32-bit and 64-bit linux. */
		return cmdq_ioctl(pFile, code, param);
	case CMDQ_IOCTL_LOCK_MUTEX:
	case CMDQ_IOCTL_UNLOCK_MUTEX:
		CMDQ_ERR("[COMPAT]deprecated ioctl 0x%08x\n", code);
		return -ENOIOCTLCMD;
	default:
		CMDQ_ERR("[COMPAT]unrecognized ioctl 0x%08x\n", code);
		return -ENOIOCTLCMD;
	}

	CMDQ_ERR("[COMPAT]unrecognized ioctl 0x%08x\n", code);
	return -ENOIOCTLCMD;
}

static int __init cmdq_init(void)
{
	int status;

	CMDQ_MSG("CMDQ driver init begin\n");

	/* Initialize group callback */
	cmdqCoreInitGroupCB();

	/* Register MDP callback */
	cmdqCoreRegisterCB(CMDQ_GROUP_MDP,
			   cmdqMdpClockOn, cmdqMdpDumpInfo, cmdqMdpResetEng, cmdqMdpClockOff);

	/* Register VENC callback */
	cmdqCoreRegisterCB(CMDQ_GROUP_VENC, NULL, cmdqVEncDumpInfo, NULL, NULL);

	status = platform_driver_register(&gCmdqDriver);
	if (0 != status) {
		CMDQ_ERR("Failed to register the CMDQ driver(%d)\n", status);
		return -ENODEV;
	}

	/* register pm notifier */
	status = register_pm_notifier(&cmdq_pm_notifier_block);
	if (0 != status) {
		CMDQ_ERR("Failed to register_pm_notifier(%d)\n", status);
		return -ENODEV;
	}

	CMDQ_MSG("CMDQ driver init end\n");

	return 0;
}

static void cmdq_driver_process_read_address_request(struct cmdqReadAddressStruct *req_user)
{
	/* create kernel-space buffer for working */
	uint32_t *addrs = NULL;
	uint32_t *values = NULL;
	dma_addr_t pa = 0;
	int i = 0;

	CMDQ_LOG("[READ_PA] cmdq_driver_process_read_address_request()\n");

	do {
		if (NULL == req_user ||
		    0 == req_user->count || NULL == CMDQ_U32_PTR(req_user->values)
		    || NULL == CMDQ_U32_PTR(req_user->dmaAddresses)) {
			CMDQ_ERR("[READ_PA] invalid req_user\n");
			break;
		}

		addrs = kcalloc(req_user->count, sizeof(uint32_t), GFP_KERNEL);
		if (NULL == addrs) {
			CMDQ_ERR("[READ_PA] fail to alloc addr buf\n");
			break;
		}

		values = kcalloc(req_user->count, sizeof(uint32_t), GFP_KERNEL);
		if (NULL == values) {
			CMDQ_ERR("[READ_PA] fail to alloc value buf\n");
			break;
		}
		/* copy from user */
		if (copy_from_user
		    (addrs, CMDQ_U32_PTR(req_user->dmaAddresses),
		     req_user->count * sizeof(uint32_t))) {
			CMDQ_ERR("[READ_PA] fail to copy user dmaAddresses\n");
			break;
		}
		/* actually read these PA write buffers */
		for (i = 0; i < req_user->count; ++i) {
			pa = (0xFFFFFFFF & addrs[i]);
			CMDQ_LOG("[READ_PA] req read dma address 0x%pa\n", &pa);
			values[i] = cmdqCoreReadWriteAddress(pa);
		}

		/* copy value to user */
		if (copy_to_user
		    (CMDQ_U32_PTR(req_user->values), values, req_user->count * sizeof(uint32_t))) {
			CMDQ_ERR("[READ_PA] fail to copy to user value buf\n");
			break;
		}

	} while (0);

	kfree(addrs);
	kfree(values);
}

static long cmdq_driver_create_secure_medadata(cmdqCommandStruct *pCommand)
{
	void *pAddrMetadatas = NULL;
	const uint32_t length =
	    (pCommand->secData.addrMetadataCount) * sizeof(cmdqSecAddrMetadataStruct);

	/* verify parameter */
	if ((false == pCommand->secData.isSecure) && (0 != pCommand->secData.addrMetadataCount)) {

		/* normal path with non-zero secure metadata */
		CMDQ_ERR
		    ("[secData]mismatch secData.isSecure(%d) and secData.addrMetadataCount(%d)\n",
		     pCommand->secData.isSecure, pCommand->secData.addrMetadataCount);
		return -EFAULT;
	}

	/* revise max count field */
	pCommand->secData.addrMetadataMaxCount = pCommand->secData.addrMetadataCount;

	/* bypass 0 metadata case */
	if (0 == pCommand->secData.addrMetadataCount) {
		pCommand->secData.addrMetadatas = (cmdqU32Ptr_t) (unsigned long)NULL;
		return 0;
	}

	/* create kernel-space buffer for working */
	pAddrMetadatas = kzalloc(length, GFP_KERNEL);
	if (NULL == pAddrMetadatas) {
		CMDQ_ERR("[secData]kzalloc for addrMetadatas failed, count:%d, alloacted_size:%d\n",
			 pCommand->secData.addrMetadataCount, length);
		return -ENOMEM;
	}

	/* copy from user */
	if (copy_from_user(pAddrMetadatas, CMDQ_U32_PTR(pCommand->secData.addrMetadatas), length)) {

		CMDQ_ERR("[secData]fail to copy user addrMetadatas\n");

		/* replace buffer first to ensure that */
		/* addrMetadatas is valid kernel space buffer address when free it */
		pCommand->secData.addrMetadatas = (cmdqU32Ptr_t) (unsigned long)pAddrMetadatas;
		/* free secure path metadata */
		cmdq_driver_destroy_secure_medadata(pCommand);
		return -EFAULT;
	}

	/* replace buffer */
	pCommand->secData.addrMetadatas = (cmdqU32Ptr_t) (unsigned long)pAddrMetadatas;

#if 0
	cmdq_core_dump_secure_metadata(&(pCommand->secData));
#endif

	return 0;
}

int32_t cmdqRecProfileMarker(cmdqRecHandle handle, const char *tag)
{
#ifdef CMDQ_PROFILE_MARKER_SUPPORT
	int32_t status;
	int32_t index;
	cmdqBackupSlotHandle hSlot;
	dma_addr_t allocatedStartPA;

	do {
		allocatedStartPA = 0;
		status = 0;

		/* allocate temp slot for GCE to store timestamp info */
		/* those timestamp info will copy to record strute after task execute done */
		if ((0 == handle->profileMarker.count) && (0 == handle->profileMarker.hSlot)) {
			status = cmdqCoreAllocWriteAddress(CMDQ_MAX_PROFILE_MARKER_IN_TASK, &allocatedStartPA);
			if(0 > status) {
				CMDQ_ERR("[REC][PROF_MARKER]allocate failed, status:%d\n", status);
				break;
			}

			handle->profileMarker.hSlot = 0LL | (allocatedStartPA);

			CMDQ_VERBOSE("[REC][PROF_MARKER]update handle(%p) slot start PA:%pa(0x%llx)\n",
				handle, &allocatedStartPA, handle->profileMarker.hSlot);
		}

		/* insert instruciton */
		index = handle->profileMarker.count;
		hSlot = (cmdqBackupSlotHandle)(handle->profileMarker.hSlot);

		if (index >= CMDQ_MAX_PROFILE_MARKER_IN_TASK) {
			CMDQ_ERR("[REC][PROF_MARKER]insert profile maker failed since already reach max count\n");
			status = -EFAULT;
			break;
		}

		CMDQ_VERBOSE("[REC][PROF_MARKER]inserting profile instr, handle:%p, slot:%pa(0x%llx), index:%d, tag:%s\n",
			handle, &hSlot, handle->profileMarker.hSlot, index, tag);

		cmdqRecBackupRegisterToSlot(handle, hSlot, index, CMDQ_APXGPT2_COUNT);

		handle->profileMarker.tag[index] = tag;
		handle->profileMarker.count += 1;
	} while(0);

	return status;
#else
	CMDQ_ERR("func:%s failed since CMDQ dosen't enable profile marker\n", __func__);
	return -EFAULT;
#endif
}

static int cmdq_sec_sectrace_map(void *va, size_t size)
{
	int status;
	enum mc_result mcRet;

	CMDQ_LOG("[sectrace]-->map: start, va:%p, size:%d\n", va, (int)size);

	status = 0;
	cmdq_sec_lock_secure_path();
	do {
		/* HACK: submit a dummy message to ensure secure path init done */
		status = cmdq_sec_submit_to_secure_world_async_unlocked(
			CMD_CMDQ_TL_TEST_HELLO_TL, NULL, CMDQ_INVALID_THREAD, NULL, NULL);

		/* map log buffer in NWd */
		mcRet = mc_map(&(gCmdqSecContextHandle->sessionHandle),
					va,
					(uint32_t)size,
					&gCmdqSectraceMappedInfo);
		if (MC_DRV_OK != mcRet) {
			CMDQ_ERR("[sectrace]map: failed in NWd, mc_map err: 0x%x\n", mcRet);
			status = -EFAULT;
			break;
		}

		CMDQ_LOG("[sectrace]map: mc_map sectrace buffer done, gCmdqSectraceMappedInfo(va:0x%08x, size:%d)\n",
			gCmdqSectraceMappedInfo.secure_virt_addr, gCmdqSectraceMappedInfo.secure_virt_len);

		/* ask secure CMDQ to map sectrace log buffer */
		status = cmdq_sec_submit_to_secure_world_async_unlocked(
					CMD_CMDQ_TL_SECTRACE_MAP,
					NULL, CMDQ_INVALID_THREAD,
					cmdq_sec_fill_iwc_command_sectrace_unlocked,
					NULL);
		if(0 > status) {
			CMDQ_ERR("[sectrace]map: failed in SWd: %d\n", status);
			mc_unmap(&(gCmdqSecContextHandle->sessionHandle), va, &gCmdqSectraceMappedInfo);
			status = -EFAULT;
			break;
		}
	} while(0);

	cmdq_sec_unlock_secure_path();

	CMDQ_LOG("[sectrace]<--map: status: %d\n", status);

	return status;
}

int32_t cmdq_sec_execute_session_unlocked(struct mc_session_handle *pSessionHandle,
					  CMDQ_IWC_STATE_ENUM *pIwcState, int32_t timeout_ms)
{
	enum mc_result mcRet;
	int32_t status = 0;
	const int32_t secureWoldTimeout_ms = (0 < timeout_ms) ?
										(timeout_ms) :
										(MC_INFINITE_TIMEOUT);

	CMDQ_PROF_START("CMDQ_SEC_EXE");

	do {
		/* notify to secure world */
		mcRet = mc_notify(pSessionHandle);
		if (MC_DRV_OK != mcRet) {
			CMDQ_ERR("[SEC]EXEC: mc_notify err[0x%x]\n", mcRet);
			status = -1;
			break;
		} else {
			CMDQ_MSG("[SEC]EXEC: mc_notify ret[0x%x]\n", mcRet);
		}
		(*pIwcState) = IWC_SES_TRANSACTED;


		/* wait respond */
		mcRet = mc_wait_notification(pSessionHandle, secureWoldTimeout_ms);
		if (MC_DRV_ERR_TIMEOUT == mcRet) {
			CMDQ_ERR
			    ("[SEC]EXEC: mc_wait_notification timeout, err[0x%x], secureWoldTimeout_ms[%d]\n",
			     mcRet, secureWoldTimeout_ms);
			status = -ETIMEDOUT;
			break;
		}

		if (MC_DRV_OK != mcRet) {
			CMDQ_ERR("[SEC]EXEC: mc_wait_notification err[0x%x]\n", mcRet);
			status = -1;
			break;
		} else {
			CMDQ_MSG("[SEC]EXEC: mc_wait_notification err[%d]\n", mcRet);
		}
		(*pIwcState) = IWC_SES_ON_TRANSACTED;
	} while (0);

	CMDQ_PROF_END("CMDQ_SEC_EXE");

	return status;
}

/********************************************************************************
 * operator API
 *******************************************************************************/
int32_t cmdq_sec_open_mobicore_impl(uint32_t deviceId)
{
	int32_t status;
	enum mc_result mcRet = MC_DRV_ERR_UNKNOWN;
	int retryCnt = 0;
	do {
		status = 0;
		mcRet = mc_open_device(deviceId);

		/* Currently, a process context limits to open mobicore device once, */
		/* and mc_open_device dose not support reference cout */
		/* so skip the false alarm error.... */
		if (MC_DRV_ERR_INVALID_OPERATION == mcRet) {
			CMDQ_MSG("[SEC]_MOBICORE_OPEN: already opened, continue to execution\n");
			status = -EEXIST;
		} else if (MC_DRV_OK != mcRet) {
			CMDQ_ERR("[SEC]_MOBICORE_OPEN: err[0x%x]\n", mcRet);
			status = -1;
			retryCnt++;
			continue;
		}
		CMDQ_MSG("[SEC]_MOBICORE_OPEN: status[%d], ret[0x%x]\n", status, mcRet);
		break;
	}while(retryCnt < 30);
	
	return status;
}

const int32_t cmdq_core_can_module_entry_suspend(EngineStruct *engineList)
{
	int32_t status = 0;
	int i;
	CMDQ_ENG_ENUM e = 0;

	CMDQ_ENG_ENUM mdpEngines[] = {
		CMDQ_ENG_ISP_IMGI,
		CMDQ_ENG_MDP_RDMA0,
		CMDQ_ENG_MDP_RSZ0,
		CMDQ_ENG_MDP_RSZ1,
		CMDQ_ENG_MDP_TDSHP0,
		CMDQ_ENG_MDP_WROT0,
		CMDQ_ENG_MDP_WDMA
	};

	for (i = 0; i < (sizeof(mdpEngines) / sizeof(CMDQ_ENG_ENUM)); ++i) {
		e = mdpEngines[i];
		if (0 != engineList[e].userCount) {
			CMDQ_ERR("suspend but engine %d has userCount %d, owner=%d\n",
				 e, engineList[e].userCount, engineList[e].currOwner);
			status = -EBUSY;
		}
	}

	return status;
}

static int cmdq_driver_create_reg_address_buffer(struct cmdqCommandStruct *pCommand)
{
	int status = 0;
	uint32_t totalRegCount = 0;
	uint32_t *regAddrBuf = NULL;

	uint32_t *kernelRegAddr = NULL;
	uint32_t kernelRegCount = 0;

	const uint32_t userRegCount = pCommand->regRequest.count;


	if (0 != pCommand->debugRegDump) {
		/* get kernel dump request count */
		status = cmdqCoreDebugRegDumpBegin(pCommand->debugRegDump,
						   &kernelRegCount, &kernelRegAddr);
		if (0 != status) {
			CMDQ_ERR
			    ("cmdqCoreDebugRegDumpBegin returns %d, ignore kernel reg dump request\n",
			     status);
			kernelRegCount = 0;
			kernelRegAddr = NULL;
		}
	}
	/* how many register to dump? */
	totalRegCount = kernelRegCount + userRegCount;

	if (0 == totalRegCount) {
		/* no need to dump register */
		pCommand->regRequest.count = 0;
		pCommand->regValue.count = 0;
		pCommand->regRequest.regAddresses = (cmdqU32Ptr_t) (unsigned long)NULL;
		pCommand->regValue.regValues = (cmdqU32Ptr_t) (unsigned long)NULL;
	} else {
		regAddrBuf = kcalloc(totalRegCount, sizeof(uint32_t), GFP_KERNEL);
		if (NULL == regAddrBuf)
			return -ENOMEM;

		/* collect user space dump request */
		if (userRegCount) {
			if (copy_from_user
			    (regAddrBuf, CMDQ_U32_PTR(pCommand->regRequest.regAddresses),
			     userRegCount * sizeof(uint32_t))) {
				kfree(regAddrBuf);
				return -EFAULT;
			}
		}
		/* collect kernel space dump request, concatnate after user space request */
		if (kernelRegCount) {
			memcpy(regAddrBuf + userRegCount, kernelRegAddr,
			       kernelRegCount * sizeof(uint32_t));
		}

		/* replace address buffer and value address buffer with kzalloc memory */
		pCommand->regRequest.regAddresses = (cmdqU32Ptr_t) (unsigned long)(regAddrBuf);
		pCommand->regRequest.count = totalRegCount;
	}

	return 0;
}

static irqreturn_t cmdq_irq_handler(int IRQ, void *pDevice)
{
	int index;
	uint32_t irqStatus;
	bool handled = false;	/* we share IRQ bit with CQ-DMA, */
	/* so it is possible that this handler */
	/* is called but GCE does not have IRQ flag. */
	do {
		if (cmdq_dev_get_irq_id() == IRQ) {
			irqStatus = CMDQ_REG_GET32(CMDQ_CURR_IRQ_STATUS) & 0x0FFFF;
			for (index = 0; (irqStatus != 0xFFFF) && index < CMDQ_MAX_THREAD_COUNT;
			     index++) {
				/* STATUS bit set to 0 means IRQ asserted */
				if (irqStatus & (1 << index))
					continue;

				/* so we mark irqStatus to 1 to denote finished processing */
				/* and we can early-exit if no more threads being asserted */
				irqStatus |= (1 << index);

				cmdqCoreHandleIRQ(index);
				handled = true;
			}
		} else if (cmdq_dev_get_irq_secure_id() == IRQ) {
			CMDQ_ERR("receive secure IRQ %d in NWD\n", IRQ);
		}
	} while (0);

	if (handled) {
		cmdq_core_add_consume_task();
		return IRQ_HANDLED;
	}
	/* allow CQ-DMA to process this IRQ bit */
	return IRQ_NONE;
}