sparc: leon3: Reimplemented AMBA Plug&Play scanning routines.

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

7 files changed, 1225 insertions, 415 deletions

diff --git a/arch/sparc/cpu/leon3/Makefile b/arch/sparc/cpu/leon3/Makefile
index 4f13ec3..c5068a8 100644
--- a/arch/sparc/cpu/leon3/Makefile
+++ b/arch/sparc/cpu/leon3/Makefile
@@ -6,4 +6,5 @@
 #
 
 extra-y	= start.o
-obj-y	= cpu_init.o serial.o cpu.o ambapp.o interrupts.o prom.o usb_uhci.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
diff --git a/arch/sparc/cpu/leon3/ambapp.c b/arch/sparc/cpu/leon3/ambapp.c
index a30d1f2..bb81338 100644
--- a/arch/sparc/cpu/leon3/ambapp.c
+++ b/arch/sparc/cpu/leon3/ambapp.c
@@ -1,343 +1,315 @@
-/* Gaisler AMBA Plug&Play bus scanning. Functions
- * ending on _nomem is inteded to be used only during
- * initialization, only registers are used (no ram).
+/* GRLIB AMBA Plug&Play information scanning, relies on assembler
+ * routines.
  *
- * (C) Copyright 2007
- * Daniel Hellstrom, Gaisler Research, daniel@gaisler.com
+ * (C) Copyright 2010, 2015
+ * Daniel Hellstrom, Cobham Gaisler, daniel@gaisler.com.
  *
  * SPDX-License-Identifier:	GPL-2.0+
  */
 
+/* #define DEBUG */
+
 #include <common.h>
-#include <command.h>
 #include <ambapp.h>
+#include <config.h>
+
+/************ C INTERFACE OF ASSEMBLER SCAN ROUTINES ************/
+struct ambapp_find_apb_info {
+	/* Address of APB device Plug&Play information */
+	struct ambapp_pnp_apb	*pnp;
+	/* AHB Bus index of where the APB-Master Bridge device was found */
+	int			ahb_bus_index;
+	int			dec_index;
+};
 
-#if defined(CONFIG_CMD_AMBAPP)
-extern void ambapp_print_apb(apbctrl_pp_dev * apb,
-			     ambapp_ahbdev * apbmst, int index);
-extern void ambapp_print_ahb(ahbctrl_pp_dev * ahb, int index);
-extern int ambapp_apb_print;
-extern int ambapp_ahb_print;
-#endif
-
-static int ambapp_apb_scan(unsigned int vendor,	/* Plug&Play Vendor ID */
-			   unsigned int driver,	/* Plug&Play Device ID */
-			   ambapp_apbdev * dev,	/* Result(s) is placed here */
-			   int index,	/* Index of device to start copying Plug&Play
-					 * info into dev
-					 */
-			   int max_cnt	/* Maximal count that dev can hold, if dev
-					 * is NULL function will stop scanning after
-					 * max_cnt devices are found.
-					 */
-    )
-{
-	int i, cnt = 0;
-	unsigned int apbmst_base;
-	ambapp_ahbdev apbmst;
-	apbctrl_pp_dev *apb;
-
-	if (max_cnt == 0)
-		return 0;
-
-	/* Get AMBA APB Master */
-	if (ambapp_ahbslv_first(VENDOR_GAISLER, GAISLER_APBMST, &apbmst) != 1) {
-		return 0;
-	}
+struct ambapp_find_ahb_info {
+	/* Address of AHB device Plug&Play information */
+	struct ambapp_pnp_ahb	*pnp;
+	/* AHB Bus index of where the AHB device was found */
+	int			ahb_bus_index;
+	int			dec_index;
+};
 
-	/* Get APB CTRL Plug&Play info area */
-	apbmst_base = apbmst.address[0] & LEON3_IO_AREA;
-	apb = (apbctrl_pp_dev *) (apbmst_base | LEON3_CONF_AREA);
+extern void ambapp_find_buses(unsigned int ioarea, struct ambapp_bus *abus);
 
-	for (i = 0; i < LEON3_APB_SLAVES; i++) {
-#if defined(CONFIG_CMD_AMBAPP)
-		if (ambapp_apb_print && amba_vendor(apb->conf)
-		    && amba_device(apb->conf)) {
-			ambapp_print_apb(apb, &apbmst, i);
-		}
-#endif
-		if ((amba_vendor(apb->conf) == vendor) &&
-		    (amba_device(apb->conf) == driver) && ((index < 0)
-							   || (index-- == 0))) {
-			/* Convert Plug&Play info into a more readable format */
-			cnt++;
-			if (dev) {
-				dev->irq = amba_irq(apb->conf);
-				dev->ver = amba_ver(apb->conf);
-				dev->address =
-				    (apbmst_base |
-				     (((apb->
-					bar & 0xfff00000) >> 12))) & (((apb->
-									bar &
-									0x0000fff0)
-								       << 4) |
-								      0xfff00000);
-				dev++;
-			}
-			/* found max devices? */
-			if (cnt >= max_cnt)
-				return cnt;
-		}
-		/* Get next Plug&Play entry */
-		apb++;
-	}
-	return cnt;
-}
+extern int ambapp_find_apb(struct ambapp_bus *abus, unsigned int dev_vend,
+	int index, struct ambapp_find_apb_info *result);
 
-unsigned int ambapp_apb_next_nomem(register unsigned int vendor,	/* Plug&Play Vendor ID */
-				   register unsigned int driver,	/* Plug&Play Device ID */
-				   register int index)
-{
-	register int i;
-	register ahbctrl_pp_dev *apbmst;
-	register apbctrl_pp_dev *apb;
-	register unsigned int apbmst_base;
-
-	/* APBMST is a AHB Slave */
-	apbmst = ambapp_ahb_next_nomem(VENDOR_GAISLER, GAISLER_APBMST, 1, 0);
-	if (!apbmst)
-		return 0;
-
-	apbmst_base = amba_membar_start(apbmst->bars[0]);
-	if (amba_membar_type(apbmst->bars[0]) == AMBA_TYPE_AHBIO)
-		apbmst_base = AMBA_TYPE_AHBIO_ADDR(apbmst_base);
-	apbmst_base &= LEON3_IO_AREA;
-
-	/* Find the vendor/driver device on the first APB bus */
-	apb = (apbctrl_pp_dev *) (apbmst_base | LEON3_CONF_AREA);
-
-	for (i = 0; i < LEON3_APB_SLAVES; i++) {
-		if ((amba_vendor(apb->conf) == vendor) &&
-		    (amba_device(apb->conf) == driver) && ((index < 0)
-							   || (index-- == 0))) {
-			/* Convert Plug&Play info info a more readable format */
-			return (apbmst_base | (((apb->bar & 0xfff00000) >> 12)))
-			    & (((apb->bar & 0x0000fff0) << 4) | 0xfff00000);
-		}
-		/* Get next Plug&Play entry */
-		apb++;
-	}
-	return 0;
-}
+extern int ambapp_find_ahb(struct ambapp_bus *abus, unsigned int dev_vend,
+	int index, int type, struct ambapp_find_ahb_info *result);
 
-/****************************** APB SLAVES ******************************/
+/************ C ROUTINES USED BY U-BOOT AMBA CORE DRIVERS ************/
+struct ambapp_bus ambapp_plb;
 
-int ambapp_apb_count(unsigned int vendor, unsigned int driver)
+void ambapp_bus_init(
+	unsigned int ioarea,
+	unsigned int freq,
+	struct ambapp_bus *abus)
 {
-	return ambapp_apb_scan(vendor, driver, NULL, 0, LEON3_APB_SLAVES);
+	int i;
+
+	ambapp_find_buses(ioarea, abus);
+	for (i = 0; i < 6; i++)
+		if (abus->ioareas[i] == 0)
+			break;
+	abus->buses = i;
+	abus->freq = freq;
 }
 
-int ambapp_apb_first(unsigned int vendor,
-		     unsigned int driver, ambapp_apbdev * dev)
+/* Parse APB PnP Information */
+void ambapp_apb_parse(struct ambapp_find_apb_info *info, ambapp_apbdev *dev)
 {
-	return ambapp_apb_scan(vendor, driver, dev, 0, 1);
+	struct ambapp_pnp_apb *apb = info->pnp;
+	unsigned int apbbase = (unsigned int)apb & 0xfff00000;
+
+	dev->vendor = amba_vendor(apb->id);
+	dev->device = amba_device(apb->id);
+	dev->irq = amba_irq(apb->id);
+	dev->ver = amba_ver(apb->id);
+	dev->address = (apbbase | (((apb->iobar & 0xfff00000) >> 12))) &
+			(((apb->iobar &	0x0000fff0) << 4) | 0xfff00000);
+	dev->mask = amba_apb_mask(apb->iobar);
+	dev->ahb_bus_index = info->ahb_bus_index - 1;
 }
 
