[PATCH kvm-unit-tests 8/8] arm/psci: add smccc 1.1 tests

[Andrew Jones <drjones@xxxxxxxxxx>]

Signed-off-by: Andrew Jones <drjones@xxxxxxxxxx>
---
 arm/psci.c | 69 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++--
 1 file changed, 67 insertions(+), 2 deletions(-)

diff --git a/arm/psci.c b/arm/psci.c
index a092f00c2cab..af96a82cedd0 100644
--- a/arm/psci.c
+++ b/arm/psci.c
@@ -14,9 +14,14 @@
 #include <asm/psci.h>
 #include <asm/delay.h>
 
-static cpumask_t halted;
+#define ARM_SMCCC_VERSION_FUNC_ID		((1U << 31) | 0x0000)
+#define ARM_SMCCC_ARCH_FEATURES_FUNC_ID		((1U << 31) | 0x0001)
+#define ARM_SMCCC_ARCH_WORKAROUND_1		((1U << 31) | 0x8000)
 
+static int psci_version;
+static cpumask_t halted;
 static bool invalid_function_exception;
+static bool have_workaround1;
 
 #ifdef __arm__
 static void invalid_function_handler(struct pt_regs *regs __unused)
@@ -115,12 +120,60 @@ static bool psci_cpu_on_test(void)
 	return !failed;
 }
 
+static void workaround1_test(void)
+{
+#ifdef __aarch64__
+	unsigned long ret;
+
+	if (!have_workaround1)
+		return;
+
+	asm volatile(
+	"	mov	w0, " xstr(ARM_SMCCC_ARCH_WORKAROUND_1) "\n"
+	"	hvc	#0\n"
+	"	mov	%0, x0\n"
+	: "=r" (ret) : : "x0");
+
+	report("workaround1_test", ret == 0);
+#endif
+}
+
+static void smccc11_test(void)
+{
+	int ret;
+
+	ret = psci_invoke(PSCI_1_0_FN_PSCI_FEATURES, PSCI_1_0_FN_PSCI_FEATURES, 0, 0);
+	if (ret != 0) {
+		ret = 0;
+		goto report_version;
+	}
+
+	ret = psci_invoke(PSCI_1_0_FN_PSCI_FEATURES, ARM_SMCCC_VERSION_FUNC_ID, 0, 0);
+	if (ret != 0) {
+		ret = 0;
+		goto report_version;
+	}
+
+	ret = psci_invoke(ARM_SMCCC_VERSION_FUNC_ID, 0, 0, 0);
+	report_info("SMCCC version %d.%d", PSCI_VERSION_MAJOR(ret), PSCI_VERSION_MINOR(ret));
+	ret = PSCI_VERSION_MAJOR(ret) >= 1 && PSCI_VERSION_MINOR(ret) >= 1;
+
+report_version:
+	report("smccc: version >= 1.1", ret);
+	have_workaround1 = ret && psci_invoke(ARM_SMCCC_ARCH_FEATURES_FUNC_ID, ARM_SMCCC_ARCH_WORKAROUND_1, 0, 0);
+}
+
 static void general_check(void)
 {
 	report("invalid-function", psci_invalid_function());
 	report("affinity-info-on", psci_affinity_info(0) == PSCI_0_2_AFFINITY_LEVEL_ON);
 	report("affinity-info-off", psci_affinity_info(1) == PSCI_0_2_AFFINITY_LEVEL_OFF);
 
+	if (PSCI_VERSION_MAJOR(psci_version) >= 1)
+		smccc11_test();
+
+	workaround1_test();
+
 	if (ERRATA(6c7a5dce22b3))
 		report("cpu-on", psci_cpu_on_test());
 	else
@@ -159,6 +212,11 @@ static void migrate_prepare(void)
 		cpu_relax();
 	assert(psci_affinity_info(2) == PSCI_0_2_AFFINITY_LEVEL_ON);
 
+	if (PSCI_VERSION_MAJOR(psci_version) >= 1)
+		smccc11_test();
+
+	workaround1_test();
+
 	mdelay(500);
 	do_migration();
 	mdelay(500);
@@ -170,6 +228,13 @@ static void migrate_check(void)
 	report("CPU1 stopped", psci_affinity_info(1) == PSCI_0_2_AFFINITY_LEVEL_OFF);
 	report("CPU2 not stopped", psci_affinity_info(2) == PSCI_0_2_AFFINITY_LEVEL_ON);
 	report("CPU2 still halted", cpumask_test_cpu(2, &halted));
+
+	int version = psci_invoke(PSCI_0_2_FN_PSCI_VERSION, 0, 0, 0);
+	report_info("PSCI version %d.%d", PSCI_VERSION_MAJOR(version),
+					  PSCI_VERSION_MINOR(version));
+	if (PSCI_VERSION_MAJOR(psci_version) >= 1 || PSCI_VERSION_MAJOR(version) >= 1)
+		smccc11_test();
+	workaround1_test();
 }
 
 static struct test {
@@ -204,9 +269,9 @@ static struct test *get_test(const char *name)
 
 int main(int ac, char **av)
 {
-	int psci_version = psci_invoke(PSCI_0_2_FN_PSCI_VERSION, 0, 0, 0);
 	struct test *test = get_test("general");
 
+	psci_version = psci_invoke(PSCI_0_2_FN_PSCI_VERSION, 0, 0, 0);
 	report_info("PSCI version %d.%d", PSCI_VERSION_MAJOR(psci_version),
 					  PSCI_VERSION_MINOR(psci_version));
 
-- 
2.13.6