Merge with /home/wd/git/u-boot/custodian/u-boot-microblaze

11 files changed, 838 insertions, 8 deletions

diff --git a/cpu/microblaze/Makefile b/cpu/microblaze/Makefile
index fd544254f..db1afa553 100644
--- a/cpu/microblaze/Makefile
+++ b/cpu/microblaze/Makefile
@@ -26,7 +26,8 @@ include $(TOPDIR)/config.mk
 LIB	= $(obj)lib$(CPU).a
 
 START	= start.o
-COBJS	= cpu.o interrupts.o
+SOBJS	= dcache.o icache.o irq.o disable_int.o enable_int.o
+COBJS	= cpu.o interrupts.o cache.o exception.o timer.o
 
 SRCS	:= $(START:.o=.S) $(SOBJS:.o=.S) $(COBJS:.o=.c)
 OBJS	:= $(addprefix $(obj),$(SOBJS) $(COBJS))
diff --git a/cpu/microblaze/cache.c b/cpu/microblaze/cache.c
new file mode 100644
index 000000000..fc388ebb5
--- /dev/null
+++ b/cpu/microblaze/cache.c
@@ -0,0 +1,48 @@
+/*
+ * (C) Copyright 2007 Michal Simek
+ *
+ * Michal SIMEK <moonstr@monstr.eu>
+ *
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307 USA
+ */
+
+#include <common.h>
+
+#if (CONFIG_COMMANDS & CFG_CMD_CACHE)
+
+int dcache_status (void)
+{
+	int i = 0;
+	int mask = 0x80;
+	__asm__ __volatile__ ("mfs %0,rmsr"::"r" (i):"memory");
+	/* i&=0x80 */
+	__asm__ __volatile__ ("and %0,%0,%1"::"r" (i), "r" (mask):"memory");
+	return i;
+}
+
+int icache_status (void)
+{
+	int i = 0;
+	int mask = 0x20;
+	__asm__ __volatile__ ("mfs %0,rmsr"::"r" (i):"memory");
+	/* i&=0x20 */
+	__asm__ __volatile__ ("and %0,%0,%1"::"r" (i), "r" (mask):"memory");
+	return i;
+}
+#endif
diff --git a/cpu/microblaze/dcache.S b/cpu/microblaze/dcache.S
new file mode 100644
index 000000000..eaf96717e
--- /dev/null
+++ b/cpu/microblaze/dcache.S
@@ -0,0 +1,68 @@
+/*
+ * (C) Copyright 2007 Michal Simek
+ *
+ * Michal  SIMEK <monstr@monstr.eu>
+ *
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307 USA
+ */
+	.text
+	.globl	dcache_enable
+	.ent	dcache_enable
+	.align	2
+dcache_enable:
+	/* Make space on stack for a temporary */
+	addi	r1, r1, -4
+	/* Save register r12 */
+	swi	r12, r1, 0
+	/* Read the MSR register */
+	mfs	r12, rmsr
+	/* Set the instruction enable bit */
+	ori	r12, r12, 0x80
+	/* Save the MSR register */
+	mts	rmsr, r12
+	/* Load register r12 */
+	lwi	r12, r1, 0
+	/* Return */
+	rtsd	r15, 8
+	/* Update stack in the delay slot */
+	addi	r1, r1, 4
+	.end	dcache_enable
+
+	.text
+	.globl	dcache_disable
+	.ent	dcache_disable
+	.align	2
+dcache_disable:
+	/* Make space on stack for a temporary */
+	addi	r1, r1, -4
+	/* Save register r12 */
+	swi	r12, r1, 0
+	/* Read the MSR register */
+	mfs	r12, rmsr
+	/* Clear the data cache enable bit */
+	andi	r12, r12, ~0x80
+	/* Save the MSR register */
+	mts	rmsr, r12
+	/* Load register r12 */
+	lwi	r12, r1, 0
+	/* Return */
+	rtsd	r15, 8
+	/* Update stack in the delay slot */
+	addi	r1, r1, 4
+	.end	dcache_disable
diff --git a/cpu/microblaze/disable_int.S b/cpu/microblaze/disable_int.S
new file mode 100644
index 000000000..aecd79513
--- /dev/null
+++ b/cpu/microblaze/disable_int.S
@@ -0,0 +1,46 @@
+/*
+ * (C) Copyright 2007 Michal Simek
+ *
+ * Michal  SIMEK <monstr@monstr.eu>
+ *
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307 USA
+ */
+
+	.text
+	.globl	microblaze_disable_interrupts
+	.ent	microblaze_disable_interrupts
+	.align	2
+microblaze_disable_interrupts:
+	#Make space on stack for a temporary
+	addi	r1, r1, -4
+	#Save register r12
+	swi	r12, r1, 0
+	#Read the MSR register
+	mfs	r12, rmsr
+	#Clear the interrupt enable bit
+	andi	r12, r12, ~2
+	#Save the MSR register
+	mts	rmsr, r12
+	#Load register r12
+	lwi	r12, r1, 0
+	#Return
+	rtsd	r15, 8
+	#Update stack in the delay slot
+	addi	r1, r1, 4
+	.end	microblaze_disable_interrupts
diff --git a/cpu/microblaze/enable_int.S b/cpu/microblaze/enable_int.S
new file mode 100644
index 000000000..c096c6c3c
--- /dev/null
+++ b/cpu/microblaze/enable_int.S
@@ -0,0 +1,38 @@
+/*
+ * (C) Copyright 2007 Michal Simek
+ *
+ * Michal  SIMEK <monstrmonstr.eu>
+ *
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307 USA
+ */
+
+	.text
+	.globl	microblaze_enable_interrupts
+	.ent	microblaze_enable_interrupts
+	.align	2
+microblaze_enable_interrupts:
+	addi	r1, r1, -4
+	swi	r12, r1, 0
+	mfs	r12, rmsr
+	ori	r12, r12, 2
+	mts	rmsr, r12
+	lwi	r12, r1, 0
+	rtsd	r15, 8
+	addi	r1, r1, 4
+	.end	microblaze_enable_interrupts
diff --git a/cpu/microblaze/exception.c b/cpu/microblaze/exception.c
new file mode 100644
index 000000000..b135acbad
--- /dev/null
+++ b/cpu/microblaze/exception.c
@@ -0,0 +1,68 @@
+/*
+ * (C) Copyright 2007 Michal Simek
+ *
+ * Michal  SIMEK <monstr@monstr.eu>
+ *
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307 USA
+ */
+
+#include <common.h>
+
+void _hw_exception_handler (void)
+{
+	int address = 0;
+	int state = 0;
+	/* loading address of exception EAR */
+	__asm__ __volatile ("mfs %0,rear"::"r" (address):"memory");
+	/* loading excetpion state register ESR */
+	__asm__ __volatile ("mfs %0,resr"::"r" (state):"memory");
+	printf ("Hardware exception at 0x%x address\n", address);
+	switch (state & 0x1f) {	/* mask on exception cause */
+	case 0x1:
+		puts ("Unaligned data access exception\n");
+		break;
+	case 0x2:
+		puts ("Illegal op-code exception\n");
+		break;
+	case 0x3:
+		puts ("Instruction bus error exception\n");
+		break;
+	case 0x4:
+		puts ("Data bus error exception\n");
+		break;
+	case 0x5:
+		puts ("Divide by zero exception\n");
+		break;
+	default:
+		puts ("Undefined cause\n");
+		break;
+	}
+	printf ("Unaligned %sword access\n", ((state & 0x800) ? "" : "half"));
+	printf ("Unaligned %s access\n", ((state & 0x400) ? "store" : "load"));
+	printf ("Register R%x\n", (state & 0x3E) >> 5);
+	hang ();
+}
+
+#ifdef CFG_USR_EXCEP
+void _exception_handler (void)
+{
+	puts ("User vector_exception\n");
+	hang ();
+}
+#endif
diff --git a/cpu/microblaze/icache.S b/cpu/microblaze/icache.S
new file mode 100644
index 000000000..25940d106
--- /dev/null
+++ b/cpu/microblaze/icache.S
@@ -0,0 +1,69 @@
+/*
+ * (C) Copyright 2007 Michal Simek
+ *
+ * Michal  SIMEK <monstr@monstr.eu>
+ *
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307 USA
+ */
+
+	.text
+	.globl	icache_enable
+	.ent	icache_enable
+	.align	2
+icache_enable:
+	/* Make space on stack for a temporary */
+	addi	r1, r1, -4
+	/* Save register r12 */
+	swi	r12, r1, 0
+	/* Read the MSR register */
+	mfs	r12, rmsr
+	/* Set the instruction enable bit */
+	ori	r12, r12, 0x20
+	/* Save the MSR register */
+	mts	rmsr, r12
+	/* Load register r12 */
+	lwi	r12, r1, 0
+	/* Return */
+	rtsd	r15, 8
+	/* Update stack in the delay slot */
+	addi	r1, r1, 4
+	.end	icache_enable
+
+	.text
+	.globl	icache_disable
+	.ent	icache_disable
+	.align	2
+icache_disable:
+	/* Make space on stack for a temporary */
+	addi	r1, r1, -4
+	/* Save register r12 */
+	swi	r12, r1, 0
+	/* Read the MSR register */
+	mfs	r12, rmsr
+	/* Clear the instruction enable bit */
+	andi	r12, r12, ~0x20
+	/* Save the MSR register */
+	mts	rmsr, r12
+	/* Load register r12 */
+	lwi	r12, r1, 0
+	/* Return */
+	rtsd	r15, 8
+	/* Update stack in the delay slot */
+	addi	r1, r1, 4
+	.end	icache_disable
diff --git a/cpu/microblaze/interrupts.c b/cpu/microblaze/interrupts.c
index ccf67e172..2db847cd0 100644
--- a/cpu/microblaze/interrupts.c
+++ b/cpu/microblaze/interrupts.c
@@ -1,6 +1,8 @@
 /*
+ * (C) Copyright 2007 Michal Simek
  * (C) Copyright 2004 Atmark Techno, Inc.
  *
+ * Michal  SIMEK <monstr@monstr.eu>
  * Yasushi SHOJI <yashi@atmark-techno.com>
  *
  * See file CREDITS for list of people who contributed to this
@@ -13,7 +15,7 @@
  *
  * This program is distributed in the hope that it will be useful,
  * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.	 See the
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  * GNU General Public License for more details.
  *
  * You should have received a copy of the GNU General Public License
@@ -22,11 +24,185 @@
  * MA 02111-1307 USA
  */
 
