core: sm: support SMCCC v1.1 specification

SMCCC v1.1 specification: support defined function IDs with weak
handlers platform can override, as other PSCI function handler.
We could state we support v1.2 but Linux kernel v5.7-rc1 expects
strict v1.1 support.

unsigned long arm_arch_version(void);
    returns SMCCC_V_1_1

unsigned long arm_arch_feature(unsigned long a1);
    default supports version only

unsigned long arm_arch_soc_id(void);
unsigned long arm_arch_workaround_1(void);
unsigned long arm_arch_workaround_2(void);
    default return ARM_SMCCC_RET_NOT_SUPPORTED

This helper is needed by Linux kernel (U-Boot) drivers that rely on
arm_smccc_v1_1() supports.

Signed-off-by: Etienne Carriere <etienne.carriere@linaro.org>
Reviewed-by: Jens Wiklander <jens.wiklander@linaro.org>
Acked-by: Jerome Forissier <jerome@forissier.org>

2 files changed, 64 insertions, 6 deletions

diff --git a/core/arch/arm/include/sm/std_smc.h b/core/arch/arm/include/sm/std_smc.h
index c3d738c8..ce8606dc 100644
--- a/core/arch/arm/include/sm/std_smc.h
+++ b/core/arch/arm/include/sm/std_smc.h
@@ -10,6 +10,21 @@
 /*                                      0x8400ff02 is reserved */
 #define ARM_STD_SVC_VERSION		0x8400ff03
 
+#define ARM_SMCCC_VERSION		0x80000000
+#define ARM_SMCCC_ARCH_FEATURES		0x80000001
+#define ARM_SMCCC_ARCH_SOC_ID		0x80000002
+#define ARM_SMCCC_ARCH_WORKAROUND_1	0x80008000
+#define ARM_SMCCC_ARCH_WORKAROUND_2	0x80007fff
+
+#define ARM_SMCCC_RET_SUCCESS		0
+#define ARM_SMCCC_RET_NOT_SUPPORTED	0xffffffff
+#define ARM_SMCCC_RET_NOT_REQUIRED	0xfffffffe
+#define ARM_SMCCC_RET_INVALID_PARAMETER	0xfffffffd
+
+#define SMCCC_V_1_0			0x10000
+#define SMCCC_V_1_1			0x10001
+#define SMCCC_V_1_2			0x10002
+
 /* ARM Standard Service Calls version numbers */
 #define STD_SVC_VERSION_MAJOR		0x0
 #define STD_SVC_VERSION_MINOR		0x1
diff --git a/core/arch/arm/sm/sm.c b/core/arch/arm/sm/sm.c
index 15f77eb2..a668d15e 100644
--- a/core/arch/arm/sm/sm.c
+++ b/core/arch/arm/sm/sm.c
@@ -1,11 +1,13 @@
 // SPDX-License-Identifier: BSD-2-Clause
 /*
- * Copyright (c) 2016, Linaro Limited
+ * Copyright (c) 2016-2020, Linaro Limited
  * Copyright (c) 2014, STMicroelectronics International N.V.
  */
 #include <arm.h>
 #include <compiler.h>
+#include <config.h>
 #include <kernel/misc.h>
+#include <kernel/thread.h>
 #include <platform_config.h>
 #include <sm/optee_smc.h>
 #include <sm/sm.h>
@@ -13,9 +15,43 @@
 #include <string.h>
 #include "sm_private.h"
 
+static void smc_arch_handler(struct thread_smc_args *args)
+{
+	uint32_t smc_fid = args->a0;
+	uint32_t feature_fid = args->a1;
+
+	switch (smc_fid) {
+	case ARM_SMCCC_VERSION:
+		args->a0 = SMCCC_V_1_1;
+		break;
+	case ARM_SMCCC_ARCH_FEATURES:
+		switch (feature_fid) {
+		case ARM_SMCCC_VERSION:
+		case ARM_SMCCC_ARCH_SOC_ID:
+			args->a0 = ARM_SMCCC_RET_SUCCESS;
+			break;
+		default:
+			args->a0 = ARM_SMCCC_RET_NOT_SUPPORTED;
+			break;
+		}
+		break;
+	case ARM_SMCCC_ARCH_SOC_ID:
+		args->a0 = ARM_SMCCC_RET_NOT_SUPPORTED;
+		break;
+	case ARM_SMCCC_ARCH_WORKAROUND_1:
+	case ARM_SMCCC_ARCH_WORKAROUND_2:
+		args->a0 = ARM_SMCCC_RET_NOT_REQUIRED;
+		break;
+	default:
+		args->a0 = OPTEE_SMC_RETURN_UNKNOWN_FUNCTION;
+		break;
+	}
+}
+
 uint32_t sm_from_nsec(struct sm_ctx *ctx)
 {
 	uint32_t *nsec_r0 = (uint32_t *)(&ctx->nsec.r0);
+	struct thread_smc_args *args = (struct thread_smc_args *)nsec_r0;
 
 	/*
 	 * Check that struct sm_ctx has the different parts properly
@@ -31,17 +67,24 @@ uint32_t sm_from_nsec(struct sm_ctx *ctx)
 		return SM_EXIT_TO_NON_SECURE;
 #endif
 
-#ifdef CFG_PSCI_ARM32
-	if (OPTEE_SMC_OWNER_NUM(*nsec_r0) == OPTEE_SMC_OWNER_STANDARD) {
-		smc_std_handler((struct thread_smc_args *)nsec_r0, &ctx->nsec);
+	switch (OPTEE_SMC_OWNER_NUM(args->a0)) {
+	case OPTEE_SMC_OWNER_STANDARD:
+		if (IS_ENABLED(CFG_PSCI_ARM32)) {
+			smc_std_handler(args, &ctx->nsec);
+			return SM_EXIT_TO_NON_SECURE;
+		}
+		break;
+	case OPTEE_SMC_OWNER_ARCH:
+		smc_arch_handler(args);
 		return SM_EXIT_TO_NON_SECURE;
+	default:
+		break;
 	}
-#endif
 
 	sm_save_unbanked_regs(&ctx->nsec.ub_regs);
 	sm_restore_unbanked_regs(&ctx->sec.ub_regs);
 
-	memcpy(&ctx->sec.r0, nsec_r0, sizeof(uint32_t) * 8);
+	memcpy(&ctx->sec.r0, args, sizeof(*args));
 	if (OPTEE_SMC_IS_FAST_CALL(ctx->sec.r0))
 		ctx->sec.mon_lr = (uint32_t)&thread_vector_table.fast_smc_entry;
 	else