[U-Boot] [PATCH 3/3] ARM: rmobile: Add possibility to debug main PSCI commands

[Oleksandr Tyshchenko]

From: Oleksandr Tyshchenko <oleksandr_tyshchenko at epam.com>

Also take care of the fact that Lager and Stout boards use
different serial interface for console.

Signed-off-by: Oleksandr Tyshchenko <oleksandr_tyshchenko at epam.com>
---
 arch/arm/mach-rmobile/debug.h | 91 +++++++++++++++++++++++++++++++++++++++++++
 arch/arm/mach-rmobile/psci.c  | 23 +++++++++++
 2 files changed, 114 insertions(+)
 create mode 100644 arch/arm/mach-rmobile/debug.h

diff --git a/arch/arm/mach-rmobile/debug.h b/arch/arm/mach-rmobile/debug.h
new file mode 100644
index 0000000..5d4ec77
--- /dev/null
+++ b/arch/arm/mach-rmobile/debug.h
@@ -0,0 +1,91 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Contains functions used for PSCI debug.
+ *
+ * Copyright (C) 2018 EPAM Systems Inc.
+ *
+ * Based on arch/arm/mach-uniphier/debug.h
+ */
+
+#ifndef __DEBUG_H__
+#define __DEBUG_H__
+
+#include <asm/io.h>
+
+/* SCIFA definitions */
+#define SCIFA_BASE		0xe6c40000
+
+#define SCIFA_SCASSR	0x14	/* Serial status register */
+#define SCIFA_SCAFTDR	0x20	/* Transmit FIFO data register */
+
+/* SCIF definitions */
+#define SCIF_BASE		0xe6e60000
+
+#define SCIF_SCFSR		0x10	/* Serial status register */
+#define SCIF_SCFTDR		0x0c	/* Transmit FIFO data register */
+
+/* Common for both interfaces definitions */
+#define SCFSR_TDFE		BIT(5) /* Transmit FIFO Data Empty */
+#define SCFSR_TEND		BIT(6) /* Transmission End */
+
+#ifdef CONFIG_SCIF_A
+#define UART_BASE			SCIFA_BASE
+#define UART_STATUS_REG		SCIFA_SCASSR
+#define UART_TX_FIFO_REG	SCIFA_SCAFTDR
+#else
+#define UART_BASE			SCIF_BASE
+#define UART_STATUS_REG		SCIF_SCFSR
+#define UART_TX_FIFO_REG	SCIF_SCFTDR
+#endif
+
+/* All functions are inline so that they can be called from .secure section. */
+
+#ifdef DEBUG
+static inline void debug_putc(int c)
+{
+	void __iomem *base = (void __iomem *)UART_BASE;
+
+	while (!(readw(base + UART_STATUS_REG) & SCFSR_TDFE))
+		;
+
+	writeb(c, base + UART_TX_FIFO_REG);
+	writew(readw(base + UART_STATUS_REG) & ~(SCFSR_TEND | SCFSR_TDFE),
+	       base + UART_STATUS_REG);
+}
+
+static inline void debug_puts(const char *s)
+{
+	while (*s) {
+		if (*s == '\n')
+			debug_putc('\r');
+
+		debug_putc(*s++);
+	}
+}
+
+static inline void debug_puth(unsigned long val)
+{
+	int i;
+	unsigned char c;
+
+	for (i = 8; i--; ) {
+		c = ((val >> (i * 4)) & 0xf);
+		c += (c >= 10) ? 'a' - 10 : '0';
+		debug_putc(c);
+	}
+}
+#else
+static inline void debug_putc(int c)
+{
+}
+
+static inline void debug_puts(const char *s)
+{
+}
+
+static inline void debug_puth(unsigned long val)
+{
+}
+#endif
+
+#endif /* __DEBUG_H__ */
diff --git a/arch/arm/mach-rmobile/psci.c b/arch/arm/mach-rmobile/psci.c
index 95850b8..4d78b0f 100644
--- a/arch/arm/mach-rmobile/psci.c
+++ b/arch/arm/mach-rmobile/psci.c
@@ -14,6 +14,7 @@
 #include <asm/secure.h>
 
 #include "pm-r8a7790.h"
+#include "debug.h"
 
 #define R8A7790_PSCI_NR_CPUS	8
 #if R8A7790_PSCI_NR_CPUS > CONFIG_ARMV7_PSCI_NR_CPUS
@@ -105,6 +106,16 @@ int __secure psci_cpu_on(u32 function_id, u32 target_cpu, u32 entry_point,
 {
 	int cpu;
 
+	debug_puts("[U-Boot PSCI] cpu_on: cpu=");
+	debug_puth(get_current_cpu());
+	debug_puts(", target_cpu=");
+	debug_puth(target_cpu);
+	debug_puts(", entry_point=");
+	debug_puth(entry_point);
+	debug_puts(", context_id=");
+	debug_puth(context_id);
+	debug_puts("\n");
+
 	cpu = mpidr_to_cpu_index(target_cpu);
 	if (cpu == -1)
 		return ARM_PSCI_RET_INVAL;
@@ -130,6 +141,10 @@ int __secure psci_cpu_off(void)
 {
 	int cpu = get_current_cpu();
 
+	debug_puts("[U-Boot PSCI] cpu_off: cpu=");
+	debug_puth(cpu);
+	debug_puts("\n");
+
 	/*
 	 * Place the CPU interface in a state where it can never make a CPU
 	 * exit WFI as result of an asserted interrupt.
@@ -154,6 +169,10 @@ int __secure psci_cpu_off(void)
 
 void __secure psci_system_reset(u32 function_id)
 {
+	debug_puts("[U-Boot PSCI] system_reset: cpu=");
+	debug_puth(get_current_cpu());
+	debug_puts("\n");
+
 	r8a7790_system_reset();
 
 	/* Drain the WB before WFI */
@@ -166,6 +185,10 @@ void __secure psci_system_reset(u32 function_id)
 
 void __secure psci_system_off(u32 function_id)
 {
+	debug_puts("[U-Boot PSCI] system_off: cpu=");
+	debug_puth(get_current_cpu());
+	debug_puts("\n");
+
 	/* Drain the WB before WFI */
 	dsb();
 
-- 
2.7.4