-void enable_interrupts(void)
+#include <common.h>
+#include <command.h>
+#include <asm/microblaze_intc.h>
+
+#undef DEBUG_INT
+
+extern void microblaze_disable_interrupts (void);
+extern void microblaze_enable_interrupts (void);
+
+void enable_interrupts (void)
 {
+	microblaze_enable_interrupts ();
 }
 
-int disable_interrupts(void)
+int disable_interrupts (void)
 {
+	microblaze_disable_interrupts ();
 	return 0;
 }
+
+#ifdef CFG_INTC_0
+#ifdef CFG_TIMER_0
+extern void timer_init (void);
+#endif
+
+static struct irq_action vecs[CFG_INTC_0_NUM];
+
+/* mapping structure to interrupt controller */
+microblaze_intc_t *intc = (microblaze_intc_t *) (CFG_INTC_0_ADDR);
+
+/* default handler */
+void def_hdlr (void)
+{
+	puts ("def_hdlr\n");
+}
+
+void enable_one_interrupt (int irq)
+{
+	int mask;
+	int offset = 1;
+	offset <<= irq;
+	mask = intc->ier;
+	intc->ier = (mask | offset);
+#ifdef DEBUG_INT
+	printf ("Enable one interrupt irq %x - mask %x,ier %x\n", offset, mask,
+		intc->ier);
+	printf ("INTC isr %x, ier %x, iar %x, mer %x\n", intc->isr, intc->ier,
+		intc->iar, intc->mer);
+#endif
+}
+
+void disable_one_interrupt (int irq)
+{
+	int mask;
+	int offset = 1;
+	offset <<= irq;
+	mask = intc->ier;
+	intc->ier = (mask & ~offset);
+#ifdef DEBUG_INT
+	printf ("Disable one interrupt irq %x - mask %x,ier %x\n", irq, mask,
+		intc->ier);
+	printf ("INTC isr %x, ier %x, iar %x, mer %x\n", intc->isr, intc->ier,
+		intc->iar, intc->mer);
+#endif
+}
+
+/* adding new handler for interrupt */
+void install_interrupt_handler (int irq, interrupt_handler_t * hdlr, void *arg)
+{
+	struct irq_action *act;
+	/* irq out of range */
+	if ((irq < 0) || (irq > CFG_INTC_0_NUM)) {
+		puts ("IRQ out of range\n");
+		return;
+	}
+	act = &vecs[irq];
+	if (hdlr) {		/* enable */
+		act->handler = hdlr;
+		act->arg = arg;
+		act->count = 0;
+		enable_one_interrupt (irq);
+	} else {		/* disable */
+
+		act->handler = (interrupt_handler_t *) def_hdlr;
+		act->arg = (void *)irq;
+		disable_one_interrupt (irq);
+	}
+}
+
+/* initialization interrupt controller - hardware */
+void intc_init (void)
+{
+	intc->mer = 0;
+	intc->ier = 0;
+	intc->iar = 0xFFFFFFFF;
+	/* XIntc_Start - hw_interrupt enable and all interrupt enable */
+	intc->mer = 0x3;
+#ifdef DEBUG_INT
+	printf ("INTC isr %x, ier %x, iar %x, mer %x\n", intc->isr, intc->ier,
+		intc->iar, intc->mer);
+#endif
+}
+
+int interrupts_init (void)
+{
+	int i;
+	/* initialize irq list */
+	for (i = 0; i < CFG_INTC_0_NUM; i++) {
+		vecs[i].handler = (interrupt_handler_t *) def_hdlr;
+		vecs[i].arg = (void *)i;
+		vecs[i].count = 0;
+	}
+	/* initialize intc controller */
+	intc_init ();
+#ifdef CFG_TIMER_0
+	timer_init ();
+#endif
+	enable_interrupts ();
+	return 0;
+}
+
+void interrupt_handler (void)
+{
+	int irqs;
+	irqs = (intc->isr & intc->ier);	/* find active interrupt */
+
+#ifdef DEBUG_INT
+	printf ("INTC isr %x, ier %x, iar %x, mer %x\n", intc->isr, intc->ier,
+		intc->iar, intc->mer);
+	printf ("Interrupt handler on %x line, r14 %x\n", irqs, value);
+#endif
+	struct irq_action *act = vecs;
+	while (irqs) {
+		if (irqs & 1) {
+#ifdef DEBUG_INT
+			printf
+			    ("Jumping to interrupt handler rutine addr %x,count %x,arg %x\n",
+			     act->handler, act->count, act->arg);
+#endif
+			act->handler (act->arg);
+			act->count++;
+		}
+		irqs >>= 1;
+		act++;
+	}
+	intc->iar = 0xFFFFFFFF;	/* erase all events */
+#ifdef DEBUG
+	printf ("Dump INTC reg, isr %x, ier %x, iar %x, mer %x\n", intc->isr,
+		intc->ier, intc->iar, intc->mer);
+	printf ("Interrupt handler on %x line, r14\n", irqs);
+#endif
+}
+#endif
+
+#if (CONFIG_COMMANDS & CFG_CMD_IRQ)
+#ifdef CFG_INTC_0
+int do_irqinfo (cmd_tbl_t * cmdtp, int flag, int argc, char *argv[])
+{
+	int i;
+	struct irq_action *act = vecs;
+
+	puts ("\nInterrupt-Information:\n\n"
+	      "Nr  Routine   Arg       Count\n"
+	      "-----------------------------\n");
+
+	for (i = 0; i < CFG_INTC_0_NUM; i++) {
+		if (act->handler != (interrupt_handler_t*) def_hdlr) {
+			printf ("%02d  %08lx  %08lx  %d\n", i,
+				(int)act->handler, (int)act->arg, act->count);
+		}
+		act++;
+	}
+	puts ("\n");
+	return (0);
+}
+#else
+int do_irqinfo (cmd_tbl_t * cmdtp, int flag, int argc, char *argv[])
+{
+	puts ("Undefined interrupt controller\n");
+}
+#endif
+#endif				/* CONFIG_COMMANDS & CFG_CMD_IRQ */
diff --git a/cpu/microblaze/irq.S b/cpu/microblaze/irq.S
new file mode 100644
index 000000000..a4e3fbfad
--- /dev/null
+++ b/cpu/microblaze/irq.S
@@ -0,0 +1,165 @@
+/*
+ * (C) Copyright 2007 Michal Simek
+ *
+ * Michal  SIMEK <monstr@monstr.eu>
+ *
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307 USA
+ */
+
+#include <config.h>
+	.text
+	.global _interrupt_handler
+_interrupt_handler:
+	addi	r1, r1, -4
+	swi	r2, r1, 0
+	addi	r1, r1, -4
+	swi	r3, r1, 0
+	addi	r1, r1, -4
+	swi	r4, r1, 0
+	addi	r1, r1, -4
+	swi	r5, r1, 0
+	addi	r1, r1, -4
+	swi	r6, r1, 0
+	addi	r1, r1, -4
+	swi	r7, r1, 0
+	addi	r1, r1, -4
+	swi	r8, r1, 0
+	addi	r1, r1, -4
+	swi	r9, r1, 0
+	addi	r1, r1, -4
+	swi	r10, r1, 0
+	addi	r1, r1, -4
+	swi	r11, r1, 0
+	addi	r1, r1, -4
+	swi	r12, r1, 0
+	addi	r1, r1, -4
+	swi	r13, r1, 0
+	addi	r1, r1, -4
+	swi	r14, r1, 0
+	addi	r1, r1, -4
+	swi	r15, r1, 0
+	addi	r1, r1, -4
+	swi	r16, r1, 0
+	addi	r1, r1, -4
+	swi	r17, r1, 0
+	addi	r1, r1, -4
+	swi	r18, r1, 0
+	addi	r1, r1, -4
+	swi	r19, r1, 0
+	addi	r1, r1, -4
+	swi	r20, r1, 0
+	addi	r1, r1, -4
+	swi	r21, r1, 0
+	addi	r1, r1, -4
+	swi	r22, r1, 0
+	addi	r1, r1, -4
+	swi	r23, r1, 0
+	addi	r1, r1, -4
+	swi	r24, r1, 0
+	addi	r1, r1, -4
+	swi	r25, r1, 0
+	addi	r1, r1, -4
+	swi	r26, r1, 0
+	addi	r1, r1, -4
+	swi	r27, r1, 0
+	addi	r1, r1, -4
+	swi	r28, r1, 0
+	addi	r1, r1, -4
+	swi	r29, r1, 0
+	addi	r1, r1, -4
+	swi	r30, r1, 0
+	addi	r1, r1, -4
+	swi	r31, r1, 0
+	brlid	r15, interrupt_handler
+	nop
+	nop
+	lwi	r31, r1, 0
+	addi	r1, r1, 4
+	lwi	r30, r1, 0
+	addi	r1, r1, 4
+	lwi	r29, r1, 0
+	addi	r1, r1, 4
+	lwi	r28, r1, 0
+	addi	r1, r1, 4
+	lwi	r27, r1, 0
+	addi	r1, r1, 4
+	lwi	r26, r1, 0
+	addi	r1, r1, 4
+	lwi	r25, r1, 0
+	addi	r1, r1, 4
+	lwi	r24, r1, 0
+	addi	r1, r1, 4
+	lwi	r23, r1, 0
+	addi	r1, r1, 4
+	lwi	r22, r1, 0
+	addi	r1, r1, 4
+	lwi	r21, r1, 0
+	addi	r1, r1, 4
+	lwi	r20, r1, 0
+	addi	r1, r1, 4
+	lwi	r19, r1, 0
+	addi	r1, r1, 4
+	lwi	r18, r1, 0
+	addi	r1, r1, 4
+	lwi	r17, r1, 0
+	addi	r1, r1, 4
+	lwi	r16, r1, 0
+	addi	r1, r1, 4
+	lwi	r15, r1, 0
+	addi	r1, r1, 4
+	lwi	r14, r1, 0
+	addi	r1, r1, 4
+	lwi	r13, r1, 0
+	addi	r1, r1, 4
+	lwi	r12, r1, 0
+	addi	r1, r1, 4
+	lwi	r11, r1, 0
+	addi	r1, r1, 4
+	lwi	r10, r1, 0
+	addi	r1, r1, 4
+	lwi	r9, r1, 0
+	addi	r1, r1, 4
+	lwi	r8, r1, 0
+	addi	r1, r1, 4
+	lwi	r7, r1, 0
+	addi	r1, r1, 4
+	lwi	r6, r1, 0
+	addi	r1, r1, 4
+	lwi	r5, r1, 0
+	addi	r1, r1, 4
+	lwi	r4, r1, 0
+	addi	r1, r1, 4
+	lwi	r3, r1, 0
+	addi	r1, r1, 4
+	lwi	r2, r1, 0
+	addi	r1, r1, 4
+
+	/* enable_interrupt */
+	addi	r1, r1, -4
+	swi	r12, r1, 0
+	mfs	r12, rmsr
+	ori	r12, r12, 2
+	mts	rmsr, r12
+	lwi	r12, r1, 0
+	addi	r1, r1, 4
+	nop
+	bra	r14
+	nop
+	nop
+	.size _interrupt_handler,.-_interrupt_handler
diff --git a/cpu/microblaze/start.S b/cpu/microblaze/start.S
index 7efdbb097..29481af9a 100644
--- a/cpu/microblaze/start.S
+++ b/cpu/microblaze/start.S
@@ -1,6 +1,8 @@
 /*
+ * (C) Copyright 2007 Michal Simek
  * (C) Copyright 2004 Atmark Techno, Inc.
  *
+ * Michal  SIMEK <monstr@monstr.eu>
  * Yasushi SHOJI <yashi@atmark-techno.com>
  *
  * See file CREDITS for list of people who contributed to this
@@ -13,7 +15,7 @@
  *
  * This program is distributed in the hope that it will be useful,
  * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  * GNU General Public License for more details.
  *
  * You should have received a copy of the GNU General Public License
@@ -27,10 +29,91 @@
 	.text
 	.global _start
 _start:
+	mts	rmsr, r0	/* disable cache */
+	addi	r1, r0, CFG_INIT_SP_OFFSET
+	addi	r1, r1, -4	/* Decrement SP to top of memory */
+	/* add opcode instruction for 32bit jump - 2 instruction imm & brai*/
+	addi	r6, r0, 0xb000	/* hex b000 opcode imm */
+	bslli	r6, r6, 16	/* shift */
+	swi	r6, r0, 0x0	/* reset address */
+	swi	r6, r0, 0x8	/* user vector exception */
+	swi	r6, r0, 0x10	/* interrupt */
+	swi	r6, r0, 0x20	/* hardware exception */
 
