[DEBUG] KVM: arm64: Add debug UART hacks at EL2

Provide a standalone mechanism for debug UART access at EL2. It's a
debug hack, so please try not to get too offended by it.

[ qperret: re-map UART at EL2 in protected mode ]

Signed-off-by: Will Deacon <will@kernel.org>
Signed-off-by: Quentin Perret <qperret@google.com>
diff --git a/arch/arm64/kernel/image-vars.h b/arch/arm64/kernel/image-vars.h
index c96a9a0..4291feb 100644
--- a/arch/arm64/kernel/image-vars.h
+++ b/arch/arm64/kernel/image-vars.h
@@ -134,6 +134,10 @@
 /* pKVM static key */
 KVM_NVHE_ALIAS(kvm_protected_mode_initialized);
 
+#ifdef CONFIG_KVM_ARM_HYP_DEBUG_UART
+KVM_NVHE_ALIAS(kvm_hyp_debug_uart_set_basep);
+#endif
+
 #endif /* CONFIG_KVM */
 
 #endif /* __ARM64_KERNEL_IMAGE_VARS_H */
diff --git a/arch/arm64/kvm/Kconfig b/arch/arm64/kvm/Kconfig
index 7634c1e..4d6952d 100644
--- a/arch/arm64/kvm/Kconfig
+++ b/arch/arm64/kvm/Kconfig
@@ -58,6 +58,14 @@
 
 	  If unsure, say N.
 
+config KVM_ARM_HYP_DEBUG_UART
+	bool "Hack up some basic UART functionality at EL2"
+
+config KVM_ARM_HYP_DEBUG_UART_ADDR
+	hex "Physical address of the PL011 for EL2 to use"
+	depends on KVM_ARM_HYP_DEBUG_UART
+	default 0x09000000
+
 endif # KVM
 
 endif # VIRTUALIZATION