-int ambapp_apb_next(unsigned int vendor,
-		    unsigned int driver, ambapp_apbdev * dev, int index)
+/* Parse AHB PnP information */
+void ambapp_ahb_parse(struct ambapp_find_ahb_info *info, ambapp_ahbdev *dev)
 {
-	return ambapp_apb_scan(vendor, driver, dev, index, 1);
+	struct ambapp_pnp_ahb *ahb = info->pnp;
+	unsigned int ahbbase = (unsigned int)ahb & 0xfff00000;
+	int i, type;
+	unsigned int addr, mask, mbar;
+
+	dev->vendor = amba_vendor(ahb->id);
+	dev->device = amba_device(ahb->id);
+	dev->irq = amba_irq(ahb->id);
+	dev->ver = amba_ver(ahb->id);
+	dev->userdef[0] = ahb->custom[0];
+	dev->userdef[1] = ahb->custom[1];
+	dev->userdef[2] = ahb->custom[2];
+	dev->ahb_bus_index = info->ahb_bus_index - 1;
+	for (i = 0; i < 4; i++) {
+		mbar = ahb->mbar[i];
+		addr = amba_membar_start(mbar);
+		type = amba_membar_type(mbar);
+		if (type == AMBA_TYPE_AHBIO) {
+			addr = amba_ahbio_adr(addr, ahbbase);
+			mask = (((unsigned int)
+				(amba_membar_mask((~mbar))<<8)|0xff))+1;
+		} else {
+			/* AHB memory area, absolute address */
+			mask = (~((unsigned int)
+				(amba_membar_mask(mbar)<<20)))+1;
+		}
+		dev->address[i] = addr;
+		dev->mask[i] = mask;
+		dev->type[i] = type;
+	}
 }
 
-int ambapp_apbs_first(unsigned int vendor,
-		      unsigned int driver, ambapp_apbdev * dev, int max_cnt)
+int ambapp_apb_find(struct ambapp_bus *abus, int vendor, int device,
+	int index, ambapp_apbdev *dev)
 {
-	return ambapp_apb_scan(vendor, driver, dev, 0, max_cnt);
-}
+	unsigned int devid = AMBA_PNP_ID(vendor, device);
+	int found;
+	struct ambapp_find_apb_info apbdev;
 
-enum {
-	AHB_SCAN_MASTER = 0,
-	AHB_SCAN_SLAVE = 1
-};
-
-/* Scan AMBA Plug&Play bus for AMBA AHB Masters or AHB Slaves
- * for a certain matching Vendor and Device ID.
- *
- * Return number of devices found.
- *
- * Compact edition...
- */
-static int ambapp_ahb_scan(unsigned int vendor,	/* Plug&Play Vendor ID */
-			   unsigned int driver,	/* Plug&Play Device ID */
-			   ambapp_ahbdev * dev,	/* Result(s) is placed here */
-			   int index,	/* Index of device to start copying Plug&Play
-					 * info into dev
-					 */
-			   int max_cnt,	/* Maximal count that dev can hold, if dev
-					 * is NULL function will stop scanning after
-					 * max_cnt devices are found.
-					 */
-			   int type	/* Selectes what type of devices to scan.
-					 * 0=AHB Masters
-					 * 1=AHB Slaves
-					 */
-    )
-{
-	int i, j, cnt = 0, max_pp_devs;
-	unsigned int addr;
-	ahbctrl_info *info = (ahbctrl_info *) (LEON3_IO_AREA | LEON3_CONF_AREA);
-	ahbctrl_pp_dev *ahb;
-
-	if (max_cnt == 0)
-		return 0;
-
-	if (type == 0) {
-		max_pp_devs = LEON3_AHB_MASTERS;
-		ahb = info->masters;
-	} else {
-		max_pp_devs = LEON3_AHB_SLAVES;
-		ahb = info->slaves;
-	}
+	found = ambapp_find_apb(abus, devid, index, &apbdev);
+	if (found == 1)
+		ambapp_apb_parse(&apbdev, dev);
 
-	for (i = 0; i < max_pp_devs; i++) {
-#if defined(CONFIG_CMD_AMBAPP)
-		if (ambapp_ahb_print && amba_vendor(ahb->conf) &&
-		    amba_device(ahb->conf)) {
-			ambapp_print_ahb(ahb, i);
-		}
-#endif
-		if ((amba_vendor(ahb->conf) == vendor) &&
-		    (amba_device(ahb->conf) == driver) &&
-		    ((index < 0) || (index-- == 0))) {
-			/* Convert Plug&Play info info a more readable format */
-			cnt++;
-			if (dev) {
-				dev->irq = amba_irq(ahb->conf);
-				dev->ver = amba_ver(ahb->conf);
-				dev->userdef[0] = ahb->userdef[0];
-				dev->userdef[1] = ahb->userdef[1];
-				dev->userdef[2] = ahb->userdef[2];
-				for (j = 0; j < 4; j++) {
-					addr = amba_membar_start(ahb->bars[j]);
-					if (amba_membar_type(ahb->bars[j]) ==
-					    AMBA_TYPE_AHBIO)
-						addr =
-						    AMBA_TYPE_AHBIO_ADDR(addr);
-					dev->address[j] = addr;
-				}
-				dev++;
-			}
-			/* found max devices? */
-			if (cnt >= max_cnt)
-				return cnt;
-		}
-		/* Get next Plug&Play entry */
-		ahb++;
-	}
-	return cnt;
+	return found;
 }
 
-unsigned int ambapp_ahb_get_info(ahbctrl_pp_dev * ahb, int info)
+int ambapp_apb_count(struct ambapp_bus *abus, int vendor, int device)
 {
-	register unsigned int ret;
-
-	if (!ahb)
-		return 0;
-
-	switch (info) {
-	default:
-		info = 0;
-	case 0:
-	case 1:
-	case 2:
-	case 3:
-		/* Get Address from PnP Info */
-		ret = amba_membar_start(ahb->bars[info]);
-		if (amba_membar_type(ahb->bars[info]) == AMBA_TYPE_AHBIO)
-			ret = AMBA_TYPE_AHBIO_ADDR(ret);
-		return ret;
-	}
-	return 0;
-
+	unsigned int devid = AMBA_PNP_ID(vendor, device);
+	int found;
+	struct ambapp_find_apb_info apbdev;
+
+	found = ambapp_find_apb(abus, devid, 63, &apbdev);
+	if (found == 1)
+		return 64;
+	else
+		return 63 - apbdev.dec_index;
 }
 
-ahbctrl_pp_dev *ambapp_ahb_next_nomem(register unsigned int vendor,	/* Plug&Play Vendor ID */
-				      register unsigned int driver,	/* Plug&Play Device ID */
-				      register unsigned int opts,	/* 1=slave, 0=master */
-				      register int index)
+int ambapp_ahb_find(struct ambapp_bus *abus, int vendor, int device,
+	int index, ambapp_ahbdev *dev, int type)
 {
-	register ahbctrl_pp_dev *ahb;
-	register ahbctrl_info *info =
-	    (ahbctrl_info *) (LEON3_IO_AREA | LEON3_CONF_AREA);
-	register int i;
-	register int max_pp_devs;
-
-	if (opts == 0) {
-		max_pp_devs = LEON3_AHB_MASTERS;
-		ahb = info->masters;
-	} else {
-		max_pp_devs = LEON3_AHB_SLAVES;
-		ahb = info->slaves;
-	}
+	int found;
+	struct ambapp_find_ahb_info ahbdev;
+	unsigned int devid = AMBA_PNP_ID(vendor, device);
 
-	for (i = 0; i < max_pp_devs; i++) {
-		if ((amba_vendor(ahb->conf) == vendor) &&
-		    (amba_device(ahb->conf) == driver) &&
-		    ((index < 0) || (index-- == 0))) {
-			/* Convert Plug&Play info info a more readable format */
-			return ahb;
-		}
-		/* Get next Plug&Play entry */
-		ahb++;
-	}
-	return 0;
+	found = ambapp_find_ahb(abus, devid, index, type, &ahbdev);
+	if (found == 1)
+		ambapp_ahb_parse(&ahbdev, dev);
+
+	return found;
 }
 
-/****************************** AHB MASTERS ******************************/
-int ambapp_ahbmst_count(unsigned int vendor, unsigned int driver)
+int ambapp_ahbmst_find(struct ambapp_bus *abus, int vendor, int device,
+	int index, ambapp_ahbdev *dev)
 {
-	/* Get number of devices of this vendor&device ID */
-	return ambapp_ahb_scan(vendor, driver, NULL, 0, LEON3_AHB_MASTERS,
-			       AHB_SCAN_MASTER);
+	return ambapp_ahb_find(abus, vendor, device, index, dev, DEV_AHB_MST);
 }
 
-int ambapp_ahbmst_first(unsigned int vendor, unsigned int driver,
-			ambapp_ahbdev * dev)
+int ambapp_ahbslv_find(struct ambapp_bus *abus, int vendor, int device,
+	int index, ambapp_ahbdev *dev)
 {
-	/* find first device of this */
-	return ambapp_ahb_scan(vendor, driver, dev, 0, 1, AHB_SCAN_MASTER);
+	return ambapp_ahb_find(abus, vendor, device, index, dev, DEV_AHB_SLV);
 }
 