-	addi	r1, r0, CFG_SDRAM_BASE	/* init stack pointer */
-	addi	r1, r1, CFG_SDRAM_SIZE	/* set sp to high up */
+	addi	r6, r0, 0xb808	/* hew b808 opcode brai*/
+	bslli	r6, r6, 16
+	swi	r6, r0, 0x4	/* reset address */
+	swi	r6, r0, 0xC	/* user vector exception */
+	swi	r6, r0, 0x14	/* interrupt */
+	swi	r6, r0, 0x24	/* hardware exception */
 
-	brai	board_init
+#ifdef CFG_RESET_ADDRESS
+	/* reset address */
+	addik	r6, r0, CFG_RESET_ADDRESS
+	sw	r6, r1, r0
+	lhu	r7, r1, r0
+	shi	r7, r0, 0x2
+	shi	r6, r0, 0x6
+/* 
+ * Copy U-Boot code to TEXT_BASE
+ * solve problem with sbrk_base
+ */
+#if (CFG_RESET_ADDRESS != TEXT_BASE)
+	addi	r4, r0, __end
+	addi	r5, r0, __text_start
+	rsub	r4, r5, r4	/* size = __end - __text_start */
+	addi	r6, r0, CFG_RESET_ADDRESS	/* source address */
+	addi	r7, r0, 0	/* counter */
+4:
+	lw	r8, r6, r7
+	sw	r8, r5, r7
+	addi	r7, r7, 0x4
+	cmp	r8, r4, r7
+	blti	r8, 4b
+#endif
+#endif
+
+#ifdef CFG_USR_EXCEP
+	/* user_vector_exception */
+	addik	r6, r0, _exception_handler
+	sw	r6, r1, r0
+	lhu	r7, r1, r0
+	shi	r7, r0, 0xa
+	shi	r6, r0, 0xe
+#endif
+
+#ifdef CFG_INTC_0
+	/* interrupt_handler */
+	addik	r6, r0, _interrupt_handler
+	sw	r6, r1, r0
+	lhu	r7, r1, r0
+	shi	r7, r0, 0x12
+	shi	r6, r0, 0x16
+#endif
 