diff --git a/arch/arm64/kvm/hyp/debug-pl011.h b/arch/arm64/kvm/hyp/debug-pl011.h
new file mode 100644
index 0000000..c76108a
--- /dev/null
+++ b/arch/arm64/kvm/hyp/debug-pl011.h
@@ -0,0 +1,227 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * Stand-alone header for basic debug output on the PL011 UART.  To use it,
+ * ensure that CONFIG_KVM_ARM_HYP_DEBUG_UART is enabled and that
+ * CONFIG_KVM_ARM_HYP_DEBUG_UART_ADDR is the physical address of the PL011
+ * UART that you want to use. Then just include this header and try not to
+ * vomit at the state of the macros and functions it provides.
+ *
+ * The C functions only work when the MMU is enabled, but the assembly macros
+ * should work pretty much everywhere.
+ *
+ * It's slow and racy, but you'll be fine. Patches unwelcome.
+ */
+
+#ifndef __ARM64_KVM_HYP_DEBUG_PL011_H__
+#define __ARM64_KVM_HYP_DEBUG_PL011_H__
+
+#ifdef CONFIG_KVM_ARM_HYP_DEBUG_UART
+
+#define HYP_PL011_BASE_PHYS	CONFIG_KVM_ARM_HYP_DEBUG_UART_ADDR
+#define HYP_PL011_UARTFR	0x18
+
+#define HYP_PL011_UARTFR_BUSY	3
+#define HYP_PL011_UARTFR_FULL	5
+
+#ifdef __ASSEMBLY__
+
+.macro hyp_pl011_base, tmp
+	mrs		\tmp, sctlr_el2
+	tbz		\tmp, #0, 9990f
+	isb
+
+alternative_cb kvm_hyp_debug_uart_set_basep
+	movz		\tmp, #0
+	movk		\tmp, #0, lsl #16
+	movk		\tmp, #0, lsl #32
+	movk		\tmp, #0, lsl #48
+alternative_cb_end
+
+	kern_hyp_va	\tmp
+	ldr		\tmp, [\tmp]
+	b		9991f
+9990:	mov		\tmp, HYP_PL011_BASE_PHYS
+9991:
+.endm
+
+/*
+ * 'c' is a W register containing the character to transmit. Preserved.
+ * 'tmpnr' is the number of another scratch register. Clobbered.
+ */
+.macro hyp_putc, c, tmpnr
+9992:	hyp_pl011_base	x\tmpnr
+	ldr		w\tmpnr, [x\tmpnr, HYP_PL011_UARTFR]
+	tbnz		w\tmpnr, HYP_PL011_UARTFR_FULL, 9992b
+	hyp_pl011_base	x\tmpnr
+	str		\c, [x\tmpnr]
+9992:	hyp_pl011_base	x\tmpnr
+	ldr		w\tmpnr, [x\tmpnr, HYP_PL011_UARTFR]
+	tbnz		w\tmpnr, HYP_PL011_UARTFR_BUSY, 9992b
+.endm
+
+/*
+ * 's' is an X register containing the address of the string to print.
+ * 'tmpnr1' and  'tmpnr2' are numbers of other scratch registers.
+ * All three registers clobbered.
+ *
+ * The string must be mapped, so it's best to use a combination of '.ascii'
+ * and PC-relative addressing (i.e. an ADR instruction)
+ */
+.macro hyp_puts, s, tmpnr1, tmpnr2
+9993:	ldrb		w\tmpnr1, [\s]
+	cbz		w\tmpnr1, 9993f
+	hyp_putc	w\tmpnr1, \tmpnr2
+	add		\s, \s, #1
+	b		9993b
+9993:	mov		w\tmpnr1, '\n'
+	hyp_putc	w\tmpnr1, \tmpnr2
+.endm
+
+.macro __hyp_putx4, xnr, tmpnr
+	bic		x\xnr, x\xnr, #0xfffffff0
+	sub		w\tmpnr, w\xnr, #10
+	tbnz		w\tmpnr, #31, 9994f
+	add		x\xnr, x\xnr, #0x27
+9994:	add		x\xnr, x\xnr, #0x30
+	hyp_putc	w\xnr, \tmpnr
+.endm
+
+/*
+ * 'x' is an X register containing a value to printed in hex. Preserved.
+ * 'tmpnr1' and  'tmpnr2' are numbers of other scratch registers. Clobbered.
+ */
+.macro hyp_putx64, x, tmpnr1, tmpnr2
+	mov		w\tmpnr1, '0'
+	hyp_putc	w\tmpnr1, \tmpnr2
+	mov		w\tmpnr1, 'x'
+	hyp_putc	w\tmpnr1, \tmpnr2
+	movz		x\tmpnr1, #15, lsl #32
+9995:	bfxil		x\tmpnr1, \x, #60, #4
+	ror		\x, \x, #60
+	__hyp_putx4	\tmpnr1, \tmpnr2
+	ror		x\tmpnr1, x\tmpnr1, #32
+	cbz		w\tmpnr1, 9995f
+	sub		x\tmpnr1, x\tmpnr1, #1
+	ror		x\tmpnr1, x\tmpnr1, #32
+	b		9995b
+9995:	mov		w\tmpnr1, '\n'
+	hyp_putc	w\tmpnr1, \tmpnr2
+.endm
+
+#else
+
+static inline void *__hyp_pl011_base(void)
+{
+	unsigned long ioaddr;
+
+	asm volatile(ALTERNATIVE_CB(
+		"movz	%0, #0\n"
+		"movk	%0, #0, lsl #16\n"
+		"movk	%0, #0, lsl #32\n"
+		"movk	%0, #0, lsl #48",
+		kvm_hyp_debug_uart_set_basep)
+		: "=r" (ioaddr));
+
+	return *((void **)kern_hyp_va(ioaddr));
+}
+
+static inline unsigned int __hyp_readw(void *ioaddr)
+{
+	unsigned int val;
+	asm volatile("ldr %w0, [%1]" : "=r" (val) : "r" (ioaddr));
+	return val;
+}
+
+static inline void __hyp_writew(unsigned int val, void *ioaddr)
+{
+	asm volatile("str %w0, [%1]" : : "r" (val), "r" (ioaddr));
+}
+
+static inline void hyp_putc(char c)
+{
+	unsigned int val;
+	void *base = __hyp_pl011_base();
+
+	do {
+		val = __hyp_readw(base + HYP_PL011_UARTFR);
+	} while (val & (1U << HYP_PL011_UARTFR_FULL));
+
+	__hyp_writew(c, base);
+
+	do {
+		val = __hyp_readw(base + HYP_PL011_UARTFR);
+	} while (val & (1U << HYP_PL011_UARTFR_BUSY));
+}
+
+/*
+ * Caller needs to ensure string is mapped. If it lives in .rodata, you should
+ * be good as long as we're using PC-relative addressing (probably true).
+ */
+static inline void hyp_puts(char *s)
+{
+	while (*s)
+		hyp_putc(*s++);
+	hyp_putc('\n');
+}
+
+static inline void __hyp_putx4(unsigned int x)
+{
+	x &= 0xf;
+	if (x <= 9)
+		x += '0';
+	else
+		x += ('a' - 0xa);
+	hyp_putc(x);
+}
+
+static inline void __hyp_putx4n(unsigned long x, int n)
+{
+	int i = n >> 2;
+
+	hyp_putc('0');
+	hyp_putc('x');
+
+	while (i--)
+		__hyp_putx4(x >> (4 * i));
+
+	hyp_putc('\n');
+}
+
+static inline void hyp_putx32(unsigned int x)
+{
+	__hyp_putx4n(x, 32);
+}
+
+static inline void hyp_putx64(unsigned long x)
+{
+	__hyp_putx4n(x, 64);
+}
+
+#endif
+
+#else
+
+#warning "Please don't include debug-pl011.h if you're not debugging"
+
+#ifdef __ASSEMBLY__
+
+.macro hyp_putc, c, tmpnr
+.endm
+
+.macro hyp_puts, s, tmpnr1, tmpnr2
+.endm
+
+.macro hyp_putx64, x, tmpnr1, tmpnr2
+.endm
+
+#else
+
+static inline void hyp_putc(char c) { }
+static inline void hyp_puts(char *s) { }
+static inline void hyp_putx32(unsigned int x) { }
+static inline void hyp_putx64(unsigned long x) { }
+
+#endif
+
+#endif	/* CONFIG_KVM_ARM_HYP_DEBUG_UART */
+#endif	/* __ARM64_KVM_HYP_DEBUG_PL011_H__ */
diff --git a/arch/arm64/kvm/hyp/nvhe/setup.c b/arch/arm64/kvm/hyp/nvhe/setup.c
index 57c2784..f0e6a13 100644
--- a/arch/arm64/kvm/hyp/nvhe/setup.c
+++ b/arch/arm64/kvm/hyp/nvhe/setup.c
@@ -22,6 +22,25 @@
 #define hyp_percpu_size ((unsigned long)__per_cpu_end - \
 			 (unsigned long)__per_cpu_start)
 