-int ambapp_ahbmst_next(unsigned int vendor,
-		       unsigned int driver, ambapp_ahbdev * dev, int index)
+int ambapp_ahb_count(struct ambapp_bus *abus, int vendor, int device, int type)
 {
-	/* find first device of this */
-	return ambapp_ahb_scan(vendor, driver, dev, index, 1, AHB_SCAN_MASTER);
+	int found;
+	struct ambapp_find_ahb_info ahbdev;
+	unsigned int devid = AMBA_PNP_ID(vendor, device);
+
+	found = ambapp_find_ahb(abus, devid, 63, type, &ahbdev);
+	if (found == 1)
+		return 64;
+	else
+		return 63 - ahbdev.dec_index;
 }
 
-int ambapp_ahbmsts_first(unsigned int vendor,
-			 unsigned int driver, ambapp_ahbdev * dev, int max_cnt)
+int ambapp_ahbmst_count(struct ambapp_bus *abus, int vendor, int device)
 {
-	/* find first device of this */
-	return ambapp_ahb_scan(vendor, driver, dev, 0, max_cnt,
-			       AHB_SCAN_MASTER);
+	return ambapp_ahb_count(abus, vendor, device, DEV_AHB_MST);
 }
 
-/****************************** AHB SLAVES ******************************/
-int ambapp_ahbslv_count(unsigned int vendor, unsigned int driver)
+int ambapp_ahbslv_count(struct ambapp_bus *abus, int vendor, int device)
 {
-	/* Get number of devices of this vendor&device ID */
-	return ambapp_ahb_scan(vendor, driver, NULL, 0, LEON3_AHB_SLAVES,
-			       AHB_SCAN_SLAVE);
+	return ambapp_ahb_count(abus, vendor, device, DEV_AHB_SLV);
 }
 
-int ambapp_ahbslv_first(unsigned int vendor, unsigned int driver,
-			ambapp_ahbdev * dev)
+/* The define CONFIG_SYS_GRLIB_SINGLE_BUS may be defined on GRLIB systems
+ * where only one AHB Bus is available - no bridges are present. This option
+ * is available only to reduce the footprint.
+ *
+ * Defining this on a multi-bus GRLIB system may also work depending on the
+ * design.
+ */
+
+#ifndef CONFIG_SYS_GRLIB_SINGLE_BUS
+
+/* GAISLER AHB2AHB Version 1 Bridge Definitions */
+#define AHB2AHB_V1_FLAG_FFACT     0x0f0	/* Frequency factor against top bus */
+#define AHB2AHB_V1_FLAG_FFACT_DIR 0x100	/* Factor direction, 0=down, 1=up */
+#define AHB2AHB_V1_FLAG_MBUS      0x00c	/* Master bus number mask */
+#define AHB2AHB_V1_FLAG_SBUS      0x003	/* Slave bus number mask */
+
+/* Get Parent bus frequency. Note that since we go from a "child" bus
+ * to a parent bus, the frequency factor direction is inverted.
+ */
+unsigned int gaisler_ahb2ahb_v1_freq(ambapp_ahbdev *ahb, unsigned int freq)
 {
-	/* find first device of this */
-	return ambapp_ahb_scan(vendor, driver, dev, 0, 1, AHB_SCAN_SLAVE);
+	int dir;
+	unsigned char ffact;
+
+	/* Get division/multiple factor */
+	ffact = (ahb->userdef[0] & AHB2AHB_V1_FLAG_FFACT) >> 4;
+	if (ffact != 0) {
+		dir = ahb->userdef[0] & AHB2AHB_V1_FLAG_FFACT_DIR;
+
+		/* Calculate frequency by dividing or
+		 * multiplying system frequency
+		 */
+		if (dir)
+			freq = freq * ffact;
+		else
+			freq = freq / ffact;
+	}
+
+	return freq;
 }
 
-int ambapp_ahbslv_next(unsigned int vendor,
-		       unsigned int driver, ambapp_ahbdev * dev, int index)
+/* AHB2AHB and L2CACHE ver 2 is not supported yet. */
+unsigned int gaisler_ahb2ahb_v2_freq(ambapp_ahbdev *ahb, unsigned int freq)
 {
-	/* find first device of this */
-	return ambapp_ahb_scan(vendor, driver, dev, index, 1, AHB_SCAN_SLAVE);
+	panic("gaisler_ahb2ahb_v2_freq: AHB2AHB ver 2 not supported\n");
+	return -1;
 }
+#endif
 
-int ambapp_ahbslvs_first(unsigned int vendor,
-			 unsigned int driver, ambapp_ahbdev * dev, int max_cnt)
+/* Return the frequency of a AHB bus identified by index found
+ * note that this is not the AHB Bus number.
+ */
+unsigned int ambapp_bus_freq(struct ambapp_bus *abus, int ahb_bus_index)
 {
-	/* find first device of this */
-	return ambapp_ahb_scan(vendor, driver, dev, 0, max_cnt, AHB_SCAN_SLAVE);
+	unsigned int freq = abus->freq;
+#ifndef CONFIG_SYS_GRLIB_SINGLE_BUS
+	unsigned int ioarea, ioarea_parent, bridge_pnp_ofs;
+	struct ambapp_find_ahb_info ahbinfo;
+	ambapp_ahbdev ahb;
+	int parent;
+
+	debug("ambapp_bus_freq: get freq on bus %d\n", ahb_bus_index);
+
+	while (ahb_bus_index != 0) {
+		debug("  BUS[0]: 0x%08x\n", abus->ioareas[0]);
+		debug("  BUS[1]: 0x%08x\n", abus->ioareas[1]);
+		debug("  BUS[2]: 0x%08x\n", abus->ioareas[2]);
+		debug("  BUS[3]: 0x%08x\n", abus->ioareas[3]);
+		debug("  BUS[4]: 0x%08x\n", abus->ioareas[4]);
+		debug("  BUS[5]: 0x%08x\n", abus->ioareas[5]);
+
+		/* Get I/O area of AHB bus */
+		ioarea = abus->ioareas[ahb_bus_index];
+
+		printf("  IOAREA: 0x%08x\n", ioarea);
+
+		/* Get parent bus */
+		parent = (ioarea & 0x7);
+		if (parent == 0) {
+			panic("%s: parent=0 indicates no parent! Stopping.\n",
+				__func__);
+			return -1;
+		}
+		parent = parent - 1;
+		bridge_pnp_ofs = ioarea & 0x7e0;
+
+		debug("  PARENT: %d\n", parent);
+		debug("  BRIDGE_OFS: 0x%08x\n", bridge_pnp_ofs);
+
+		/* Get AHB/AHB bridge PnP address */
+		ioarea_parent = (abus->ioareas[parent] & 0xfff00000) |
+				AMBA_CONF_AREA | AMBA_AHB_SLAVE_CONF_AREA;
+		ahbinfo.pnp = (struct ambapp_pnp_ahb *)
+				(ioarea_parent | bridge_pnp_ofs);
+
+		debug("  IOAREA PARENT: 0x%08x\n", ioarea_parent);
+		debug("  BRIDGE PNP: 0x%p\n", ahbinfo.pnp);
+
+		/* Parse the AHB information */
+		ahbinfo.ahb_bus_index = parent;
+		ambapp_ahb_parse(&ahbinfo, &ahb);
+
+		debug("  BRIDGE ID: VENDOR=%d(0x%x), DEVICE=%d(0x%x)\n",
+			ahb.vendor, ahb.vendor, ahb.device, ahb.device);
+
+		/* Different bridges may convert frequency differently */
+		if ((ahb.vendor == VENDOR_GAISLER) &&
+			((ahb.device == GAISLER_AHB2AHB) ||
+			(ahb.device == GAISLER_L2CACHE))) {
+			/* Get new frequency */
+			if (ahb.ver > 1)
+				freq = gaisler_ahb2ahb_v2_freq(&ahb, freq);
+			else
+				freq = gaisler_ahb2ahb_v1_freq(&ahb, freq);
+
+			debug("  NEW FREQ: %dHz\n", freq);
+		} else {
+			panic("%s: unsupported AMBA bridge\n", __func__);
+			return -1;
+		}
+
+		/* Step upwards towards system top bus */
+		ahb_bus_index = parent;
+	}
+#endif
+
+	debug("ambapp_bus_freq: %dHz\n", freq);
+
+	return freq;
 }
