1 files changed, 181 insertions, 55 deletions

diff --git a/arch/arm/common/fiq_debugger.c b/arch/arm/common/fiq_debugger.c
index 3ed18ae2ed8..1908ba95a90 100644
--- a/arch/arm/common/fiq_debugger.c
+++ b/arch/arm/common/fiq_debugger.c
@@ -25,6 +25,7 @@
 #include <linux/kernel_stat.h>
 #include <linux/irq.h>
 #include <linux/delay.h>
+#include <linux/reboot.h>
 #include <linux/sched.h>
 #include <linux/slab.h>
 #include <linux/smp.h>
@@ -37,8 +38,6 @@
 #include <asm/fiq_glue.h>
 #include <asm/stacktrace.h>
 
-#include <mach/system.h>
-
 #include <linux/uaccess.h>
 
 #include "fiq_debugger_ringbuf.h"
@@ -46,6 +45,8 @@
 #define DEBUG_MAX 64
 #define MAX_UNHANDLED_FIQ_COUNT 1000000
 
+#define MAX_FIQ_DEBUGGER_PORTS 4
+
 #define THREAD_INFO(sp) ((struct thread_info *) \
 		((unsigned long)(sp) & ~(THREAD_SIZE - 1)))
 
@@ -82,7 +83,6 @@ struct fiq_debugger_state {
 
 #ifdef CONFIG_FIQ_DEBUGGER_CONSOLE
 	struct console console;
-	struct tty_driver *tty_driver;
 	struct tty_struct *tty;
 	int tty_open_count;
 	struct fiq_debugger_ringbuf *tty_rbuf;
@@ -93,6 +93,10 @@ struct fiq_debugger_state {
 	unsigned int last_local_timer_irqs[NR_CPUS];
 };
 
+#ifdef CONFIG_FIQ_DEBUGGER_CONSOLE
+struct tty_driver *fiq_tty_driver;
+#endif
+
 #ifdef CONFIG_FIQ_DEBUGGER_NO_SLEEP
 static bool initial_no_sleep = true;
 #else
@@ -107,9 +111,12 @@ static bool initial_debug_enable;
 static bool initial_console_enable;
 #endif
 
+static bool fiq_kgdb_enable;
+
 module_param_named(no_sleep, initial_no_sleep, bool, 0644);
 module_param_named(debug_enable, initial_debug_enable, bool, 0644);
 module_param_named(console_enable, initial_console_enable, bool, 0644);
+module_param_named(kgdb_enable, fiq_kgdb_enable, bool, 0644);
 
 #ifdef CONFIG_FIQ_DEBUGGER_WAKEUP_IRQ_ALWAYS_ON
 static inline void enable_wakeup_irq(struct fiq_debugger_state *state) {}
@@ -175,13 +182,18 @@ static void debug_uart_flush(struct fiq_debugger_state *state)
 		state->pdata->uart_flush(state->pdev);
 }
 
+static void debug_putc(struct fiq_debugger_state *state, char c)
+{
+	state->pdata->uart_putc(state->pdev, c);
+}
+
 static void debug_puts(struct fiq_debugger_state *state, char *s)
 {
 	unsigned c;
 	while ((c = *s++)) {
 		if (c == '\n')
-			state->pdata->uart_putc(state->pdev, '\r');
-		state->pdata->uart_putc(state->pdev, c);
+			debug_putc(state, '\r');
+		debug_putc(state, c);
 	}
 }
 
@@ -357,7 +369,6 @@ static void dump_allregs(struct fiq_debugger_state *state, unsigned *regs)
 static void dump_irqs(struct fiq_debugger_state *state)
 {
 	int n;
-	unsigned int cpu;
 
 	debug_printf(state, "irqnr       total  since-last   status  name\n");
 	for (n = 0; n < NR_IRQS; n++) {
@@ -371,16 +382,6 @@ static void dump_irqs(struct fiq_debugger_state *state)
 			(act && act->name) ? act->name : "???");
 		state->last_irqs[n] = kstat_irqs(n);
 	}