+	/* hardware exception */
+	addik	r6, r0, _hw_exception_handler
+	sw	r6, r1, r0
+	lhu	r7, r1, r0
+	shi	r7, r0, 0x22
+	shi	r6, r0, 0x26
+
+	/* enable instruction and data cache */
+	mfs	r12, rmsr
+	ori	r12, r12, 0xa0
+	mts	rmsr, r12
+
+clear_bss:
+	/* clear BSS segments */
+	addi	r5, r0, __bss_start
+	addi	r4, r0, __bss_end
+	cmp	r6, r5, r4
+	beqi	r6, 3f
+2:
+	swi     r0, r5, 0 /* write zero to loc */
+	addi    r5, r5, 4 /* increment to next loc */
+	cmp     r6, r5, r4 /* check if we have reach the end */
+	bnei    r6, 2b
+3:	/* jumping to board_init */
+	brai	board_init
 1:	bri	1b
diff --git a/cpu/microblaze/timer.c b/cpu/microblaze/timer.c
new file mode 100644
index 000000000..be4fd57cc
--- /dev/null
+++ b/cpu/microblaze/timer.c
@@ -0,0 +1,68 @@
+/*
+ * (C) Copyright 2007 Michal Simek
+ *
+ * Michal  SIMEK <monstr@monstr.eu>
+ *
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307 USA
+ */
+
+#include <common.h>
+#include <asm/microblaze_timer.h>
+
+volatile int timestamp = 0;
+
+void reset_timer (void)
+{
+	timestamp = 0;
+}
+
+ulong get_timer (ulong base)
+{
+	return (timestamp - base);
+}
+
+void set_timer (ulong t)
+{
+	timestamp = t;
+}
+
+#ifdef CFG_INTC_0
+#ifdef CFG_TIMER_0
+extern void install_interrupt_handler (int irq, interrupt_handler_t * hdlr,
+				       void *arg);
+
+microblaze_timer_t *tmr = (microblaze_timer_t *) (CFG_TIMER_0_ADDR);
+
+void timer_isr (void *arg)
+{
+	timestamp++;
+	tmr->control = tmr->control | TIMER_INTERRUPT;
+}
+
+void timer_init (void)
+{
+	tmr->loadreg = CFG_TIMER_0_PRELOAD;
+	tmr->control = TIMER_INTERRUPT | TIMER_RESET;
+	tmr->control =
+	    TIMER_ENABLE | TIMER_ENABLE_INTR | TIMER_RELOAD | TIMER_DOWN_COUNT;
+	reset_timer ();
+	install_interrupt_handler (CFG_TIMER_0_IRQ, timer_isr, (void *)tmr);
+}
+#endif
+#endif