From 3d0ee0c47571029d5dcdaaf72d375518a1fefa2f Mon Sep 17 00:00:00 2001
From: hhb <hhb@rock-chips.com>
Date: Sat, 31 Mar 2012 19:26:05 +0800
Subject: [PATCH] rk30: fiq serial debugger: add command io Raw memory i/o
 utility

---
 arch/arm/common/fiq_debugger.c | 108 ++++++++++++++++++++++++++++++++-
 1 file changed, 105 insertions(+), 3 deletions(-)

diff --git a/arch/arm/common/fiq_debugger.c b/arch/arm/common/fiq_debugger.c
index d0a8aa8a3216..037dd9a11cca 100644
--- a/arch/arm/common/fiq_debugger.c
+++ b/arch/arm/common/fiq_debugger.c
@@ -544,7 +544,7 @@ static void do_sysrq(struct fiq_debugger_state *state, char rq)
 	handle_sysrq(rq);
 	end_syslog_dump(state);
 }
-
+static void fiq_io(struct fiq_debugger_state *state);
 /* This function CANNOT be called in FIQ context */
 static void debug_irq_exec(struct fiq_debugger_state *state, char *cmd)
 {
@@ -588,6 +588,8 @@ static char cmd_buf[][16] = {
 		{"cpu"},
 		{"ps"},
 		{"sysrq"},
+		{"help"},
+		{"io"},
 };
 
 static void debug_help(struct fiq_debugger_state *state)
@@ -605,7 +607,8 @@ static void debug_help(struct fiq_debugger_state *state)
 				" nosleep       Disable sleep while in FIQ\n"
 				" console       Switch terminal to console\n"
 				" cpu           Current CPU\n"
-				" cpu <number>  Switch to CPU<number>\n");
+				" cpu <number>  Switch to CPU<number>\n"
+			    " io            Raw memory i/o utility\n");
 	debug_printf(state,	" ps            Process list\n"
 				" sysrq         sysrq options\n"
 				" sysrq <param> Execute sysrq with <param>\n");
@@ -640,6 +643,104 @@ static void switch_cpu(struct fiq_debugger_state *state, int cpu)
 	state->current_cpu = cpu;
 }
 
+
+static void memread_memory(struct fiq_debugger_state *state, unsigned long phys_addr,
+								unsigned char *addr, int len, int iosize)
+{
+	int i;
+
+	while (len > 0) {
+		debug_printf(state, "%08lx: ", phys_addr);
+		i = 0;
+		while (i < 16 && len > 0) {
+			switch(iosize) {
+			case 1:
+				debug_printf(state, " %02x", readb(addr));
+				break;
+			case 2:
+				debug_printf(state, " %04x", readw(addr));
+				break;
+			case 4:
+				debug_printf(state, " %08lx", readl(addr));
+				break;
+			}
+			i += iosize;
+			addr += iosize;
+			len -= iosize;
+		}
+		phys_addr += 16;
+		debug_printf(state, "\n");
+	}
+}
+
+
+static void write_memory(unsigned long phys_addr, unsigned char *addr,
+								int len, int iosize, unsigned long value)
+{
+	switch(iosize) {
+	case 1:
+		while (len > 0) {
+			writeb(value, addr);
+			len -= iosize;
+			addr += iosize;
+		}
+		break;
+	case 2:
+		while (len > 0) {
+			writew(value, addr);
+			len -= iosize;
+			addr += iosize;
+		}
+		break;
+	case 4:
+		while (len > 0) {
+			writel(value, addr);
+			len -= iosize;
+			addr += iosize;
+		}
+		break;
+	}
+}
+
+static void fiq_io(struct fiq_debugger_state *state)
+{
+	int mem_read = 1;  //1 mean read, 0 mean write
+	int len = 0, iosize = 4;
+	int value = 0;
+	void __iomem * addr;
+	unsigned long phys_addr;
+	char *s;
+
+	if (strstr(state->debug_buf, "-h")) {
+		debug_printf(state, "io [-w/-r] [-l size] phys_addr [var] \n"
+							"io -w -l 0x04 0x20064004 0x05 \n");
+		return;
+	}
+
+	if (strstr(state->debug_buf, "-w")) {
+		mem_read = 0;
+	}
+
+	s = strstr(state->debug_buf, "-l");
+	if (s) {
+		s = skip_spaces(s+2);
+		s += sscanf(s, "0x%x 0x%lx", &len, &phys_addr);
+	}
+
+	addr = ioremap_nocache(phys_addr, len);
+
+	if (mem_read == 0) {
+		s = skip_spaces(s+1);
+		sscanf(s, "0x%x", &value);
+		write_memory(phys_addr, addr, len, iosize, value);
+	} else {
+		memread_memory(state, phys_addr, addr, len, iosize);
+	}
+
+	iounmap(addr);
+}
+
+
 static bool debug_fiq_exec(struct fiq_debugger_state *state,
 			const char *cmd, unsigned *regs, void *svc_sp)
 {
@@ -682,6 +783,8 @@ static bool debug_fiq_exec(struct fiq_debugger_state *state,
 		else
 			debug_printf(state, "invalid cpu\n");
 		debug_printf(state, "cpu %d\n", state->current_cpu);
+	} else if (!strncmp(cmd, "io ", 3)) {
+		fiq_io(state);
 	} else {
 		if (state->debug_busy) {
 			debug_printf(state,
@@ -691,7 +794,6 @@ static bool debug_fiq_exec(struct fiq_debugger_state *state,
 			strcpy(state->debug_cmd, cmd);
 			state->debug_busy = 1;
 		}
-
 		return true;
 	}
 	if (!state->console_enable)
-- 
2.34.1