static int svc_normal_to_secure_thread()

in stratix10-svc.c [336:512]

• 155 lines of code
• 27 McCabe index (conditional complexity)

static int svc_normal_to_secure_thread(void *data)
{
	struct stratix10_svc_controller
			*ctrl = (struct stratix10_svc_controller *)data;
	struct stratix10_svc_data *pdata;
	struct stratix10_svc_cb_data *cbdata;
	struct arm_smccc_res res;
	unsigned long a0, a1, a2;
	int ret_fifo = 0;

	pdata =  kmalloc(sizeof(*pdata), GFP_KERNEL);
	if (!pdata)
		return -ENOMEM;

	cbdata = kmalloc(sizeof(*cbdata), GFP_KERNEL);
	if (!cbdata) {
		kfree(pdata);
		return -ENOMEM;
	}

	/* default set, to remove build warning */
	a0 = INTEL_SIP_SMC_FPGA_CONFIG_LOOPBACK;
	a1 = 0;
	a2 = 0;

	pr_debug("smc_hvc_shm_thread is running\n");

	while (!kthread_should_stop()) {
		ret_fifo = kfifo_out_spinlocked(&ctrl->svc_fifo,
						pdata, sizeof(*pdata),
						&ctrl->svc_fifo_lock);

		if (!ret_fifo)
			continue;

		pr_debug("get from FIFO pa=0x%016x, command=%u, size=%u\n",
			 (unsigned int)pdata->paddr, pdata->command,
			 (unsigned int)pdata->size);

		switch (pdata->command) {
		case COMMAND_RECONFIG_DATA_CLAIM:
			svc_thread_cmd_data_claim(ctrl, pdata, cbdata);
			continue;
		case COMMAND_RECONFIG:
			a0 = INTEL_SIP_SMC_FPGA_CONFIG_START;
			pr_debug("conf_type=%u\n", (unsigned int)pdata->flag);
			a1 = pdata->flag;
			a2 = 0;
			break;
		case COMMAND_RECONFIG_DATA_SUBMIT:
			a0 = INTEL_SIP_SMC_FPGA_CONFIG_WRITE;
			a1 = (unsigned long)pdata->paddr;
			a2 = (unsigned long)pdata->size;
			break;
		case COMMAND_RECONFIG_STATUS:
			a0 = INTEL_SIP_SMC_FPGA_CONFIG_ISDONE;
			a1 = 0;
			a2 = 0;
			break;
		case COMMAND_RSU_STATUS:
			a0 = INTEL_SIP_SMC_RSU_STATUS;
			a1 = 0;
			a2 = 0;
			break;
		case COMMAND_RSU_UPDATE:
			a0 = INTEL_SIP_SMC_RSU_UPDATE;
			a1 = pdata->arg[0];
			a2 = 0;
			break;
		case COMMAND_RSU_NOTIFY:
			a0 = INTEL_SIP_SMC_RSU_NOTIFY;
			a1 = pdata->arg[0];
			a2 = 0;
			break;
		case COMMAND_RSU_RETRY:
			a0 = INTEL_SIP_SMC_RSU_RETRY_COUNTER;
			a1 = 0;
			a2 = 0;
			break;
		case COMMAND_RSU_MAX_RETRY:
			a0 = INTEL_SIP_SMC_RSU_MAX_RETRY;
			a1 = 0;
			a2 = 0;
			break;
		case COMMAND_RSU_DCMF_VERSION:
			a0 = INTEL_SIP_SMC_RSU_DCMF_VERSION;
			a1 = 0;
			a2 = 0;
			break;
		default:
			pr_warn("it shouldn't happen\n");
			break;
		}
		pr_debug("%s: before SMC call -- a0=0x%016x a1=0x%016x",
			 __func__, (unsigned int)a0, (unsigned int)a1);
		pr_debug(" a2=0x%016x\n", (unsigned int)a2);

		ctrl->invoke_fn(a0, a1, a2, 0, 0, 0, 0, 0, &res);

		pr_debug("%s: after SMC call -- res.a0=0x%016x",
			 __func__, (unsigned int)res.a0);
		pr_debug(" res.a1=0x%016x, res.a2=0x%016x",
			 (unsigned int)res.a1, (unsigned int)res.a2);
		pr_debug(" res.a3=0x%016x\n", (unsigned int)res.a3);

		if (pdata->command == COMMAND_RSU_STATUS) {
			if (res.a0 == INTEL_SIP_SMC_RSU_ERROR)
				cbdata->status = BIT(SVC_STATUS_ERROR);
			else
				cbdata->status = BIT(SVC_STATUS_OK);

			cbdata->kaddr1 = &res;
			cbdata->kaddr2 = NULL;
			cbdata->kaddr3 = NULL;
			pdata->chan->scl->receive_cb(pdata->chan->scl, cbdata);
			continue;
		}

		switch (res.a0) {
		case INTEL_SIP_SMC_STATUS_OK:
			svc_thread_recv_status_ok(pdata, cbdata, res);
			break;
		case INTEL_SIP_SMC_STATUS_BUSY:
			switch (pdata->command) {
			case COMMAND_RECONFIG_DATA_SUBMIT:
				svc_thread_cmd_data_claim(ctrl,
							  pdata, cbdata);
				break;
			case COMMAND_RECONFIG_STATUS:
				svc_thread_cmd_config_status(ctrl,
							     pdata, cbdata);
				break;
			default:
				pr_warn("it shouldn't happen\n");
				break;
			}
			break;
		case INTEL_SIP_SMC_STATUS_REJECTED:
			pr_debug("%s: STATUS_REJECTED\n", __func__);
			break;
		case INTEL_SIP_SMC_STATUS_ERROR:
		case INTEL_SIP_SMC_RSU_ERROR:
			pr_err("%s: STATUS_ERROR\n", __func__);
			cbdata->status = BIT(SVC_STATUS_ERROR);
			cbdata->kaddr1 = NULL;
			cbdata->kaddr2 = NULL;
			cbdata->kaddr3 = NULL;
			pdata->chan->scl->receive_cb(pdata->chan->scl, cbdata);
			break;
		default:
			pr_warn("Secure firmware doesn't support...\n");

			/*
			 * be compatible with older version firmware which
			 * doesn't support RSU notify or retry
			 */
			if ((pdata->command == COMMAND_RSU_RETRY) ||
			    (pdata->command == COMMAND_RSU_MAX_RETRY) ||
				(pdata->command == COMMAND_RSU_NOTIFY)) {
				cbdata->status =
					BIT(SVC_STATUS_NO_SUPPORT);
				cbdata->kaddr1 = NULL;
				cbdata->kaddr2 = NULL;
				cbdata->kaddr3 = NULL;
				pdata->chan->scl->receive_cb(
					pdata->chan->scl, cbdata);
			}
			break;

		}
	}

	kfree(cbdata);
	kfree(pdata);

	return 0;
}