-
-	for (cpu = 0; cpu < NR_CPUS; cpu++) {
-
-		debug_printf(state, "LOC %d: %10u %11u\n", cpu,
-			     __IRQ_STAT(cpu, local_timer_irqs),
-			     __IRQ_STAT(cpu, local_timer_irqs) -
-			     state->last_local_timer_irqs[cpu]);
-		state->last_local_timer_irqs[cpu] =
-			__IRQ_STAT(cpu, local_timer_irqs);
-	}
 }
 
 struct stacktrace_state {
@@ -533,11 +534,29 @@ static void end_syslog_dump(struct fiq_debugger_state *state)
 
 static void do_sysrq(struct fiq_debugger_state *state, char rq)
 {
+	if ((rq == 'g' || rq == 'G') && !fiq_kgdb_enable) {
+		debug_printf(state, "sysrq-g blocked\n");
+		return;
+	}
 	begin_syslog_dump(state);
 	handle_sysrq(rq);
 	end_syslog_dump(state);
 }
 
+#ifdef CONFIG_KGDB
+static void do_kgdb(struct fiq_debugger_state *state)
+{
+	if (!fiq_kgdb_enable) {
+		debug_printf(state, "kgdb through fiq debugger not enabled\n");
+		return;
+	}
+
+	debug_printf(state, "enabling console and triggering kgdb\n");
+	state->console_enable = true;
+	handle_sysrq('g');
+}
+#endif
+
 /* This function CANNOT be called in FIQ context */
 static void debug_irq_exec(struct fiq_debugger_state *state, char *cmd)
 {
@@ -547,6 +566,10 @@ static void debug_irq_exec(struct fiq_debugger_state *state, char *cmd)
 		do_sysrq(state, 'h');
 	if (!strncmp(cmd, "sysrq ", 6))
 		do_sysrq(state, cmd[6]);
+#ifdef CONFIG_KGDB
+	if (!strcmp(cmd, "kgdb"))
+		do_kgdb(state);
+#endif
 }
 
 static void debug_help(struct fiq_debugger_state *state)
@@ -568,6 +591,9 @@ static void debug_help(struct fiq_debugger_state *state)
 	debug_printf(state,	" ps            Process list\n"
 				" sysrq         sysrq options\n"
 				" sysrq <param> Execute sysrq with <param>\n");
+#ifdef CONFIG_KGDB
+	debug_printf(state,	" kgdb          Enter kernel debugger\n");
+#endif
 }
 
 static void take_affinity(void *info)
@@ -604,8 +630,17 @@ static bool debug_fiq_exec(struct fiq_debugger_state *state,
 		dump_allregs(state, regs);
 	} else if (!strcmp(cmd, "bt")) {
 		dump_stacktrace(state, (struct pt_regs *)regs, 100, svc_sp);
-	} else if (!strcmp(cmd, "reboot")) {
-		arch_reset(0, 0);
+	} else if (!strncmp(cmd, "reboot", 6)) {
+		cmd += 6;
+		while (*cmd == ' ')
+			cmd++;
+		if (*cmd) {
+			char tmp_cmd[32];
+			strlcpy(tmp_cmd, cmd, sizeof(tmp_cmd));
+			kernel_restart(tmp_cmd);
+		} else {
+			kernel_restart(NULL);
+		}
 	} else if (!strcmp(cmd, "irqs")) {
 		dump_irqs(state);
 	} else if (!strcmp(cmd, "kmsg")) {
@@ -722,7 +757,8 @@ static void debug_handle_irq_context(struct fiq_debugger_state *state)
 #endif
 	if (state->debug_busy) {
 		debug_irq_exec(state, state->debug_cmd);
-		debug_prompt(state);
+		if (!state->console_enable)
+			debug_prompt(state);
 		state->debug_busy = 0;
 	}
 }
