From 8977dca853ea35e94f63adae55e3feba6cf08443 Mon Sep 17 00:00:00 2001
From: Andy Gross <agross@codeaurora.org>
Date: Thu, 2 Jul 2015 17:09:09 -0500
Subject: [PATCH] firmware: qcom: scm: Add boot APIs

This patch adds the warm/cold boot and cpu shutdown APIs.

Signed-off-by: Andy Gross <agross@codeaurora.org>
---
 drivers/firmware/qcom_scm-64.c | 79 +++++++++++++++++++++++++++++++++-
 drivers/firmware/qcom_scm.h    |  7 +++
 2 files changed, 84 insertions(+), 2 deletions(-)

diff --git a/drivers/firmware/qcom_scm-64.c b/drivers/firmware/qcom_scm-64.c
index 8531c32bfa91..7f985ca78eaa 100644
--- a/drivers/firmware/qcom_scm-64.c
+++ b/drivers/firmware/qcom_scm-64.c
@@ -307,18 +307,93 @@ static int qcom_scm_call(u32 svc_id, u32 cmd_id, struct qcom_scm_desc *desc)
 	return 0;
 }
 
+static int qcom_scm_call_atomic(u32 s, u32 c, struct qcom_scm_desc *desc)
+{
+	int arglen = desc->arginfo & 0xf;
+	int ret;
+	u32 fn_id = QCOM_SCM_SIP_FNID(s, c);
+	u64 x0;
+
+	ret = allocate_extra_arg_buffer(desc, GFP_ATOMIC);
+	if (ret)
+		return ret;
+
+	x0 = fn_id | BIT(SMC_ATOMIC_SYSCALL) | qcom_scm_version_mask;
+
+	pr_debug("qcom_scm_call: func id %#llx, args: %#x, %#llx, %#llx, %#llx, %#llx\n",
+			x0, desc->arginfo, desc->args[0], desc->args[1],
+			desc->args[2], desc->x5);
+
+	if (qcom_scm_version == QCOM_SCM_ARMV8_64)
+		ret = __qcom_scm_call_armv8_64(x0, desc->arginfo, desc->args[0],
+						desc->args[1], desc->args[2],
+						desc->x5, &desc->ret[0],
+						&desc->ret[1], &desc->ret[2]);
+	else
+		ret = __qcom_scm_call_armv8_32(x0, desc->arginfo, desc->args[0],
+						desc->args[1], desc->args[2],
+						desc->x5, &desc->ret[0],
+						&desc->ret[1], &desc->ret[2]);
+	if (ret < 0)
+		pr_err("qcom_scm_call failed: func id %#llx, arginfo: %#x, args: %#llx, %#llx, %#llx, %#llx, ret: %d, syscall returns: %#llx, %#llx, %#llx\n", 
+			x0, desc->arginfo, desc->args[0], desc->args[1],
+			desc->args[2], desc->x5, ret, desc->ret[0],
+			desc->ret[1], desc->ret[2]);
+
+	if (arglen > N_REGISTER_ARGS)
+		kfree(desc->extra_arg_buf);
+	if (ret < 0)
+		return qcom_scm_remap_error(ret);
+	return ret;
+}
+
+static int qcom_scm_set_boot_addr(void *entry, const cpumask_t *cpus, int flags)
+{
+	struct qcom_scm_desc desc = {0};
+	unsigned int cpu = cpumask_first(cpus);
+	u64 mpidr_el1 = cpu_logical_map(cpu);
+
+	/* For now we assume only a single cpu is set in the mask */
+	WARN_ON(cpumask_weight(cpus) != 1);
+
+	if (mpidr_el1 & ~MPIDR_HWID_BITMASK) {
+		pr_err("CPU%d:Failed to set boot address\n", cpu);
+		return -ENOSYS;
+	}
+
+	desc.args[0] = virt_to_phys(entry);
+	desc.args[1] = BIT(MPIDR_AFFINITY_LEVEL(mpidr_el1, 0));
+	desc.args[2] = BIT(MPIDR_AFFINITY_LEVEL(mpidr_el1, 1));
+	desc.args[3] = BIT(MPIDR_AFFINITY_LEVEL(mpidr_el1, 2));
+	desc.args[4] = ~0ULL;
+	desc.args[5] = QCOM_SCM_FLAG_HLOS | flags;
+	desc.arginfo = QCOM_SCM_ARGS(6);
+
+	return qcom_scm_call(QCOM_SCM_SVC_BOOT, QCOM_SCM_BOOT_ADDR_MC, &desc);
+}
+
 int __qcom_scm_set_cold_boot_addr(void *entry, const cpumask_t *cpus)
 {
-	return -EOPNOTSUPP;
+	int flags = QCOM_SCM_FLAG_COLDBOOT_MC;
+
+	return qcom_scm_set_boot_addr(entry, cpus, flags);
 }
 
 int __qcom_scm_set_warm_boot_addr(void *entry, const cpumask_t *cpus)
 {
-	return -EOPNOTSUPP;
+	int flags = QCOM_SCM_FLAG_WARMBOOT_MC;
+
+	return qcom_scm_set_boot_addr(entry, cpus, flags);
 }
 
 void __qcom_scm_cpu_power_down(u32 flags)
 {
+	struct qcom_scm_desc desc = {0};
+	desc.args[0] = QCOM_SCM_CMD_CORE_HOTPLUGGED |
+			(flags & QCOM_SCM_FLUSH_FLAG_MASK);
+	desc.arginfo = QCOM_SCM_ARGS(1);
+
+	qcom_scm_call_atomic(QCOM_SCM_SVC_BOOT, QCOM_SCM_CMD_TERMINATE_PC, &desc);
 }
 
 int __qcom_scm_is_call_available(u32 svc_id, u32 cmd_id)
diff --git a/drivers/firmware/qcom_scm.h b/drivers/firmware/qcom_scm.h
index 7dcc73381b7a..5fb75fab81d3 100644
--- a/drivers/firmware/qcom_scm.h
+++ b/drivers/firmware/qcom_scm.h
@@ -43,6 +43,9 @@ extern int __qcom_scm_hdcp_req(struct qcom_scm_hdcp_req *req, u32 req_cnt,
 #define QCOM_SCM_EINVAL_ARG	-2
 #define QCOM_SCM_ERROR		-1
 #define QCOM_SCM_INTERRUPTED	1
+#define QCOM_SCM_EBUSY         -55
+#define QCOM_SCM_V2_EBUSY      -12
+
 
 static inline int qcom_scm_remap_error(int err)
 {
@@ -56,6 +59,10 @@ static inline int qcom_scm_remap_error(int err)
 		return -EOPNOTSUPP;
 	case QCOM_SCM_ENOMEM:
 		return -ENOMEM;
+	case QCOM_SCM_EBUSY:
+		return QCOM_SCM_EBUSY;
+	case QCOM_SCM_V2_EBUSY:
+		return QCOM_SCM_V2_EBUSY;
 	}
 	return -EINVAL;
 }
-- 
2.39.5