4 files changed, 138 insertions, 17 deletions

diff --git a/cpu/nios/interrupts.c b/cpu/nios/interrupts.c
index 443680582..48fc81e58 100644
--- a/cpu/nios/interrupts.c
+++ b/cpu/nios/interrupts.c
@@ -179,7 +179,7 @@ int do_irqinfo (cmd_tbl_t *cmdtp, int flag, int argc, char *argv[])
 	int vec;
 
 	printf ("\nInterrupt-Information:\n");
-	printf ("Nr  Routine   Arg       CouIt's ok to cnt\n");
+	printf ("Nr  Routine   Arg       Count\n");
 
 	for (vec=0; vec<64; vec++) {
 		if (irq_vecs[vec].handler != NULL) {
diff --git a/cpu/nios/serial.c b/cpu/nios/serial.c
index 34257a2fe..4bdda2500 100644
--- a/cpu/nios/serial.c
+++ b/cpu/nios/serial.c
@@ -26,6 +26,48 @@
 #include <watchdog.h>
 #include <nios-io.h>
 
+/*------------------------------------------------------------------
+ * JTAG acts as the serial port
+ *-----------------------------------------------------------------*/
+#if defined(CONFIG_CONSOLE_JTAG)
+
+static nios_jtag_t *jtag = (nios_jtag_t *)CFG_NIOS_CONSOLE;
+
+void serial_setbrg( void ){ return; }
+int serial_init( void ) { return(0);}
+
+void serial_putc (char c)
+{
+	while ((jtag->txcntl & NIOS_JTAG_TRDY) != 0)
+		WATCHDOG_RESET ();
+	jtag->txcntl = NIOS_JTAG_TRDY | (unsigned char)c;
+}
+
+void serial_puts (const char *s)
+{
+	while (*s != 0)
+		serial_putc (*s++);
+}
+
+int serial_tstc (void)
+{
+	return (jtag->rxcntl & NIOS_JTAG_RRDY);
+}
+
+int serial_getc (void)
+{
+	int c;
+	while (serial_tstc() == 0)
+		WATCHDOG_RESET ();
+	c = jtag->rxcntl & 0x0ff;
+	jtag->rxcntl = 0;
+	return (c);
+}
+
+/*------------------------------------------------------------------
+ * UART the serial port
+ *-----------------------------------------------------------------*/
+#else
 
 static nios_uart_t *uart = (nios_uart_t *)CFG_NIOS_CONSOLE;
 
@@ -34,12 +76,12 @@ static nios_uart_t *uart = (nios_uart_t *)CFG_NIOS_CONSOLE;
 /* Everything's already setup for fixed-baud PTF
  * assignment
  */
-void serial_setbrg( void ){ return; }
-int serial_init( void ) { return(0);}
+void serial_setbrg (void){ return; }
+int serial_init (void) { return (0);}
 
 #else
 
-void serial_setbrg( void )
+void serial_setbrg (void)
 {
 	DECLARE_GLOBAL_DATA_PTR;
 	unsigned div;
@@ -49,39 +91,44 @@ void serial_setbrg( void )
 	return;
 }
 
-int serial_init( void )
+int serial_init (void)
 {
-	serial_setbrg();
-	return(0);
+	serial_setbrg ();
+	return (0);
 }
 
 #endif /* CFG_NIOS_FIXEDBAUD */
 
 
-void serial_putc( char c )
+/*-----------------------------------------------------------------------
+ * UART CONSOLE
+ *---------------------------------------------------------------------*/
+void serial_putc (char c)
 {
 	if (c == '\n')
-		serial_putc('\r');
-	while( (uart->status & NIOS_UART_TRDY) == 0 )
+		serial_putc ('\r');
+	while ((uart->status & NIOS_UART_TRDY) == 0)
 		WATCHDOG_RESET ();
 	uart->txdata = (unsigned char)c;
 }
 
-void serial_puts( const char *s )
+void serial_puts (const char *s)
 {
-	while( *s != 0 ) {
-		serial_putc( *s++ );
+	while (*s != 0) {
+		serial_putc (*s++);
 	}
 }
 
-int serial_tstc( void )
+int serial_tstc (void)
 {
-	return( uart->status & NIOS_UART_RRDY);
+	return (uart->status & NIOS_UART_RRDY);
 }
 
-int serial_getc( void )
+int serial_getc (void)
 {
-	while( serial_tstc() == 0 )
+	while (serial_tstc () == 0)
 		WATCHDOG_RESET ();
 	return( uart->rxdata & 0x00ff );
 }
+
+#endif /* CONFIG_JTAG_CONSOLE */
diff --git a/cpu/nios/start.S b/cpu/nios/start.S
index 7cbd1a78f..cb1af3c8b 100644
--- a/cpu/nios/start.S
+++ b/cpu/nios/start.S
@@ -93,6 +93,14 @@ _start:
 	subi	%g6, 4			/* %g6 <- src addr */
 	ld	%g7, [%g7]		/* %g7 <- dst addr */
 
+	/* No need to move text sections if we're already located
+	 * at the proper address.
+	 */
+	cmp	%g7, %g6
+	ifs	cc_z
+	br	reloc
+	nop				/* delay slot */
+
 1:	cmp	%g7, %g5
 	skps	cc_nz
 	br	2f
@@ -114,6 +122,7 @@ _start:
 	 pfx	%xhi(reloc@h)
 	 movhi	%g0, %xlo(reloc@h)
 	 jmp	%g0
+	 nop				/* delay slot */
 reloc:
 
 	/*
@@ -141,6 +150,48 @@ reloc:
 4:
 
 	/*
+	 * INIT VECTOR TABLE
+	 */
+	pfx	%hi(CFG_VECT_BASE)
+	movi	%g0, %lo(CFG_VECT_BASE)
+	pfx	%xhi(CFG_VECT_BASE)
+	movhi	%g0, %xlo(CFG_VECT_BASE)	/* dst */
+	mov	%l0, %g0
+
+	pfx	%hi(_vectors)
+	movi	%g1, %lo(_vectors)
+	pfx	%xhi(_vectors)
+	movhi	%g1, %xlo(_vectors)	/* src */
+	bgen	%g2, 6			/* cnt = 64 */
+
+	ldp	%g3, [%l0, 3]		/* bkpt vector */
+	ldp	%g4, [%l0, 4]		/* single step vector */
+
+5:	ld	%g7, [%g1]
+	addi	%g1, 4			/* src++ */
+	st	[%g0], %g7
+	addi	%g0, 4			/* dst++ */
+
+	subi	%g2, 1			/* cnt-- */
+	ifrnz	%g2
+	br	5b
+	nop				/* delay slot */
+
+#if defined(CONFIG_ROM_STUBS)
+	/* Restore the breakpoint and single step exception
+	 * vectors to their original values.
+	 */
+	stp	[%l0,3], %g3		/* breakpoint */
+	stp	[%l0,4], %g4		/* single step */
+#endif
+
+	/* For debug startup convenience ... software breakpoints
+	 * set prior to this point may not succeed ;-)
+	 */
+	.global __start
+__start:
+
+	/*
 	 * Call board_init -- never returns
 	 */
 	pfx	%hi(board_init@h)
diff --git a/cpu/nios/traps.S b/cpu/nios/traps.S
index 655fc6358..bc4d3f66d 100644
--- a/cpu/nios/traps.S
+++ b/cpu/nios/traps.S
@@ -557,3 +557,26 @@ _timebase_int:
 	mov	%fp, %sp
 
 	tret	%o7			/* Done */
+
+/*************************************************************************
+ * GDB stubs
+ ************************************************************************/
+	.text
+	.global _brkpt_hw_int, _brkpt_sw_int
+	.align	4
+
+_brkpt_hw_int:
+	movi	%l1, 9
+	pfx	3
+	wrctl	%l1
+	pfx	4
+	wrctl	%l1
+
+_brkpt_sw_int:
+	movi	%l1, 9
+	pfx	3
+	wrctl	%l1
+	pfx	4
+	wrctl	%l1
+
+	tret	%o7