microblaze: Add checking mechanism for MSR instruction

It was necessary to use fourth parameter(r8) in early_printk
to show messages on console.

Signed-off-by: Michal Simek <monstr@monstr.eu>

3 files changed, 27 insertions, 4 deletions

diff --git a/arch/microblaze/include/asm/setup.h b/arch/microblaze/include/asm/setup.h
index 27f8dafd8c34..ed67c9ed15b8 100644
--- a/arch/microblaze/include/asm/setup.h
+++ b/arch/microblaze/include/asm/setup.h
@@ -38,7 +38,7 @@ extern void early_console_reg_tlb_alloc(unsigned int addr);
 void time_init(void);
 void init_IRQ(void);
 void machine_early_init(const char *cmdline, unsigned int ram,
-						unsigned int fdt);
+			unsigned int fdt, unsigned int msr);
 
 void machine_restart(char *cmd);
 void machine_shutdown(void);
diff --git a/arch/microblaze/kernel/head.S b/arch/microblaze/kernel/head.S
index e41c6ce2a7be..bfc7ea801cc4 100644
--- a/arch/microblaze/kernel/head.S
+++ b/arch/microblaze/kernel/head.S
@@ -55,6 +55,19 @@ ENTRY(_start)
 	andi	r1, r1, ~2
 	mts	rmsr, r1
 
+/*
+ * Here is checking mechanism which check if Microblaze has msr instructions
+ * We load msr and compare it with previous r1 value - if is the same,
+ * msr instructions works if not - cpu don't have them.
+ */
+	or	r8, r0, r0 /* 0 - I have msr instr, 1 - I don't have */
+	or	r12, r0, r0
+	msrset	r12, 0	/* set nothing - just read msr for test */
+	cmpu	r12, r12, r1
+	beqi	r12, 1f
+	ori	r8, r0, 1 /* I don't have msr */
+1:
+
 /* r7 may point to an FDT, or there may be one linked in.
    if it's in r7, we've got to save it away ASAP.
    We ensure r7 points to a valid FDT, just in case the bootloader
@@ -209,8 +222,8 @@ start_here:
 	 * Please see $(ARCH)/mach-$(SUBARCH)/setup.c for
 	 * the function.
 	 */
-	la	r8, r0, machine_early_init
-	brald	r15, r8
+	la	r9, r0, machine_early_init
+	brald	r15, r9
 	nop
 
 #ifndef CONFIG_MMU
diff --git a/arch/microblaze/kernel/setup.c b/arch/microblaze/kernel/setup.c
index 2a97bf513b64..8c1e0f4dcf18 100644
--- a/arch/microblaze/kernel/setup.c
+++ b/arch/microblaze/kernel/setup.c
@@ -94,7 +94,7 @@ inline unsigned get_romfs_len(unsigned *addr)
 #endif	/* CONFIG_MTD_UCLINUX_EBSS */
 
 void __init machine_early_init(const char *cmdline, unsigned int ram,
-		unsigned int fdt)
+		unsigned int fdt, unsigned int msr)
 {
 	unsigned long *src, *dst = (unsigned long *)0x0;
 
@@ -157,6 +157,16 @@ void __init machine_early_init(const char *cmdline, unsigned int ram,
 	early_printk("New klimit: 0x%08x\n", (unsigned)klimit);
 #endif
 
+#if CONFIG_XILINX_MICROBLAZE0_USE_MSR_INSTR
+	if (msr)
+		early_printk("!!!Your kernel has setup MSR instruction but "
+				"CPU don't have it %d\n", msr);
+#else
+	if (!msr)
+		early_printk("!!!Your kernel not setup MSR instruction but "
+				"CPU have it %d\n", msr);
+#endif
+
 	for (src = __ivt_start; src < __ivt_end; src++, dst++)
 		*dst = *src;