From 00e0d84258ec5dfd5ca1cf6d09dd30246767bf39 Mon Sep 17 00:00:00 2001
From: Sumit Garg <sumit.garg@nxp.com>
Date: Wed, 4 May 2016 13:25:47 -0400
Subject: [PATCH 2/4] ARMv7: Enable standard psci fast call handling

For ARMv7, secure monitor lies in OP-TEE OS so standard psci (Power
State Configuration Interface) fast call handling is required to be
handled in OP-TEE OS. So enabled generic framework for handling standard
psci fast calls which need to be implemented by corresponding platform.

Signed-off-by: Sumit Garg <sumit.garg@nxp.com>
---
 core/arch/arm/include/sm/psci.h |  77 ++++++++++++++++++++++++++++
 core/arch/arm/plat-ls/main.c    |  28 +++++++++-
 core/arch/arm/sm/psci.c         | 110 ++++++++++++++++++++++++++++++++++++++++
 core/arch/arm/sm/sub.mk         |   1 +
 4 files changed, 215 insertions(+), 1 deletion(-)
 create mode 100644 core/arch/arm/include/sm/psci.h
 create mode 100644 core/arch/arm/sm/psci.c

diff --git a/core/arch/arm/include/sm/psci.h b/core/arch/arm/include/sm/psci.h
new file mode 100644
index 0000000..b7cd542
--- /dev/null
+++ b/core/arch/arm/include/sm/psci.h
@@ -0,0 +1,77 @@
+/*
+ * Copyright (c) 2016, Freescale Semiconductor, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef PSCI_H
+#define PSCI_H
+
+#include <kernel/thread.h>
+
+/* PSCI 1.0 supported */
+#define SMC_PSCI_VERSION			0x00010000
+
+#define SMC_PSCI_FN_BASE			0x84000000
+#define SMC_PSCI_FN(n)				(SMC_PSCI_FN_BASE + (n))
+
+/* PSCI v0.1 interface */
+#define SMC_PSCI_FN_CPU_SUSPEND			SMC_PSCI_FN(1)
+#define SMC_PSCI_FN_CPU_OFF			SMC_PSCI_FN(2)
+#define SMC_PSCI_FN_CPU_ON			SMC_PSCI_FN(3)
+#define SMC_PSCI_FN_MIGRATE			SMC_PSCI_FN(5)
+
+/* PSCI v0.2 interface */
+#define SMC_PSCI_FN_PSCI_VERSION		SMC_PSCI_FN(0)
+#define SMC_PSCI_FN_AFFINITY_INFO		SMC_PSCI_FN(4)
+#define SMC_PSCI_FN_MIGRATE_INFO_TYPE		SMC_PSCI_FN(6)
+#define SMC_PSCI_FN_MIGRATE_INFO_UP_CPU		SMC_PSCI_FN(7)
+#define SMC_PSCI_FN_SYSTEM_OFF			SMC_PSCI_FN(8)
+#define SMC_PSCI_FN_SYSTEM_RESET		SMC_PSCI_FN(9)
+
+/* PSCI v1.0 interface */
+#define SMC_PSCI_FN_FEATURES			SMC_PSCI_FN(10)
+#define SMC_PSCI_FN_SYSTEM_SUSPEND		SMC_PSCI_FN(14)
+
+/* PSCI return values (inclusive of all PSCI versions) */
+#define SMC_PSCI_RET_SUCCESS			0
+#define SMC_PSCI_RET_NOT_SUPPORTED		(-1)
+#define SMC_PSCI_RET_INVALID_PARAMS		(-2)
+#define SMC_PSCI_RET_DENIED			(-3)
+#define SMC_PSCI_RET_ALREADY_ON			(-4)
+#define SMC_PSCI_RET_ON_PENDING			(-5)
+#define SMC_PSCI_RET_INTERNAL_FAILURE		(-6)
+#define SMC_PSCI_RET_NOT_PRESENT		(-7)
+#define SMC_PSCI_RET_DISABLED			(-8)
+
+void handle_psci_standard_calls(struct thread_smc_args *args);
+
+void smc_psci_cpu_off(void) __noreturn;
+void smc_psci_cpu_on(struct thread_smc_args *args);
+void smc_psci_affinity_info(struct thread_smc_args *args);
+void smc_psci_system_off(void) __noreturn;
+void smc_psci_system_reset(void) __noreturn;
+void smc_psci_feature_query(struct thread_smc_args *args);
+
+#endif /*PSCI_H*/
diff --git a/core/arch/arm/plat-ls/main.c b/core/arch/arm/plat-ls/main.c
index 7588402..8b6f56b 100644
--- a/core/arch/arm/plat-ls/main.c
+++ b/core/arch/arm/plat-ls/main.c
@@ -30,19 +30,28 @@
 #include <console.h>
 #include <drivers/ns16550.h>
 #include <kernel/generic_boot.h>
-#include <kernel/thread.h>
 #include <kernel/panic.h>
 #include <kernel/pm_stubs.h>
 #include <mm/core_mmu.h>
 #include <tee/entry_std.h>
 #include <tee/entry_fast.h>
 #include <sm/sm.h>
+#include <sm/optee_smc.h>
+#include <sm/psci.h>
 
 static void main_fiq(void);
 
