1 files changed, 67 insertions, 1 deletions

diff --git a/arch/mips/fw/cfe/cfe_api.c b/arch/mips/fw/cfe/cfe_api.c
index 0c9c97ab291e..dcdfd962dbde 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,64 @@ 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, ...)
+{
+	unsigned int prid, __maybe_unused rev;
+	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;
+
+	prid = read_c0_prid();
+	if ((prid & PRID_COMP_MASK) != PRID_COMP_BROADCOM)
+		goto no_cfe;
+
+	rev = prid & PRID_REV_MASK;
+
+	/* disable XKS01 so that CFE can access the registers */
+	switch (prid & PRID_IMP_MASK) {
+#ifdef CONFIG_CPU_BMIPS4380
+	case PRID_IMP_BMIPS43XX:
+		if (rev >= PRID_REV_BMIPS4380_LO &&
+		    rev <= PRID_REV_BMIPS4380_HI)
+			__write_32bit_c0_register($22, 3,
+				__read_32bit_c0_register($22, 3) & ~BIT(12));
+		break;
+#endif
+#ifdef CONFIG_CPU_BMIPS5000
+	case PRID_IMP_BMIPS5000:
+	case PRID_IMP_BMIPS5200:
+		__write_32bit_c0_register($22, 5,
+			__read_32bit_c0_register($22, 5) & ~BIT(8));
+		break;
+#endif
+	default:
+		break;
+	}
+
+	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);
+}