diff --git a/arch/sparc/cpu/leon3/ambapp_low.S b/arch/sparc/cpu/leon3/ambapp_low.S
new file mode 100644
index 0000000..2863586
--- /dev/null
+++ b/arch/sparc/cpu/leon3/ambapp_low.S
@@ -0,0 +1,784 @@
+/* GRLIB AMBA Plug&Play information scanning implemented without
+ * using memory (stack) and one register window. The code scan
+ * the PnP info and inserts the AHB bridges/buses into register
+ * i0-i5.
+ * The code support
+ *  - up to 6 AHB buses
+ *  - multiple APB buses
+ *  - support for AHB2AHB & L2CACHE bridges
+ *
+ * (C) Copyright 2010, 2015
+ * Daniel Hellstrom, Cobham Gaisler, daniel@gaisler.com.
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <ambapp.h>
+
+	.seg	"text"
+	.globl	_nomem_amba_init
+	.globl	_nomem_ambapp_find_buses
+	.globl	_nomem_find_apb
+	.globl	_nomem_find_ahb
+
+/* Overview
+ * ========
+ *
+ * _nomem_amba_init         - Init AMBA bus and calls _nomem_ambapp_find_buses
+ * _nomem_ambapp_find_buses - Scan AMBA PnP info for AHB buses/bridges and
+ *                            place them in i0-i5, see below
+ * _nomem_find_apb          - Find one APB device identified by VENDOR:DEVICE
+ *                            ID and an index.
+ * _nomem_find_ahb          - Find one AHB Master or Slave device identified
+ *                            by VENDOR:DEVICE ID and an index.
+ * init_ahb_bridges         - Local function. Clears i0-i5
+ * insert_ahb_bridge        - Local function. Insert a new AHB bus into first
+ *                            free register in i0-i5. It also checks that the
+ *                            bus has not already been added.
+ * get_ahb_bridge           - Local function. Get AHB bus from registers,
+ *                            return register iN, where N is defined by o0.
+ *
+ * The _nomem_find_apb and _nomem_find_ahb function requires that i0-i5
+ * are populated with the AHB buses of the system. The registers are
+ * initialized by _nomem_ambapp_find_buses.
+ *
+ * AHB Bus result and requirements of i0-i5
+ * ========================================
+ *
+ * i0: AHB BUS0 IOAREA, no parent bus
+ * i1: AHB BUS1 IOAREA, parent bus is always i0 (AHB BUS0) and bridge address
+ * i2: AHB BUS2 IOAREA, 3-bit parent bus number and bridge address
+ * i3: AHB BUS3 IOAREA, 3-bit parent bus number and bridge address
+ * i4: AHB BUS4 IOAREA, 3-bit parent bus number and bridge address
+ * i5: AHB BUS5 IOAREA, 3-bit parent bus number and bridge address
+ *
+ * AHB BUS
+ * -------
+ * Bits 31-20 (0xfff00000) contain the found bus I/O Area (AHB PnP area).
+ *
+ * 3-bit Parent bus
+ * ----------------
+ * Bits 2-0 (0x00000007) contain parent bus number. Zero if no parent
+ * bus, 1 = parent is AHB BUS 0 (i0), 2 = parent is AHB BUS 1 (i1)..
+ *
+ * Bridge Address
+ * --------------
+ * Bits 10-5 (0x000007e0) contain the index of the Bridge's PnP
+ * information on the parent. Since all bridges are found in the
+ * PnP information they all have a PnP entry. Together with the
+ * parent bus number the PnP entry can be found:
+ *  PnPEntry = (BRIDGE_ADDRESS + (iN & 0xfff00000)) | 0x000ff800
+ *  where N is the parent bus minus one.
+ *
+ */
+
+/* Function initializes the AHB Bridge I/O AREA storage. (Clears i0-i5)
+ *
+ * Arguments
+ *  none
+ *
+ * Results
+ *  none
+ *
+ * Clobbered
+ *  none
+ */
+
+init_ahb_bridges:
+	mov	%g0, %i0
+	mov	%g0, %i1
+	mov	%g0, %i2
+	mov	%g0, %i3
+	mov	%g0, %i4
+	retl
+	 mov	%g0, %i5
+
+/* Function returns AHB Bridge I/O AREA for specified bus.
+ *
+ * Arguments
+ *  - o0 = bus number
+ *
+ * Results
+ *  - o0 = I/O AREA
+ *
+ * Clobbered
+ *  none
+ */
+get_ahb_bridge:
+	cmp	%o0, 1
+	be,a	L1
+	 mov	%i0, %o0
+
+	cmp	%o0, 2
+	be,a	L1
+	 mov	%i1, %o0
+
+	cmp	%o0, 3
+	be,a	L1
+	 mov	%i2, %o0
+
+	cmp	%o0, 4
+	be,a	L1
+	 mov	%i3, %o0
+
+	cmp	%o0, 5
+	be,a	L1
+	 mov	%i4, %o0
+
+	cmp	%o0, 6
+	be,a	L1
+	 mov	%i5, %o0
+
+	/* o0 > 6: only 6 buses supported */
+	mov	%g0, %o0
+L1:
+	retl
+	 nop
+
+/* Function adds a AHB Bridge I/O AREA to the i0-i5 registers if
+ * not already added. It stores the bus PnP start information.
+ *
+ * Arguments
+ *  - o0 = AHB Bridge I/O area
+ *
+ * Results
+ *  none
+ *
+ * Clobbered
+ *  o2, o3
+ */
+insert_ahb_bridge:
+	/* Check that bridge hasn't already been added */
+	andn	%o0, 0x7ff, %o2
+	andn	%i0, 0x7ff, %o3
+	cmp	%o3, %o2
+	be	L2
+	 andn	%i1, 0x7ff, %o3
+	cmp	%o3, %o2
+	be	L2
+	 andn	%i2, 0x7ff, %o3
+	cmp	%o3, %o2
+	be	L2
+	 andn	%i3, 0x7ff, %o3
+	cmp	%o3, %o2
+	be	L2
+	 andn	%i4, 0x7ff, %o3
+	cmp	%o3, %o2
+	be	L2
+	 andn	%i5, 0x7ff, %o3
+	cmp	%o3, %o2
+	be	L2
+
+	/* Insert into first free posistion */
+	 cmp	%i0, %g0
+	be,a	L2
+	 mov	%o0, %i0
+
+	cmp	%i1, %g0
+	be,a	L2
+	 mov	%o0, %i1
+
+	cmp	%i2, %g0
+	be,a	L2
+	 mov	%o0, %i2
+
+	cmp	%i3, %g0
+	be,a	L2
+	 mov	%o0, %i3
+
+	cmp	%i4, %g0
+	be,a	L2
+	 mov	%o0, %i4
+
+	cmp	%i5, %g0
+	be,a	L2
+	 mov	%o0, %i5
+L2:
+	retl
+	 nop
+
+/* FUNCTION int _nomem_find_ahb_bus(
+ *	unsigned int bridge,
+ *	int vendor_device,
+ *	int index,
+ *	void **pconf,
+ *	int not_used,
+ *	int option
+ *	)
+ *
+ * Scans the AHB Master or Slave area for a matching VENDOR:DEVICE, the
+ * index is decremented when a matching device is found but index is
+ * greater than zero. When index is zero and a matching DEVICE:VENDOR
+ * is found the AHB configuration address and AHB I/O area is returned.
+ *
+ * i0-i7,l0,l1,l2,l3,l4,g2,o6 is not available for use.
+ * o1,o5 Must be left untouched
+ *
+ * Results
+ *  - o0 Number of found devices (1 or 0)
+ *  - o2 is decremented for each matching VENDOR:DEVICE found, zero if found
+ *  - o3 Address of the AHB PnP configuration entry (Only valid if o0=1)
+ *
+ * Clobbered
+ *  - o3 (Clobbered when no device was found)
+ *  - o4 (Number of Devices left to search)
+ *  - o0 (Bus ID, PnP ID, Device)
+ */
+_nomem_find_ahb_bus:
+
+	/* Get the number of Slaves/Masters.
+	 * Only AHB Bus 0 has 64 AHB Masters/Slaves the
+	 * other AHB buses has 16 slaves and 16 masters.
+	 */
+	add	%g0, 16, %o4		/* Defaulting to 16 */
+	andcc	%o0, 0x7, %g0		/* 3-bit bus id */
+	be,a	.L_maxloops_detected
+	 add	%g0, 64, %o4		/* AHB Bus 0 has 64 AHB Masters/Slaves */
+.L_maxloops_detected:
+
+	/* Get start address of AHB Slave or AHB Master area depending on what
+	 * we are searching for.
+	 */
+	andn	%o0, 0x7ff, %o0		/* Remove Bus ID and 5-bit AHB/AHB
+					 * Bridge PnP Address to get I/O Area */
+	set	AMBA_CONF_AREA,	%o3
+	or	%o3, %o0, %o3		/* Master area address */
+
+	cmp	%o5, DEV_AHB_SLV
+	be,a	.L_conf_area_calculated
+	 or	%o3, AMBA_AHB_SLAVE_CONF_AREA, %o3	/* Add 0x800 to get to slave area */
+.L_conf_area_calculated:
+
+	/* Iterate over all AHB device and try to find matching DEVICE:VENDOR
+	 * o1 - VENDOR|DEVICE
+	 * o2 - Index
+	 * o3 - Current AHB Device Configuration address
+	 * o5 - Type (leave untouched)
+	 *
+	 * o4 - Number of AHB device left to process
+	 * o0 - tmp
+	 */
+.L_process_one_conf:
+	ld	[%o3], %o0
+	andn	%o0, 0xfff, %o0
+	cmp	%o0, 0			/* No device if zero */
+	beq	.L_next_conf
+	 cmp	%o1, 0			/* If VENDOR:DEVICE==0, consider all matching */
+	beq	.L_process_ahb_dev_found
+	 cmp	%o0, %o1		/* Does VENDOR and DEVICE Match? */
+	bne	.L_next_conf
+	 nop
+.L_process_ahb_dev_found:
+	/* Found a Matching VENDOR:DEVICE, index must also match */
+	cmp	%o2, %g0
+	bne	.L_next_conf
+	 dec	%o2
+	/* Index matches also, return happy with o3 set to AHB Conf Address */
+	mov	%g0, %o2
+	retl
+	 add	%g0, 1, %o0
+
+.L_next_conf:
+	subcc	%o4, 1, %o4		/* One device has been processed,
+					 * Are there more devices to process? */
+	bne	.L_process_one_conf
+	 add	%o3, AMBA_AHB_CONF_LENGH, %o3	/* Next Configuration entry */
+	/* No Matching device found */
+	retl
+	 mov	%g0, %o0
+
+/* FUNCTION int _nomem_find_ahb(
+ *      int unused,
+ *	int vendor_device,
+ *	int index,
+ *	void **pconf,
+ *	int *ahb_bus_index,
+ *	int option,
+ *	)
+ *
+ * Find a AHB Master or AHB Slave device, it puts the address of the AHB PnP
+ * configuration in o3 (pconf), the I/O Area base address in o4 (pioarea).
+ *
+ * Calls _nomem_find_ahb_bus for every AHB bus.
+ *
+ * i0-i7, l0, l1, o6, g1, g4-g7 is not available for use.
+ *
+ * Arguments
+ *  - o0 Unused
+ *
+ * Results
+ *  - o0 Number of found devices (1 or 0)
+ *  - o2 Decremented Index (Zero if found)
+ *  - o3 Address of the AHB PnP configuration entry
+ *  - o4 AHB Bus index the device was found on (if o0=1)
+ *  - o5 Left untouched
+ *
+ * Clobbered
+ *  - o0 (AHB Bridge and used by _nomem_find_ahb_bus)
+ *  - o2 (index is decremented)
+ *  - l2 (Current AHB Bus index)
+ *  - g2 (return address)
+ */
+_nomem_find_ahb:
+	mov	%o7, %g2		/* Save return address */
+	/* Scan all AHB Buses found for the AHB Master/Slave matching VENDOR:DEVICE */
+	clr	%l2
+.L_search_next_ahb_bus:
+	add	%l2, 1, %l2
+	call	get_ahb_bridge			/* Get bus %l0 I/O Area */
+	 mov	%l2, %o0
+	cmp	%o0, %g0
+	be	.L_no_device_found		/* If no more AHB bus is left to be scanned, proceed */
+	 nop
+	call	_nomem_find_ahb_bus		/* Scan AHB bus %o0 for VENDOR:DEVICE. Index in o3 is decremented  */
+	 nop
+	cmp	%o0, %g0			/* If VENDOR:DEVICE was not found scan next AHB Bus */
+	be	.L_search_next_ahb_bus		/* Do next bus is o0=0 (not found) */
+	 nop
+	/* The device was found, o0 is 1 */
+	mov	%g2, %o7		/* Restore return address */
+	retl
+	 mov	%l2, %o4		/* The AHB bus index the device was found on */
+
+	/* No device found matching */
+.L_no_device_found:
+	mov	%g2, %o7		/* Restore return address */
+	retl
+	 mov	%g0, %o0
+
+
+/* FUNCTION int _nomem_find_apb_bus(
+ *      int apbmst,
+ *	int vendor_device,
+ *	int index,
+ *	void **pconf
+ *	)
+ *
+ * Find a APB Slave device, it puts the address of the APB PnP configuration
+ * in o3 (pconf).
+ *
+ * Calls _nomem_find_ahb_bus for every AHB bus searching for AHB/APB Bridges.
+ * The AHB/APB bridges are AHB Slaves with ID GAISLER_APBMST.
+ *
+ * Results
+ *  - o0 Number of found devices (1 or 0)
+ *  - o2 Decremented Index
+ *  - o3 Address of the found APB device PnP configuration entry
+ *
+ * Clobbered
+ *  - o5 PnP VENDOR:DEVICE ID
+ */
+
+_nomem_find_apb_bus:
+	set	AMBA_CONF_AREA, %o3
+	or	%o0, %o3, %o3		/* Calc start of APB device PnP info */
+	add	%g0, 16, %o0		/* o0, number of APB Slaves left to scan */
+.L_process_one_apb_conf:
+	ld	[%o3], %o5
+	andn	%o5, 0xfff, %o5
+	cmp	%o5, 0			/* No device if zero */
+	beq	.L_process_apb_dev_not_found
+	 cmp	%o1, 0			/* If VENDOR:DEVICE == -1, consider all matching */
+	beq	.L_process_apb_dev_found
+	 cmp	%o1, %o5		/* Found VENDOR:DEVICE */
+	bne	.L_process_apb_dev_not_found
+	 nop
+
+.L_process_apb_dev_found:
+	/* Found matching device, compare index */
+	cmp	%o2, %g0
+	bne	.L_process_apb_dev_not_found
+	 dec	%o2
+	/* Matching index and VENDOR:DEVICE */
+	retl
+	 add	%g0, 1, %o0
+
+.L_process_apb_dev_not_found:
+	subcc	%o0, 1, %o0
+	bne	.L_process_one_apb_conf
+	 add	%o3, 8, %o3
+	retl
+	 mov	%g0, %o0
+
+/* FUNCTION int _nomem_find_apb(
+ *      int unused,
+ *	int vendor_device,
+ *	int index,
+ *	void **pconf,
+ *	int *ahb_bus_index
+ *	)
+ *
+ * Find a APB Slave device, it puts the address of the APB PnP configuration
+ * in o3 (pconf), the APB Master I/O Area base address in o4 (papbarea).
+ *
+ * Calls _nomem_find_ahb_bus for every AHB bus searching for AHB/APB Bridges.
+ * The AHB/APB bridges are AHB Slaves with ID GAISLER_APBMST.
+ *
+ * i0-i7, l0, l1, o6 is not available for use.
+ *
+ * Arguments
+ *  - o0 Unused
+ *
+ * Results
+ *  - o0 Number of found devices (1 or 0)
+ *  - o2 Decremented Index if not found
+ *  - o3 Address of the APB PnP configuration entry
+ *  - o4 AHB Bus index of APB Bridge/APB Device
+ *
+ * Clobbered
+ *  - o0 (AHB Bridge)
+ *  - o2 (index is decremented)
+ *  - l2 (APB DEV Index [7..4] : APBMST AHB Index [3..0])
+ *  - l3 (Current AHB Bus index)
+ *  - l4 (temporary storage for APB VENDOR:DEVICE)
+ *  - o5 (AHB Slave ID)
+ *  - o0 (clobbered by _nomem_find_ahb_bus)
+ *  - g2 (Return address)
+ */
+_nomem_find_apb:
+	/* Scan all AHB Buses found for AHB/APB Bridges */
+	mov	%o7, %g2		/* Save return address */
+	mov	%o1, %l4		/* Save APB VENDOR:DEVICE */
+	sll	%o2, 4, %l2		/* APB MST index = 0 */
+	add	%g0, 1, %l3		/* AHB Bus index = 0 */
+.L2_search_next_ahb_bus:
+	call	get_ahb_bridge		/* Get bus %l3 I/O Area */
+	 mov	%l3, %o0
+	cmp	%o0, %g0
+	be	.L2_no_device_found	/* If no more AHB bus is left to be scanned, proceed */
+	 add	%g0, DEV_AHB_SLV, %o5	/* Search for AHB Slave */
+	sethi	%hi(AMBA_PNP_ID(VENDOR_GAISLER, GAISLER_APBMST)), %o1
+	call	_nomem_find_ahb_bus	/* Scan AHB bus %o0 for VENDOR:DEVICE. Index in o3 is decremented */
+	 and	%l2, 0xf, %o2		/* Set APBMST index */
+	cmp	%o0, %g0		/* If no AHB/APB Bridge was not found, scan next AHB Bus */
+	be	.L_no_apb_bridge_found	/* Do next bus */
+	 nop
+
+	/* The AHB/APB Bridge was found.
+         * Search for the requested APB Device on the APB bus using
+	 * find_apb_bus, it will decrement the index.
+         */
+	ld	[%o3 + AMBA_AHB_MBAR0_OFS], %o3
+	sll	%o3, 16, %o0
+	and	%o0, %o3, %o0		/* Address AND Address Mask */
+	sethi	%hi(0xfff00000), %o3
+	and	%o0, %o3, %o0		/* AHB/APB Bridge address */
+
+	srl	%l2, 4, %o2		/* APB DEV Index */
+	call	_nomem_find_apb_bus
+	 mov	%l4, %o1		/* APB VENDOR:DEVICE */
+	cmp	%o0, %g0
+	be	.L_apb_dev_not_found
+	 mov	%g2, %o7		/* Restore return address */
+	/* APB Device found
+	 * o0 1
+	 * o2 Index is decremented to zero
+	 * o3 APB configuration address,
+	 * o4 APB Bridge Configuration address.
+	 */
+	mov	%g0, %o2
+	retl
+	 mov	%l3, %o4
+
+.L_apb_dev_not_found:
+	/* Update APB DEV Index by saving output from find_apb_bus
+	 * (index parameter) into bits [31..4] in L2.
+	 */
+	sll	%o2, 4, %o2
+	and	%l2, 0xf, %l2
+	or	%o2, %l2, %l2
+	/* Try finding the next AHB/APB Bridge on the same AHB bus
+	 * to find more APB devices
+	 */
+	ba	.L2_search_next_ahb_bus	/* Find next AHB/APB bridge */
+	 inc	%l2
+
+.L_no_apb_bridge_found:
+	inc	%l3			/* Next AHB Bus */
+	ba	.L2_search_next_ahb_bus	/* Process next AHB bus */
+	 andn	%l2, 0xf, %l2		/* Start at APB Bridge index 0 at every AHB Bus */
+	/* No device found matching */
+.L2_no_device_found:
+	mov	%g2, %o7		/* Restore return address */
+	srl	%l2, 4, %o2		/* APB DEV Index */
+	retl
+	 mov	%g0, %o0
+
+
+
+/* FUNCTION _nomem_amba_scan_gaisler_ahb2ahb_bridge(unsigned int bridge, int bus)
+ *
+ * Constraints:
+ *   - o1 may not be used
+ *   - o0, o2, o3 may be used.
+ *
+ * Arguments
+ *  - o0 PnP Address of Bridge AHB device
+ *  - o2 PnP ID of AHB device
+ *
+ * Results
+ *  - o0 Address of new bus PnP area or a 1 if AHB device is no bridge
+ *
+ * Clobbered
+ *   - o0, o2
+ *
+ */
+_nomem_amba_scan_gaisler_ahb2ahb_bridge:
+	andn	%o2, 0xfff, %o2
+	sethi	%hi(AMBA_PNP_ID(VENDOR_GAISLER,GAISLER_AHB2AHB)), %o3
+	cmp	%o2, %o3
+	beq	.L_is_ahb2ahb_bridge
+	 nop
+
+	retl
+	 add	%g0, 1, %o0
+
+.L_is_ahb2ahb_bridge:
+	/* Found a GAISLER AHB2AHB bridge */
+	retl
+	 ld	[%o0 + AMBA_AHB_CUSTOM1_OFS], %o0 /* Get address of bridge PnP area */
+
+
+/* FUNCTION _nomem_amba_scan_gaisler_l2cache_bridge(unsigned int bridge, int bus)
+ *
+ * Constraints:
+ *   - o1 may not be used
+ *   - o0, o2, o3 may be used.
+ *
+ * Arguments
+ *  - o0 PnP Address of Bridge AHB device
+ *  - o2 PnP ID of AHB device
+ *
+ * Results
+ *  - o0 Address of new bus PnP area or a 1 if AHB device is no bridge
+ *
+ * Clobbered
+ *   - o0, o2
+ *
+ */
+_nomem_amba_scan_gaisler_l2cache_bridge:
+	andn	%o2, 0xfff, %o2
+	sethi	%hi(AMBA_PNP_ID(VENDOR_GAISLER,GAISLER_L2CACHE)), %o3
+	cmp	%o2, %o3
+	beq	.L_is_l2cache_bridge
+	 nop
+
+	retl
+	 add	%g0, 1, %o0
+
+.L_is_l2cache_bridge:
+	/* Found a GAISLER l2cache bridge */
+	retl
+	 ld	[%o0 + AMBA_AHB_CUSTOM1_OFS], %o0 /* Get address of bridge PnP area */
+
+
+/* FUNCTION _nomem_amba_scan(unsigned int bridge, int bus)
+ *
+ * Constraints:
+ *  i0-i7, l0 is used by caller
+ *  o5-o7 may not be used.
+ *
+ * Arguments
+ *  - o0 Bridge Information: I/O AREA and parent bus
+ *  - o1 Bus
+ *
+ * Results
+ *  - o0 Number of AHB bridges found
+ *
+ * Clobbered
+ *  - o0 (Current AHB slave conf address)
+ *  - o2 (Used by insert_bridge)
+ *  - o3 (Used by insert_bridge)
+ *  - l1 (Number of AHB Slaves left to process)
+ *  - l2 (Current AHB slave conf address)
+ *  - g2 (Return address)
+ */
+_nomem_amba_scan:
+	mov	%o7, %g2	/* Save return address */
+	set	16, %l1
+	cmp	%o1, 1
+	be,a	.L2_maxloops_detected
+	 add	%g0, 64, %l1
+.L2_maxloops_detected:
+
+	/* Clear 3-bit parent bus from bridge to get I/O AREA, then or
+	 * (AMBA_CONF_AREA | AMBA_AHB_SLAVE_CONF_AREA) to get first AHB slave
+	 * conf address.
+	 */
+	andn	%o0, 0x7ff, %o0
+	set	(AMBA_CONF_AREA | AMBA_AHB_SLAVE_CONF_AREA), %l2
+	or	%o0, %l2, %l2
+
+	/* Scan AHB Slave area for AHB<->AHB bridges. For each AHB device
+	 * all "bridge drivers" are called, the driver function interface:
+	 *
+	 * Input:
+	 *   - o0 PnP Address of Bridge AHB device
+	 *   - o2 PnP ID of AHB device
+	 * Return values:
+	 *   - o0 Address of new bus PnP area, returning a 1 in o2 means not found
+	 *
+	 * Constraints:
+	 *   - o1 may not be used
+	 *   - o0, o2, o3 may be used.
+	 *
+	 */
+.L_scan_one_ahb_slave:
+	ld	[%l2], %o2
+
+	cmp	%o2, %g0
+	beq	.L_scan_next_ahb_slave
+	 nop
+
+	/* Call the GAISLER AHB2AHB bridge driver */
+	call	_nomem_amba_scan_gaisler_ahb2ahb_bridge
+	 mov	%l2, %o0
+	cmp	%o0, 1
+	bne	.L_found_bridge
+	 ld	[%l2], %o2
+
+	/* Call the GAISLER L2CACHE bridge driver */
+	call	_nomem_amba_scan_gaisler_l2cache_bridge
+	 mov	%l2, %o0
+	cmp	%o0, 1
+	bne	.L_found_bridge
+	 ld	[%l2], %o2
+
+	/* Insert next bridge "driver" function here */
+
+
+	/* The PnP ID did not match a bridge - a new bus was not found ==>
+	 * step to next AHB device */
+	ba	.L_scan_next_ahb_slave
+	 nop
+
+	/* Add Found bus */
+.L_found_bridge:
+	and	%l2, 0x7e0, %o2
+	or	%o2, %o0, %o0		/* Add AHB/AHB Bridge PnP address */
+	call	insert_ahb_bridge	/* Insert Bridge into found buses storage */
+	 or	%o1, %o0, %o0		/* Add parent bus LSB 3-bits */
+
+.L_scan_next_ahb_slave:
+	/* More Slaves to process? */
+	subcc	%l1, 1, %l1
+	bne	.L_scan_one_ahb_slave
+	 add	%l2, AMBA_AHB_CONF_LENGH, %l2
+
+	/* No more AHB devices to process */
+	mov	%g2, %o7	/* Restore return address */
+	retl
+	 nop
+
+/* FUNCTION _nomem_ambapp_find_buses(unsigned int ioarea)
+ *
+ * Find AMBA AHB buses.
+ *
+ * Constraints:
+ *  i6-i7, l7 is used by caller
+ *
+ * Arguments
+ *  - o0 Bridge Information: I/O AREA and parent bus
+ *
+ * Results
+ *  - o0 Number of AHB bridges found
+ *  - i0-i5 initialized
+ *
+ * Clobbered
+ *  - o0 (Current AHB slave conf address)
+ *  - o2 (Used by insert_bridge)
+ *  - o3 (Used by insert_bridge)
+ *  - l0 (Current AHB Bus)
+ *  - l1 (Used by nomem_amba_scan)
+ *  - l2 (Used by nomem_amba_scan)
+ *  - l3 (Used by nomem_amba_scan)
+ *  - l4 (Used by nomem_amba_scan)
+ *
+ *  - g1 (level 1 return address)
+ *  - g2 (Used by nomem_amba_scan)
+ */
+_nomem_ambapp_find_buses:
+	mov	%o7, %g1	/* Save return address */
+
+	/* Initialize AHB Bus storage */
+	call	init_ahb_bridges
+	 nop
+
+	/* Insert AHB Bus 0 */
+	call	insert_ahb_bridge
+	 nop			/* Argument already prepared by caller */
+
+	/* Scan AHB Bus 0 for AHB Bridges */
+	call	_nomem_amba_scan
+	 add	%g0, 1, %o1
+
+	/* Scan all AHB Buses found for more AHB Bridges */
+	add	%g0, 2, %l0
+.L100_search_next_ahb_bus:
+	call	get_ahb_bridge			/* Get bus %l0 I/O Area */
+	 mov	%l0, %o0
+	cmp	%o0, %g0
+	be	.L100_return			/* If no more AHB bus is left to be scanned, proceed */
+	 nop
+	call	_nomem_amba_scan		/* Scan bus %l0 for AHB Bridges. i0-i7,l0 is used */
+	 mov	%l0, %o1			/* I/O AREA untouched in o0 */
+	ba	.L100_search_next_ahb_bus	/* Do next bus */
+	 add	%l0, 1, %l0
+
+.L100_return:
+	mov	%g1, %o7
+	retl
+	 nop
+
+
+/* FUNCTION _nomem_amba_init(unsigned int ioarea)
+ *
+ *  Find all AHB buses
+ *
+ * Constraints:
+ *  i6, i7, o6, o7, l7, l6, g3, g4, g5, g6, g7 is used by caller
+ *
+ * Arguments
+ *  - o0 Bridge Information: I/O AREA and parent bus
+ *
+ * Results
+ *  - o0 Number of AHB bridges found
+ *
+ * Clobbered
+ *  - l0, l1, l2, l3, l4, g1, g2 (used by _nomem_ambapp_find_buses)
+ *  - o0, o1, o2, o3 (Used as arguments)
+ *  - o5 (return address)
+ *  - g1 (level 1 return address)
+ *  - g2 (level 2 return address)
+ */
+_nomem_amba_init:
+	mov	%o7, %o5	/* Save return address, o5 not used */
+
+	/* Scan for buses, it will init i0-i5 */
+	call	_nomem_ambapp_find_buses
+	 nop
+
+	mov	%o5, %o7
+	retl
+	 nop
+
+/* Call tree and their return address register
+ *
+ *_nomem_amba_scan           (g1)
+ * -> init_ahb_bridges       (o7)
+ * -> insert_ahb_bridge      (o7)
+ * -> _nomem_amba_scan       (g2)
+ *    -> insert_ahb_bridge   (o7)
+ * -> get_ahb_bridge         (o7)
+ *
+ *
+ * -> _nomem_find_apb        (g2)
+ *    -> get_ahb_bridge      (o7)
+ *    -> _nomem_find_ahb_bus (o7)
+ *    -> _nomem_find_apb_bus (o7)
+ * -> _nomem_find_ahb        (g2)
+ *    -> get_ahb_bridge      (o7)
+ *    -> _nomem_find_ahb_bus (o7)
+ * -> mem_handler.func()     (o7)
+ *
+ */
diff --git a/arch/sparc/cpu/leon3/ambapp_low_c.S b/arch/sparc/cpu/leon3/ambapp_low_c.S
new file mode 100644
index 0000000..42288fa
--- /dev/null
+++ b/arch/sparc/cpu/leon3/ambapp_low_c.S
@@ -0,0 +1,113 @@
+/* C-interface for AMBA PnP scanning functions implemented in
+ * ambapp_low.S. At the point the memory and stack can be
+ * used.
+ *
+ * (C) Copyright 2010, 2015
+ * Daniel Hellstrom, Cobham Gaisler, daniel@gaisler.com.
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+
+	.seg	"text"
+	.extern	_nomem_ambapp_find_buses
+	.extern	_nomem_find_apb
+	.extern	_nomem_find_ahb
+
+	.globl	ambapp_find_buses
+	.globl	ambapp_find_apb
+	.globl	ambapp_find_ahb
+
+
+/* C-interface for _nomem_ambapp_find_buses used when memory is available.
+ */
+ambapp_find_buses:
+	save	%sp, -104, %sp
+	mov	%i1, %l7	/* Save second argument */
+	call _nomem_ambapp_find_buses
+	 mov	%i0, %o0
+
+	/* Store result */
+	st	%g0, [%l7+0x00]
+	st	%i0, [%l7+0x04]
+	st	%i1, [%l7+0x08]
+	st	%i2, [%l7+0x0c]
+	st	%i3, [%l7+0x10]
+	st	%i4, [%l7+0x14]
+	st	%i5, [%l7+0x18]
+
+	ret
+	 restore
+
+/* C-interface for _nomem_find_apb used when memory is available.
+ *
+ * void ambapp_find_apb(
+ *	struct ambapp_bus *abus,
+ *	unsigned int dev_vend,
+ *	int index,
+ *	struct ambapp_find_apb_info *result
+ *	);
+ *
+ */
+ambapp_find_apb:
+	save	%sp, -104, %sp
+
+	mov	%i3, %l7	/* Save second argument */
+	mov	%i1, %o1
+	mov	%i2, %o2
+
+	/* Initialize buses available in system */
+	ld	[%i0+0x08], %i1
+	ld	[%i0+0x0c], %i2
+	ld	[%i0+0x10], %i3
+	ld	[%i0+0x14], %i4
+	ld	[%i0+0x18], %i5
+
+	call _nomem_find_apb
+	 ld	[%i0+0x04], %i0
+
+	st	%o2, [%l7+0x08]	/* Decremented Index */
+	st	%o3, [%l7]	/* PnP configuration address of APB Device */
+	st	%o4, [%l7+0x04]	/* AHB Bus Index of AHB/APB bridge and APB Device */
+	mov	%o0, %i0
+	ret
+	 restore
+
+/* C-interface for _nomem_find_ahb used when memory is available.
+ *
+ * void ambapp_find_ahb(
+ *	struct ambapp_bus *abus,
+ *	unsigned int dev_vend,
+ *	int index,
+ *	int type,
+ *	struct ambapp_find_ahb_info *result
+ *	);
+ *
+ */
+ambapp_find_ahb:
+	save	%sp, -104, %sp
+
+	mov	%i4, %l7	/* Save second argument */
+	clr	%o0
+	mov	%i1, %o1
+	mov	%i2, %o2
+	clr	%o3
+	clr	%o4
+	mov	%i3, %o5
+
+	/* Initialize buses available in system */
+	ld	[%i0+0x08], %i1
+	ld	[%i0+0x0c], %i2
+	ld	[%i0+0x10], %i3
+	ld	[%i0+0x14], %i4
+	ld	[%i0+0x18], %i5
+
+	call _nomem_find_ahb
+	 ld	[%i0+0x04], %i0
+
+	st	%o2, [%l7+0x08]	/* Decremented Index */
+	st	%o3, [%l7]	/* PnP configuration address of AHB Device */
+	st	%o4, [%l7+0x04]	/* AHB Bus Index of AHB Device */
+	mov	%o0, %i0
+	ret
+	 restore
diff --git a/arch/sparc/cpu/leon3/cpu_init.c b/arch/sparc/cpu/leon3/cpu_init.c
index 2f41d88..f23ecb8 100644
--- a/arch/sparc/cpu/leon3/cpu_init.c
+++ b/arch/sparc/cpu/leon3/cpu_init.c
@@ -1,8 +1,8 @@
 /* Initializes CPU and basic hardware such as memory
  * controllers, IRQ controller and system timer 0.
  *
- * (C) Copyright 2007
- * Daniel Hellstrom, Gaisler Research, daniel@gaisler.com
+ * (C) Copyright 2007, 2015
+ * Daniel Hellstrom, Cobham Gaisler, daniel@gaisler.com
  *
  * SPDX-License-Identifier:	GPL-2.0+
  */
