qemu: Implement qemu_system_off via semihosting.
This makes the PSCI SYSTEM_OFF call work on QEMU. It assumes that QEMU has
semihosting enabled, but that is already assumed by the image loader.
Signed-off-by: Andrew Walbran <qwandor@google.com>
Change-Id: I0fb7cf7909262b675c3143efeac07f4d60730b03
diff --git a/include/lib/semihosting.h b/include/lib/semihosting.h
index 006c7b7..24b030c 100644
--- a/include/lib/semihosting.h
+++ b/include/lib/semihosting.h
@@ -23,6 +23,7 @@
#define SEMIHOSTING_SYS_REMOVE 0x0E
#define SEMIHOSTING_SYS_SYSTEM 0x12
#define SEMIHOSTING_SYS_ERRNO 0x13
+#define SEMIHOSTING_SYS_EXIT 0x18
#define FOPEN_MODE_R 0x0
#define FOPEN_MODE_RB 0x1
@@ -54,5 +55,6 @@
void semihosting_write_char(char character);
void semihosting_write_string(char *string);
char semihosting_read_char(void);
+void semihosting_exit(uint32_t reason, uint32_t subcode);
#endif /* SEMIHOSTING_H */
diff --git a/lib/semihosting/semihosting.c b/lib/semihosting/semihosting.c
index 051dd00..60fc52a 100644
--- a/lib/semihosting/semihosting.c
+++ b/lib/semihosting/semihosting.c
@@ -15,7 +15,7 @@
#endif
long semihosting_call(unsigned long operation,
- void *system_block_address);
+ uintptr_t system_block_address);
typedef struct {
const char *file_name;
@@ -53,7 +53,7 @@
open_block.name_length = strlen(file_name);
return semihosting_call(SEMIHOSTING_SYS_OPEN,
- (void *) &open_block);
+ (uintptr_t) &open_block);
}
long semihosting_file_seek(long file_handle, ssize_t offset)
@@ -65,7 +65,7 @@
seek_block.location = offset;
result = semihosting_call(SEMIHOSTING_SYS_SEEK,
- (void *) &seek_block);
+ (uintptr_t) &seek_block);
if (result)
result = semihosting_call(SEMIHOSTING_SYS_ERRNO, 0);
@@ -86,7 +86,7 @@
read_block.length = *length;
result = semihosting_call(SEMIHOSTING_SYS_READ,
- (void *) &read_block);
+ (uintptr_t) &read_block);
if (result == *length) {
return -EINVAL;
@@ -112,7 +112,7 @@
write_block.length = *length;
result = semihosting_call(SEMIHOSTING_SYS_WRITE,
- (void *) &write_block);
+ (uintptr_t) &write_block);
*length = result;
@@ -122,28 +122,28 @@
long semihosting_file_close(long file_handle)
{
return semihosting_call(SEMIHOSTING_SYS_CLOSE,
- (void *) &file_handle);
+ (uintptr_t) &file_handle);
}
long semihosting_file_length(long file_handle)
{
return semihosting_call(SEMIHOSTING_SYS_FLEN,
- (void *) &file_handle);
+ (uintptr_t) &file_handle);
}
char semihosting_read_char(void)
{
- return semihosting_call(SEMIHOSTING_SYS_READC, NULL);
+ return semihosting_call(SEMIHOSTING_SYS_READC, 0);
}
void semihosting_write_char(char character)
{
- semihosting_call(SEMIHOSTING_SYS_WRITEC, (void *) &character);
+ semihosting_call(SEMIHOSTING_SYS_WRITEC, (uintptr_t) &character);
}
void semihosting_write_string(char *string)
{
- semihosting_call(SEMIHOSTING_SYS_WRITE0, (void *) string);
+ semihosting_call(SEMIHOSTING_SYS_WRITE0, (uintptr_t) string);
}
long semihosting_system(char *command_line)
@@ -154,7 +154,7 @@
system_block.command_length = strlen(command_line);
return semihosting_call(SEMIHOSTING_SYS_SYSTEM,
- (void *) &system_block);
+ (uintptr_t) &system_block);
}
long semihosting_get_flen(const char *file_name)
@@ -216,3 +216,15 @@
semihosting_file_close(file_handle);
return ret;
}
+
+void semihosting_exit(uint32_t reason, uint32_t subcode)
+{
+#ifdef __aarch64__
+ uint64_t parameters[] = {reason, subcode};
+
+ (void) semihosting_call(SEMIHOSTING_SYS_EXIT, (uintptr_t) ¶meters);
+#else
+ /* The subcode is not supported on AArch32. */
+ (void) semihosting_call(SEMIHOSTING_SYS_EXIT, reason);
+#endif
+}
diff --git a/plat/qemu/common/qemu_pm.c b/plat/qemu/common/qemu_pm.c
index a199688..116211c 100644
--- a/plat/qemu/common/qemu_pm.c
+++ b/plat/qemu/common/qemu_pm.c
@@ -10,10 +10,13 @@
#include <arch_helpers.h>
#include <common/debug.h>
#include <lib/psci/psci.h>
+#include <lib/semihosting.h>
#include <plat/common/platform.h>
#include "qemu_private.h"
+#define ADP_STOPPED_APPLICATION_EXIT 0x20026
+
/*
* The secure entry point to be used on warm reset.
*/
@@ -191,7 +194,8 @@
******************************************************************************/
static void __dead2 qemu_system_off(void)
{
- ERROR("QEMU System Off: operation not handled.\n");
+ semihosting_exit(ADP_STOPPED_APPLICATION_EXIT, 0);
+ ERROR("QEMU System Off: semihosting call unexpectedly returned.\n");
panic();
}
diff --git a/plat/qemu/qemu/platform.mk b/plat/qemu/qemu/platform.mk
index eaeb72c..b95bf5a 100644
--- a/plat/qemu/qemu/platform.mk
+++ b/plat/qemu/qemu/platform.mk
@@ -151,6 +151,8 @@
BL31_SOURCES += lib/cpus/aarch64/aem_generic.S \
lib/cpus/aarch64/cortex_a53.S \
lib/cpus/aarch64/cortex_a57.S \
+ lib/semihosting/semihosting.c \
+ lib/semihosting/${ARCH}/semihosting_call.S \
plat/common/plat_psci_common.c \
${PLAT_QEMU_COMMON_PATH}/qemu_pm.c \
${PLAT_QEMU_COMMON_PATH}/topology.c \
diff --git a/plat/qemu/qemu_sbsa/platform.mk b/plat/qemu/qemu_sbsa/platform.mk
index f34c7e1..51832d0 100644
--- a/plat/qemu/qemu_sbsa/platform.mk
+++ b/plat/qemu/qemu_sbsa/platform.mk
@@ -71,6 +71,8 @@
BL31_SOURCES += lib/cpus/aarch64/aem_generic.S \
lib/cpus/aarch64/cortex_a53.S \
lib/cpus/aarch64/cortex_a57.S \
+ lib/semihosting/semihosting.c \
+ lib/semihosting/${ARCH}/semihosting_call.S \
plat/common/plat_psci_common.c \
${PLAT_QEMU_COMMON_PATH}/qemu_pm.c \
${PLAT_QEMU_COMMON_PATH}/topology.c \