@@ -780,19 +816,19 @@ static bool debug_handle_uart_interrupt(struct fiq_debugger_state *state,
 		} else if ((c >= ' ') && (c < 127)) {
 			if (state->debug_count < (DEBUG_MAX - 1)) {
 				state->debug_buf[state->debug_count++] = c;
-				state->pdata->uart_putc(state->pdev, c);
+				debug_putc(state, c);
 			}
 		} else if ((c == 8) || (c == 127)) {
 			if (state->debug_count > 0) {
 				state->debug_count--;
-				state->pdata->uart_putc(state->pdev, 8);
-				state->pdata->uart_putc(state->pdev, ' ');
-				state->pdata->uart_putc(state->pdev, 8);
+				debug_putc(state, 8);
+				debug_putc(state, ' ');
+				debug_putc(state, 8);
 			}
 		} else if ((c == 13) || (c == 10)) {
 			if (c == '\r' || (c == '\n' && last_c != '\r')) {
-				state->pdata->uart_putc(state->pdev, '\r');
-				state->pdata->uart_putc(state->pdev, '\n');
+				debug_putc(state, '\r');
+				debug_putc(state, '\n');
 			}
 			if (state->debug_count) {
 				state->debug_buf[state->debug_count] = 0;
@@ -882,10 +918,8 @@ static void debug_resume(struct fiq_glue_handler *h)
 #if defined(CONFIG_FIQ_DEBUGGER_CONSOLE)
 struct tty_driver *debug_console_device(struct console *co, int *index)
 {
-	struct fiq_debugger_state *state;
-	state = container_of(co, struct fiq_debugger_state, console);
-	*index = 0;
-	return state->tty_driver;
+	*index = co->index;
+	return fiq_tty_driver;
 }
 
 static void debug_console_write(struct console *co,
@@ -901,8 +935,8 @@ static void debug_console_write(struct console *co,
 	debug_uart_enable(state);
 	while (count--) {
 		if (*s == '\n')
-			state->pdata->uart_putc(state->pdev, '\r');
-		state->pdata->uart_putc(state->pdev, *s++);
+			debug_putc(state, '\r');
+		debug_putc(state, *s++);
 	}
 	debug_uart_flush(state);
 	debug_uart_disable(state);
@@ -917,7 +951,9 @@ static struct console fiq_debugger_console = {
 
 int fiq_tty_open(struct tty_struct *tty, struct file *filp)
 {
-	struct fiq_debugger_state *state = tty->driver->driver_state;
+	int line = tty->index;
+	struct fiq_debugger_state **states = tty->driver->driver_state;
+	struct fiq_debugger_state *state = states[line];
 	if (state->tty_open_count++)
 		return 0;
 
@@ -944,7 +980,7 @@ int  fiq_tty_write(struct tty_struct *tty, const unsigned char *buf, int count)
 
 	debug_uart_enable(state);
 	for (i = 0; i < count; i++)
-		state->pdata->uart_putc(state->pdev, *buf++);
+		debug_putc(state, *buf++);
 	debug_uart_disable(state);
 
 	return count;
@@ -955,43 +991,115 @@ int  fiq_tty_write_room(struct tty_struct *tty)
 	return 1024;
 }
 
+#ifdef CONFIG_CONSOLE_POLL
+static int fiq_tty_poll_init(struct tty_driver *driver, int line, char *options)
+{
+	return 0;
+}
+
+static int fiq_tty_poll_get_char(struct tty_driver *driver, int line)
+{
+	struct fiq_debugger_state *state = driver->ttys[line]->driver_data;
+	int c = NO_POLL_CHAR;
+
+	debug_uart_enable(state);
+	if (debug_have_fiq(state)) {
+		int count = fiq_debugger_ringbuf_level(state->tty_rbuf);
+		if (count > 0) {
+			c = fiq_debugger_ringbuf_peek(state->tty_rbuf, 0);
+			fiq_debugger_ringbuf_consume(state->tty_rbuf, 1);
+		}
+	} else {
+		c = debug_getc(state);
+		if (c == FIQ_DEBUGGER_NO_CHAR)
+			c = NO_POLL_CHAR;
+	}
+	debug_uart_disable(state);
+
+	return c;
+}
+
+static void fiq_tty_poll_put_char(struct tty_driver *driver, int line, char ch)
+{
+	struct fiq_debugger_state *state = driver->ttys[line]->driver_data;
+	debug_uart_enable(state);
+	debug_putc(state, ch);
+	debug_uart_disable(state);
+}
+#endif
+
 static const struct tty_operations fiq_tty_driver_ops = {
 	.write = fiq_tty_write,
 	.write_room = fiq_tty_write_room,
 	.open = fiq_tty_open,
 	.close = fiq_tty_close,
+#ifdef CONFIG_CONSOLE_POLL
+	.poll_init = fiq_tty_poll_init,
+	.poll_get_char = fiq_tty_poll_get_char,
+	.poll_put_char = fiq_tty_poll_put_char,
+#endif
 };
 
-static int fiq_debugger_tty_init(struct fiq_debugger_state *state)
+static int fiq_debugger_tty_init(void)
 {
-	int ret = -EINVAL;
+	int ret;
+	struct fiq_debugger_state **states = NULL;
 
-	state->tty_driver = alloc_tty_driver(1);
-	if (!state->tty_driver) {
-		pr_err("Failed to allocate fiq debugger tty\n");
+	states = kzalloc(sizeof(*states) * MAX_FIQ_DEBUGGER_PORTS, GFP_KERNEL);
+	if (!states) {
+		pr_err("Failed to allocate fiq debugger state structres\n");
 		return -ENOMEM;
 	}
 
-	state->tty_driver->owner		= THIS_MODULE;
-	state->tty_driver->driver_name	= "fiq-debugger";
-	state->tty_driver->name		= "ttyFIQ";
-	state->tty_driver->type		= TTY_DRIVER_TYPE_SERIAL;
-	state->tty_driver->subtype	= SERIAL_TYPE_NORMAL;
-	state->tty_driver->init_termios	= tty_std_termios;
-	state->tty_driver->init_termios.c_cflag =
+	fiq_tty_driver = alloc_tty_driver(MAX_FIQ_DEBUGGER_PORTS);
+	if (!fiq_tty_driver) {
+		pr_err("Failed to allocate fiq debugger tty\n");
+		ret = -ENOMEM;
+		goto err_free_state;
+	}
+
+	fiq_tty_driver->owner		= THIS_MODULE;
+	fiq_tty_driver->driver_name	= "fiq-debugger";
+	fiq_tty_driver->name		= "ttyFIQ";
+	fiq_tty_driver->type		= TTY_DRIVER_TYPE_SERIAL;
+	fiq_tty_driver->subtype		= SERIAL_TYPE_NORMAL;
+	fiq_tty_driver->init_termios	= tty_std_termios;
+	fiq_tty_driver->flags		= TTY_DRIVER_REAL_RAW |
+					  TTY_DRIVER_DYNAMIC_DEV;
+	fiq_tty_driver->driver_state	= states;
+
+	fiq_tty_driver->init_termios.c_cflag =
 					B115200 | CS8 | CREAD | HUPCL | CLOCAL;
-	state->tty_driver->init_termios.c_ispeed =
-		state->tty_driver->init_termios.c_ospeed = 115200;
-	state->tty_driver->flags		= TTY_DRIVER_REAL_RAW;
-	tty_set_operations(state->tty_driver, &fiq_tty_driver_ops);
-	state->tty_driver->driver_state = state;
+	fiq_tty_driver->init_termios.c_ispeed = 115200;
+	fiq_tty_driver->init_termios.c_ospeed = 115200;
+
+	tty_set_operations(fiq_tty_driver, &fiq_tty_driver_ops);
 
-	ret = tty_register_driver(state->tty_driver);
+	ret = tty_register_driver(fiq_tty_driver);
 	if (ret) {
 		pr_err("Failed to register fiq tty: %d\n", ret);
-		goto err;
+		goto err_free_tty;
 	}
 
+	pr_info("Registered FIQ tty driver\n");
+	return 0;
+
+err_free_tty:
+	put_tty_driver(fiq_tty_driver);
+	fiq_tty_driver = NULL;
+err_free_state:
+	kfree(states);
+	return ret;
+}
+
+static int fiq_debugger_tty_init_one(struct fiq_debugger_state *state)
+{
+	int ret;
+	struct device *tty_dev;
+	struct fiq_debugger_state **states = fiq_tty_driver->driver_state;
+
+	states[state->pdev->id] = state;
+
 	state->tty_rbuf = fiq_debugger_ringbuf_alloc(1024);
 	if (!state->tty_rbuf) {
 		pr_err("Failed to allocate fiq debugger ringbuf\n");
@@ -999,13 +1107,23 @@ static int fiq_debugger_tty_init(struct fiq_debugger_state *state)
 		goto err;
 	}
 
-	pr_info("Registered FIQ tty driver %p\n", state->tty_driver);
+	tty_dev = tty_register_device(fiq_tty_driver, state->pdev->id,
+		&state->pdev->dev);
+	if (IS_ERR(tty_dev)) {
+		pr_err("Failed to register fiq debugger tty device\n");
+		ret = PTR_ERR(tty_dev);
+		goto err;
+	}
+
+	device_set_wakeup_capable(tty_dev, 1);
+
+	pr_info("Registered fiq debugger ttyFIQ%d\n", state->pdev->id);
+
 	return 0;
 
 err:
 	fiq_debugger_ringbuf_free(state->tty_rbuf);
 	state->tty_rbuf = NULL;
-	put_tty_driver(state->tty_driver);
 	return ret;
 }
 #endif
@@ -1038,6 +1156,9 @@ static int fiq_debugger_probe(struct platform_device *pdev)
 	int fiq;
 	int uart_irq;
 
+	if (pdev->id >= MAX_FIQ_DEBUGGER_PORTS)
+		return -EINVAL;
+
 	if (!pdata->uart_getc || !pdata->uart_putc)
 		return -EINVAL;
 	if ((pdata->uart_enable && !pdata->uart_disable) ||
@@ -1155,8 +1276,12 @@ static int fiq_debugger_probe(struct platform_device *pdev)
 
 #if defined(CONFIG_FIQ_DEBUGGER_CONSOLE)
 	state->console = fiq_debugger_console;
+	state->console.index = pdev->id;
+	if (!console_set_on_cmdline)
+		add_preferred_console(state->console.name,
+			state->console.index, NULL);
 	register_console(&state->console);
-	fiq_debugger_tty_init(state);
+	fiq_debugger_tty_init_one(state);
 #endif
 	return 0;
 
@@ -1190,6 +1315,7 @@ static struct platform_driver fiq_debugger_driver = {
 
 static int __init fiq_debugger_init(void)
 {
+	fiq_debugger_tty_init();
 	return platform_driver_register(&fiq_debugger_driver);
 }