diff mbox

[U-Boot,v1,09/12] sparc: leon3: Added memory controller initialization using new AMBA PnP routines.

Message ID 1446565726-10777-10-git-send-email-fgretief@spaceteq.co.za
State Changes Requested
Delegated to: Tom Rini
Headers show

Commit Message

Francois Retief Nov. 3, 2015, 3:48 p.m. UTC
From: Daniel Hellstrom <daniel@gaisler.com>

Signed-off-by: Daniel Hellstrom <daniel@gaisler.com>
Signed-off-by: Francois Retief <fgretief@spaceteq.co.za>
---

 arch/sparc/cpu/leon3/Makefile     |   2 +-
 arch/sparc/cpu/leon3/memcfg.c     | 237 +++++++++++++++++++++++++++++++++++
 arch/sparc/cpu/leon3/memcfg.h     |  90 ++++++++++++++
 arch/sparc/cpu/leon3/memcfg_low.S | 253 ++++++++++++++++++++++++++++++++++++++
 arch/sparc/cpu/leon3/start.S      |  41 +++++-
 include/configs/gr_cpci_ax2000.h  |  27 ++--
 include/configs/gr_ep2s60.h       |  32 +++--
 include/configs/gr_xc3s_1500.h    |  23 ++--
 include/configs/grsim.h           |  26 ++--
 9 files changed, 688 insertions(+), 43 deletions(-)
 create mode 100644 arch/sparc/cpu/leon3/memcfg.c
 create mode 100644 arch/sparc/cpu/leon3/memcfg.h
 create mode 100644 arch/sparc/cpu/leon3/memcfg_low.S

--
2.4.3
diff mbox

Patch

diff --git a/arch/sparc/cpu/leon3/Makefile b/arch/sparc/cpu/leon3/Makefile
index c5068a8..f4cf43c 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 0000000..b9eda44
--- /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 0000000..a524896
--- /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 0000000..84a81a9
--- /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 be9b3fb..2031149 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
diff --git a/include/configs/gr_cpci_ax2000.h b/include/configs/gr_cpci_ax2000.h
index 8461787..5bbf1aa 100644
--- a/include/configs/gr_cpci_ax2000.h
+++ b/include/configs/gr_cpci_ax2000.h
@@ -312,31 +312,36 @@ 

 #define CONFIG_SYS_GRLIB_SDRAM    0

+/* No SDRAM Configuration */
+#undef CONFIG_SYS_GRLIB_GAISLER_SDCTRL1
+
 /* See, GRLIB Docs (grip.pdf) on how to set up
  * These the memory controller registers.
  */
-#define CONFIG_SYS_GRLIB_MEMCFG1   (0x10f800ff | (1<<11))
+#define CONFIG_SYS_GRLIB_ESA_MCTRL1
+#define CONFIG_SYS_GRLIB_ESA_MCTRL1_CFG1   (0x10f800ff | (1<<11))
 #if CONFIG_LEON_RAM_SELECT == CONFIG_LEON_RAM_SDRAM_NOSRAM
-#define CONFIG_SYS_GRLIB_MEMCFG2   0x82206000
+#define CONFIG_SYS_GRLIB_ESA_MCTRL1_CFG2   0x82206000
 #else
-#define CONFIG_SYS_GRLIB_MEMCFG2   0x82205260
+#define CONFIG_SYS_GRLIB_ESA_MCTRL1_CFG2   0x82205260
 #endif
-#define CONFIG_SYS_GRLIB_MEMCFG3   0x0809a000
+#define CONFIG_SYS_GRLIB_ESA_MCTRL1_CFG3   0x0809a000

-#define CONFIG_SYS_GRLIB_FT_MEMCFG1   (0x10f800ff | (1<<11))
+/* GRLIB FT-MCTRL configuration */
+#define CONFIG_SYS_GRLIB_GAISLER_FTMCTRL1
+#define CONFIG_SYS_GRLIB_GAISLER_FTMCTRL1_CFG1   (0x10f800ff | (1<<11))
 #if CONFIG_LEON_RAM_SELECT == CONFIG_LEON_RAM_SDRAM_NOSRAM
-#define CONFIG_SYS_GRLIB_FT_MEMCFG2   0x82206000
+#define CONFIG_SYS_GRLIB_GAISLER_FTMCTRL1_CFG2   0x82206000
 #else
-#define CONFIG_SYS_GRLIB_FT_MEMCFG2   0x82205260
+#define CONFIG_SYS_GRLIB_GAISLER_FTMCTRL1_CFG2   0x82205260
 #endif
-#define CONFIG_SYS_GRLIB_FT_MEMCFG3   0x0809a000
+#define CONFIG_SYS_GRLIB_GAISLER_FTMCTRL1_CFG3   0x0809a000

 /* no DDR controller */
-#define CONFIG_SYS_GRLIB_DDR_CFG   0x00000000
+#undef CONFIG_SYS_GRLIB_GAISLER_DDRSPA1

 /* no DDR2 Controller */
-#define CONFIG_SYS_GRLIB_DDR2_CFG1 0x00000000
-#define CONFIG_SYS_GRLIB_DDR2_CFG3 0x00000000
+#undef CONFIG_SYS_GRLIB_GAISLER_DDR2SPA1

 /* Identification string */
 #define CONFIG_IDENT_STRING "GAISLER LEON3 GR-CPCI-AX2000"
diff --git a/include/configs/gr_ep2s60.h b/include/configs/gr_ep2s60.h
index 2f953ae..b55ca77 100644
--- a/include/configs/gr_ep2s60.h
+++ b/include/configs/gr_ep2s60.h
@@ -289,21 +289,29 @@ 

 #define CONFIG_SYS_GRLIB_SDRAM    0

+/* No SDRAM Configuration */
+#undef CONFIG_SYS_GRLIB_GAISLER_SDCTRL1
+
 /* See, GRLIB Docs (grip.pdf) on how to set up
  * These the memory controller registers.
  */
-#define CONFIG_SYS_GRLIB_MEMCFG1  (0x10f800ff | (1<<11))
-#define CONFIG_SYS_GRLIB_MEMCFG2  0x00000000
-#define CONFIG_SYS_GRLIB_MEMCFG3  0x00000000
-
-#define CONFIG_SYS_GRLIB_FT_MEMCFG1  (0x10f800ff | (1<<11))
-#define CONFIG_SYS_GRLIB_FT_MEMCFG2  0x00000000
-#define CONFIG_SYS_GRLIB_FT_MEMCFG3  0x00000000
-
-#define CONFIG_SYS_GRLIB_DDR_CFG  0xa900830a
-
-#define CONFIG_SYS_GRLIB_DDR2_CFG1 0x00000000
-#define CONFIG_SYS_GRLIB_DDR2_CFG3 0x00000000
+#define CONFIG_SYS_GRLIB_ESA_MCTRL1
+#define CONFIG_SYS_GRLIB_ESA_MCTRL1_CFG1  (0x10f800ff | (1<<11))
+#define CONFIG_SYS_GRLIB_ESA_MCTRL1_CFG2  0x00000000
+#define CONFIG_SYS_GRLIB_ESA_MCTRL1_CFG3  0x00000000
+
+/* GRLIB FT-MCTRL configuration */
+#define CONFIG_SYS_GRLIB_GAISLER_FTMCTRL1
+#define CONFIG_SYS_GRLIB_GAISLER_FTMCTRL1_CFG1  (0x10f800ff | (1<<11))
+#define CONFIG_SYS_GRLIB_GAISLER_FTMCTRL1_CFG2  0x00000000
+#define CONFIG_SYS_GRLIB_GAISLER_FTMCTRL1_CFG3  0x00000000
+
+/* DDR controller */
+#define CONFIG_SYS_GRLIB_GAISLER_DDRSPA1
+#define CONFIG_SYS_GRLIB_GAISLER_DDRSPA1_CTRL  0xa900830a
+
+/* no DDR2 Controller */
+#undef CONFIG_SYS_GRLIB_GAISLER_DDR2SPA1

 /* Identification string */
 #define CONFIG_IDENT_STRING "GAISLER LEON3 EP2S60"
diff --git a/include/configs/gr_xc3s_1500.h b/include/configs/gr_xc3s_1500.h
index 428b532..d086b69 100644
--- a/include/configs/gr_xc3s_1500.h
+++ b/include/configs/gr_xc3s_1500.h
@@ -252,23 +252,28 @@ 

 #define CONFIG_SYS_GRLIB_SDRAM    0

+/* No SDRAM Configuration */
+#undef CONFIG_SYS_GRLIB_GAISLER_SDCTRL1
+
 /* See, GRLIB Docs (grip.pdf) on how to set up
  * These the memory controller registers.
  */
-#define CONFIG_SYS_GRLIB_MEMCFG1   (0x000000ff | (1<<11))
-#define CONFIG_SYS_GRLIB_MEMCFG2   0x82206000
-#define CONFIG_SYS_GRLIB_MEMCFG3   0x00136000
+#define CONFIG_SYS_GRLIB_ESA_MCTRL1
+#define CONFIG_SYS_GRLIB_ESA_MCTRL1_CFG1   (0x000000ff | (1<<11))
+#define CONFIG_SYS_GRLIB_ESA_MCTRL1_CFG2   0x82206000
+#define CONFIG_SYS_GRLIB_ESA_MCTRL1_CFG3   0x00136000

-#define CONFIG_SYS_GRLIB_FT_MEMCFG1   (0x000000ff | (1<<11))
-#define CONFIG_SYS_GRLIB_FT_MEMCFG2   0x82206000
-#define CONFIG_SYS_GRLIB_FT_MEMCFG3   0x00136000
+/* GRLIB FT-MCTRL configuration */
+#define CONFIG_SYS_GRLIB_GAISLER_FTMCTRL1
+#define CONFIG_SYS_GRLIB_GAISLER_FTMCTRL1_CFG1   (0x000000ff | (1<<11))
+#define CONFIG_SYS_GRLIB_GAISLER_FTMCTRL1_CFG2   0x82206000
+#define CONFIG_SYS_GRLIB_GAISLER_FTMCTRL1_CFG3   0x00136000

 /* no DDR controller */
-#define CONFIG_SYS_GRLIB_DDR_CFG   0x00000000
+#undef CONFIG_SYS_GRLIB_GAISLER_DDRSPA1

 /* no DDR2 Controller */
-#define CONFIG_SYS_GRLIB_DDR2_CFG1 0x00000000
-#define CONFIG_SYS_GRLIB_DDR2_CFG3 0x00000000
+#undef CONFIG_SYS_GRLIB_GAISLER_DDR2SPA1

 /* Identification string */
 #define CONFIG_IDENT_STRING "GAISLER LEON3 GR-XC3S-1500"
diff --git a/include/configs/grsim.h b/include/configs/grsim.h
index e07b816..e1f7dc3 100644
--- a/include/configs/grsim.h
+++ b/include/configs/grsim.h
@@ -264,25 +264,33 @@ 
 #define CONFIG_SYS_GRLIB_SDRAM     0

 #define CONFIG_SYS_GRLIB_MEMCFG1   (0x000000ff | (1<<11))
+
+/* No SDRAM Configuration */
+#undef CONFIG_SYS_GRLIB_GAISLER_SDCTRL1
+
+/* LEON2 MCTRL configuration */
+#define CONFIG_SYS_GRLIB_ESA_MCTRL1
+#define CONFIG_SYS_GRLIB_ESA_MCTRL1_CFG1   (0x000000ff | (1<<11))
 #if CONFIG_GRSIM
 /* GRSIM configuration */
-#define CONFIG_SYS_GRLIB_MEMCFG2   0x82206000
+#define CONFIG_SYS_GRLIB_ESA_MCTRL1_CFG2   0x82206000
 #else
 /* TSIM configuration */
-#define CONFIG_SYS_GRLIB_MEMCFG2   0x81805220
+#define CONFIG_SYS_GRLIB_ESA_MCTRL1_CFG2   0x81805220
 #endif
-#define CONFIG_SYS_GRLIB_MEMCFG3   0x00136000
+#define CONFIG_SYS_GRLIB_ESA_MCTRL1_CFG3   0x00136000

-#define CONFIG_SYS_GRLIB_FT_MEMCFG1   (0x000000ff | (1<<11))
-#define CONFIG_SYS_GRLIB_FT_MEMCFG2   0x82206000
-#define CONFIG_SYS_GRLIB_FT_MEMCFG3   0x00136000
+/* GRLIB FT-MCTRL configuration */
+#define CONFIG_SYS_GRLIB_GAISLER_FTMCTRL1
+#define CONFIG_SYS_GRLIB_GAISLER_FTMCTRL1_CFG1   (0x000000ff | (1<<11))
+#define CONFIG_SYS_GRLIB_GAISLER_FTMCTRL1_CFG2   0x82206000
+#define CONFIG_SYS_GRLIB_GAISLER_FTMCTRL1_CFG3   0x00136000

 /* no DDR controller */
-#define CONFIG_SYS_GRLIB_DDR_CFG   0x00000000
+#undef CONFIG_SYS_GRLIB_GAISLER_DDRSPA1

 /* no DDR2 Controller */
-#define CONFIG_SYS_GRLIB_DDR2_CFG1 0x00000000
-#define CONFIG_SYS_GRLIB_DDR2_CFG3 0x00000000
+#undef CONFIG_SYS_GRLIB_GAISLER_DDR2SPA1

 /* default kernel command line */
 #define CONFIG_DEFAULT_KERNEL_COMMAND_LINE "console=ttyS0,38400\0\0"