+#if defined(CFG_ARMV7_PSCI)
+static void entry_fast_smc(struct thread_smc_args *args);
+#endif
+
 static const struct thread_handlers handlers = {
 	.std_smc = tee_entry_std,
+#if defined(CFG_ARMV7_PSCI)
+	.fast_smc = entry_fast_smc,
+#else
 	.fast_smc = tee_entry_fast,
+#endif
 	.fiq = main_fiq,
 	.cpu_on = pm_panic,
 	.cpu_off = pm_panic,
@@ -67,6 +76,23 @@ static void main_fiq(void)
 	panic();
 }
 
+#if defined(CFG_ARMV7_PSCI)
+static void entry_fast_smc(struct thread_smc_args *args)
+{
+	switch (OPTEE_SMC_OWNER_NUM(args->a0)) {
+
+	/* Handle standard service fast calls */
+	case OPTEE_SMC_OWNER_STANDARD:
+		handle_psci_standard_calls(args);
+		break;
+	/* TEE specific FASTCALL handled by generic TEE function */
+	default:
+		tee_entry_fast(args);
+		break;
+	}
+}
+#endif
+
 void init_sec_mon(uint32_t nsec_entry)
 {
 	struct plat_nsec_ctx *plat_ctx = (struct plat_nsec_ctx *)nsec_entry;
diff --git a/core/arch/arm/sm/psci.c b/core/arch/arm/sm/psci.c
new file mode 100644
index 0000000..ba9d4a0
--- /dev/null
+++ b/core/arch/arm/sm/psci.c
@@ -0,0 +1,110 @@
+/*
+ * Copyright (c) 2016, Freescale Semiconductor, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <sm/psci.h>
+#include <trace.h>
+#include <kernel/panic.h>
+
+void __weak __noreturn smc_psci_cpu_off(void)
+{
+	IMSG("SMC PSCI cpu off platform specific implementation required\n");
+	while(1);
+}
+
+void __weak smc_psci_cpu_on(struct thread_smc_args *args)
+{
+	IMSG("SMC PSCI cpu on platform specific implementation required\n");
+	args->a0 = SMC_PSCI_RET_NOT_SUPPORTED;
+}
+
+void __weak smc_psci_affinity_info(struct thread_smc_args *args)
+{
+	IMSG("SMC PSCI affinity info platform specific implementation required\n");
+	args->a0 = SMC_PSCI_RET_NOT_SUPPORTED;
+}
+
+void __weak __noreturn smc_psci_system_off(void)
+{
+	IMSG("SMC PSCI system off platform specific implementation required\n");
+	while(1);
+}
+
+void __weak __noreturn smc_psci_system_reset(void)
+{
+	IMSG("SMC PSCI system reset platform specific implementation required\n");
+	while(1);
+}
+
+void __weak smc_psci_feature_query(struct thread_smc_args *args)
+{
+	args->a0 = SMC_PSCI_RET_NOT_SUPPORTED;
+}
+
+void handle_psci_standard_calls(struct thread_smc_args *args)
+{
+	switch (args->a0) {
+	case SMC_PSCI_FN_CPU_SUSPEND:
+		args->a0 = SMC_PSCI_RET_NOT_SUPPORTED;
+		break;
+	case SMC_PSCI_FN_CPU_OFF:
+		smc_psci_cpu_off();
+		break;
+	case SMC_PSCI_FN_CPU_ON:
+		smc_psci_cpu_on(args);
+		args->a0 = SMC_PSCI_RET_SUCCESS;
+		break;
+	case SMC_PSCI_FN_MIGRATE:
+		args->a0 = SMC_PSCI_RET_NOT_SUPPORTED;
+		break;
+	case SMC_PSCI_FN_PSCI_VERSION:
+		args->a0 = SMC_PSCI_VERSION;
+		break;
+	case SMC_PSCI_FN_AFFINITY_INFO:
+		smc_psci_affinity_info(args);
+		break;
+	case SMC_PSCI_FN_MIGRATE_INFO_TYPE:
+		args->a0 = SMC_PSCI_RET_NOT_SUPPORTED;
+		break;
+	case SMC_PSCI_FN_MIGRATE_INFO_UP_CPU:
+		args->a0 = SMC_PSCI_RET_NOT_SUPPORTED;
+		break;
+	case SMC_PSCI_FN_SYSTEM_OFF:
+		smc_psci_system_off();
+		break;
+	case SMC_PSCI_FN_SYSTEM_RESET:
+		smc_psci_system_reset();
+		break;
+	case SMC_PSCI_FN_FEATURES:
+		break;
+	case SMC_PSCI_FN_SYSTEM_SUSPEND:
+		args->a0 = SMC_PSCI_RET_NOT_SUPPORTED;
+		break;
+	default:
+		args->a0 = SMC_PSCI_RET_NOT_SUPPORTED;
+		break;
+	}
+}
diff --git a/core/arch/arm/sm/sub.mk b/core/arch/arm/sm/sub.mk
index 685739e..fdb70a1 100644
--- a/core/arch/arm/sm/sub.mk
+++ b/core/arch/arm/sm/sub.mk
@@ -1,2 +1,3 @@
 srcs-y += sm_a32.S
 srcs-y += sm.c
+srcs-$(CFG_ARMV7_PSCI) += psci.c
-- 
1.8.1.4