+#ifdef CONFIG_KVM_ARM_HYP_DEBUG_UART
+unsigned long arm64_kvm_hyp_debug_uart_addr;
+static int create_hyp_debug_uart_mapping(void)
+{
+	phys_addr_t base = CONFIG_KVM_ARM_HYP_DEBUG_UART_ADDR;
+	unsigned long haddr;
+
+	haddr = __pkvm_create_private_mapping(base, PAGE_SIZE, PAGE_HYP_DEVICE);
+	if (!haddr)
+		return -1;
+
+	arm64_kvm_hyp_debug_uart_addr = haddr;
+
+	return 0;
+}
+#else
+static int create_hyp_debug_uart_mapping(void) { return 0; }
+#endif
+
 static void *vmemmap_base;
 static void *hyp_pgt_base;
 static void *host_s2_pgt_base;
@@ -128,6 +147,10 @@
 	if (ret)
 		return ret;
 
+	ret = create_hyp_debug_uart_mapping();
+	if (ret)
+		return ret;
+
 	return 0;
 }
 
diff --git a/arch/arm64/kvm/mmu.c b/arch/arm64/kvm/mmu.c
index 98c6c38..d11f8c3 100644
--- a/arch/arm64/kvm/mmu.c
+++ b/arch/arm64/kvm/mmu.c
@@ -393,6 +393,44 @@
 	return ret;
 }
 
+#ifdef CONFIG_KVM_ARM_HYP_DEBUG_UART
+extern unsigned long __kvm_nvhe_arm64_kvm_hyp_debug_uart_addr;
+
+void __init kvm_hyp_debug_uart_set_basep(struct alt_instr *alt,
+					 __le32 *origptr, __le32 *updptr,
+					 int nr_inst)
+{
+	int i;
+	u64 addr = (u64)kvm_ksym_ref(&__kvm_nvhe_arm64_kvm_hyp_debug_uart_addr);
+
+	BUG_ON(nr_inst != 4);
+
+	for (i = 0; i < 4; ++i) {
+		u32 insn = le32_to_cpu(origptr[i]);
+
+		insn = aarch64_insn_encode_immediate(AARCH64_INSN_IMM_16,
+						     insn,
+						     addr & 0xffff);
+		BUG_ON(insn == AARCH64_BREAK_FAULT);
+		updptr[i] = cpu_to_le32(insn);
+		addr >>= 16;
+	}
+}
+
+static int create_hyp_debug_uart_mapping(void)
+{
+	if (is_kernel_in_hyp_mode())
+		return -EBUSY;
+
+	return __create_hyp_private_mapping(CONFIG_KVM_ARM_HYP_DEBUG_UART_ADDR,
+					    PAGE_SIZE,
+					    &__kvm_nvhe_arm64_kvm_hyp_debug_uart_addr,
+					    PAGE_HYP_DEVICE);
+}
+#else
+static int create_hyp_debug_uart_mapping(void) { return 0; }
+#endif
+
 /**
  * create_hyp_io_mappings - Map IO into both kernel and HYP
  * @phys_addr:	The physical start address which gets mapped
@@ -1508,6 +1546,7 @@
 		goto out_destroy_pgtable;
 
 	io_map_base = hyp_idmap_start;
+	WARN_ON(create_hyp_debug_uart_mapping());
 	return 0;
 
 out_destroy_pgtable: