sparc: leon3: Added memory controller initialization using new AMBA PnP routines.

Signed-off-by: Daniel Hellstrom <daniel@gaisler.com>

5 files changed, 621 insertions, 2 deletions

diff --git a/arch/sparc/cpu/leon3/Makefile b/arch/sparc/cpu/leon3/Makefile
index c5068a8ecfe..f4cf43cfec3 100644
--- a/arch/sparc/cpu/leon3/Makefile
+++ b/arch/sparc/cpu/leon3/Makefile
@@ -7,4 +7,4 @@
 
 extra-y	= start.o
 obj-y	= cpu_init.o serial.o cpu.o ambapp.o ambapp_low.o ambapp_low_c.o \
-	interrupts.o prom.o usb_uhci.o
+	interrupts.o prom.o usb_uhci.o memcfg.o memcfg_low.o
diff --git a/arch/sparc/cpu/leon3/memcfg.c b/arch/sparc/cpu/leon3/memcfg.c
new file mode 100644
index 00000000000..b9eda440e22
--- /dev/null
+++ b/arch/sparc/cpu/leon3/memcfg.c
@@ -0,0 +1,237 @@
+/* GRLIB Memory controller setup. The register values are used
+ * from the associated low level assembler routine implemented
+ * in memcfg_low.S.
+ *
+ * (C) Copyright 2010, 2015
+ * Daniel Hellstrom, Cobham Gaisler, daniel@gaisler.com.
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <ambapp.h>
+#include "memcfg.h"
+#include <config.h>
+
+#ifdef CONFIG_SYS_GRLIB_ESA_MCTRL1
+struct mctrl_setup esa_mctrl1_cfg = {
+	.reg_mask = 0x7,
+	.regs = {
+		{
+			.mask = 0x00000300,
+			.value = CONFIG_SYS_GRLIB_ESA_MCTRL1_CFG1,
+		},
+		{
+			.mask = 0x00000000,
+			.value = CONFIG_SYS_GRLIB_ESA_MCTRL1_CFG2,
+		},
+		{
+			.mask = 0x00000000,
+			.value = CONFIG_SYS_GRLIB_ESA_MCTRL1_CFG3,
+		},
+	}
+};
+#ifdef CONFIG_SYS_GRLIB_ESA_MCTRL2
+struct mctrl_setup esa_mctrl2_cfg = {
+	.reg_mask = 0x7,
+	.regs = {
+		{
+			.mask = 0x00000300,
+			.value = CONFIG_SYS_GRLIB_ESA_MCTRL2_CFG1,
+		},
+		{
+			.mask = 0x00000000,
+			.value = CONFIG_SYS_GRLIB_ESA_MCTRL2_CFG2,
+		},
+		{
+			.mask = 0x00000000,
+			.value = CONFIG_SYS_GRLIB_ESA_MCTRL2_CFG3,
+		},
+	}
+};
+#endif
+#endif
+
+#ifdef CONFIG_SYS_GRLIB_GAISLER_FTMCTRL1
+struct mctrl_setup gaisler_ftmctrl1_cfg = {
+	.reg_mask = 0x7,
+	.regs = {
+		{
+			.mask = 0x00000300,
+			.value = CONFIG_SYS_GRLIB_GAISLER_FTMCTRL1_CFG1,
+		},
+		{
+			.mask = 0x00000000,
+			.value = CONFIG_SYS_GRLIB_GAISLER_FTMCTRL1_CFG2,
+		},
+		{
+			.mask = 0x00000000,
+			.value = CONFIG_SYS_GRLIB_GAISLER_FTMCTRL1_CFG3,
+		},
+	}
+};
+#ifdef CONFIG_SYS_GRLIB_GAISLER_FTMCTRL2
+struct mctrl_setup gaisler_ftmctrl2_cfg = {
+	.reg_mask = 0x7,
+	.regs = {
+		{
+			.mask = 0x00000300,
+			.value = CONFIG_SYS_GRLIB_GAISLER_FTMCTRL2_CFG1,
+		},
+		{
+			.mask = 0x00000000,
+			.value = CONFIG_SYS_GRLIB_GAISLER_FTMCTRL2_CFG2,
+		},
+		{
+			.mask = 0x00000000,
+			.value = CONFIG_SYS_GRLIB_GAISLER_FTMCTRL2_CFG3,
+		},
+	}
+};
+#endif
+#endif
+
+#ifdef CONFIG_SYS_GRLIB_GAISLER_SDCTRL1
+struct mctrl_setup gaisler_sdctrl1_cfg = {
+	.reg_mask = 0x1,
+	.regs = {
+		{
+			.mask = 0x00000000,
+			.value = CONFIG_SYS_GRLIB_GAISLER_SDCTRL1_CTRL,
+		},
+	}
+};
+#ifdef CONFIG_SYS_GRLIB_GAISLER_SDCTRL2
+struct mctrl_setup gaisler_sdctrl2_cfg = {
+	.reg_mask = 0x1,
+	.regs = {
+		{
+			.mask = 0x00000000,
+			.value = CONFIG_SYS_GRLIB_GAISLER_SDCTRL2_CTRL,
+		},
+	}
+};
+#endif
+#endif
+
+#ifdef CONFIG_SYS_GRLIB_GAISLER_DDR2SPA1
+struct ahbmctrl_setup gaisler_ddr2spa1_cfg = {
+	.ahb_mbar_no = 1,
+	.reg_mask = 0xd,
+	.regs = {
+		{
+			.mask = 0x00000000,
+			.value = CONFIG_SYS_GRLIB_GAISLER_DDR2SPA1_CFG1,
+		},
+		{ 0x00000000, 0},
+		{
+			.mask = 0x00000000,
+			.value = CONFIG_SYS_GRLIB_GAISLER_DDR2SPA1_CFG3,
+		},
+		{
+			.mask = 0x00000000,
+			.value = CONFIG_SYS_GRLIB_GAISLER_DDR2SPA1_CFG4,
+		},
+	}
+};
+#ifdef CONFIG_SYS_GRLIB_GAISLER_DDR2SPA2
+struct ahbmctrl_setup gaisler_ddr2spa2_cfg = {
+	.ahb_mbar_no = 1,
+	.reg_mask = 0xd,
+	.regs = {
+		{
+			.mask = 0x00000000,
+			.value = CONFIG_SYS_GRLIB_GAISLER_DDR2SPA2_CFG1,
+		},
+		{ 0x00000000, 0},
+		{
+			.mask = 0x00000000,
+			.value = CONFIG_SYS_GRLIB_GAISLER_DDR2SPA2_CFG3,
+		},
+		{
+			.mask = 0x00000000,
+			.value = CONFIG_SYS_GRLIB_GAISLER_DDR2SPA2_CFG4,
+		},
+	}
+};
+#endif
+#endif
+
+#ifdef CONFIG_SYS_GRLIB_GAISLER_DDRSPA1
+struct ahbmctrl_setup gaisler_ddrspa1_cfg = {
+	.ahb_mbar_no = 1,
+	.reg_mask = 0x1,
+	.regs = {
+		{
+			.mask = 0x00000000,
+			.value = CONFIG_SYS_GRLIB_GAISLER_DDRSPA1_CTRL,
+		},
+	}
+};
+#ifdef CONFIG_SYS_GRLIB_GAISLER_DDRSPA2
+struct ahbmctrl_setup gaisler_ddrspa2_cfg = {
+	.ahb_mbar_no = 1,
+	.reg_mask = 0x1,
+	.regs = {
+		{
+			.mask = 0x00000000,
+			.value = CONFIG_SYS_GRLIB_GAISLER_DDRSPA2_CTRL,
+		},
+	}
+};
+#endif
+#endif
+
+struct grlib_mctrl_handler grlib_mctrl_handlers[] = {
+/* ESA MCTRL (PROM/FLASH/IO/SRAM/SDRAM) */
+#ifdef CONFIG_SYS_GRLIB_ESA_MCTRL1
+	{DEV_APB_SLV, 0, MH_UNUSED, AMBA_PNP_ID(VENDOR_ESA, ESA_MCTRL),
+	_nomem_mctrl_init, (void *)&esa_mctrl1_cfg},
+#ifdef CONFIG_SYS_GRLIB_ESA_MCTRL2
+	{DEV_APB_SLV, 1, MH_UNUSED, AMBA_PNP_ID(VENDOR_ESA, ESA_MCTRL),
+	_nomem_mctrl_init, (void *)&esa_mctrl2_cfg},
+#endif
+#endif
+
+/* GAISLER Fault Tolerant Memory controller (PROM/FLASH/IO/SRAM/SDRAM) */
+#ifdef CONFIG_SYS_GRLIB_GAISLER_FTMCTRL1
+	{DEV_APB_SLV, 0, MH_UNUSED, AMBA_PNP_ID(VENDOR_GAISLER, GAISLER_FTMCTRL),
+	_nomem_mctrl_init, (void *)&gaisler_ftmctrl1_cfg},
+#ifdef CONFIG_SYS_GRLIB_GAISLER_FTMCTRL2
+	{DEV_APB_SLV, 1, MH_UNUSED, AMBA_PNP_ID(VENDOR_GAISLER, GAISLER_FTMCTRL),
+	_nomem_mctrl_init, (void *)&gaisler_ftmctrl2_cfg},
+#endif
+#endif
+
+/* GAISLER SDRAM-only Memory controller (SDRAM) */
+#ifdef CONFIG_SYS_GRLIB_GAISLER_SDCTRL1
+	{DEV_APB_SLV, 0, MH_UNUSED, AMBA_PNP_ID(VENDOR_GAISLER, GAISLER_SDCTRL),
+	_nomem_mctrl_init, (void *)&gaisler_sdctrl1_cfg},
+#ifdef CONFIG_SYS_GRLIB_GAISLER_SDCTRL2
+	{DEV_APB_SLV, 1, MH_UNUSED, AMBA_PNP_ID(VENDOR_GAISLER, GAISLER_SDCTRL),
+	_nomem_mctrl_init, (void *)&gaisler_sdctrl2_cfg},
+#endif
+#endif
+
+/* GAISLER DDR Memory controller (DDR) */
+#ifdef CONFIG_SYS_GRLIB_GAISLER_DDRSPA1
+	{DEV_AHB_SLV, 0, MH_UNUSED, AMBA_PNP_ID(VENDOR_GAISLER, GAISLER_DDRSP),
+	_nomem_ahbmctrl_init, (void *)&gaisler_ddrspa1_cfg},
+#ifdef CONFIG_SYS_GRLIB_GAISLER_DDRSPA2
+	{DEV_AHB_SLV, 1, MH_UNUSED, AMBA_PNP_ID(VENDOR_GAISLER, GAISLER_DDRSP),
+	_nomem_ahbmctrl_init, (void *)&gaisler_ddrspa2_cfg},
+#endif
+#endif
+
+/* GAISLER DDR2 Memory controller (DDR2) */
+#ifdef CONFIG_SYS_GRLIB_GAISLER_DDR2SPA1
+	{DEV_AHB_SLV, 0, MH_UNUSED, AMBA_PNP_ID(VENDOR_GAISLER, GAISLER_DDR2SP),
+	_nomem_ahbmctrl_init, (void *)&gaisler_ddr2spa1_cfg},
+#ifdef CONFIG_SYS_GRLIB_GAISLER_DDR2SPA2
+	{DEV_AHB_SLV, 1, MH_UNUSED, AMBA_PNP_ID(VENDOR_GAISLER, GAISLER_DDR2SP),
+	_nomem_ahbmctrl_init, (void *)&gaisler_ddr2spa2_cfg},
+#endif
+#endif
+
+	/* Mark end */
+	MH_END
+};
diff --git a/arch/sparc/cpu/leon3/memcfg.h b/arch/sparc/cpu/leon3/memcfg.h
new file mode 100644
index 00000000000..a524896e5ad
--- /dev/null
+++ b/arch/sparc/cpu/leon3/memcfg.h
@@ -0,0 +1,90 @@
+/* GRLIB Memory controller setup structures
+ *
+ * (C) Copyright 2010, 2015
+ * Daniel Hellstrom, Cobham Gaisler, daniel@gaisler.com.
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#ifndef __MEMCFG_H__
+#define __MEMCFG_H__
+
+/*********** Low Level Memory Controller Initalization ***********/
+
+#ifndef __ASSEMBLER__
+
+struct grlib_mctrl_handler;
+
+typedef void (*mctrl_handler_t)(
+	struct grlib_mctrl_handler *dev,
+	void *conf,
+	unsigned int ioarea
+	);
+
+/* Memory Controller Handler Structure */
+struct grlib_mctrl_handler {
+	unsigned char	type;		/* 0x00. MASK: AHB MST&SLV, APB SLV */
+	char		index;		/* 0x01. Unit number, 0, 1, 2... */
+	char		unused[2];	/* 0x02 */
+	unsigned int	ven_dev;	/* 0x04. Device and Vendor */
+	mctrl_handler_t	func;		/* 0x08. Memory Controller Handler */
+	void		*priv;		/* 0x0c. Optional private data, ptr to
+					 * info how to set up controller */
+};
+
+extern struct grlib_mctrl_handler grlib_mctrl_handlers[];
+
+#endif
+
+#define MH_STRUCT_SIZE		(4*4)
+#define MH_TYPE			0x00
+#define MH_INDEX		0x01
+#define MH_VENDOR_DEVICE	0x04
+#define MH_FUNC			0x08
+#define MH_PRIV			0x0c
+
+#define MH_TYPE_NONE	DEV_NONE
+#define MH_TYPE_AHB_MST	DEV_AHB_MST
+#define MH_TYPE_AHB_SLV	DEV_AHB_SLV
+#define MH_TYPE_APB_SLV	DEV_APB_SLV
+
+#define MH_UNUSED	{0, 0}
+#define MH_END		{DEV_NONE, 0, MH_UNUSED, AMBA_PNP_ID(0, 0), 0, 0}
+
+/*********** Low Level Memory Controller Initalization Handlers ***********/
+
+#ifndef __ASSEMBLER__
+extern void _nomem_mctrl_init(
+	struct grlib_mctrl_handler *dev,
+	void *conf,
+	unsigned int ioarea_apbmst);
+
+struct mctrl_setup {
+	unsigned int reg_mask;		/* Which registers to write */
+	struct {
+		unsigned int mask;	/* Mask used keep reg bits unchanged */
+		unsigned int value;	/* Value written to register */
+	} regs[8];
+};
+
+extern void _nomem_ahbmctrl_init(
+	struct grlib_mctrl_handler *dev,
+	void *conf,
+	unsigned int ioarea_apbmst);
+
+struct ahbmctrl_setup {
+	int ahb_mbar_no;		/* MBAR to get register address from */
+	unsigned int reg_mask;		/* Which registers to write */
+	struct {
+		unsigned int mask;	/* Mask used keep reg bits unchanged */
+		unsigned int value;	/* Value written to register */
+	} regs[8];
+};
+#endif
+
+/* mctrl_setup data structure defines */
+#define NREGS_OFS 0
+#define REGS_OFS 0x4
+#define REGS_SIZE 8
+
+#endif
diff --git a/arch/sparc/cpu/leon3/memcfg_low.S b/arch/sparc/cpu/leon3/memcfg_low.S
new file mode 100644
index 00000000000..84a81a90a9a
--- /dev/null
+++ b/arch/sparc/cpu/leon3/memcfg_low.S
@@ -0,0 +1,253 @@
+/* This is the memory initialization functions, the function
+ * implemented below initializes each memory controller
+ * found and specified by the input grlib_mctrl_handler structure.
+ *
+ * After the memory controllers have been initialized the stack
+ * can be used.
+ *
+ * (C) Copyright 2010, 2015
+ * Daniel Hellstrom, Cobham Gaisler, daniel@gaisler.com.
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <ambapp.h>
+#include "memcfg.h"
+#include <config.h>
+
+	.seg	"text"
+	.globl	_nomem_memory_ctrl_init
+	.globl	_nomem_mctrl_init, _nomem_ahbmctrl_init
+	.extern	_nomem_find_apb
+	.extern	_nomem_find_ahb
+
+
+/* FUNCTION
+ *   _nomem_memory_controller_init(struct grlib_mctrl_handler *mem_handlers)
+ *
+ * Initialize AMBA devices, _nomem_amba_init() has prepared i0-i5
+ * with the AHB buses on the system.
+ *
+ * For each entry in mem_handlers find the VENDOR:DEVICE and handle it
+ * by calling the handler function pointer.
+ *
+ * Constraints:
+ *  i6, i7, o6, l7, l6, g3, g4, g5, g6, g7 is used by caller
+ *  o7 is return address
+ *  l5 reserved for this function for future use.
+ *
+ * Arguments
+ *  - o0 Pointer to memory handler array
+ *
+ * Results
+ *  - o0 Number of memory controllers found
+ *
+ * Clobbered
+ *  - o0 (Current AHB slave conf address)
+ *  - l0 (mem handler entry address)
+ *  - l1 (Return value, number of memory controllers found)
+ *  - o7 (function pointer)
+ *  - l0, l1, l2, l3, l4, g1, g2 (used by _nomem_ambapp_find_buses)
+ *  - o0, o1, o2, o3, o4, o5 (Used as arguments)
+ *
+ *  - g1 ( level 1 return address)
+ *  - g2 ( level 2 return address)
+ */
+
+_nomem_memory_ctrl_init:
+	/* At this point all AHB buses has been found and the I/O Areas of
+	 * all AHB buses is stored in the i0-i5 registers. Max 6 buses. Next,
+	 * memory controllers are found by searching all buses for matching
+	 * VENDOR:DEVICE. The VENDOR:DEVICE to search for are taken from the
+	 * mem_handlers array. For each match the function pointer stored in
+	 * the mem_handler entry is called to handle the hardware setup.
+	 */
+	mov	%o7, %g1	/* Save return address */
+	mov	%o0, %l0
+	mov	%g0, %l1	/* The return value */
+
+.L_do_one_mem_handler:
+	ld	[%l0 + MH_FUNC], %o7
+	cmp	%o7, %g0
+	be	.L_all_mctrl_handled
+	 nop
+
+	/*** Scan for memory controller ***/
+
+	/* Set up argments, o5 not used by _nomem_find_apb */
+	ldub	[%l0 + MH_TYPE], %o5
+	clr	%o4
+	clr	%o3
+	ldub	[%l0 + MH_INDEX], %o2
+	ld	[%l0 + MH_VENDOR_DEVICE], %o1
+
+	/* An empty config? */
+	cmp	%o5, DEV_NONE
+	beq	.L_all_mctrl_next
+
+	/* Select function (APB or AHB) */
+	 cmp	%o5, DEV_APB_SLV
+	bne	.L_find_ahb_memctrl
+	 clr	%o0
+.L_find_apb_memctrl:
+	call	_nomem_find_apb			/* Scan for APB slave device */
+	 nop
+
+	/* o3 = iobar address
+	 * o4 = AHB Bus index
+	 *
+	 * REG ADR = ((iobar >> 12) & (iobar << 4) & 0xfff00) | "APB Base"
+	 */
+	ld	[%o3 + AMBA_APB_IOBAR_OFS], %o5
+	srl	%o5, 12, %o2
+	sll	%o5, 4, %o5
+	and	%o2, %o5, %o5
+	set	0xfff00, %o2
+	and	%o2, %o5, %o5
+	sethi	%hi(0xfff00000), %o2
+	and	%o3, %o2, %o2
+	or	%o5, %o2, %o5	/* Register base address */
+
+	ba	.L_call_one_mem_handler
+	 nop
+
+.L_find_ahb_memctrl:
+	call	_nomem_find_ahb		/* Scan for AHB Slave or Master.
+					 * o5 determine type. */
+	 nop
+	clr	%o5
+
+	/* Call the handler function if the hardware was found
+	 *
+	 * o0 = mem_handler
+	 * o1 = Configuration address
+	 * o2 = AHB Bus index
+	 * o3 = APB Base register (if APB Slave)
+	 *
+	 * Constraints:
+	 * i0-i7, l0, l1, l5, g1, g3-g7 may no be used.
+	 */
+.L_call_one_mem_handler:
+	cmp	%o0, %g0
+	be	.L_all_mctrl_next
+	 mov	%l0, %o0			/* Mem handler pointer */
+	mov	%o3, %o1			/* AMBA PnP Configuration address */
+	mov	%o4, %o2			/* AHB Bus index */
+	ld	[%l0 + MH_FUNC], %o7	/* Get Function pointer */
+	call	%o7
+	 mov	%o5, %o3			/* APB Register Base Address */
+
+	inc	%l1				/* Number of Memory controllers
+						 * handled. */
+
+	/* Do next entry in mem_handlers */
+.L_all_mctrl_next:
+	ba	.L_do_one_mem_handler
+	 add	%l0, MH_STRUCT_SIZE, %l0
+
+.L_all_mctrl_handled:
+	mov	%g1, %o7	/* Restore return address */
+	retl
+	 mov	%l1, %o0
+
+
+
+/* Generic Memory controller initialization routine (APB Registers)
+ *
+ * o0 = mem_handler structure pointer
+ * o1 = Configuration address
+ * o2 = AHB Bus index
+ * o3 = APB Base register
+ *
+ * Clobbered
+ *  o0-o4
+ */
+_nomem_mctrl_init:
+	ld	[%o0 + MH_PRIV], %o0	/* Get Private structure */
+	ld	[%o0], %o1		/* Get Reg Mask */
+	and	%o1, 0xff, %o1
+	add	%o0, REGS_OFS, %o0	/* Point to first reg */
+.L_do_one_reg:
+	andcc	%o1, 0x1, %g0
+	beq	.L_do_next_reg
+	 ld	[%o0], %o2
+	ld	[%o3], %o4
+	and	%o4, %o2, %o4
+	ld	[%o0 + 4], %o2
+	or	%o4, %o2, %o4
+	st	%o4, [%o3]
+
+.L_do_next_reg:
+	add	%o0, REGS_SIZE, %o0
+	add	%o3, 4, %o3
+	srl	%o1, 1, %o1
+	cmp	%o1, 0
+	bne	.L_do_one_reg
+	 nop
+
+	/* No more registers to write */
+	retl
+	 nop
+
+
+
+/* Generic Memory controller initialization routine (AHB Registers)
+ *
+ * o0 = mem_handler structure pointer
+ * o1 = Configuration address of memory controller
+ * o2 = AHB Bus index
+ *
+ * Clobbered
+ *  o0-o5
+ */
+_nomem_ahbmctrl_init:
+	ld	[%o0 + MH_PRIV], %o0		/* Get Private structure */
+
+	/* Get index of AHB MBAR to get registers from */
+	ld	[%o0], %o5
+	add	%o0, 4, %o0
+
+	/* Get Address of MBAR in PnP info */
+	add	%o5, 4, %o5
+	sll	%o5, 2, %o5
+	add	%o5, %o1, %o5			/* Address of MBAR */
+
+	/* Get Address of registers from PnP information
+	 * Address is in AHB I/O format, i.e. relative to bus
+	 *
+	 * ADR = (iobar & (iobar << 16) & 0xfff00000)
+	 * IOADR = (ADR >> 12) | "APB Base"
+	 */
+	ld	[%o5], %o5
+	sll	%o5, 16, %o4
+	and	%o5, %o4, %o5
+	sethi	%hi(0xfff00000), %o4
+	and	%o5, %o4, %o5			/* ADR */
+	and	%o4, %o1, %o4
+	srl	%o5, 12, %o5
+	or	%o5, %o4, %o3			/* IOADR in o3 */
+
+	ld	[%o0], %o1			/* Get Reg Mask */
+	and	%o1, 0xff, %o1
+	add	%o0, REGS_OFS, %o0		/* Point to first reg */
+.L_do_one_ahbreg:
+	andcc	%o1, 0x1, %g0
+	beq	.L_do_next_reg
+	 ld	[%o0], %o2
+	ld	[%o3], %o4
+	and	%o4, %o2, %o4
+	ld	[%o0 + 4], %o2
+	or	%o4, %o2, %o4
+	st	%o4, [%o3]
+
+.L_do_next_ahbreg:
+	add	%o0, REGS_SIZE, %o0
+	add	%o3, 4, %o3
+	srl	%o1, 1, %o1
+	cmp	%o1, 0
+	bne	.L_do_one_reg
+	 nop
+
+	/* No more registers to write */
+	retl
+	 nop
diff --git a/arch/sparc/cpu/leon3/start.S b/arch/sparc/cpu/leon3/start.S
index be9b3fb20b0..203114970b2 100644
--- a/arch/sparc/cpu/leon3/start.S
+++ b/arch/sparc/cpu/leon3/start.S
@@ -13,6 +13,12 @@
 #include <asm/stack.h>
 #include <asm/leon.h>
 #include <version.h>
+#include <ambapp.h>
+
+/* Default Plug&Play I/O area */
+#ifndef CONFIG_AMBAPP_IOAREA
+#define CONFIG_AMBAPP_IOAREA AMBA_DEFAULT_IOAREA
+#endif
 
 /* Entry for traps which jump to a programmer-specified trap handler.  */
 #define TRAPR(H)  \
@@ -208,6 +214,7 @@ version_string:
 	.ascii U_BOOT_VERSION_STRING, "\0"
 
 	.section	".text"
+	.extern	_nomem_amba_init, _nomem_memory_ctrl_init
 	.align 4
 
 _hardreset:
@@ -259,6 +266,38 @@ stackp:
 	andn	%fp, 0x0f, %fp
 	sub	%fp, 64, %sp
 
+/* Obtain the address of _GLOBAL_OFFSET_TABLE_ */
+	SPARC_PIC_THUNK_CALL(l7)
+
+/* Scan AMBA Bus for AMBA buses using PnP information. All found
+ * AMBA buses I/O area will be located in i0-i5 upon return.
+ * The i0-i5 registers are later used by _nomem_amba_init2
+ */
+ambainit:
+	call	_nomem_amba_init
+	 sethi	%hi(CONFIG_AMBAPP_IOAREA), %o0
+
+/* Scan AMBA Buses for memory controllers, then initialize the
+ * memory controllers. Note that before setting up the memory controller
+ * the stack can not be used.
+ */
+memory_ctrl_init:
+	SPARC_LOAD_ADDRESS(grlib_mctrl_handlers, l7, o0)
+
+	call	_nomem_memory_ctrl_init
+	 nop
+
+/* The return valu indicate how many memory controllers where found and
+ * initialized, if no memory controller was initialized, we can not continue
+ * because from here on we expect memory to be working.
+ */
+	cmp	%o0, 0
+memory_ctrl_init_failed:
+	beq	memory_ctrl_init_failed
+	 nop
+
+/*** From now on the stack can be used. ***/
+
 cpu_init_unreloc:
 	call	cpu_init_f
 	 nop
@@ -269,7 +308,6 @@ cpu_init_unreloc:
 /* un relocated end address of monitor */
 #define DATA_END __init_end
 
-	SPARC_PIC_THUNK_CALL(l7)
 reloc:
 	SPARC_LOAD_ADDRESS(TEXT_START, l7, g2)
 	SPARC_LOAD_ADDRESS(DATA_END, l7, g3)
@@ -389,6 +427,7 @@ jump:
 
 	SPARC_LOAD_ADDRESS(board_init_f, l7, o1)
 	set	CONFIG_SYS_RELOC_MONITOR_BASE,%o2
+	SPARC_LOAD_ADDRESS(TEXT_START, l7, g1)
 	add	%o1,%o2,%o1
 	sub	%o1,%g1,%o1
 	call	%o1