[PATCH 1/2] MIPS: CFE: Add cfe_die()

[Florian Fainelli <f.fainelli@xxxxxxxxx>]

Add a cfe_die() implementation which is useful when the kernel does an
early panic and no console is registered. This allows us to print
useful diagnostics such as an invalid DTB having been
configured/selected.

Signed-off-by: Florian Fainelli <f.fainelli@xxxxxxxxx>
---
 arch/mips/fw/cfe/cfe_api.c             | 48 +++++++++++++++++++++++++-
 arch/mips/include/asm/fw/cfe/cfe_api.h |  2 ++
 2 files changed, 49 insertions(+), 1 deletion(-)

diff --git a/arch/mips/fw/cfe/cfe_api.c b/arch/mips/fw/cfe/cfe_api.c
index 0c9c97ab291e..6e7883f1d47e 100644
--- a/arch/mips/fw/cfe/cfe_api.c
+++ b/arch/mips/fw/cfe/cfe_api.c
@@ -13,10 +13,15 @@
  *
  * Authors:  Mitch Lichtenberg, Chris Demetriou
  */
-
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/printk.h>
+#include <asm/mipsregs.h>
 #include <asm/fw/cfe/cfe_api.h>
 #include "cfe_api_int.h"
 
+unsigned long __initdata cfe_seal;
+
 /* Cast from a native pointer to a cfe_xptr_t and back.	 */
 #define XPTR_FROM_NATIVE(n)	((cfe_xptr_t) (intptr_t) (n))
 #define NATIVE_FROM_XPTR(x)	((void *) (intptr_t) (x))
@@ -412,3 +417,44 @@ int cfe_writeblk(int handle, s64 offset, const char *buffer, int length)
 		return xiocb.xiocb_status;
 	return xiocb.plist.xiocb_buffer.buf_retlen;
 }
+
+void __init cfe_die(char *fmt, ...)
+{
+	char msg[128];
+	va_list ap;
+	int handle;
+	unsigned int count;
+
+	va_start(ap, fmt);
+	vsprintf(msg, fmt, ap);
+	strcat(msg, "\r\n");
+
+	if (cfe_seal != CFE_EPTSEAL)
+		goto no_cfe;
+
+	/* disable XKS01 so that CFE can access the registers */
+#if defined(CONFIG_CPU_BMIPS4380)
+	__write_32bit_c0_register($22, 3,
+		__read_32bit_c0_register($22, 3) & ~BIT(12));
+#elif defined(CONFIG_CPU_BMIPS5000)
+	__write_32bit_c0_register($22, 5,
+		__read_32bit_c0_register($22, 5) & ~BIT(8));
+#endif
+	handle = cfe_getstdhandle(CFE_STDHANDLE_CONSOLE);
+	if (handle < 0)
+		goto no_cfe;
+
+	cfe_write(handle, msg, strlen(msg));
+
+	for (count = 0; count < 0x7fffffff; count++)
+		mb();
+	cfe_exit(0, 1);
+	while (1)
+		;
+
+no_cfe:
+	/* probably won't print anywhere useful */
+	panic("%s", msg);
+
+	va_end(ap);
+}
diff --git a/arch/mips/include/asm/fw/cfe/cfe_api.h b/arch/mips/include/asm/fw/cfe/cfe_api.h
index 6457f36897a2..25df2f4deb31 100644
--- a/arch/mips/include/asm/fw/cfe/cfe_api.h
+++ b/arch/mips/include/asm/fw/cfe/cfe_api.h
@@ -105,5 +105,7 @@ int cfe_setenv(char *name, char *val);
 int cfe_write(int handle, const char *buffer, int length);
 int cfe_writeblk(int handle, int64_t offset, const char *buffer,
 		 int length);
+extern unsigned long cfe_seal;
+__printf(1, 2) void cfe_die(char *fmt, ...);
 
 #endif				/* CFE_API_H */
-- 
2.25.1