From d1b8743b377b151eab9d4a79b54f2e48a217b9dc Mon Sep 17 00:00:00 2001
From: Peng Fan <peng.fan@nxp.com>
Date: Tue, 1 Mar 2022 15:54:13 +0800
Subject: [PATCH] drivers: console: add jailhouse debug console

Support jailhouse hypervisor debug console

Signed-off-by: Peng Fan <peng.fan@nxp.com>
---
 CODEOWNERS                                |  1 +
 drivers/console/CMakeLists.txt            |  1 +
 drivers/console/Kconfig                   |  9 ++++
 drivers/console/jailhouse_debug_console.c | 58 +++++++++++++++++++++++
 4 files changed, 69 insertions(+)
 create mode 100644 drivers/console/jailhouse_debug_console.c

diff --git a/CODEOWNERS b/CODEOWNERS
index f705eaf32cb..7d72cf85dde 100644
--- a/CODEOWNERS
+++ b/CODEOWNERS
@@ -230,6 +230,7 @@
 /drivers/counter/                         @nordic-krch
 /drivers/console/ipm_console.c            @finikorg
 /drivers/console/semihost_console.c       @luozhongyao
+/drivers/console/jailhouse_debug_console.c @MrVan
 /drivers/counter/counter_cmos.c           @dcpleung
 /drivers/counter/counter_ll_stm32_timer.c @kentjhall
 /drivers/crypto/*nrf_ecb*                 @maciekfabia @anangl
diff --git a/drivers/console/CMakeLists.txt b/drivers/console/CMakeLists.txt
index 1741831d978..e1242bf5e95 100644
--- a/drivers/console/CMakeLists.txt
+++ b/drivers/console/CMakeLists.txt
@@ -5,6 +5,7 @@ zephyr_library_sources_ifdef(CONFIG_GSM_MUX gsm_mux.c)
 zephyr_library_sources_ifdef(CONFIG_IPM_CONSOLE_RECEIVER ipm_console_receiver.c)
 zephyr_library_sources_ifdef(CONFIG_IPM_CONSOLE_SENDER ipm_console_sender.c)
 zephyr_library_sources_ifdef(CONFIG_IPM_CONSOLE ipm_console.c)
+zephyr_library_sources_ifdef(CONFIG_JAILHOUSE_DEBUG_CONSOLE jailhouse_debug_console.c)
 zephyr_library_sources_ifdef(CONFIG_NATIVE_POSIX_CONSOLE native_posix_console.c)
 zephyr_library_sources_ifdef(CONFIG_RAM_CONSOLE ram_console.c)
 zephyr_library_sources_ifdef(CONFIG_RTT_CONSOLE rtt_console.c)
diff --git a/drivers/console/Kconfig b/drivers/console/Kconfig
index 8f0b66169fe..33b8ab8e7f9 100644
--- a/drivers/console/Kconfig
+++ b/drivers/console/Kconfig
@@ -87,6 +87,15 @@ config UART_CONSOLE_INPUT_EXPIRED_TIMEOUT
 	  Fixed amount of time which unit is milliseconds to keep the UART
 	  console in use flag true.
 
+config JAILHOUSE_DEBUG_CONSOLE
+	bool "Use JAILHOUSE_DEBUG console"
+	select CONSOLE_HAS_DRIVER
+	depends on ARM64
+	help
+	  Emit console messages to a jailhouse hypervisor debug console.
+	  Useful in board bring-up if there aren't any working serial
+	  drivers.
+
 config RAM_CONSOLE
 	bool "Use RAM console"
 	select CONSOLE_HAS_DRIVER
diff --git a/drivers/console/jailhouse_debug_console.c b/drivers/console/jailhouse_debug_console.c
new file mode 100644
index 00000000000..29d6a61d09e
--- /dev/null
+++ b/drivers/console/jailhouse_debug_console.c
@@ -0,0 +1,58 @@
+/*
+ * Copyright 2022 NXP
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include <kernel.h>
+#include <sys/printk.h>
+#include <device.h>
+#include <init.h>
+
+#if defined(CONFIG_PRINTK) || defined(CONFIG_STDOUT_CONSOLE)
+/**
+ * @brief Output one character to SIMULATOR console
+ * @param c Character to output
+ * @return The character passed as input.
+ */
+static int console_out(int c)
+{
+	register unsigned long x0 __asm__("x0") = 8;
+	register unsigned long x1 __asm__("x1") = c;
+
+	__asm__ volatile ("hvc #0x4a48\r\n"
+			  : "+r" (x0), "+r" (x1) : : );
+	return c;
+}
+#endif
+
+#if defined(CONFIG_STDOUT_CONSOLE)
+extern void __stdout_hook_install(int (*hook)(int));
+#else
+#define __stdout_hook_install(x)		\
+	do {/* nothing */			\
+	} while ((0))
+#endif
+
+#if defined(CONFIG_PRINTK)
+extern void __printk_hook_install(int (*fn)(int));
+#else
+#define __printk_hook_install(x)		\
+	do {/* nothing */			\
+	} while ((0))
+#endif
+
+/**
+ * @brief Initialize the console/debug port
+ * @return 0 if successful, otherwise failed.
+ */
+static int jailhouse_console_init(const struct device *arg)
+{
+	ARG_UNUSED(arg);
+	__stdout_hook_install(console_out);
+	__printk_hook_install(console_out);
+	return 0;
+}
+
+SYS_INIT(jailhouse_console_init,
+	 PRE_KERNEL_1,
+	 CONFIG_KERNEL_INIT_PRIORITY_DEFAULT);