[PATCH kvm-unit-tests 2/2] arm/arm64: psci: support testing tcg

[Andrew Jones <drjones@redhat.com>]

Signed-off-by: Andrew Jones <drjones@redhat.com>
---
 arm/psci.c | 34 +++++++++++++++++++++++++++++++++-
 1 file changed, 33 insertions(+), 1 deletion(-)

diff --git a/arm/psci.c b/arm/psci.c
index e571afb92c7e..3c5f43877fde 100644
--- a/arm/psci.c
+++ b/arm/psci.c
@@ -8,12 +8,44 @@
  * This work is licensed under the terms of the GNU LGPL, version 2.
  */
 #include <libcflat.h>
+#include <asm/processor.h>
 #include <asm/smp.h>
 #include <asm/psci.h>
 
+static bool invalid_function_exception;
+
+#ifdef __arm__
+static void invalid_function_handler(struct pt_regs *regs __unused)
+{
+	invalid_function_exception = true;
+}
+#else
+static void invalid_function_handler(struct pt_regs *regs, unsigned int esr __unused)
+{
+	invalid_function_exception = true;
+	regs->pc += 4;
+}
+#endif
+
+static void install_invalid_function_handler(exception_fn handler)
+{
+#ifdef __arm__
+	install_exception_handler(EXCPTN_UND, handler);
+#else
+	install_exception_handler(EL1H_SYNC, ESR_EL1_EC_UNKNOWN, handler);
+#endif
+}
+
 static bool psci_invalid_function(void)
 {
-	return psci_invoke(1337, 0, 0, 0) == PSCI_RET_NOT_SUPPORTED;
+	bool pass;
+
+	install_invalid_function_handler(invalid_function_handler);
+
+	pass = psci_invoke(1337, 0, 0, 0) == PSCI_RET_NOT_SUPPORTED || invalid_function_exception;
+
+	install_invalid_function_handler(NULL);
+	return pass;
 }
 
 static int psci_affinity_info(unsigned long target_affinity, uint32_t lowest_affinity_level)
-- 
2.9.3