From e69f66c6ebe82bbbd1da766bc4eda40ec7ee5af1 Mon Sep 17 00:00:00 2001
From: Michal Simek <monstr@monstr.eu>
Date: Tue, 8 May 2007 15:57:43 +0200
Subject: [PATCH] add: reading special purpose registers

---
 common/cmd_mfsl.c            | 43 +++++++++++++++++++++++++++---------
 cpu/microblaze/exception.c   |  4 ++--
 include/asm-microblaze/asm.h | 17 +++++---------
 3 files changed, 41 insertions(+), 23 deletions(-)

diff --git a/common/cmd_mfsl.c b/common/cmd_mfsl.c
index 3032489f08b..26786aa8cd7 100644
--- a/common/cmd_mfsl.c
+++ b/common/cmd_mfsl.c
@@ -350,22 +350,41 @@ int do_fwr (cmd_tbl_t * cmdtp, int flag, int argc, char *argv[])
 
 }
 
-int do_rmsr (cmd_tbl_t * cmdtp, int flag, int argc, char *argv[])
+int do_rspr (cmd_tbl_t * cmdtp, int flag, int argc, char *argv[])
 {
+	unsigned int reg = 0;
 	unsigned int val = 0;
 
-	val = (unsigned int)simple_strtoul (argv[1], NULL, 16);
+	reg = (unsigned int)simple_strtoul (argv[1], NULL, 16);
+	val = (unsigned int)simple_strtoul (argv[2], NULL, 16);
 	if (argc < 1) {
 		printf ("Usage:\n%s\n", cmdtp->usage);
 		return 1;
 	}
-	if (argc > 1) {
-		MTS (val);
-		MFS (val);
-	} else {
-		MFS (val);
+	switch (reg) {
+	case 0x1:
+		if (argc > 2) {
+			MTS (val, rmsr);
+			NOP;
+			MFS (val, rmsr);
+
+		} else {
+			MFS (val, rmsr);
+		}
+		puts ("MSR");
+		break;
+	case 0x3:
+		MFS (val, rear);
+		puts ("EAR");
+		break;
+	case 0x5:
+		MFS (val, resr);
+		puts ("ESR");
+		break;
+	default:
+		return 1;
 	}
-	printf ("rmsr: 0x%08lx\n", val);
+	printf (": 0x%08lx\n", val);
 	return 0;
 }
 
@@ -388,7 +407,11 @@ U_BOOT_CMD (fwr, 4, 1, do_fwr,
 		" 2 - blocking data write\n"
 		" 3 - blocking control write\n");
 
-U_BOOT_CMD (rmsr, 2, 1, do_rmsr,
-	    "rmsr    - read MSR register\n", "- read MSR register.\n");
+U_BOOT_CMD (rspr, 3, 1, do_rspr,
+		"rmsr    - read/write special purpose register\n",
+		"- reg_num [write value] read/write special purpose register\n"
+		" 0 - MSR - Machine status register\n"
+		" 1 - EAR - Exception address register\n"
+		" 2 - ESR - Exception status register\n");
 
 #endif				/* CONFIG_MICROBLAZE & CFG_CMD_MFSL */
diff --git a/cpu/microblaze/exception.c b/cpu/microblaze/exception.c
index 87ecbeaa49a..d76b05a5262 100644
--- a/cpu/microblaze/exception.c
+++ b/cpu/microblaze/exception.c
@@ -30,9 +30,9 @@ void _hw_exception_handler (void)
 	int address = 0;
 	int state = 0;
 	/* loading address of exception EAR */
-	MFSEAR(address);
+	MFS (address, rear);
 	/* loading excetpion state register ESR */
-	MFSESR(state);
+	MFS (state, resr);
 	printf ("Hardware exception at 0x%x address\n", address);
 	switch (state & 0x1f) {	/* mask on exception cause */
 	case 0x1:
diff --git a/include/asm-microblaze/asm.h b/include/asm-microblaze/asm.h
index 8302ed88507..f10f89c941a 100755
--- a/include/asm-microblaze/asm.h
+++ b/include/asm-microblaze/asm.h
@@ -49,23 +49,18 @@
 
 /* CPU dependent */
 /* machine status register */
-#define MFS(val) \
-	__asm__ __volatile__ ("mfs %0, rmsr":"=r" (val));
+#define MFS(val, reg) \
+	__asm__ __volatile__ ("mfs %0," #reg :"=r" (val));
 
-#define MTS(val) \
-	__asm__ __volatile__ ("mts rmsr, %0"::"r" (val));
-
-/* exception status register */
-#define MFSEAR(val) \
-	__asm__ __volatile ("mfs %0,rear":"=r" (val));
-
-#define MFSESR(val) \
-	__asm__ __volatile ("mfs %0,resr":"=r" (val));
+#define MTS(val, reg) \
+	__asm__ __volatile__ ("mts " #reg ", %0"::"r" (val));
 
 /* get return address from interrupt */
 #define R14(val) \
 	__asm__ __volatile__ ("addi %0, r14, 0":"=r" (val));
 
+#define NOP	__asm__ __volatile__ ("nop");
+
 /* use machine status registe USE_MSR_REG */
 #ifdef XILINX_USE_MSR_INSTR
 #define MSRSET(val) \