@@ -14,6 +14,11 @@
 
 #include <config.h>
 
+/* Default Plug&Play I/O area */
+#ifndef CONFIG_AMBAPP_IOAREA
+#define CONFIG_AMBAPP_IOAREA AMBA_DEFAULT_IOAREA
+#endif
+
 #define TIMER_BASE_CLK 1000000
 #define US_PER_TICK (1000000 / CONFIG_SYS_HZ)
 
@@ -39,64 +44,30 @@ struct {
 /*
  * Breath some life into the CPU...
  *
- * Set up the memory map,
- * initialize a bunch of registers.
- *
  * Run from FLASH/PROM:
  *  - until memory controller is set up, only registers available
+ *  - memory controller has already been setup up, stack can be used
  *  - no global variables available for writing
  *  - constants available
  */
-
 void cpu_init_f(void)
 {
-	/* these varaiable must not be initialized */
-	ambapp_dev_irqmp *irqmp;
-	ambapp_apbdev apbdev;
-	register unsigned int apbmst;
-
-	/* find AMBA APB Master */
-	apbmst = (unsigned int)
-	    ambapp_ahb_next_nomem(VENDOR_GAISLER, GAISLER_APBMST, 1, 0);
-	if (!apbmst) {
-		/*
-		 * no AHB/APB bridge, something is wrong
-		 * ==> jump to start (or hang)
-		 */
-		while (1) ;
-	}
-	/* Init memory controller */
-	if (init_memory_ctrl()) {
-		while (1) ;
-	}
-
-	/****************************************************
-	 * From here we can use the main memory and the stack.
-	 */
-
-	/* Find AMBA APB IRQMP Controller */
-	if (ambapp_apb_first(VENDOR_GAISLER, GAISLER_IRQMP, &apbdev) != 1) {
-		/* no IRQ controller, something is wrong
-		 * ==> jump to start (or hang)
-		 */
-		while (1) ;
-	}
-	irqmp = (ambapp_dev_irqmp *) apbdev.address;
 
-	/* initialize the IRQMP */
-	irqmp->ilevel = 0xf;	/* all IRQ off */
-	irqmp->iforce = 0;
-	irqmp->ipend = 0;
-	irqmp->iclear = 0xfffe;	/* clear all old pending interrupts */
-	irqmp->cpu_mask[0] = 0;	/* mask all IRQs on CPU 0 */
-	irqmp->cpu_force[0] = 0;	/* no force IRQ on CPU 0 */
-
-	/* cache */
 }
 
+/* Routine called from start.S,
+ *
+ * Run from FLASH/PROM:
+ *  - memory controller has already been setup up, stack can be used
+ *  - global variables available for read/writing
+ *  - constants avaiable
+	 */
 void cpu_init_f2(void)
 {
-
+	/* Initialize the AMBA Plug & Play bus structure, the bus
+	 * structure represents the AMBA bus that the CPU is located at.
+	 */
+	ambapp_bus_init(CONFIG_AMBAPP_IOAREA, CONFIG_SYS_CLK_FREQ, &ambapp_plb);
 }
 
 /*
@@ -105,95 +76,58 @@ void cpu_init_f2(void)
 int cpu_init_r(void)
 {
 	ambapp_apbdev apbdev;
+	int index, cpu;
+	ambapp_dev_gptimer *timer = NULL;
+	unsigned int bus_freq;
 
 	/*
 	 * Find AMBA APB IRQMP Controller,
-	 * When we come so far we know there is a IRQMP available
 	 */
-	ambapp_apb_first(VENDOR_GAISLER, GAISLER_IRQMP, &apbdev);
-	irqmp = (ambapp_dev_irqmp *) apbdev.address;
-
-	/* timer */
-	if (ambapp_apb_first(VENDOR_GAISLER, GAISLER_GPTIMER, &apbdev) != 1) {
-		printf("cpu_init_r: gptimer not found!\n");
-		return 1;
-	}
-	gptimer = (ambapp_dev_gptimer *) apbdev.address;
-	gptimer_irq = apbdev.irq;
-
-	/* initialize prescaler common to all timers to 1MHz */
-	gptimer->scalar = gptimer->scalar_reload =
-	    (((CONFIG_SYS_CLK_FREQ / 1000) + 500) / 1000) - 1;
-
-	return (0);
-}
-
-/* find & setup memory controller */
-int init_memory_ctrl()
-{
-	register ambapp_dev_mctrl *mctrl;
-	register ambapp_dev_sdctrl *sdctrl;
-	register ambapp_dev_ddrspa *ddrspa;
-	register ambapp_dev_ddr2spa *ddr2spa;
-	register ahbctrl_pp_dev *ahb;
-	register unsigned int base;
-	register int not_found_mctrl = -1;
-
-	/* find ESA Memory controller */
-	base = ambapp_apb_next_nomem(VENDOR_ESA, ESA_MCTRL, 0);
-	if (base) {
-		mctrl = (ambapp_dev_mctrl *) base;
-
-		/* config MCTRL memory controller */
-		mctrl->mcfg1 = CONFIG_SYS_GRLIB_MEMCFG1 | (mctrl->mcfg1 & 0x300);
-		mctrl->mcfg2 = CONFIG_SYS_GRLIB_MEMCFG2;
-		mctrl->mcfg3 = CONFIG_SYS_GRLIB_MEMCFG3;
-		not_found_mctrl = 0;
+	if (ambapp_apb_find(&ambapp_plb, VENDOR_GAISLER,
+		GAISLER_IRQMP, 0, &apbdev) != 1) {
+		panic("%s: IRQ controller not found\n", __func__);
+		return -1;
 	}
+	irqmp = (ambapp_dev_irqmp *)apbdev.address;
 
-	/* find Gaisler Fault Tolerant Memory controller */
-	base = ambapp_apb_next_nomem(VENDOR_GAISLER, GAISLER_FTMCTRL, 0);
-	if (base) {
-		mctrl = (ambapp_dev_mctrl *) base;
-
-		/* config MCTRL memory controller */
-		mctrl->mcfg1 = CONFIG_SYS_GRLIB_FT_MEMCFG1 | (mctrl->mcfg1 & 0x300);
-		mctrl->mcfg2 = CONFIG_SYS_GRLIB_FT_MEMCFG2;
-		mctrl->mcfg3 = CONFIG_SYS_GRLIB_FT_MEMCFG3;
-		not_found_mctrl = 0;
+	/* initialize the IRQMP */
+	irqmp->ilevel = 0xf;	/* all IRQ off */
+	irqmp->iforce = 0;
+	irqmp->ipend = 0;
+	irqmp->iclear = 0xfffe;	/* clear all old pending interrupts */
+	for (cpu = 0; cpu < 16; cpu++) {
+		/* mask and clear force for all IRQs on CPU[N] */
+		irqmp->cpu_mask[cpu] = 0;
+		irqmp->cpu_force[cpu] = 0;
 	}
 
-	/* find SDRAM controller */
-	base = ambapp_apb_next_nomem(VENDOR_GAISLER, GAISLER_SDCTRL, 0);
-	if (base) {
-		sdctrl = (ambapp_dev_sdctrl *) base;
-
-		/* config memory controller */
-		sdctrl->sdcfg = CONFIG_SYS_GRLIB_SDRAM;
-		not_found_mctrl = 0;
-	}
+	/* timer */
+	index = 0;
+	while (ambapp_apb_find(&ambapp_plb, VENDOR_GAISLER, GAISLER_GPTIMER,
+		index, &apbdev) == 1) {
+		timer = (ambapp_dev_gptimer *)apbdev.address;
+		if (gptimer == NULL) {
+			gptimer = timer;
+			gptimer_irq = apbdev.irq;
+		}
+
+		/* Different buses may have different frequency, the
+		 * frequency of the bus tell in which frequency the timer
+		 * prescaler operates.
+		 */
+		bus_freq = ambapp_bus_freq(&ambapp_plb, apbdev.ahb_bus_index);
 
-	ahb = ambapp_ahb_next_nomem(VENDOR_GAISLER, GAISLER_DDR2SPA, 1, 0);
-	if (ahb) {
-		ddr2spa = (ambapp_dev_ddr2spa *) ambapp_ahb_get_info(ahb, 1);
+		/* initialize prescaler common to all timers to 1MHz */
+		timer->scalar = timer->scalar_reload =
+		    (((bus_freq / 1000) + 500) / 1000) - 1;
 
-		/* Config DDR2 memory controller */
-		ddr2spa->cfg1 = CONFIG_SYS_GRLIB_DDR2_CFG1;
-		ddr2spa->cfg3 = CONFIG_SYS_GRLIB_DDR2_CFG3;
-		not_found_mctrl = 0;
+		index++;
 	}
-
-	ahb = ambapp_ahb_next_nomem(VENDOR_GAISLER, GAISLER_DDRSPA, 1, 0);
-	if (ahb) {
-		ddrspa = (ambapp_dev_ddrspa *) ambapp_ahb_get_info(ahb, 1);
-
-		/* Config DDR memory controller */
-		ddrspa->ctrl = CONFIG_SYS_GRLIB_DDR_CFG;
-		not_found_mctrl = 0;
+	if (!gptimer) {
+		printf("%s: gptimer not found!\n", __func__);
+		return 1;
 	}
-
-	/* failed to find any memory controller */
-	return not_found_mctrl;
+	return 0;
 }
 
 /* Uses Timer 0 to get accurate
diff --git a/arch/sparc/cpu/leon3/serial.c b/arch/sparc/cpu/leon3/serial.c
index f1dcab3..c4a49ed 100644
--- a/arch/sparc/cpu/leon3/serial.c
+++ b/arch/sparc/cpu/leon3/serial.c
@@ -14,6 +14,11 @@
 
 DECLARE_GLOBAL_DATA_PTR;
 
+/* Select which UART that will become u-boot console */
+#ifndef CONFIG_SYS_GRLIB_APBUART_INDEX
+#define CONFIG_SYS_GRLIB_APBUART_INDEX 0
+#endif
+
 static int leon3_serial_init(void)
 {
 	ambapp_dev_apbuart *uart;
@@ -21,7 +26,8 @@ static int leon3_serial_init(void)
 	unsigned int tmp;
 
 	/* find UART */
-	if (ambapp_apb_first(VENDOR_GAISLER, GAISLER_APBUART, &apbdev) != 1)
+	if (ambapp_apb_find(&ambapp_plb, VENDOR_GAISLER, GAISLER_APBUART,
+		CONFIG_SYS_GRLIB_APBUART_INDEX, &apbdev) != 1)
 		return -1; /* didn't find hardware */
 
 	/* found apbuart, let's init .. */
diff --git a/arch/sparc/cpu/leon3/usb_uhci.c b/arch/sparc/cpu/leon3/usb_uhci.c
index ca7d6e8..1be84c6 100644
--- a/arch/sparc/cpu/leon3/usb_uhci.c
+++ b/arch/sparc/cpu/leon3/usb_uhci.c
@@ -690,11 +690,11 @@ void handle_usb_interrupt(void)
  */
 int usb_lowlevel_init(int index, enum usb_init_type init, void **controller)
 {
-	unsigned char temp;
 	ambapp_ahbdev ahbdev;
 
 	/* Find GRUSB core using AMBA Plug&Play information */
-	if (ambapp_ahbslv_first(VENDOR_GAISLER, GAISLER_UHCI, &ahbdev) != 1) {
+	if (ambapp_ahbslv_find(&ambapp_plb, VENDOR_GAISLER, GAISLER_UHCI,
+		CONFIG_SYS_GRLIB_GRUSB_INDEX, &ahbdev) != 1) {
 		printf("USB UHCI: Failed to find GRUSB controller\n");
 		return -1;
 	}