grlib: Move source files

Update #3678.

10 files changed, 8696 insertions, 0 deletions

diff --git a/bsps/shared/grlib/pci/gr_701.c b/bsps/shared/grlib/pci/gr_701.c
new file mode 100644
index 0000000000..c9ac0db0f2
--- /dev/null
+++ b/bsps/shared/grlib/pci/gr_701.c
@@ -0,0 +1,618 @@
+/*  GR-701 PCI Target driver.
+ * 
+ *  COPYRIGHT (c) 2008.
+ *  Cobham Gaisler AB.
+ *
+ *  Configures the GR-701 interface PCI board.
+ *  This driver provides a AMBA PnP bus by using the general part
+ *  of the AMBA PnP bus driver (ambapp_bus.c).
+ *
+ *  Driver resources for the AMBA PnP bus provided can be set using
+ *  gr701_set_resources().
+ *
+ *  The license and distribution terms for this file may be
+ *  found in found in the file LICENSE in this distribution or at
+ *  http://www.rtems.org/license/LICENSE.
+ */
+
+#include <inttypes.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+
+#include <bsp.h>
+#include <rtems/bspIo.h>
+#include <pci.h>
+#include <pci/access.h>
+
+#include <grlib/ambapp.h>
+
+#include <grlib/ambapp.h>
+#include <drvmgr/drvmgr.h>
+#include <grlib/ambapp_bus.h>
+#include <drvmgr/pci_bus.h>
+#include <grlib/bspcommon.h>
+#include <grlib/genirq.h>
+
+#include <grlib/gr_701.h>
+
+#include <grlib/grlib_impl.h>
+
+/* Offset from 0x80000000 (dual bus version) */
+#define AHB1_BASE_ADDR 0x80000000
+#define AHB1_IOAREA_BASE_ADDR 0x80100000
+
+/* #define DEBUG 1 */
+
+#ifdef DEBUG
+#define DBG(x...) printk(x)
+#else
+#define DBG(x...) 
+#endif
+
+int gr701_init1(struct drvmgr_dev *dev);
+int gr701_init2(struct drvmgr_dev *dev);
+void gr701_interrupt(void *arg);
+
+#define READ_REG(address) (*(volatile unsigned int *)address)
+
+/* PCI bride reg layout on AMBA side */
+struct amba_bridge_regs {
+	volatile unsigned int bar0;
+	volatile unsigned int bar1;
+	volatile unsigned int bar2;
+	volatile unsigned int bar3;
+	volatile unsigned int bar4;/* 0x10 */
+	
+	volatile unsigned int unused[4*3-1];
+	
+	volatile unsigned int ambabars[1]; /* 0x40 */
+};
+
+/* PCI bride reg layout on PCI side */
+struct pci_bridge_regs {
+	volatile unsigned int bar0;
+	volatile unsigned int bar1;
+	volatile unsigned int bar2;
+	volatile unsigned int bar3;
+	volatile unsigned int bar4; /* 0x10 */
+
+	volatile unsigned int ilevel;
+	volatile unsigned int ipend;
+	volatile unsigned int iforce;
+	volatile unsigned int istatus;
+	volatile unsigned int iclear;
+	volatile unsigned int imask;
+};
+
+/* Private data structure for driver */
+struct gr701_priv {
+	/* Driver management */
+	struct drvmgr_dev		*dev;
+	char				prefix[16];
+	SPIN_DECLARE(devlock);
+
+	struct pci_bridge_regs		*pcib;
+	struct amba_bridge_regs		*ambab;
+
+	/* PCI */
+	pci_dev_t			pcidev;
+	struct pci_dev_info		*devinfo;	
+
+	/* IRQ */
+	genirq_t			genirq;
+	int				interrupt_cnt;
+
+	/* GR-701 Address translation */
+	struct drvmgr_map_entry		bus_maps_up[2];
+	struct drvmgr_map_entry		bus_maps_down[2];
+
+	/* AMBA Plug&Play information on GR-701 */
+	struct ambapp_bus		abus;
+	struct ambapp_mmap		amba_maps[3];
+        struct ambapp_config		config;
+};
+
+int ambapp_gr701_int_register(
+	struct drvmgr_dev *dev,
+	int irq,
+	const char *info,
+	drvmgr_isr handler,
+	void *arg);
+int ambapp_gr701_int_unregister(
+	struct drvmgr_dev *dev,
+	int irq,
+	drvmgr_isr isr,
+	void *arg);
+int ambapp_gr701_int_unmask(
+	struct drvmgr_dev *dev,
+	int irq);
+int ambapp_gr701_int_mask(
+	struct drvmgr_dev *dev,
+	int irq);
+int ambapp_gr701_int_clear(
+	struct drvmgr_dev *dev,
+	int irq);
+int ambapp_gr701_get_params(
+	struct drvmgr_dev *dev,
+	struct drvmgr_bus_params *params);
+
+struct ambapp_ops ambapp_gr701_ops = {
+	.int_register = ambapp_gr701_int_register,
+	.int_unregister = ambapp_gr701_int_unregister,
+	.int_unmask = ambapp_gr701_int_unmask,
+	.int_mask = ambapp_gr701_int_mask,
+	.int_clear = ambapp_gr701_int_clear,
+	.get_params = ambapp_gr701_get_params
+};
+
+struct drvmgr_drv_ops gr701_ops = 
+{
+	.init = {gr701_init1, gr701_init2, NULL, NULL},
+	.remove = NULL,
+	.info = NULL
+};
+
+struct pci_dev_id_match gr701_ids[] = 
+{
+	PCIID_DEVVEND(PCIID_VENDOR_GAISLER, PCIID_DEVICE_GR_701),
+	PCIID_END_TABLE /* Mark end of table */
+};
+
+struct pci_drv_info gr701_info =
+{
+	{
+		DRVMGR_OBJ_DRV,			/* Driver */
+		NULL,				/* Next driver */
+		NULL,				/* Device list */
+		DRIVER_PCI_GAISLER_GR701_ID,	/* Driver ID */
+		"GR-701_DRV",			/* Driver Name */
+		DRVMGR_BUS_TYPE_PCI,		/* Bus Type */
+		&gr701_ops,
+		NULL,				/* Funcs */
+		0,				/* No devices yet */
+		0,
+	},
+	&gr701_ids[0]
+};
+
+/* Driver resources configuration for the AMBA bus on the GR-701 board.
+ * It is declared weak so that the user may override it from the project file,
+ * if the default settings are not enough.
+ *
+ * The configuration consists of an array of configuration pointers, each
+ * pointer determine the configuration of one GR-701 board. Pointer
+ * zero is for board0, pointer 1 for board1 and so on.
+ *
+ * The array must end with a NULL pointer.
+ */
+struct drvmgr_bus_res *gr701_resources[] __attribute__((weak)) =
+{
+	NULL
+};
+
+void gr701_register_drv(void)
+{
+	DBG("Registering GR-701 PCI driver\n");
+	drvmgr_drv_register(&gr701_info.general);
+}
+
+void gr701_interrupt(void *arg)
+{
+	struct gr701_priv *priv = arg;
+	unsigned int status;
+	int irq = 0;
+	SPIN_ISR_IRQFLAGS(irqflags);
+
+	SPIN_LOCK(&priv->devlock, irqflags);
+	while ( (status=priv->pcib->istatus) != 0 ) {
+		priv->interrupt_cnt++;	/* An interrupt was generated */
+		irq = status;
+		genirq_doirq(priv->genirq, irq);
+		/* ACK interrupt */
+		priv->pcib->istatus = 0;
+	}
+	SPIN_UNLOCK(&priv->devlock, irqflags);
+
+	/* ACK interrupt, this is because PCI is Level, so the IRQ Controller still drives the IRQ. */
+	if ( irq )
+		drvmgr_interrupt_clear(priv->dev, 0);
+}
+
+static int gr701_hw_init(struct gr701_priv *priv)
+{
+	uint32_t com1;
+	struct pci_bridge_regs *pcib;
+	struct amba_bridge_regs *ambab;
+	int mst;
+	unsigned int pci_freq_hz;
+	pci_dev_t pcidev = priv->pcidev;
+	struct pci_dev_info *devinfo = priv->devinfo;
+
+	/* Set up PCI ==> AMBA */
+	priv->pcib = pcib = (void *)devinfo->resources[0].address;
+	pcib->bar0 = 0xfc000000;
+
+	/* Set up GR701 AMBA Masters connection to PCI */
+	priv->ambab = ambab = (struct amba_bridge_regs *)(
+		devinfo->resources[1].address + 0x400);
+
+	/* Init all msters, max 16 */
+	for (mst=0; mst<16; mst++) {
+		ambab->ambabars[mst] = 0x40000000;
+		if (READ_REG(&ambab->ambabars[mst]) != 0x40000000)
+			break;
+	}
+
+	/* Setup Address translation for AMBA bus, assume that PCI BAR
+	 * are mapped 1:1 to CPU.
+	 */
+
+	priv->amba_maps[0].size = 0x04000000;
+	priv->amba_maps[0].local_adr = devinfo->resources[1].address;
+	priv->amba_maps[0].remote_adr = 0xfc000000;
+
+	/* Mark end of table */
+	priv->amba_maps[1].size=0;
+	priv->amba_maps[1].local_adr = 0;
+	priv->amba_maps[1].remote_adr = 0;
+
+	/* Setup DOWN-streams address translation */
+	priv->bus_maps_down[0].name = "PCI BAR1 -> AMBA";
+	priv->bus_maps_down[0].size = priv->amba_maps[0].size;
+	priv->bus_maps_down[0].from_adr = (void *)devinfo->resources[1].address;
+	priv->bus_maps_down[0].to_adr = (void *)0xfc000000;
+
+	/* Setup UP-streams address translation */
+	priv->bus_maps_up[0].name = "AMBA PCIF Window";
+	priv->bus_maps_up[0].size = 0x10000000;
+	priv->bus_maps_up[0].from_adr = (void *)0xe0000000;
+	priv->bus_maps_up[0].to_adr = (void *)0x40000000;
+
+	/* Mark end of translation tables */
+	priv->bus_maps_down[1].size = 0;
+	priv->bus_maps_up[1].size = 0;
+
+	/* Enable I/O and Mem accesses */
+	pci_cfg_r32(pcidev, PCIR_COMMAND, &com1);
+	com1 |= PCIM_CMD_PORTEN | PCIM_CMD_MEMEN;
+	pci_cfg_w32(pcidev, PCIR_COMMAND, com1);
+
+	/* Start AMBA PnP scan at first AHB bus */
+	ambapp_scan(&priv->abus, devinfo->resources[1].address + 0x3f00000,
+			NULL, &priv->amba_maps[0]);
+
+	/* Frequency is the same as the PCI bus frequency */
+	drvmgr_freq_get(priv->dev, 0, &pci_freq_hz);
+
+	/* Initialize Frequency of AMBA bus */
+	ambapp_freq_init(&priv->abus, NULL, pci_freq_hz);
+
+	/* Init IRQ controller (avoid IRQ generation) */
+	pcib->imask = 0x0000;
+	pcib->ipend = 0;
+	pcib->iclear = 0xffff;
+	pcib->iforce = 0;
+	pcib->ilevel = 0x0;
+
+	/* Successfully registered the GR-701 board */
+	return 0;
+}
+
+static void gr701_hw_init2(struct gr701_priv *priv)
+{
+	/* Enable PCI Master (for DMA) */
+	pci_master_enable(priv->pcidev);
+}
+
+/* Called when a PCI target is found with the PCI device and vendor ID 
+ * given in gr701_ids[].
+ */
+int gr701_init1(struct drvmgr_dev *dev)
+{
+	struct gr701_priv *priv;
+	struct pci_dev_info *devinfo;
+	uint32_t bar0, bar1, bar0_size, bar1_size;
+	int resources_cnt;
+
+	priv = grlib_calloc(1, sizeof(*priv));
+	if ( !priv )
+		return DRVMGR_NOMEM;
+
+	dev->priv = priv;
+	priv->dev = dev;
+
+	/* Determine number of configurations */
+	resources_cnt = get_resarray_count(gr701_resources);
+
+	/* Generate Device prefix */
+	strcpy(priv->prefix, "/dev/gr701_0");
+	priv->prefix[11] += dev->minor_drv;
+	mkdir(priv->prefix, S_IRWXU | S_IRWXG | S_IRWXO);
+	priv->prefix[12] = '/';
+	priv->prefix[13] = '\0';
+
+	priv->devinfo = devinfo = (struct pci_dev_info *)dev->businfo;
+	priv->pcidev = devinfo->pcidev;
+	bar0 = devinfo->resources[0].address;
+	bar0_size = devinfo->resources[0].size;
+	bar1 = devinfo->resources[1].address;
+	bar1_size = devinfo->resources[1].size;
+	printk("\n\n--- GR-701[%d] ---\n", dev->minor_drv);
+	printk(" PCI BUS: 0x%x, SLOT: 0x%x, FUNCTION: 0x%x\n",
+		PCI_DEV_EXPAND(priv->pcidev));
+	printk(" PCI VENDOR: 0x%04x, DEVICE: 0x%04x\n\n\n",
+		devinfo->id.vendor, devinfo->id.device);
+	printk(" PCI BAR[0]: 0x%" PRIx32 " - 0x%" PRIx32 "\n",
+		bar0, bar0 + bar0_size - 1);
+	printk(" PCI BAR[1]: 0x%" PRIx32 " - 0x%" PRIx32 "\n",
+		bar1, bar1 + bar1_size - 1);
+	printk(" IRQ: %d\n\n\n", devinfo->irq);
+
+	/* all neccessary space assigned to GR-701 target? */
+	if ((bar0_size == 0) || (bar1_size == 0))
+		return DRVMGR_ENORES;
+
+	/* Initialize spin-lock for this PCI perihperal device. This is to
+	 * protect the Interrupt Controller Registers. The genirq layer is
+         * protecting its own internals and ISR dispatching.
+         */
+	SPIN_INIT(&priv->devlock, priv->prefix);
+
+	priv->genirq = genirq_init(16);
+	if ( priv->genirq == NULL ) {
+		free(priv);
+		dev->priv = NULL;
+		return DRVMGR_FAIL;
+	}
+
+	if ( gr701_hw_init(priv) ) {
+		genirq_destroy(priv->genirq);
+		free(priv);
+		dev->priv = NULL;
+		printk(" Failed to initialize GR-701 HW\n");
+		return DRVMGR_FAIL;
+	}
+
+	/* Init amba bus */
+	priv->config.abus = &priv->abus;
+	priv->config.ops = &ambapp_gr701_ops;
+	priv->config.maps_up = &priv->bus_maps_up[0];
+	priv->config.maps_down = &priv->bus_maps_down[0];
+	if ( priv->dev->minor_drv < resources_cnt ) {
+		priv->config.resources = gr701_resources[priv->dev->minor_drv];
+	} else {
+		priv->config.resources = NULL;
+	}
+
+	/* Create and register AMBA PnP bus. */
+	return ambapp_bus_register(dev, &priv->config);
+}
+
+/* Called when a PCI target is found with the PCI device and vendor ID 
+ * given in gr701_ids[].
+ */
+int gr701_init2(struct drvmgr_dev *dev)
+{
+	struct gr701_priv *priv = dev->priv;
+
+	/* Clear any old interrupt requests */
+	drvmgr_interrupt_clear(dev, 0);
+
+	/* Enable System IRQ so that GR-701 PCI target interrupt goes through.
+	 *
+	 * It is important to enable it in stage init2. If interrupts were
+	 * enabled in init1 this might hang the system when more than one PCI
+	 * board is connected, this is because PCI interrupts might be shared
+	 * and PCI target 2 have not initialized and might therefore drive
+	 * interrupt already when entering init1().
+	 */
+	drvmgr_interrupt_register(dev, 0, "gr701", gr701_interrupt, priv);
+
+	gr701_hw_init2(priv);
+
+	return DRVMGR_OK;
+}
+
+int ambapp_gr701_int_register(
+	struct drvmgr_dev *dev,
+	int irq,
+	const char *info,
+	drvmgr_isr handler,
+	void *arg)
+{
+	struct gr701_priv *priv = dev->parent->dev->priv;
+	SPIN_IRQFLAGS(irqflags);
+	int status;
+	void *h;
+
+	h = genirq_alloc_handler(handler, arg);
+	if ( h == NULL )
+		return DRVMGR_FAIL;
+
+	SPIN_LOCK_IRQ(&priv->devlock, irqflags);
+
+	status = genirq_register(priv->genirq, irq, h);
+	if ( status == 0 ) {
+		/* Clear IRQ for first registered handler */
+		priv->pcib->iclear = (1<<irq);
+	} else if ( status == 1 )
+		status = 0;
+
+	if (status != 0) {
+		SPIN_UNLOCK_IRQ(&priv->devlock, irqflags);
+		genirq_free_handler(h);
+		return DRVMGR_FAIL;
+	}
+
+	status = genirq_enable(priv->genirq, irq, handler, arg);
+	if ( status == 0 ) {
+		/* Enable IRQ for first enabled handler only */
+		priv->pcib->imask |= (1<<irq); /* unmask interrupt source */
+	} else if ( status == 1 )
+		status = DRVMGR_OK;
+
+	SPIN_UNLOCK_IRQ(&priv->devlock, irqflags);
+
+	return status;
+}
+
+int ambapp_gr701_int_unregister(
+	struct drvmgr_dev *dev,
+	int irq,
+	drvmgr_isr isr,
+	void *arg)
+{
+	struct gr701_priv *priv = dev->parent->dev->priv;
+	SPIN_IRQFLAGS(irqflags);
+	int status;
+	void *handler;
+
+	SPIN_LOCK_IRQ(&priv->devlock, irqflags);
+
+	status = genirq_disable(priv->genirq, irq, isr, arg);
+	if ( status == 0 ) {
+		/* Disable IRQ only when no enabled handler exists */
+		priv->pcib->imask &= ~(1<<irq); /* mask interrupt source */
+	}
+
+	handler = genirq_unregister(priv->genirq, irq, isr, arg);
+	if ( handler == NULL )
+		status = DRVMGR_FAIL;
+	else
+		status = DRVMGR_OK;
+
+	SPIN_UNLOCK_IRQ(&priv->devlock, irqflags);
+
+	if (handler)
+		genirq_free_handler(handler);
+
+	return status;
+}
+
+int ambapp_gr701_int_unmask(
+	struct drvmgr_dev *dev,
+	int irq)
+{
+	struct gr701_priv *priv = dev->parent->dev->priv;
+	SPIN_IRQFLAGS(irqflags);
+
+	DBG("GR-701 IRQ %d: enable\n", irq);
+
+	if ( genirq_check(priv->genirq, irq) )
+		return DRVMGR_FAIL;
+
+	SPIN_LOCK_IRQ(&priv->devlock, irqflags);
+
+	/* Enable IRQ */
+	priv->pcib->imask |= (1<<irq); /* unmask interrupt source */
+
+	SPIN_UNLOCK_IRQ(&priv->devlock, irqflags);
+
+	return DRVMGR_OK;
+}
+
+int ambapp_gr701_int_mask(
+	struct drvmgr_dev *dev,
+	int irq)
+{
+	struct gr701_priv *priv = dev->parent->dev->priv;
+	SPIN_IRQFLAGS(irqflags);
+
+	DBG("GR-701 IRQ %d: disable\n", irq);
+
+	if ( genirq_check(priv->genirq, irq) )
+		return DRVMGR_FAIL;
+
+	SPIN_LOCK_IRQ(&priv->devlock, irqflags);
+
+	/* Disable IRQ */
+	priv->pcib->imask &= ~(1<<irq); /* mask interrupt source */
+
+	SPIN_UNLOCK_IRQ(&priv->devlock, irqflags);
+
+	return DRVMGR_OK;
+}
+
+int ambapp_gr701_int_clear(
+	struct drvmgr_dev *dev,
+	int irq)
+{
+	struct gr701_priv *priv = dev->parent->dev->priv;
+
+	if ( genirq_check(priv->genirq, irq) )
+		return DRVMGR_FAIL;
+
+	priv->pcib->iclear = (1<<irq);
+
+	return DRVMGR_OK;
+}
+
+int ambapp_gr701_get_params(struct drvmgr_dev *dev, struct drvmgr_bus_params *params)
+{
+	struct gr701_priv *priv = dev->parent->dev->priv;
+
+	/* Device name prefix pointer, skip /dev */
+	params->dev_prefix = &priv->prefix[5];
+
+	return 0;
+}
+
+void gr701_print_dev(struct drvmgr_dev *dev, int options)
+{
+	struct gr701_priv *priv = dev->priv;
+	struct pci_dev_info *devinfo = priv->devinfo;
+	unsigned int freq_hz;
+	uint32_t bar0, bar1, bar0_size, bar1_size;
+
+	/* Print */
+	printf("--- GR-701 [bus 0x%x, dev 0x%x, fun 0x%x] ---\n",
+		PCI_DEV_EXPAND(priv->pcidev));
+	bar0 = devinfo->resources[0].address;
+	bar0_size = devinfo->resources[0].size;
+	bar1 = devinfo->resources[1].address;
+	bar1_size = devinfo->resources[1].size;
+
+	printf(" PCI BAR[0]: 0x%" PRIx32 " - 0x%" PRIx32 "\n",
+		bar0, bar0 + bar0_size - 1);
+	printf(" PCI BAR[1]: 0x%" PRIx32 " - 0x%" PRIx32 "\n",
+		bar1, bar1 + bar1_size - 1);
+	printf(" IRQ:             %d\n", devinfo->irq);
+
+	/* Frequency is the same as the PCI bus frequency */
+	drvmgr_freq_get(dev, 0, &freq_hz);
+
+	printf(" FREQ:            %u Hz\n", freq_hz);
+	printf(" IMASK:           0x%08x\n", priv->pcib->imask);
+	printf(" IPEND:           0x%08x\n", priv->pcib->ipend);
+
+	/* Print amba config */
+	if ( options & GR701_OPTIONS_AMBA ) {
+		ambapp_print(&priv->abus, 10);
+	}
+
+#if 0
+	/* Print IRQ handlers and their arguments */
+	if ( options & GR701_OPTIONS_IRQ ) {
+		int i;
+		for(i=0; i<16; i++) {
+			printf(" IRQ[%02d]:         0x%x, arg: 0x%x\n", 
+				i, (unsigned int)priv->isrs[i].handler, (unsigned int)priv->isrs[i].arg);
+		}
+	}
+#endif
+}
+
+void gr701_print(int options)
+{
+	struct pci_drv_info *drv = &gr701_info;
+	struct drvmgr_dev *dev;
+
+	dev = drv->general.dev;
+	while(dev) {
+		gr701_print_dev(dev, options);
+		dev = dev->next_in_drv;
+	}
+}
diff --git a/bsps/shared/grlib/pci/gr_rasta_adcdac.c b/bsps/shared/grlib/pci/gr_rasta_adcdac.c
new file mode 100644
index 0000000000..12e35acbc5
--- /dev/null
+++ b/bsps/shared/grlib/pci/gr_rasta_adcdac.c
@@ -0,0 +1,694 @@
+/*  GR-RASTA-ADCDAC PCI Target driver.
+ * 
+ *  COPYRIGHT (c) 2008.
+ *  Cobham Gaisler AB.
+ *
+ *  Configures the GR-RASTA-ADCDAC interface PCI board.
+ *  This driver provides a AMBA PnP bus by using the general part
+ *  of the AMBA PnP bus driver (ambapp_bus.c).
+ *
+ *  Driver resources for the AMBA PnP bus provided can be set using
+ *  gr_rasta_adcdac_set_resources().
+ *
+ *  The license and distribution terms for this file may be
+ *  found in found in the file LICENSE in this distribution or at
+ *  http://www.rtems.org/license/LICENSE.
+ */
+
+#include <inttypes.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+
+#include <bsp.h>
+#include <rtems/bspIo.h>
+#include <pci.h>
+
+#include <grlib/ambapp.h>
+#include <grlib/grlib.h>
+#include <drvmgr/drvmgr.h>
+#include <grlib/ambapp_bus.h>
+#include <drvmgr/pci_bus.h>
+#include <grlib/bspcommon.h>
+#include <grlib/genirq.h>
+
+#include <grlib/gr_rasta_adcdac.h>
+
+#include <grlib/grlib_impl.h>
+
+/*#define DEBUG 1*/
+
+#ifdef DEBUG
+#define DBG(x...) printk(x)
+#else
+#define DBG(x...) 
+#endif
+
+/* Determines which PCI address the AHB masters will access, it should be
+ * set so that the masters can access the CPU RAM. Default is base of CPU RAM,
+ * CPU RAM is mapped 1:1 to PCI space.
+ */
+extern unsigned int _RAM_START;
+#define AHBMST2PCIADR (((unsigned int)&_RAM_START) & 0xf0000000)
+
+/* PCI ID */
+#define PCIID_VENDOR_GAISLER		0x1AC8
+#define PCIID_DEVICE_GR_RASTA_ADCDAC	0x0014
+
+int gr_rasta_adcdac_init1(struct drvmgr_dev *dev);
+int gr_rasta_adcdac_init2(struct drvmgr_dev *dev);
+void gr_rasta_adcdac_isr (void *arg);
+
+struct grpci_regs {
+	volatile unsigned int cfg_stat;
+	volatile unsigned int bar0;
+	volatile unsigned int page0;
+	volatile unsigned int bar1;
+	volatile unsigned int page1;
+	volatile unsigned int iomap;
+	volatile unsigned int stat_cmd;
+};
+
+struct gr_rasta_adcdac_ver {
+	const unsigned int	amba_freq_hz;	/* The frequency */
+	const unsigned int	amba_ioarea;	/* The address where the PnP IOAREA starts at */
+};
+
+/* Private data structure for driver */
+struct gr_rasta_adcdac_priv {
+	/* Driver management */
+	struct drvmgr_dev		*dev;
+	char				prefix[20];
+	SPIN_DECLARE(devlock);
+
+	/* PCI */
+	pci_dev_t			pcidev;
+	struct pci_dev_info		*devinfo;
+	uint32_t			ahbmst2pci_map;
+
+	/* IRQ */
+	genirq_t			genirq;
+
+	/* GR-RASTA-ADCDAC */
+	struct gr_rasta_adcdac_ver	*version;
+	struct irqmp_regs		*irq;
+	struct grpci_regs		*grpci;
+	struct drvmgr_map_entry		bus_maps_down[3];
+	struct drvmgr_map_entry		bus_maps_up[2];
+
+	/* AMBA Plug&Play information on GR-RASTA-ADCDAC */
+	struct ambapp_bus		abus;
+	struct ambapp_mmap		amba_maps[4];
+        struct ambapp_config		config;
+};
+
+struct gr_rasta_adcdac_ver gr_rasta_adcdac_ver0 = {
+	.amba_freq_hz		= 50000000,
+	.amba_ioarea		= 0x80100000,
+};
+
+int ambapp_rasta_adcdac_int_register(
+	struct drvmgr_dev *dev,
+	int irq,
+	const char *info,
+	drvmgr_isr handler,
+	void *arg);
+int ambapp_rasta_adcdac_int_unregister(
+	struct drvmgr_dev *dev,
+	int irq,
+	drvmgr_isr isr,
+	void *arg);
+int ambapp_rasta_adcdac_int_unmask(
+	struct drvmgr_dev *dev,
+	int irq);
+int ambapp_rasta_adcdac_int_mask(
+	struct drvmgr_dev *dev,
+	int irq);
+int ambapp_rasta_adcdac_int_clear(
+	struct drvmgr_dev *dev,
+	int irq);
+int ambapp_rasta_adcdac_get_params(
+	struct drvmgr_dev *dev,
+	struct drvmgr_bus_params *params);
+
+struct ambapp_ops ambapp_rasta_adcdac_ops = {
+	.int_register = ambapp_rasta_adcdac_int_register,
+	.int_unregister = ambapp_rasta_adcdac_int_unregister,
+	.int_unmask = ambapp_rasta_adcdac_int_unmask,
+	.int_mask = ambapp_rasta_adcdac_int_mask,
+	.int_clear = ambapp_rasta_adcdac_int_clear,
+	.get_params = ambapp_rasta_adcdac_get_params
+};
+
+struct drvmgr_drv_ops gr_rasta_adcdac_ops = 
+{	.init = {gr_rasta_adcdac_init1, gr_rasta_adcdac_init2, NULL, NULL},
+	.remove = NULL,
+	.info = NULL
+};
+
+struct pci_dev_id_match gr_rasta_adcdac_ids[] = 
+{
+	PCIID_DEVVEND(PCIID_VENDOR_GAISLER, PCIID_DEVICE_GR_RASTA_ADCDAC),
+	PCIID_END_TABLE /* Mark end of table */
+};
+
+struct pci_drv_info gr_rasta_adcdac_info =
+{
+	{
+		DRVMGR_OBJ_DRV,			/* Driver */
+		NULL,				/* Next driver */
+		NULL,				/* Device list */
+		DRIVER_PCI_GAISLER_RASTAADCDAC_ID,/* Driver ID */
+		"GR-RASTA-ADCDAC_DRV",		/* Driver Name */
+		DRVMGR_BUS_TYPE_PCI,		/* Bus Type */
+		&gr_rasta_adcdac_ops,
+		NULL,				/* Funcs */
+		0,				/* No devices yet */
+		0,
+	},
+	&gr_rasta_adcdac_ids[0]
+};
+
+/* Driver resources configuration for the AMBA bus on the GR-RASTA-ADCDAC board.
+ * It is declared weak so that the user may override it from the project file,
+ * if the default settings are not enough.
+ *
+ * The configuration consists of an array of configuration pointers, each
+ * pointer determine the configuration of one GR-RASTA-ADCDAC board. Pointer
+ * zero is for board0, pointer 1 for board1 and so on.
+ *
+ * The array must end with a NULL pointer.
+ */
+struct drvmgr_bus_res *gr_rasta_adcdac_resources[] __attribute__((weak)) =
+{
+	NULL
+};
+
+void gr_rasta_adcdac_register_drv(void)
+{
+	DBG("Registering GR-RASTA-ADCDAC PCI driver\n");
+	drvmgr_drv_register(&gr_rasta_adcdac_info.general);
+}
+
+void gr_rasta_adcdac_isr (void *arg)
+{
+	struct gr_rasta_adcdac_priv *priv = arg;
+	unsigned int status, tmp;
+	int irq;
+	SPIN_ISR_IRQFLAGS(irqflags);
+
+	tmp = status = priv->irq->ipend;
+
+	/* DBG("GR-RASTA-ADCDAC: IRQ 0x%x\n",status); */
+
+	SPIN_LOCK(&priv->devlock, irqflags);
+	for(irq=0; irq<16; irq++) {
+		if ( status & (1<<irq) ) {
+			genirq_doirq(priv->genirq, irq);
+			priv->irq->iclear = (1<<irq);
+			status &= ~(1<<irq);
+			if ( status == 0 )
+				break;
+		}
+	}
+	SPIN_UNLOCK(&priv->devlock, irqflags);
+
+	/* ACK interrupt, this is because PCI is Level, so the IRQ Controller still drives the IRQ. */
+	if ( tmp )
+		drvmgr_interrupt_clear(priv->dev, 0);
+
+	DBG("RASTA-ADCDAC-IRQ: 0x%x\n", tmp);
+}
+
+static int gr_rasta_adcdac_hw_init1(struct gr_rasta_adcdac_priv *priv)
+{
+	uint32_t data;
+	unsigned int *page0 = NULL;
+	struct ambapp_dev *tmp;
+	struct ambapp_ahb_info *ahb;
+	struct pci_dev_info *devinfo = priv->devinfo;
+	uint32_t bar0, bar0_size;
+
+	/* Select version of GR-RASTA-ADCDAC board */
+	switch (devinfo->rev) {
+		case 0:
+			priv->version = &gr_rasta_adcdac_ver0;
+			break;
+		default:
+			return -2;
+	}
+
+	bar0 = devinfo->resources[0].address;
+	bar0_size = devinfo->resources[0].size;
+	page0 = (unsigned int *)(bar0 + bar0_size/2); 
+
+	/* Point PAGE0 to start of Plug and Play information */
+	*page0 = priv->version->amba_ioarea & 0xf0000000;
+
+	/* set parity error response */
+	pci_cfg_r32(priv->pcidev, PCIR_COMMAND, &data);
+	pci_cfg_w32(priv->pcidev, PCIR_COMMAND, (data|PCIM_CMD_PERRESPEN));
+
+	/* Setup cache line size. Default cache line size will result in
+	 * poor performance (256 word fetches), 0xff will set it according
+	 * to the max size of the PCI FIFO.
+	 */
+	pci_cfg_w8(priv->pcidev, PCIR_CACHELNSZ, 0xff);
+
+	/* Scan AMBA Plug&Play */
+
+	/* AMBA MAP bar0 (in CPU) ==> 0x80000000(remote amba address) */
+	priv->amba_maps[0].size = bar0_size/2;
+	priv->amba_maps[0].local_adr = bar0;
+	priv->amba_maps[0].remote_adr = 0x80000000;
+
+	/* AMBA MAP bar1 (in CPU) ==> 0x40000000(remote amba address) */
+	priv->amba_maps[1].size = devinfo->resources[1].size;
+	priv->amba_maps[1].local_adr = devinfo->resources[1].address;
+	priv->amba_maps[1].remote_adr = 0x40000000;
+
+	/* Addresses not matching with map be untouched */
+	priv->amba_maps[2].size = 0xfffffff0;
+	priv->amba_maps[2].local_adr = 0;
+	priv->amba_maps[2].remote_adr = 0;
+
+	/* Mark end of table */
+	priv->amba_maps[3].size=0;
+	priv->amba_maps[3].local_adr = 0;
+	priv->amba_maps[3].remote_adr = 0;
+
+	/* Start AMBA PnP scan at first AHB bus */
+	/*ambapp_scan(priv->bar0 + (priv->version->amba_ioarea & ~0xf0000000),
+		NULL, &priv->amba_maps[0], NULL, &priv->abus.root, NULL);*/
+	ambapp_scan(&priv->abus,
+		bar0 + (priv->version->amba_ioarea & ~0xf0000000),
+		NULL, &priv->amba_maps[0]);
+
+	/* Initialize Frequency of AMBA bus */
+	ambapp_freq_init(&priv->abus, NULL, priv->version->amba_freq_hz);
+
+	/* Point PAGE0 to start of APB area */
+	*page0 = 0x80000000;	
+
+	/* Find GRPCI controller */
+	tmp = (struct ambapp_dev *)ambapp_for_each(&priv->abus,
+					(OPTIONS_ALL|OPTIONS_APB_SLVS),
+					VENDOR_GAISLER, GAISLER_PCIFBRG,
+					ambapp_find_by_idx, NULL);
+	if ( !tmp ) {
+		return -3;
+	}
+	priv->grpci = (struct grpci_regs *)((struct ambapp_apb_info *)tmp->devinfo)->start;
+
+	/* Set GRPCI mmap so that AMBA masters can access CPU-RAM over
+	 * the PCI window.
+	 */
+	priv->grpci->cfg_stat = (priv->grpci->cfg_stat & 0x0fffffff) |
+				(priv->ahbmst2pci_map & 0xf0000000);
+	priv->grpci->page1 = 0x40000000;
+
+	/* Find IRQ controller */
+	tmp = (struct ambapp_dev *)ambapp_for_each(&priv->abus,
+					(OPTIONS_ALL|OPTIONS_APB_SLVS),
+					VENDOR_GAISLER, GAISLER_IRQMP,
+					ambapp_find_by_idx, NULL);
+	if ( !tmp ) {
+		return -4;
+	}
+	priv->irq = (struct irqmp_regs *)DEV_TO_APB(tmp)->start;
+	/* Set up GR-RASTA-ADCDAC irq controller */
+	priv->irq->iclear = 0xffff;
+	priv->irq->ilevel = 0;
+	priv->irq->mask[0] = 0;
+
+	/* DOWN streams translation table */
+	priv->bus_maps_down[0].name = "PCI BAR0 -> AMBA";
+	priv->bus_maps_down[0].size = priv->amba_maps[0].size;
+	priv->bus_maps_down[0].from_adr = (void *)priv->amba_maps[0].local_adr;
+	priv->bus_maps_down[0].to_adr = (void *)priv->amba_maps[0].remote_adr;
+
+	priv->bus_maps_down[1].name = "PCI BAR1 -> AMBA";
+	priv->bus_maps_down[1].size = priv->amba_maps[1].size;
+	priv->bus_maps_down[1].from_adr = (void *)priv->amba_maps[1].local_adr;
+	priv->bus_maps_down[1].to_adr = (void *)priv->amba_maps[1].remote_adr;
+
+	/* Mark end of translation table */
+	priv->bus_maps_down[2].size = 0;
+
+	/* Find GRPCI controller AHB Slave interface */
+	tmp = (struct ambapp_dev *)ambapp_for_each(&priv->abus,
+					(OPTIONS_ALL|OPTIONS_AHB_SLVS),
+					VENDOR_GAISLER, GAISLER_PCIFBRG,
+					ambapp_find_by_idx, NULL);
+	if ( !tmp ) {
+		return -5;
+	}
+	ahb = (struct ambapp_ahb_info *)tmp->devinfo;
+
+	/* UP streams translation table */
+	priv->bus_maps_up[0].name = "AMBA GRPCI Window";
+	priv->bus_maps_up[0].size = ahb->mask[0]; /* AMBA->PCI Window on GR-RASTA-ADCDAC board */
+	priv->bus_maps_up[0].from_adr = (void *)ahb->start[0];
+	priv->bus_maps_up[0].to_adr = (void *)
+					(priv->ahbmst2pci_map & 0xf0000000);
+
+	/* Mark end of translation table */
+	priv->bus_maps_up[1].size = 0;
+
+	/* Successfully registered the RASTA board */
+	return 0;
+}
+
+static int gr_rasta_adcdac_hw_init2(struct gr_rasta_adcdac_priv *priv)
+{
+	/* Enable DMA by enabling PCI target as master */
+	pci_master_enable(priv->pcidev);
+
+	return DRVMGR_OK;
+}
+
+/* Called when a PCI target is found with the PCI device and vendor ID 
+ * given in gr_rasta_adcdac_ids[].
+ */
+int gr_rasta_adcdac_init1(struct drvmgr_dev *dev)
+{
+	struct gr_rasta_adcdac_priv *priv;
+	struct pci_dev_info *devinfo;
+	int status;
+	uint32_t bar0, bar1, bar0_size, bar1_size;
+	union drvmgr_key_value *value;
+	int resources_cnt;
+
+	priv = grlib_calloc(1, sizeof(*priv));
+	if ( !priv )
+		return DRVMGR_NOMEM;
+
+	dev->priv = priv;
+	priv->dev = dev;
+
+	/* Determine number of configurations */
+	resources_cnt = get_resarray_count(gr_rasta_adcdac_resources);
+
+	/* Generate Device prefix */
+
+	strcpy(priv->prefix, "/dev/rastaadcdac0");
+	priv->prefix[16] += dev->minor_drv;
+	mkdir(priv->prefix, S_IRWXU | S_IRWXG | S_IRWXO);
+	priv->prefix[17] = '/';
+	priv->prefix[18] = '\0';
+
+	priv->devinfo = devinfo = (struct pci_dev_info *)dev->businfo;
+	priv->pcidev = devinfo->pcidev;
+	bar0 = devinfo->resources[0].address;
+	bar0_size = devinfo->resources[0].size;
+	bar1 = devinfo->resources[1].address;
+	bar1_size = devinfo->resources[1].size;
+	printk("\n\n--- GR-RASTA-ADCDAC[%d] ---\n", dev->minor_drv);
+	printk(" PCI BUS: 0x%x, SLOT: 0x%x, FUNCTION: 0x%x\n",
+		PCI_DEV_EXPAND(priv->pcidev));
+	printk(" PCI VENDOR: 0x%04x, DEVICE: 0x%04x\n",
+		devinfo->id.vendor, devinfo->id.device);
+	printk(" PCI BAR[0]: 0x%" PRIx32 " - 0x%" PRIx32 "\n",
+		bar0, bar0 + bar0_size - 1);
+	printk(" PCI BAR[1]: 0x%" PRIx32 " - 0x%" PRIx32 "\n",
+		bar1, bar1 + bar1_size - 1);
+	printk(" IRQ: %d\n\n\n", devinfo->irq);
+
+	/* all neccessary space assigned to GR-RASTA-ADCDAC target? */
+	if ((bar0_size == 0) || (bar1_size == 0))
+		return DRVMGR_ENORES;
+
+	/* Initialize spin-lock for this PCI perihperal device. This is to
+	 * protect the Interrupt Controller Registers. The genirq layer is
+         * protecting its own internals and ISR dispatching.
+         */
+	SPIN_INIT(&priv->devlock, priv->prefix);
+
+	/* Let user override which PCI address the AHB masters of the
+	 * RASTA-ADCDAC board access when doing DMA to CPU RAM. The AHB masters
+	 * access the PCI Window of the AMBA bus, the MSB 4-bits of that address
+	 * is translated according this config option before the address
+	 * goes out on the PCI bus.
+	 * Only the 4 MSB bits have an effect;
+	 */
+	value = drvmgr_dev_key_get(priv->dev, "ahbmst2pci", DRVMGR_KT_INT);
+	if (value)
+		priv->ahbmst2pci_map = value->i;
+	else
+		priv->ahbmst2pci_map = AHBMST2PCIADR; /* default */
+
+	priv->genirq = genirq_init(16);
+	if ( priv->genirq == NULL ) {
+		free(priv);
+		dev->priv = NULL;
+		return DRVMGR_FAIL;
+	}
+
+	if ( (status = gr_rasta_adcdac_hw_init1(priv)) != 0 ) {
+		genirq_destroy(priv->genirq);
+		free(priv);
+		dev->priv = NULL;
+		printk(" Failed to initialize GR-RASTA-ADCDAC HW: %d\n", status);
+		return DRVMGR_FAIL;
+	}
+
+	/* Init amba bus */
+	priv->config.abus = &priv->abus;
+	priv->config.ops = &ambapp_rasta_adcdac_ops;
+	priv->config.maps_up = &priv->bus_maps_up[0];
+	priv->config.maps_down = &priv->bus_maps_down[0];
+	if ( priv->dev->minor_drv < resources_cnt ) {
+		priv->config.resources = gr_rasta_adcdac_resources[priv->dev->minor_drv];
+	} else {
+		priv->config.resources = NULL;
+	}
+
+	/* Create and register AMBA PnP bus. */
+	return ambapp_bus_register(dev, &priv->config);
+}
+
+int gr_rasta_adcdac_init2(struct drvmgr_dev *dev)
+{
+	struct gr_rasta_adcdac_priv *priv = dev->priv;
+
+	/* Clear any old interrupt requests */
+	drvmgr_interrupt_clear(dev, 0);
+
+	/* Enable System IRQ so that GR-RASTA-ADCDAC PCI target interrupt 
+	 * goes through.
+	 *
+	 * It is important to enable it in stage init2. If interrupts were
+	 * enabled in init1 this might hang the system when more than one 
+	 * PCI board is connected, this is because PCI interrupts might 
+	 * be shared and PCI board 2 have not initialized and might
+	 * therefore drive interrupt already when entering init1().
+	 */
+	drvmgr_interrupt_register(
+		dev,
+		0,
+		"gr_rasta_adcdac",
+		gr_rasta_adcdac_isr, 
+		(void *)priv);
+
+	return gr_rasta_adcdac_hw_init2(priv);
+}
+
+int ambapp_rasta_adcdac_int_register(
+	struct drvmgr_dev *dev,
+	int irq,
+	const char *info,
+	drvmgr_isr handler,
+	void *arg)
+{
+	struct gr_rasta_adcdac_priv *priv = dev->parent->dev->priv;
+	SPIN_IRQFLAGS(irqflags);
+	int status;
+	void *h;
+
+	h = genirq_alloc_handler(handler, arg);
+	if ( h == NULL )
+		return DRVMGR_FAIL;
+
+	SPIN_LOCK_IRQ(&priv->devlock, irqflags);
+
+	status = genirq_register(priv->genirq, irq, h);
+	if ( status == 0 ) {
+		/* Clear IRQ for first registered handler */
+		priv->irq->iclear = (1<<irq);
+	} else if ( status == 1 )
+		status = 0;
+
+	if (status != 0) {
+		SPIN_UNLOCK_IRQ(&priv->devlock, irqflags);
+		genirq_free_handler(h);
+		return DRVMGR_FAIL;
+	}
+
+	status = genirq_enable(priv->genirq, irq, handler, arg);
+	if ( status == 0 ) {
+		/* Enable IRQ for first enabled handler only */
+		priv->irq->mask[0] |= (1<<irq); /* unmask interrupt source */
+	} else if ( status == 1 )
+		status = 0;
+
+	SPIN_UNLOCK_IRQ(&priv->devlock, irqflags);
+
+	return status;
+}
+
+int ambapp_rasta_adcdac_int_unregister(
+	struct drvmgr_dev *dev,
+	int irq,
+	drvmgr_isr isr,
+	void *arg)
+{
+	struct gr_rasta_adcdac_priv *priv = dev->parent->dev->priv;
+	SPIN_IRQFLAGS(irqflags);
+	int status;
+	void *handler;
+
+	SPIN_LOCK_IRQ(&priv->devlock, irqflags);
+
+	status = genirq_disable(priv->genirq, irq, isr, arg);
+	if ( status == 0 ) {
+		/* Disable IRQ only when no enabled handler exists */
+		priv->irq->mask[0] &= ~(1<<irq); /* mask interrupt source */
+	}
+
+	handler = genirq_unregister(priv->genirq, irq, isr, arg);
+	if ( handler == NULL )
+		status = DRVMGR_FAIL;
+	else
+		status = DRVMGR_OK;
+
+	SPIN_UNLOCK_IRQ(&priv->devlock, irqflags);
+
+	if (handler)
+		genirq_free_handler(handler);
+
+	return status;
+}
+
+int ambapp_rasta_adcdac_int_unmask(
+	struct drvmgr_dev *dev,
+	int irq)
+{
+	struct gr_rasta_adcdac_priv *priv = dev->parent->dev->priv;
+	SPIN_IRQFLAGS(irqflags);
+
+	DBG("RASTA-ADCDAC IRQ %d: unmask\n", irq);
+
+	if ( genirq_check(priv->genirq, irq) )
+		return DRVMGR_EINVAL;
+
+	SPIN_LOCK_IRQ(&priv->devlock, irqflags);
+
+	/* Enable IRQ for first enabled handler only */
+	priv->irq->mask[0] |= (1<<irq); /* unmask interrupt source */
+
+	SPIN_UNLOCK_IRQ(&priv->devlock, irqflags);
+
+	return DRVMGR_OK;
+}
+
+int ambapp_rasta_adcdac_int_mask(
+	struct drvmgr_dev *dev,
+	int irq)
+{
+	struct gr_rasta_adcdac_priv *priv = dev->parent->dev->priv;
+	SPIN_IRQFLAGS(irqflags);
+
+	DBG("RASTA-ADCDAC IRQ %d: mask\n", irq);
+
+	if ( genirq_check(priv->genirq, irq) )
+		return DRVMGR_EINVAL;
+
+	SPIN_LOCK_IRQ(&priv->devlock, irqflags);
+
+	/* Disable/mask IRQ */
+	priv->irq->mask[0] &= ~(1<<irq); /* mask interrupt source */
+
+	SPIN_UNLOCK_IRQ(&priv->devlock, irqflags);
+
+	return DRVMGR_OK;
+}
+
+int ambapp_rasta_adcdac_int_clear(
+	struct drvmgr_dev *dev,
+	int irq)
+{
+	struct gr_rasta_adcdac_priv *priv = dev->parent->dev->priv;
+
+	if ( genirq_check(priv->genirq, irq) )
+		return DRVMGR_FAIL;
+
+	priv->irq->iclear = (1<<irq);
+
+	return DRVMGR_OK;
+}
+
+int ambapp_rasta_adcdac_get_params(struct drvmgr_dev *dev, struct drvmgr_bus_params *params)
+{
+	struct gr_rasta_adcdac_priv *priv = dev->parent->dev->priv;
+
+	/* Device name prefix pointer, skip /dev */
+	params->dev_prefix = &priv->prefix[5];
+
+	return 0;
+}
+
+void gr_rasta_adcdac_print_dev(struct drvmgr_dev *dev, int options)
+{
+	struct gr_rasta_adcdac_priv *priv = dev->priv;
+	struct pci_dev_info *devinfo = priv->devinfo;
+	uint32_t bar0, bar1, bar0_size, bar1_size;
+
+	/* Print */
+	printf("--- GR-RASTA-ADCDAC [bus 0x%x, dev 0x%x, fun 0x%x] ---\n",
+		PCI_DEV_EXPAND(priv->pcidev));
+
+	bar0 = devinfo->resources[0].address;
+	bar0_size = devinfo->resources[0].size;
+	bar1 = devinfo->resources[1].address;
+	bar1_size = devinfo->resources[1].size;
+
+	printf(" PCI BAR[0]: 0x%" PRIx32 " - 0x%" PRIx32 "\n",
+		bar0, bar0 + bar0_size - 1);
+	printf(" PCI BAR[1]: 0x%" PRIx32 " - 0x%" PRIx32 "\n",
+		bar1, bar1 + bar1_size - 1);
+	printf(" IRQ REGS:        0x%" PRIxPTR "\n", (uintptr_t)priv->irq);
+	printf(" IRQ:             %d\n", devinfo->irq);
+	printf(" PCI REVISION:    %d\n", devinfo->rev);
+	printf(" FREQ:            %d Hz\n", priv->version->amba_freq_hz);
+	printf(" IMASK:           0x%08x\n", priv->irq->mask[0]);
+	printf(" IPEND:           0x%08x\n", priv->irq->ipend);
+
+	/* Print amba config */
+	if ( options & RASTA_ADCDAC_OPTIONS_AMBA ) {
+		ambapp_print(&priv->abus, 10);
+	}
+#if 0
+	/* Print IRQ handlers and their arguments */
+	if ( options & RASTA_ADCDAC_OPTIONS_IRQ ) {
+		int i;
+		for(i=0; i<16; i++) {
+			printf(" IRQ[%02d]:         0x%x, arg: 0x%x\n", 
+				i, (unsigned int)priv->isrs[i].handler, (unsigned int)priv->isrs[i].arg);
+		}
+	}
+#endif
+}
+
+void gr_rasta_adcdac_print(int options)
+{
+	struct pci_drv_info *drv = &gr_rasta_adcdac_info;
+	struct drvmgr_dev *dev;
+
+	dev = drv->general.dev;
+	while(dev) {
+		gr_rasta_adcdac_print_dev(dev, options);
+		dev = dev->next_in_drv;
+	}
+}
diff --git a/bsps/shared/grlib/pci/gr_rasta_io.c b/bsps/shared/grlib/pci/gr_rasta_io.c
new file mode 100644
index 0000000000..f4c9d50a7b
--- /dev/null
+++ b/bsps/shared/grlib/pci/gr_rasta_io.c
@@ -0,0 +1,892 @@
+/*  GR-RASTA-IO PCI Target driver.
+ * 
+ *  COPYRIGHT (c) 2008.
+ *  Cobham Gaisler AB.
+ *
+ *  Configures the GR-RASTA-IO interface PCI board.
+ *  This driver provides a AMBA PnP bus by using the general part
+ *  of the AMBA PnP bus driver (ambapp_bus.c).
+ *
+ *  Driver resources for the AMBA PnP bus provided can be set using
+ *  gr_rasta_io_set_resources().
+ *
+ *  The license and distribution terms for this file may be
+ *  found in found in the file LICENSE in this distribution or at
+ *  http://www.rtems.org/license/LICENSE.
+ */
+
+#include <inttypes.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+
+#include <bsp.h>
+#include <rtems/bspIo.h>
+#include <pci.h>
+
+#include <grlib/ambapp.h>
+#include <grlib/grlib.h>
+#include <drvmgr/drvmgr.h>
+#include <grlib/ambapp_bus.h>
+#include <drvmgr/pci_bus.h>
+#include <grlib/bspcommon.h>
+#include <grlib/genirq.h>
+
+#include <grlib/gr_rasta_io.h>
+
+#include <grlib/grlib_impl.h>
+
+/* Determines which PCI address the AHB masters will access, it should be
+ * set so that the masters can access the CPU RAM. Default is base of CPU RAM,
+ * CPU RAM is mapped 1:1 to PCI space.
+ */
+extern unsigned int _RAM_START;
+#define AHBMST2PCIADR (((unsigned int)&_RAM_START) & 0xf0000000)
+
+/* Offset from 0x80000000 (dual bus version) */
+#define AHB1_BASE_ADDR 0x80000000
+#define AHB1_IOAREA_BASE_ADDR 0x80100000
+#define AHB1_IOAREA_OFS (AHB1_IOAREA_BASE_ADDR - AHB1_BASE_ADDR)
+
+/* Second revision constants (GRPCI2) */
+#define GRPCI2_BAR0_TO_AHB_MAP 0x04  /* Fixme */
+#define GRPCI2_BAR1_TO_AHB_MAP 0x08  /* Fixme */
+#define GRPCI2_PCI_CONFIG      0x20  /* Fixme */
+
+
+/* #define DEBUG 1 */
+
+#ifdef DEBUG
+#define DBG(x...) printk(x)
+#else
+#define DBG(x...) 
+#endif
+
+/* PCI ID */
+#define PCIID_VENDOR_GAISLER		0x1AC8
+
+int gr_rasta_io_init1(struct drvmgr_dev *dev);
+int gr_rasta_io_init2(struct drvmgr_dev *dev);
+void gr_rasta_io_isr (void *arg);
+
+struct grpci_regs {
+	volatile unsigned int cfg_stat;
+	volatile unsigned int bar0;
+	volatile unsigned int page0;
+	volatile unsigned int bar1;
+	volatile unsigned int page1;
+	volatile unsigned int iomap;
+	volatile unsigned int stat_cmd;
+};
+
+struct grpci2_regs {
+	volatile unsigned int ctrl;
+	volatile unsigned int statcap;
+	volatile unsigned int pcimstprefetch;
+	volatile unsigned int ahbtopciiomap;
+	volatile unsigned int dmactrl;
+	volatile unsigned int dmadesc;
+	volatile unsigned int dmachanact;
+	volatile unsigned int reserved;
+	volatile unsigned int pcibartoahb[6];
+	volatile unsigned int reserved2[2];
+	volatile unsigned int ahbtopcimemmap[16];
+	volatile unsigned int trcctrl;
+	volatile unsigned int trccntmode;
+	volatile unsigned int trcadpat;
+	volatile unsigned int trcadmask;
+	volatile unsigned int trcctrlsigpat;
+	volatile unsigned int trcctrlsigmask;
+	volatile unsigned int trcadstate;
+	volatile unsigned int trcctrlsigstate;
+};
+
+struct gr_rasta_io_ver {
+	const unsigned int	amba_freq_hz;	/* The frequency */
+	const unsigned int	amba_ioarea;	/* The address where the PnP IOAREA starts at */
+};
+
+/* Private data structure for driver */
+struct gr_rasta_io_priv {
+	/* Driver management */
+	struct drvmgr_dev		*dev;
+	char				prefix[16];
+	SPIN_DECLARE(devlock);
+
+	/* PCI */
+	pci_dev_t			pcidev;
+	struct pci_dev_info		*devinfo;
+	uint32_t			ahbmst2pci_map;
+
+	/* IRQ */
+	genirq_t			genirq;
+
+	/* GR-RASTA-IO */
+	struct gr_rasta_io_ver		*version;
+	struct irqmp_regs		*irq;
+	struct grpci_regs		*grpci;
+	struct grpci2_regs		*grpci2;
+	struct drvmgr_map_entry		bus_maps_down[3];
+	struct drvmgr_map_entry		bus_maps_up[2];
+
+	/* AMBA Plug&Play information on GR-RASTA-IO */
+	struct ambapp_bus		abus;
+	struct ambapp_mmap		amba_maps[4];
+        struct ambapp_config		config;
+};
+
+struct gr_rasta_io_ver gr_rasta_io_ver0 = {
+	.amba_freq_hz		= 30000000,
+	.amba_ioarea		= 0x80100000,
+};
+
+struct gr_rasta_io_ver gr_rasta_io_ver1 = {
+	.amba_freq_hz		= 50000000,
+	.amba_ioarea		= 0x80100000,
+};
+
+int ambapp_rasta_io_int_register(
+	struct drvmgr_dev *dev,
+	int irq,
+	const char *info,
+	drvmgr_isr handler,
+	void *arg);
+int ambapp_rasta_io_int_unregister(
+	struct drvmgr_dev *dev,
+	int irq,
+	drvmgr_isr handler,
+	void *arg);
+int ambapp_rasta_io_int_unmask(
+	struct drvmgr_dev *dev,
+	int irq);
+int ambapp_rasta_io_int_mask(
+	struct drvmgr_dev *dev,
+	int irq);
+int ambapp_rasta_io_int_clear(
+	struct drvmgr_dev *dev,
+	int irq);
+int ambapp_rasta_io_get_params(
+	struct drvmgr_dev *dev,
+	struct drvmgr_bus_params *params);
+
+struct ambapp_ops ambapp_rasta_io_ops = {
+	.int_register = ambapp_rasta_io_int_register,
+	.int_unregister = ambapp_rasta_io_int_unregister,
+	.int_unmask = ambapp_rasta_io_int_unmask,
+	.int_mask = ambapp_rasta_io_int_mask,
+	.int_clear = ambapp_rasta_io_int_clear,
+	.get_params = ambapp_rasta_io_get_params
+};
+
+struct drvmgr_drv_ops gr_rasta_io_ops = 
+{
+	.init = {gr_rasta_io_init1, gr_rasta_io_init2, NULL, NULL},
+	.remove = NULL,
+	.info = NULL
+};
+
+struct pci_dev_id_match gr_rasta_io_ids[] = 
+{
+	PCIID_DEVVEND(PCIID_VENDOR_GAISLER, PCIID_DEVICE_GR_RASTA_IO),
+	PCIID_DEVVEND(PCIID_VENDOR_GAISLER_OLD, PCIID_DEVICE_GR_RASTA_IO_OLD),
+	PCIID_END_TABLE /* Mark end of table */
+};
+
+struct pci_drv_info gr_rasta_io_info =
+{
+	{
+		DRVMGR_OBJ_DRV,			/* Driver */
+		NULL,				/* Next driver */
+		NULL,				/* Device list */
+		DRIVER_PCI_GAISLER_RASTAIO_ID,	/* Driver ID */
+		"GR-RASTA-IO_DRV",		/* Driver Name */
+		DRVMGR_BUS_TYPE_PCI,		/* Bus Type */
+		&gr_rasta_io_ops,
+		NULL,				/* Funcs */
+		0,				/* No devices yet */
+		0,
+	},
+	&gr_rasta_io_ids[0]
+};
+
+/* Driver resources configuration for the AMBA bus on the GR-RASTA-IO board.
+ * It is declared weak so that the user may override it from the project file,
+ * if the default settings are not enough.
+ *
+ * The configuration consists of an array of configuration pointers, each
+ * pointer determine the configuration of one GR-RASTA-IO board. Pointer
+ * zero is for board0, pointer 1 for board1 and so on.
+ *
+ * The array must end with a NULL pointer.
+ */
+struct drvmgr_bus_res *gr_rasta_io_resources[] __attribute__((weak)) =
+{
+	NULL
+};
+
+void gr_rasta_io_register_drv(void)
+{
+	DBG("Registering GR-RASTA-IO PCI driver\n");
+	drvmgr_drv_register(&gr_rasta_io_info.general);
+}
+
+void gr_rasta_io_isr (void *arg)
+{
+	struct gr_rasta_io_priv *priv = arg;
+	unsigned int status, tmp;
+	int irq;
+	SPIN_ISR_IRQFLAGS(irqflags);
+
+	tmp = status = priv->irq->ipend;
+
+	/* DBG("GR-RASTA-IO: IRQ 0x%x\n",status); */
+
+	SPIN_LOCK(&priv->devlock, irqflags);
+	for(irq=0; irq<16; irq++) {
+		if ( status & (1<<irq) ) {
+			genirq_doirq(priv->genirq, irq);
+			priv->irq->iclear = (1<<irq);
+			status &= ~(1<<irq);
+			if ( status == 0 )
+				break;
+		}
+	}
+	SPIN_UNLOCK(&priv->devlock, irqflags);
+
+	/* ACK interrupt, this is because PCI is Level, so the IRQ Controller still drives the IRQ. */
+	if ( tmp ) 
+		drvmgr_interrupt_clear(priv->dev, 0);
+
+	DBG("RASTA-IO-IRQ: 0x%x\n", tmp);
+}
+
+/* PCI Hardware (Revision 0 and 1) initialization */
+static int gr_rasta_io_hw_init(struct gr_rasta_io_priv *priv)
+{
+	unsigned int *page0 = NULL;
+	struct ambapp_dev *tmp;
+	struct ambapp_ahb_info *ahb;
+	struct pci_dev_info *devinfo = priv->devinfo;
+	uint32_t bar0, bar0_size;
+
+	bar0 = devinfo->resources[0].address;
+	bar0_size = devinfo->resources[0].size;
+	page0 = (unsigned int *)(bar0 + bar0_size/2); 
+
+	/* Point PAGE0 to start of Plug and Play information */
+	*page0 = priv->version->amba_ioarea & 0xff000000;
+
+#if 0
+	{
+		uint32_t data;
+		/* set parity error response */
+		pci_cfg_r32(priv->pcidev, PCIR_COMMAND, &data);
+		pci_cfg_w32(priv->pcidev, PCIR_COMMAND, (data|PCIM_CMD_PERRESPEN));
+	}
+#endif
+
+	/* Setup cache line size. Default cache line size will result in
+	 * poor performance (256 word fetches), 0xff will set it according
+	 * to the max size of the PCI FIFO.
+	 */
+	pci_cfg_w8(priv->pcidev, PCIR_CACHELNSZ, 0xff);
+
+	/* Scan AMBA Plug&Play */
+
+	/* AMBA MAP bar0 (in CPU) ==> 0x80000000(remote amba address) */
+	priv->amba_maps[0].size = bar0_size/2;
+	priv->amba_maps[0].local_adr = bar0;
+	priv->amba_maps[0].remote_adr = AHB1_BASE_ADDR;
+
+	/* AMBA MAP bar1 (in CPU) ==> 0x40000000(remote amba address) */
+	priv->amba_maps[1].size = devinfo->resources[1].size;
+	priv->amba_maps[1].local_adr = devinfo->resources[1].address;
+	priv->amba_maps[1].remote_adr = 0x40000000;
+
+	/* Addresses not matching with map be untouched */
+	priv->amba_maps[2].size = 0xfffffff0;
+	priv->amba_maps[2].local_adr = 0;
+	priv->amba_maps[2].remote_adr = 0;
+
+	/* Mark end of table */
+	priv->amba_maps[3].size=0;
+	priv->amba_maps[3].local_adr = 0;
+	priv->amba_maps[3].remote_adr = 0;
+
+	/* Start AMBA PnP scan at first AHB bus */
+	ambapp_scan(&priv->abus,
+			bar0 + (priv->version->amba_ioarea & ~0xff000000),
+			NULL, &priv->amba_maps[0]);
+
+	/* Initialize Frequency of AMBA bus */
+	ambapp_freq_init(&priv->abus, NULL, priv->version->amba_freq_hz);
+
+	/* Point PAGE0 to start of APB area */
+	*page0 = AHB1_BASE_ADDR;	
+
+	/* Find GRPCI controller */
+	tmp = (struct ambapp_dev *)ambapp_for_each(&priv->abus,
+					(OPTIONS_ALL|OPTIONS_APB_SLVS),
+					VENDOR_GAISLER, GAISLER_PCIFBRG,
+					ambapp_find_by_idx, NULL);
+	if ( !tmp ) {
+		return -3;
+	}
+	priv->grpci = (struct grpci_regs *)((struct ambapp_apb_info *)tmp->devinfo)->start;
+
+	/* Set GRPCI mmap so that AMBA masters can access CPU-RAM over
+	 * the PCI window.
+	 */
+	priv->grpci->cfg_stat = (priv->grpci->cfg_stat & 0x0fffffff) |
+				(priv->ahbmst2pci_map & 0xf0000000);
+	priv->grpci->page1 = 0x40000000;
+
+	/* Find IRQ controller, Clear all current IRQs */
+	tmp = (struct ambapp_dev *)ambapp_for_each(&priv->abus,
+					(OPTIONS_ALL|OPTIONS_APB_SLVS),
+					VENDOR_GAISLER, GAISLER_IRQMP,
+					ambapp_find_by_idx, NULL);
+	if ( !tmp ) {
+		return -4;
+	}
+	priv->irq = (struct irqmp_regs *)DEV_TO_APB(tmp)->start;
+	/* Set up GR-RASTA-IO irq controller */
+	priv->irq->mask[0] = 0;
+	priv->irq->iclear = 0xffff;
+	priv->irq->ilevel = 0;
+
+	/* DOWN streams translation table */
+	priv->bus_maps_down[0].name = "PCI BAR0 -> AMBA";
+	priv->bus_maps_down[0].size = priv->amba_maps[0].size;
+	priv->bus_maps_down[0].from_adr = (void *)priv->amba_maps[0].local_adr;
+	priv->bus_maps_down[0].to_adr = (void *)priv->amba_maps[0].remote_adr;
+
+	priv->bus_maps_down[1].name = "PCI BAR1 -> AMBA";
+	priv->bus_maps_down[1].size = priv->amba_maps[1].size;
+	priv->bus_maps_down[1].from_adr = (void *)priv->amba_maps[1].local_adr;
+	priv->bus_maps_down[1].to_adr = (void *)priv->amba_maps[1].remote_adr;
+
+	/* Mark end of translation table */
+	priv->bus_maps_down[2].size = 0;
+
+	/* Find GRPCI controller AHB Slave interface */
+	tmp = (struct ambapp_dev *)ambapp_for_each(&priv->abus,
+					(OPTIONS_ALL|OPTIONS_AHB_SLVS),
+					VENDOR_GAISLER, GAISLER_PCIFBRG,
+					ambapp_find_by_idx, NULL);
+	if ( !tmp ) {
+		return -5;
+	}
+	ahb = (struct ambapp_ahb_info *)tmp->devinfo;
+
+	/* UP streams translation table */
+	priv->bus_maps_up[0].name = "AMBA GRPCI Window";
+	priv->bus_maps_up[0].size = ahb->mask[0]; /* AMBA->PCI Window on GR-RASTA-IO board */
+	priv->bus_maps_up[0].from_adr = (void *)ahb->start[0];
+	priv->bus_maps_up[0].to_adr = (void *)
+					(priv->ahbmst2pci_map & 0xf0000000);
+
+	/* Mark end of translation table */
+	priv->bus_maps_up[1].size = 0;
+
+	/* Successfully registered the RASTA board */
+	return 0;
+}
+
+/* PCI Hardware (Revision 1) initialization */
+static int gr_rasta_io2_hw_init(struct gr_rasta_io_priv *priv)
+{
+	int i;
+	uint32_t data;
+	unsigned int ctrl;
+	uint8_t tmp2;
+	struct ambapp_dev *tmp;
+	struct ambapp_ahb_info *ahb;
+	uint8_t cap_ptr;
+	pci_dev_t pcidev = priv->pcidev;
+	struct pci_dev_info *devinfo = priv->devinfo;
+
+	/* Check capabilities list bit */
+	pci_cfg_r8(pcidev, PCIR_STATUS, &tmp2);
+
+	if (!((tmp2 >> 4) & 1)) {
+		/* Capabilities list not available which it should be in the
+		 * GRPCI2
+		 */
+		return -3;
+	}
+
+	/* Read capabilities pointer */
+	pci_cfg_r8(pcidev, PCIR_CAP_PTR, &cap_ptr);
+
+	/* Set AHB address mappings for target PCI bars
+	 * BAR0: 16MB  : Mapped to I/O at 0x80000000
+	 * BAR1: 256MB : Mapped to MEM at 0x40000000
+	 */
+	pci_cfg_w32(pcidev, cap_ptr+GRPCI2_BAR0_TO_AHB_MAP, AHB1_BASE_ADDR);
+	pci_cfg_w32(pcidev, cap_ptr+GRPCI2_BAR1_TO_AHB_MAP, 0x40000000);
+
+	/* Set PCI bus to be same endianess as PCI system */
+	pci_cfg_r32(pcidev, cap_ptr+GRPCI2_PCI_CONFIG, &data);
+	if (pci_endian == PCI_BIG_ENDIAN)
+		data = data & 0xFFFFFFFE;
+	else
+		data = data | 0x00000001;
+	pci_cfg_w32(pcidev, cap_ptr+GRPCI2_PCI_CONFIG, data);
+
+#if 0
+	/* set parity error response */
+	pci_cfg_r32(pcidev, PCIR_COMMAND, &data);
+	pci_cfg_w32(pcidev, PCIR_COMMAND, (data|PCIM_CMD_PERRESPEN));
+#endif
+
+	/* Scan AMBA Plug&Play */
+
+	/* AMBA MAP bar0 (in PCI) ==> 0x40000000 (remote amba address) */
+	priv->amba_maps[0].size = devinfo->resources[0].size;
+	priv->amba_maps[0].local_adr = devinfo->resources[0].address;
+	priv->amba_maps[0].remote_adr = AHB1_BASE_ADDR;
+
+	/* AMBA MAP bar0 (in PCI) ==> 0x80000000 (remote amba address) */
+	priv->amba_maps[1].size = devinfo->resources[1].size;
+	priv->amba_maps[1].local_adr = devinfo->resources[1].address;
+	priv->amba_maps[1].remote_adr = 0x40000000;
+
+	/* Addresses not matching with map be untouched */
+	priv->amba_maps[2].size = 0xfffffff0;
+	priv->amba_maps[2].local_adr = 0;
+	priv->amba_maps[2].remote_adr = 0;
+
+	/* Mark end of table */
+	priv->amba_maps[3].size=0;
+
+	/* Start AMBA PnP scan at first AHB bus */
+	ambapp_scan(
+		&priv->abus,
+		devinfo->resources[0].address + AHB1_IOAREA_OFS,
+		NULL,
+		&priv->amba_maps[0]);
+
+	/* Initialize Frequency of AMBA bus. The AMBA bus runs at same
+	 * frequency as PCI bus
+	 */
+	ambapp_freq_init(&priv->abus, NULL, priv->version->amba_freq_hz);
+
+	/* Find IRQ controller, Clear all current IRQs */
+	tmp = (struct ambapp_dev *)ambapp_for_each(&priv->abus,
+				(OPTIONS_ALL|OPTIONS_APB_SLVS),
+				VENDOR_GAISLER, GAISLER_IRQMP,
+				ambapp_find_by_idx, NULL);
+	if ( !tmp ) {
+		return -4;
+	}
+	priv->irq = (struct irqmp_regs *)DEV_TO_APB(tmp)->start;
+	/* Set up GR-RASTA-SPW-ROUTER irq controller */
+	priv->irq->mask[0] = 0;
+	priv->irq->iclear = 0xffff;
+	priv->irq->ilevel = 0;
+
+	priv->bus_maps_down[0].name = "PCI BAR0 -> AMBA";
+	priv->bus_maps_down[0].size = priv->amba_maps[0].size;
+	priv->bus_maps_down[0].from_adr = (void *)priv->amba_maps[0].local_adr;
+	priv->bus_maps_down[0].to_adr = (void *)priv->amba_maps[0].remote_adr;
+	priv->bus_maps_down[1].name = "PCI BAR1 -> AMBA";
+	priv->bus_maps_down[1].size = priv->amba_maps[1].size;
+	priv->bus_maps_down[1].from_adr = (void *)priv->amba_maps[1].local_adr;
+	priv->bus_maps_down[1].to_adr = (void *)priv->amba_maps[1].remote_adr;
+	priv->bus_maps_down[2].size = 0;
+
+	/* Find GRPCI2 controller AHB Slave interface */
+	tmp = (void *)ambapp_for_each(&priv->abus,
+					(OPTIONS_ALL|OPTIONS_AHB_SLVS),
+					VENDOR_GAISLER, GAISLER_GRPCI2,
+					ambapp_find_by_idx, NULL);
+	if ( !tmp ) {
+		return -5;
+	}
+	ahb = (struct ambapp_ahb_info *)tmp->devinfo;
+	priv->bus_maps_up[0].name = "AMBA GRPCI2 Window";
+	priv->bus_maps_up[0].size = ahb->mask[0]; /* AMBA->PCI Window on GR-RASTA-SPW-ROUTER board */
+	priv->bus_maps_up[0].from_adr = (void *)ahb->start[0];
+	priv->bus_maps_up[0].to_adr = (void *)
+				(priv->ahbmst2pci_map & ~(ahb->mask[0]-1));
+	priv->bus_maps_up[1].size = 0;
+
+	/* Find GRPCI2 controller APB Slave interface */
+	tmp = (void *)ambapp_for_each(&priv->abus,
+					(OPTIONS_ALL|OPTIONS_APB_SLVS),
+					VENDOR_GAISLER, GAISLER_GRPCI2,
+					ambapp_find_by_idx, NULL);
+	if ( !tmp ) {
+		return -6;
+	}
+	priv->grpci2 = (struct grpci2_regs *)
+		((struct ambapp_apb_info *)tmp->devinfo)->start;
+
+	/* Set AHB to PCI mapping for all AMBA AHB masters */
+	for(i = 0; i < 16; i++) {
+		priv->grpci2->ahbtopcimemmap[i] = priv->ahbmst2pci_map &
+							~(ahb->mask[0]-1);
+	}
+
+	/* Make sure dirq(0) sampling is enabled */
+	ctrl = priv->grpci2->ctrl;
+	ctrl = (ctrl & 0xFFFFFF0F) | (1 << 4);
+	priv->grpci2->ctrl = ctrl;
+
+	/* Successfully registered the RASTA-SPW-ROUTER board */
+	return 0;
+}
+
+static int gr_rasta_io_hw_init2(struct gr_rasta_io_priv *priv)
+{
+	/* Enable DMA by enabling PCI target as master */
+	pci_master_enable(priv->pcidev);
+
+	return DRVMGR_OK;
+}
+
+/* Called when a PCI target is found with the PCI device and vendor ID 
+ * given in gr_rasta_io_ids[].
+ */
+int gr_rasta_io_init1(struct drvmgr_dev *dev)
+{
+	struct gr_rasta_io_priv *priv;
+	struct pci_dev_info *devinfo;
+	int status;
+	uint32_t bar0, bar1, bar0_size, bar1_size;
+	union drvmgr_key_value *value;
+	int resources_cnt;
+
+	priv = grlib_calloc(1, sizeof(*priv));
+	if ( !priv )
+		return DRVMGR_NOMEM;
+
+	dev->priv = priv;
+	priv->dev = dev;
+
+	/* Determine number of configurations */
+	resources_cnt = get_resarray_count(gr_rasta_io_resources);
+
+	/* Generate Device prefix */
+
+	strcpy(priv->prefix, "/dev/rastaio0");
+	priv->prefix[12] += dev->minor_drv;
+	mkdir(priv->prefix, S_IRWXU | S_IRWXG | S_IRWXO);
+	priv->prefix[13] = '/';
+	priv->prefix[14] = '\0';
+
+	priv->devinfo = devinfo = (struct pci_dev_info *)dev->businfo;
+	priv->pcidev = devinfo->pcidev;
+	bar0 = devinfo->resources[0].address;
+	bar0_size = devinfo->resources[0].size;
+	bar1 = devinfo->resources[1].address;
+	bar1_size = devinfo->resources[1].size;
+	printk("\n\n--- GR-RASTA-IO[%d] ---\n", dev->minor_drv);
+	printk(" PCI BUS: 0x%x, SLOT: 0x%x, FUNCTION: 0x%x\n",
+		PCI_DEV_EXPAND(priv->pcidev));
+	printk(" PCI VENDOR: 0x%04x, DEVICE: 0x%04x\n",
+		devinfo->id.vendor, devinfo->id.device);
+	printk(" PCI BAR[0]: 0x%" PRIx32 " - 0x%" PRIx32 "\n",
+		bar0, bar0 + bar0_size - 1);
+	printk(" PCI BAR[1]: 0x%" PRIx32 " - 0x%" PRIx32 "\n",
+		bar1, bar1 + bar1_size - 1);
+	printk(" IRQ: %d\n\n\n", devinfo->irq);
+
+	/* all neccessary space assigned to GR-RASTA-IO target? */
+	if ((bar0_size == 0) || (bar1_size == 0))
+		return DRVMGR_ENORES;
+
+	/* Initialize spin-lock for this PCI peripheral device. This is to
+	 * protect the Interrupt Controller Registers. The genirq layer is
+         * protecting its own internals and ISR dispatching.
+         */
+	SPIN_INIT(&priv->devlock, priv->prefix);
+
+	/* Let user override which PCI address the AHB masters of the
+	 * GR-RASTA-IO board access when doing DMA to CPU RAM. The AHB masters
+	 * access the PCI Window of the AMBA bus, the MSB 4-bits of that address
+	 * is translated according this config option before the address
+	 * goes out on the PCI bus.
+	 * Only the 4 MSB bits have an effect;
+	 */
+	value = drvmgr_dev_key_get(priv->dev, "ahbmst2pci", DRVMGR_KT_INT);
+	if (value)
+		priv->ahbmst2pci_map = value->i;
+	else
+		priv->ahbmst2pci_map = AHBMST2PCIADR; /* default */	
+
+	priv->genirq = genirq_init(16);
+	if ( priv->genirq == NULL ) {
+		free(priv);
+		dev->priv = NULL;
+		return DRVMGR_FAIL;
+	}
+
+	/* Select version of GR-RASTA-IO board */
+	switch (devinfo->rev) {
+		case 0:
+			priv->version = &gr_rasta_io_ver0;
+			status = gr_rasta_io_hw_init(priv);
+			break;
+		case 1:
+			priv->version = &gr_rasta_io_ver1;
+			status = gr_rasta_io_hw_init(priv);
+			break;
+		case 2:
+			priv->version = &gr_rasta_io_ver1; /* same cfg as 1 */
+			status = gr_rasta_io2_hw_init(priv);
+			break;
+		default:
+			return -2;
+	}
+
+	if ( status != 0 ) {
+		genirq_destroy(priv->genirq);
+		free(priv);
+		dev->priv = NULL;
+		printk(" Failed to initialize GR-RASTA-IO HW: %d\n", status);
+		return DRVMGR_FAIL;
+	}
+
+	/* Init amba bus */
+	priv->config.abus = &priv->abus;
+	priv->config.ops = &ambapp_rasta_io_ops;
+	priv->config.maps_up = &priv->bus_maps_up[0];
+	priv->config.maps_down = &priv->bus_maps_down[0];
+	if ( priv->dev->minor_drv < resources_cnt ) {
+		priv->config.resources = gr_rasta_io_resources[priv->dev->minor_drv];
+	} else {
+		priv->config.resources = NULL;
+	}
+
+	/* Create and register AMBA PnP bus. */
+	return ambapp_bus_register(dev, &priv->config);
+}
+
+int gr_rasta_io_init2(struct drvmgr_dev *dev)
+{
+	struct gr_rasta_io_priv *priv = dev->priv;
+
+	/* Clear any old interrupt requests */
+	drvmgr_interrupt_clear(dev, 0);
+
+	/* Enable System IRQ so that GR-RASTA-IO PCI target interrupt goes
+	 * through.
+	 *
+	 * It is important to enable it in stage init2. If interrupts were
+	 * enabled in init1 this might hang the system when more than one
+	 * PCI board is connected, this is because PCI interrupts might
+	 * be shared and PCI board 2 have not initialized and
+	 * might therefore drive interrupt already when entering init1().
+	 */
+	drvmgr_interrupt_register(
+		dev,
+		0,
+		"gr_rasta_io",
+		gr_rasta_io_isr,
+		(void *)priv);
+
+	return gr_rasta_io_hw_init2(priv);
+}
+
+int ambapp_rasta_io_int_register(
+	struct drvmgr_dev *dev,
+	int irq,
+	const char *info,
+	drvmgr_isr handler,
+	void *arg)
+{
+	struct gr_rasta_io_priv *priv = dev->parent->dev->priv;
+	SPIN_IRQFLAGS(irqflags);
+	int status;
+	void *h;
+
+	h = genirq_alloc_handler(handler, arg);
+	if ( h == NULL )
+		return DRVMGR_FAIL;
+
+	SPIN_LOCK_IRQ(&priv->devlock, irqflags);
+
+	status = genirq_register(priv->genirq, irq, h);
+	if ( status == 0 ) {
+		/* Clear IRQ for first registered handler */
+		priv->irq->iclear = (1<<irq);
+	} else if ( status == 1 )
+		status = 0;
+
+	if (status != 0) {
+		SPIN_UNLOCK_IRQ(&priv->devlock, irqflags);
+		genirq_free_handler(h);
+		return DRVMGR_FAIL;
+	}
+
+	status = genirq_enable(priv->genirq, irq, handler, arg);
+	if ( status == 0 ) {
+		/* Enable IRQ for first enabled handler only */
+		priv->irq->mask[0] |= (1<<irq); /* unmask interrupt source */
+	} else if ( status == 1 )
+		status = 0;
+
+	SPIN_UNLOCK_IRQ(&priv->devlock, irqflags);
+
+	return status;
+}
+
+int ambapp_rasta_io_int_unregister(
+	struct drvmgr_dev *dev,
+	int irq,
+	drvmgr_isr isr,
+	void *arg)
+{
+	struct gr_rasta_io_priv *priv = dev->parent->dev->priv;
+	SPIN_IRQFLAGS(irqflags);
+	int status;
+	void *handler;
+
+	SPIN_LOCK_IRQ(&priv->devlock, irqflags);
+
+	status = genirq_disable(priv->genirq, irq, isr, arg);
+	if ( status == 0 ) {
+		/* Disable IRQ only when no enabled handler exists */
+		priv->irq->mask[0] &= ~(1<<irq); /* mask interrupt source */
+	}
+
+	handler = genirq_unregister(priv->genirq, irq, isr, arg);
+	if ( handler == NULL )
+		status = DRVMGR_FAIL;
+	else
+		status = DRVMGR_OK;
+
+	SPIN_UNLOCK_IRQ(&priv->devlock, irqflags);
+
+	if (handler)
+		genirq_free_handler(handler);
+
+	return status;
+}
+
+int ambapp_rasta_io_int_unmask(
+	struct drvmgr_dev *dev,
+	int irq)
+{
+	struct gr_rasta_io_priv *priv = dev->parent->dev->priv;
+	SPIN_IRQFLAGS(irqflags);
+
+	DBG("RASTA-IO IRQ %d: unmask\n", irq);
+
+	if ( genirq_check(priv->genirq, irq) )
+		return DRVMGR_EINVAL;
+
+	SPIN_LOCK_IRQ(&priv->devlock, irqflags);
+
+	/* Enable IRQ for first enabled handler only */
+	priv->irq->mask[0] |= (1<<irq); /* unmask interrupt source */
+
+	SPIN_UNLOCK_IRQ(&priv->devlock, irqflags);
+
+	return DRVMGR_OK;
+}
+
+int ambapp_rasta_io_int_mask(
+	struct drvmgr_dev *dev,
+	int irq)
+{
+	struct gr_rasta_io_priv *priv = dev->parent->dev->priv;
+	SPIN_IRQFLAGS(irqflags);
+
+	DBG("RASTA-IO IRQ %d: mask\n", irq);
+
+	if ( genirq_check(priv->genirq, irq) )
+		return DRVMGR_EINVAL;
+
+	SPIN_LOCK_IRQ(&priv->devlock, irqflags);
+
+	/* Disable/mask IRQ */
+	priv->irq->mask[0] &= ~(1<<irq); /* mask interrupt source */
+
+	SPIN_UNLOCK_IRQ(&priv->devlock, irqflags);
+
+	return DRVMGR_OK;
+}
+
+int ambapp_rasta_io_int_clear(
+	struct drvmgr_dev *dev,
+	int irq)
+{
+	struct gr_rasta_io_priv *priv = dev->parent->dev->priv;
+
+	if ( genirq_check(priv->genirq, irq) )
+		return DRVMGR_EINVAL;
+
+	priv->irq->iclear = (1<<irq);
+
+	return DRVMGR_OK;
+}
+
+int ambapp_rasta_io_get_params(struct drvmgr_dev *dev, struct drvmgr_bus_params *params)
+{
+	struct gr_rasta_io_priv *priv = dev->parent->dev->priv;
+
+	/* Device name prefix pointer, skip /dev */
+	params->dev_prefix = &priv->prefix[5];
+
+	return 0;
+}
+
+void gr_rasta_io_print_dev(struct drvmgr_dev *dev, int options)
+{
+	struct gr_rasta_io_priv *priv = dev->priv;
+	struct pci_dev_info *devinfo = priv->devinfo;
+	uint32_t bar0, bar1, bar0_size, bar1_size;
+
+	/* Print */
+	printf("--- GR-RASTA-IO [bus 0x%x, dev 0x%x, fun 0x%x] ---\n",
+		PCI_DEV_EXPAND(priv->pcidev));
+
+	bar0 = devinfo->resources[0].address;
+	bar0_size = devinfo->resources[0].size;
+	bar1 = devinfo->resources[1].address;
+	bar1_size = devinfo->resources[1].size;
+
+	printf(" PCI BAR[0]: 0x%" PRIx32 " - 0x%" PRIx32 "\n",
+		bar0, bar0 + bar0_size - 1);
+	printf(" PCI BAR[1]: 0x%" PRIx32 " - 0x%" PRIx32 "\n",
+		bar1, bar1 + bar1_size - 1);
+	printf(" IRQ REGS:        0x%" PRIxPTR "\n", (uintptr_t)priv->irq);
+	printf(" IRQ:             %d\n", devinfo->irq);
+	printf(" PCI REVISION:    %d\n", devinfo->rev);
+	printf(" FREQ:            %d Hz\n", priv->version->amba_freq_hz);
+	printf(" IMASK:           0x%08x\n", priv->irq->mask[0]);
+	printf(" IPEND:           0x%08x\n", priv->irq->ipend);
+
+	/* Print amba config */
+	if ( options & RASTA_IO_OPTIONS_AMBA ) {
+		ambapp_print(&priv->abus, 10);
+	}
+
+#if 0
+	/* Print IRQ handlers and their arguments */
+	if ( options & RASTA_IO_OPTIONS_IRQ ) {
+		int i;
+		for(i=0; i<16; i++) {
+			printf(" IRQ[%02d]:         0x%x, arg: 0x%x\n", 
+				i, (unsigned int)priv->isrs[i].handler, (unsigned int)priv->isrs[i].arg);
+		}
+	}
+#endif
+}
+
+void gr_rasta_io_print(int options)
+{
+	struct pci_drv_info *drv = &gr_rasta_io_info;
+	struct drvmgr_dev *dev;
+
+	dev = drv->general.dev;
+	while(dev) {
+		gr_rasta_io_print_dev(dev, options);
+		dev = dev->next_in_drv;
+	}
+}
diff --git a/bsps/shared/grlib/pci/gr_rasta_spw_router.c b/bsps/shared/grlib/pci/gr_rasta_spw_router.c
new file mode 100644
index 0000000000..6efbaa463d
--- /dev/null
+++ b/bsps/shared/grlib/pci/gr_rasta_spw_router.c
@@ -0,0 +1,696 @@
+/*  GR-RASTA-SPW-ROUTER PCI Target driver.
+ * 
+ *  COPYRIGHT (c) 2011.
+ *  Cobham Gaisler AB.
+ *
+ *  The license and distribution terms for this file may be
+ *  found in found in the file LICENSE in this distribution or at
+ *  http://www.rtems.org/license/LICENSE.
+ *
+ *  Configures the GR-RASTA-SPW-ROUTER interface PCI board.
+ *  This driver provides a AMBA PnP bus by using the general part
+ *  of the AMBA PnP bus driver (ambapp_bus.c). Based on the 
+ *  GR-RASTA-IO driver.
+ */
+
+#include <inttypes.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+
+#include <bsp.h>
+#include <rtems/bspIo.h>
+#include <pci.h>
+
+#include <grlib/ambapp.h>
+#include <grlib/grlib.h>
+#include <drvmgr/drvmgr.h>
+#include <grlib/ambapp_bus.h>
+#include <drvmgr/pci_bus.h>
+#include <grlib/bspcommon.h>
+#include <grlib/genirq.h>
+#include <grlib/gr_rasta_spw_router.h>
+
+#include <grlib/grlib_impl.h>
+
+/* Determines which PCI address the AHB masters will access, it should be
+ * set so that the masters can access the CPU RAM. Default is base of CPU RAM,
+ * CPU RAM is mapped 1:1 to PCI space.
+ */
+extern unsigned int _RAM_START;
+#define AHBMST2PCIADR (((unsigned int)&_RAM_START) & 0xf0000000)
+
+/* Offset from 0x80000000 (dual bus version) */
+#define AHB1_BASE_ADDR 0x80000000
+#define AHB1_IOAREA_BASE_ADDR 0x80100000
+
+#define GRPCI2_BAR0_TO_AHB_MAP 0x04  /* Fixme */
+#define GRPCI2_PCI_CONFIG      0x20  /* Fixme */
+
+/* #define DEBUG 1 */
+
+#ifdef DEBUG
+#define DBG(x...) printk(x)
+#else
+#define DBG(x...) 
+#endif
+
+/* PCI ID */
+#define PCIID_VENDOR_GAISLER		0x1AC8
+
+int gr_rasta_spw_router_init1(struct drvmgr_dev *dev);
+int gr_rasta_spw_router_init2(struct drvmgr_dev *dev);
+void gr_rasta_spw_router_isr(void *arg);
+
+struct grpci2_regs {
+	volatile unsigned int ctrl;
+	volatile unsigned int statcap;
+	volatile unsigned int pcimstprefetch;
+	volatile unsigned int ahbtopciiomap;
+	volatile unsigned int dmactrl;
+	volatile unsigned int dmadesc;
+	volatile unsigned int dmachanact;
+	volatile unsigned int reserved;
+	volatile unsigned int pcibartoahb[6];
+	volatile unsigned int reserved2[2];
+	volatile unsigned int ahbtopcimemmap[16];
+	volatile unsigned int trcctrl;
+	volatile unsigned int trccntmode;
+	volatile unsigned int trcadpat;
+	volatile unsigned int trcadmask;
+	volatile unsigned int trcctrlsigpat;
+	volatile unsigned int trcctrlsigmask;
+	volatile unsigned int trcadstate;
+	volatile unsigned int trcctrlsigstate;
+};
+
+struct gr_rasta_spw_router_ver {
+	const unsigned int	amba_freq_hz;	/* The frequency */
+	const unsigned int	amba_ioarea;	/* The address where the PnP IOAREA starts at */
+};
+
+/* Private data structure for driver */
+struct gr_rasta_spw_router_priv {
+	/* Driver management */
+	struct drvmgr_dev	*dev;
+	char			prefix[20];
+	SPIN_DECLARE(devlock);
+
+	/* PCI */
+	pci_dev_t		pcidev;
+	struct pci_dev_info	*devinfo;
+	uint32_t		ahbmst2pci_map;
+
+	/* IRQ */
+	genirq_t		genirq;
+
+	/* GR-RASTA-SPW-ROUTER */
+	struct gr_rasta_spw_router_ver	*version;
+	struct irqmp_regs	*irq;
+	struct grpci2_regs	*grpci2;
+	struct drvmgr_map_entry	bus_maps_up[2];
+	struct drvmgr_map_entry	bus_maps_down[2];
+
+	/* AMBA Plug&Play information on GR-RASTA-SPW-ROUTER */
+	struct ambapp_bus	abus;
+	struct ambapp_mmap	amba_maps[3];
+	struct ambapp_config	config;
+};
+
+struct gr_rasta_spw_router_ver gr_rasta_spw_router_ver0 = {
+	.amba_freq_hz		= 50000000,
+	.amba_ioarea		= 0xfff00000,
+};
+
+int ambapp_rasta_spw_router_int_register(
+	struct drvmgr_dev *dev,
+	int irq,
+	const char *info,
+	drvmgr_isr handler,
+	void *arg);
+int ambapp_rasta_spw_router_int_unregister(
+	struct drvmgr_dev *dev,
+	int irq,
+	drvmgr_isr handler,
+	void *arg);
+int ambapp_rasta_spw_router_int_unmask(
+	struct drvmgr_dev *dev,
+	int irq);
+int ambapp_rasta_spw_router_int_mask(
+	struct drvmgr_dev *dev,
+	int irq);
+int ambapp_rasta_spw_router_int_clear(
+	struct drvmgr_dev *dev,
+	int irq);
+int ambapp_rasta_spw_router_get_params(
+	struct drvmgr_dev *dev,
+	struct drvmgr_bus_params *params);
+
+struct ambapp_ops ambapp_rasta_spw_router_ops = {
+	.int_register = ambapp_rasta_spw_router_int_register,
+	.int_unregister = ambapp_rasta_spw_router_int_unregister,
+	.int_unmask = ambapp_rasta_spw_router_int_unmask,
+	.int_mask = ambapp_rasta_spw_router_int_mask,
+	.int_clear = ambapp_rasta_spw_router_int_clear,
+	.get_params = ambapp_rasta_spw_router_get_params
+};
+
+struct drvmgr_drv_ops gr_rasta_spw_router_ops = 
+{
+	.init = {gr_rasta_spw_router_init1, gr_rasta_spw_router_init2, NULL, NULL},
+	.remove = NULL,
+	.info = NULL
+};
+
+struct pci_dev_id_match gr_rasta_spw_router_ids[] =
+{
+	PCIID_DEVVEND(PCIID_VENDOR_GAISLER, PCIID_DEVICE_GR_RASTA_SPW_RTR),
+	PCIID_END_TABLE /* Mark end of table */
+};
+
+struct pci_drv_info gr_rasta_spw_router_info =
+{
+	{
+		DRVMGR_OBJ_DRV,			/* Driver */
+		NULL,				/* Next driver */
+		NULL,				/* Device list */
+		DRIVER_PCI_GAISLER_RASTA_SPW_ROUTER_ID,	/* Driver ID */
+		"GR-RASTA-SPW_ROUTER_DRV",	/* Driver Name */
+		DRVMGR_BUS_TYPE_PCI,		/* Bus Type */
+		&gr_rasta_spw_router_ops,
+		NULL,				/* Funcs */
+		0,				/* No devices yet */
+		sizeof(struct gr_rasta_spw_router_priv),
+	},
+	&gr_rasta_spw_router_ids[0]
+};
+
+/* Driver resources configuration for the AMBA bus on the GR-RASTA-SPW-ROUTER board.
+ * It is declared weak so that the user may override it from the project file,
+ * if the default settings are not enough.
+ *
+ * The configuration consists of an array of configuration pointers, each
+ * pointer determine the configuration of one GR-RASTA-SPW-ROUTER board. Pointer
+ * zero is for board0, pointer 1 for board1 and so on.
+ *
+ * The array must end with a NULL pointer.
+ */
+struct drvmgr_bus_res *gr_rasta_spw_router_resources[] __attribute__((weak)) =
+{
+	NULL
+};
+
+void gr_rasta_spw_router_register_drv(void)
+{
+	DBG("Registering GR-RASTA-SPW-ROUTER PCI driver\n");
+	drvmgr_drv_register(&gr_rasta_spw_router_info.general);
+}
+
+void gr_rasta_spw_router_isr(void *arg)
+{
+	struct gr_rasta_spw_router_priv *priv = arg;
+	unsigned int status, tmp;
+	int irq;
+	SPIN_ISR_IRQFLAGS(irqflags);
+
+	tmp = status = priv->irq->ipend;
+
+	/* DBG("GR-RASTA-SPW-ROUTER: IRQ 0x%x\n",status); */
+
+	SPIN_LOCK(&priv->devlock, irqflags);
+	for(irq=0; irq<16; irq++) {
+		if ( status & (1<<irq) ) {
+			genirq_doirq(priv->genirq, irq);
+			priv->irq->iclear = (1<<irq);
+			status &= ~(1<<irq);
+			if ( status == 0 )
+				break;
+		}
+	}
+	SPIN_UNLOCK(&priv->devlock, irqflags);
+
+	/* ACK interrupt, this is because PCI is Level, so the IRQ Controller
+	 * still drives the IRQ
+	 */
+	if ( tmp ) 
+		drvmgr_interrupt_clear(priv->dev, 0);
+
+	DBG("RASTA-SPW_ROUTER-IRQ: 0x%x\n", tmp);
+}
+
+static int gr_rasta_spw_router_hw_init(struct gr_rasta_spw_router_priv *priv)
+{
+	int i;
+	uint32_t data;
+	unsigned int ctrl;
+	uint8_t tmp2;
+	struct ambapp_dev *tmp;
+	struct ambapp_ahb_info *ahb;
+	uint8_t cap_ptr;
+	pci_dev_t pcidev = priv->pcidev;
+	struct pci_dev_info *devinfo = priv->devinfo;
+
+	/* Select version of GR-RASTA-SPW-ROUTER board. Currently only one
+	 * version
+	 */
+	switch (devinfo->rev) {
+		case 0:
+			priv->version = &gr_rasta_spw_router_ver0;
+			break;
+		default:
+			return -2;
+	}
+
+	/* Check capabilities list bit */
+	pci_cfg_r8(pcidev, PCIR_STATUS, &tmp2);
+
+	if (!((tmp2 >> 4) & 1)) {
+		/* Capabilities list not available which it should be in the GRPCI2 */
+		return -3;
+	}
+
+	/* Read capabilities pointer */
+	pci_cfg_r8(pcidev, PCIR_CAP_PTR, &cap_ptr);
+
+	/* Set AHB address mappings for target PCI bars */
+	pci_cfg_w32(pcidev, cap_ptr+GRPCI2_BAR0_TO_AHB_MAP, 0xffe00000);  /* APB bus, AHB I/O bus 2 MB */ 
+
+	/* Set PCI bus to be big endian */
+	pci_cfg_r32(pcidev, cap_ptr+GRPCI2_PCI_CONFIG, &data);
+	data = data & 0xFFFFFFFE;
+	pci_cfg_w32(pcidev, cap_ptr+GRPCI2_PCI_CONFIG, data);
+
+#if 0
+	/* set parity error response */
+	pci_cfg_r32(pcidev, PCIR_COMMAND, &data);
+	pci_cfg_w32(pcidev, PCIR_COMMAND, (data|PCIM_CMD_PERRESPEN));
+#endif
+
+	/* Scan AMBA Plug&Play */
+
+	/* AMBA MAP bar0 (in router) ==> 0xffe00000(remote amba address) */
+	priv->amba_maps[0].size = devinfo->resources[0].size;
+	priv->amba_maps[0].local_adr = devinfo->resources[0].address;
+	priv->amba_maps[0].remote_adr = 0xffe00000;
+
+	/* Addresses not matching with map be untouched */
+	priv->amba_maps[1].size = 0xfffffff0;
+	priv->amba_maps[1].local_adr = 0;
+	priv->amba_maps[1].remote_adr = 0;
+
+	/* Mark end of table */
+	priv->amba_maps[2].size=0;
+
+	/* Start AMBA PnP scan at first AHB bus */
+	ambapp_scan(
+		&priv->abus,
+		devinfo->resources[0].address + 0x100000,
+		NULL,
+		&priv->amba_maps[0]);
+
+	/* Initialize Frequency of AMBA bus */
+	ambapp_freq_init(&priv->abus, NULL, priv->version->amba_freq_hz);
+
+	/* Find IRQ controller, Clear all current IRQs */
+	tmp = (struct ambapp_dev *)ambapp_for_each(&priv->abus,
+				(OPTIONS_ALL|OPTIONS_APB_SLVS),
+				VENDOR_GAISLER, GAISLER_IRQMP,
+				ambapp_find_by_idx, NULL);
+	if ( !tmp ) {
+		return -4;
+	}
+	priv->irq = (struct irqmp_regs *)DEV_TO_APB(tmp)->start;
+	/* Set up GR-RASTA-SPW-ROUTER irq controller */
+	priv->irq->mask[0] = 0;
+	priv->irq->iclear = 0xffff;
+	priv->irq->ilevel = 0;
+
+	priv->bus_maps_down[0].name = "PCI BAR0 -> AMBA";
+	priv->bus_maps_down[0].size = priv->amba_maps[0].size;
+	priv->bus_maps_down[0].from_adr = (void *)priv->amba_maps[0].local_adr;
+	priv->bus_maps_down[0].to_adr = (void *)priv->amba_maps[0].remote_adr;
+	priv->bus_maps_down[1].size = 0;
+
+	/* Find GRPCI2 controller AHB Slave interface */
+	tmp = (struct ambapp_dev *)ambapp_for_each(&priv->abus,
+					(OPTIONS_ALL|OPTIONS_AHB_SLVS),
+					VENDOR_GAISLER, GAISLER_GRPCI2,
+					ambapp_find_by_idx, NULL);
+	if ( !tmp ) {
+		return -5;
+	}
+	ahb = (struct ambapp_ahb_info *)tmp->devinfo;
+	priv->bus_maps_up[0].name = "AMBA GRPCI2 Window";
+	priv->bus_maps_up[0].size = ahb->mask[0]; /* AMBA->PCI Window on GR-RASTA-SPW-ROUTER board */
+	priv->bus_maps_up[0].from_adr = (void *)ahb->start[0];
+	priv->bus_maps_up[0].to_adr = (void *)
+				(priv->ahbmst2pci_map & ~(ahb->mask[0]-1));
+	priv->bus_maps_up[1].size = 0;
+
+	/* Find GRPCI2 controller APB Slave interface */
+	tmp = (struct ambapp_dev *)ambapp_for_each(&priv->abus,
+					(OPTIONS_ALL|OPTIONS_APB_SLVS),
+					VENDOR_GAISLER, GAISLER_GRPCI2,
+					ambapp_find_by_idx, NULL);
+	if ( !tmp ) {
+		return -6;
+	}
+	priv->grpci2 = (struct grpci2_regs *)
+		((struct ambapp_apb_info *)tmp->devinfo)->start;
+
+	/* Set AHB to PCI mapping for all AMBA AHB masters */
+	for(i = 0; i < 16; i++) {
+		priv->grpci2->ahbtopcimemmap[i] = priv->ahbmst2pci_map &
+							~(ahb->mask[0]-1);
+	}
+
+	/* Make sure dirq(0) sampling is enabled */
+	ctrl = priv->grpci2->ctrl;
+	ctrl = (ctrl & 0xFFFFFF0F) | (1 << 4);
+	priv->grpci2->ctrl = ctrl;
+
+	/* Successfully registered the RASTA-SPW-ROUTER board */
+	return 0;
+}
+
+static int gr_rasta_spw_router_hw_init2(struct gr_rasta_spw_router_priv *priv)
+{
+	/* Enable DMA by enabling PCI target as master */
+	pci_master_enable(priv->pcidev);
+
+	return DRVMGR_OK;
+}
+
+/* Called when a PCI target is found with the PCI device and vendor ID 
+ * given in gr_rasta_spw_router_ids[].
+ */
+int gr_rasta_spw_router_init1(struct drvmgr_dev *dev)
+{
+	struct gr_rasta_spw_router_priv *priv;
+	struct pci_dev_info *devinfo;
+	int status;
+	uint32_t bar0, bar0_size;
+	union drvmgr_key_value *value;
+	int resources_cnt;
+
+	priv = dev->priv;
+	if (!priv)
+		return DRVMGR_NOMEM;
+
+	memset(priv, 0, sizeof(*priv));
+	dev->priv = priv;
+	priv->dev = dev;
+
+	/* Determine number of configurations */
+	resources_cnt = get_resarray_count(gr_rasta_spw_router_resources);
+
+	/* Generate Device prefix */
+
+	strcpy(priv->prefix, "/dev/spwrouter0");
+	priv->prefix[14] += dev->minor_drv;
+	mkdir(priv->prefix, S_IRWXU | S_IRWXG | S_IRWXO);
+	priv->prefix[15] = '/';
+	priv->prefix[16] = '\0';
+
+	priv->devinfo = devinfo = (struct pci_dev_info *)dev->businfo;
+	priv->pcidev = devinfo->pcidev;
+	bar0 = devinfo->resources[0].address;
+	bar0_size = devinfo->resources[0].size;
+	printk("\n\n--- GR-RASTA-SPW-ROUTER[%d] ---\n", dev->minor_drv);
+	printk(" PCI BUS: 0x%x, SLOT: 0x%x, FUNCTION: 0x%x\n",
+		PCI_DEV_EXPAND(priv->pcidev));
+	printk(" PCI VENDOR: 0x%04x, DEVICE: 0x%04x\n",
+		devinfo->id.vendor, devinfo->id.device);
+	printk(" PCI BAR[0]: 0x%08" PRIx32 " - 0x%08" PRIx32 "\n",
+		bar0, bar0 + bar0_size - 1);
+	printk(" IRQ: %d\n\n\n", devinfo->irq);
+
+	/* all neccessary space assigned to GR-RASTA-SPW-ROUTER target? */
+	if (bar0_size == 0)
+		return DRVMGR_ENORES;
+
+	/* Initialize spin-lock for this PCI peripheral device. This is to
+	 * protect the Interrupt Controller Registers. The genirq layer is
+         * protecting its own internals and ISR dispatching.
+         */
+	SPIN_INIT(&priv->devlock, priv->prefix);
+
+	/* Let user override which PCI address the AHB masters of the
+	 * GR-RASTA-SPW board access when doing DMA to CPU RAM. The AHB masters
+	 * access the PCI Window of the AMBA bus, the MSB 4-bits of that address
+	 * is translated according this config option before the address
+	 * goes out on the PCI bus.
+	 * Only the 4 MSB bits have an effect;
+	 */
+	value = drvmgr_dev_key_get(priv->dev, "ahbmst2pci", DRVMGR_KT_INT);
+	if (value)
+		priv->ahbmst2pci_map = value->i;
+	else
+		priv->ahbmst2pci_map = AHBMST2PCIADR; /* default */	
+
+	priv->genirq = genirq_init(16);
+	if ( priv->genirq == NULL )
+		return DRVMGR_FAIL;
+
+	if ((status = gr_rasta_spw_router_hw_init(priv)) != 0) {
+		genirq_destroy(priv->genirq);
+		printk(" Failed to initialize GR-RASTA-SPW-ROUTER HW: %d\n", status);
+		return DRVMGR_FAIL;
+	}
+
+	/* Init amba bus */
+	priv->config.abus = &priv->abus;
+	priv->config.ops = &ambapp_rasta_spw_router_ops;
+	priv->config.maps_up = &priv->bus_maps_up[0];
+	priv->config.maps_down = &priv->bus_maps_down[0];
+	if ( priv->dev->minor_drv < resources_cnt ) {
+		priv->config.resources = gr_rasta_spw_router_resources[priv->dev->minor_drv];
+	} else {
+		priv->config.resources = NULL;
+	}
+
+	/* Create and register AMBA PnP bus. */
+	return ambapp_bus_register(dev, &priv->config);
+}
+
+int gr_rasta_spw_router_init2(struct drvmgr_dev *dev)
+{
+	struct gr_rasta_spw_router_priv *priv = dev->priv;
+
+	/* Clear any old interrupt requests */
+	drvmgr_interrupt_clear(dev, 0);
+
+	/* Enable System IRQ so that GR-RASTA-SPW-ROUTER PCI target interrupt
+	 * goes through.
+	 *
+	 * It is important to enable it in stage init2. If interrupts were
+	 * enabled in init1 this might hang the system when more than one
+	 * PCI board is connected, this is because PCI interrupts might
+	 * be shared and PCI board 2 have not initialized and
+	 * might therefore drive interrupt already when entering init1().
+	 */
+	drvmgr_interrupt_register(
+		dev,
+		0,
+		"gr_rasta_spw_router",
+		gr_rasta_spw_router_isr,
+		(void *)priv);
+
+	return gr_rasta_spw_router_hw_init2(priv);
+}
+
+int ambapp_rasta_spw_router_int_register(
+	struct drvmgr_dev *dev,
+	int irq,
+	const char *info,
+	drvmgr_isr handler,
+	void *arg)
+{
+	struct gr_rasta_spw_router_priv *priv = dev->parent->dev->priv;
+	SPIN_IRQFLAGS(irqflags);
+	int status;
+	void *h;
+
+	h = genirq_alloc_handler(handler, arg);
+	if ( h == NULL )
+		return DRVMGR_FAIL;
+
+	SPIN_LOCK_IRQ(&priv->devlock, irqflags);
+
+	status = genirq_register(priv->genirq, irq, h);
+	if (status == 0) {
+		/* Clear IRQ for first registered handler */
+		priv->irq->iclear = (1<<irq);
+	} else if (status == 1)
+		status = 0;
+
+	if (status != 0) {
+		SPIN_UNLOCK_IRQ(&priv->devlock, irqflags);
+		genirq_free_handler(h);
+		return DRVMGR_FAIL;
+	}
+
+	status = genirq_enable(priv->genirq, irq, handler, arg);
+	if ( status == 0 ) {
+		/* Enable IRQ for first enabled handler only */
+		priv->irq->mask[0] |= (1<<irq); /* unmask interrupt source */
+	} else if ( status == 1 )
+		status = 0;
+
+	SPIN_UNLOCK_IRQ(&priv->devlock, irqflags);
+
+	return status;
+}
+
+int ambapp_rasta_spw_router_int_unregister(
+	struct drvmgr_dev *dev,
+	int irq,
+	drvmgr_isr isr,
+	void *arg)
+{
+	struct gr_rasta_spw_router_priv *priv = dev->parent->dev->priv;
+	SPIN_IRQFLAGS(irqflags);
+	int status;
+	void *handler;
+
+	SPIN_LOCK_IRQ(&priv->devlock, irqflags);
+
+	status = genirq_disable(priv->genirq, irq, isr, arg);
+	if ( status == 0 ) {
+		/* Disable IRQ only when no enabled handler exists */
+		priv->irq->mask[0] &= ~(1<<irq); /* mask interrupt source */
+	}
+
+	handler = genirq_unregister(priv->genirq, irq, isr, arg);
+	if ( handler == NULL )
+		status = DRVMGR_FAIL;
+	else
+		status = DRVMGR_OK;
+
+	SPIN_UNLOCK_IRQ(&priv->devlock, irqflags);
+
+	if (handler)
+		genirq_free_handler(handler);
+
+	return status;
+}
+
+int ambapp_rasta_spw_router_int_unmask(
+	struct drvmgr_dev *dev,
+	int irq)
+{
+	struct gr_rasta_spw_router_priv *priv = dev->parent->dev->priv;
+	SPIN_IRQFLAGS(irqflags);
+
+	DBG("RASTA-SPW-ROUTER IRQ %d: unmask\n", irq);
+
+	if ( genirq_check(priv->genirq, irq) )
+		return DRVMGR_EINVAL;
+
+	SPIN_LOCK_IRQ(&priv->devlock, irqflags);
+
+	/* Enable IRQ for first enabled handler only */
+	priv->irq->mask[0] |= (1<<irq); /* unmask interrupt source */
+
+	SPIN_UNLOCK_IRQ(&priv->devlock, irqflags);
+
+	return DRVMGR_OK;
+}
+
+int ambapp_rasta_spw_router_int_mask(
+	struct drvmgr_dev *dev,
+	int irq)
+{
+	struct gr_rasta_spw_router_priv *priv = dev->parent->dev->priv;
+	SPIN_IRQFLAGS(irqflags);
+
+	DBG("RASTA-SPW-ROUTER IRQ %d: mask\n", irq);
+
+	if ( genirq_check(priv->genirq, irq) )
+		return DRVMGR_EINVAL;
+
+	SPIN_LOCK_IRQ(&priv->devlock, irqflags);
+
+	/* Disable/mask IRQ */
+	priv->irq->mask[0] &= ~(1<<irq); /* mask interrupt source */
+
+	SPIN_UNLOCK_IRQ(&priv->devlock, irqflags);
+
+	return DRVMGR_OK;
+}
+
+int ambapp_rasta_spw_router_int_clear(
+	struct drvmgr_dev *dev,
+	int irq)
+{
+	struct gr_rasta_spw_router_priv *priv = dev->parent->dev->priv;
+
+	if ( genirq_check(priv->genirq, irq) )
+		return DRVMGR_EINVAL;
+
+	priv->irq->iclear = (1<<irq);
+
+	return DRVMGR_OK;
+}
+
+int ambapp_rasta_spw_router_get_params(struct drvmgr_dev *dev, struct drvmgr_bus_params *params)
+{
+	struct gr_rasta_spw_router_priv *priv = dev->parent->dev->priv;
+
+	/* Device name prefix pointer, skip /dev */
+	params->dev_prefix = &priv->prefix[5];
+
+	return 0;
+}
+
+void gr_rasta_spw_router_print_dev(struct drvmgr_dev *dev, int options)
+{
+	struct gr_rasta_spw_router_priv *priv = dev->priv;
+	struct pci_dev_info *devinfo = priv->devinfo;
+	uint32_t bar0, bar0_size;
+
+	/* Print */
+	printf("--- GR-RASTA-SPW-ROUTER [bus 0x%x, dev 0x%x, fun 0x%x] ---\n",
+		PCI_DEV_EXPAND(priv->pcidev));
+
+	bar0 = devinfo->resources[0].address;
+	bar0_size = devinfo->resources[0].size;
+	printf(" PCI BAR[0]: 0x%" PRIx32 " - 0x%" PRIx32 "\n",
+		bar0, bar0 + bar0_size - 1);
+	printf(" IRQ REGS:        0x%" PRIxPTR "\n", (uintptr_t)priv->irq);
+	printf(" IRQ:             %d\n", devinfo->irq);
+	printf(" PCI REVISION:    %d\n", devinfo->rev);
+	printf(" FREQ:            %d Hz\n", priv->version->amba_freq_hz);
+	printf(" IMASK:           0x%08x\n", priv->irq->mask[0]);
+	printf(" IPEND:           0x%08x\n", priv->irq->ipend);
+
+	/* Print amba config */
+	if (options & RASTA_SPW_ROUTER_OPTIONS_AMBA)
+		ambapp_print(&priv->abus, 10);
+
+#if 0
+	/* Print IRQ handlers and their arguments */
+	if (options & RASTA_SPW_ROUTER_OPTIONS_IRQ) {
+		int i;
+		for(i = 0; i < 16; i++) {
+			printf(" IRQ[%02d]:         0x%x, arg: 0x%x\n", 
+				i, (unsigned int)priv->isrs[i].handler,
+				(unsigned int)priv->isrs[i].arg);
+		}
+	}
+#endif
+}
+
+void gr_rasta_spw_router_print(int options)
+{
+	struct pci_drv_info *drv = &gr_rasta_spw_router_info;
+	struct drvmgr_dev *dev;
+
+	dev = drv->general.dev;
+	while(dev) {
+		gr_rasta_spw_router_print_dev(dev, options);
+		dev = dev->next_in_drv;
+	}
+}
diff --git a/bsps/shared/grlib/pci/gr_rasta_tmtc.c b/bsps/shared/grlib/pci/gr_rasta_tmtc.c
new file mode 100644
index 0000000000..bef2927926
--- /dev/null
+++ b/bsps/shared/grlib/pci/gr_rasta_tmtc.c
@@ -0,0 +1,897 @@
+/*  GR-RASTA-TMTC PCI Target driver.
+ * 
+ *  COPYRIGHT (c) 2008.
+ *  Cobham Gaisler AB.
+ *
+ *  Configures the GR-RASTA-TMTC interface PCI board.
+ *  This driver provides a AMBA PnP bus by using the general part
+ *  of the AMBA PnP bus driver (ambapp_bus.c).
+ *
+ *  Driver resources for the AMBA PnP bus provided can be set by overriding
+ *  the defaults by declaring gr_rasta_tmtc_resources[].
+ *
+ *  The license and distribution terms for this file may be
+ *  found in found in the file LICENSE in this distribution or at
+ *  http://www.rtems.org/license/LICENSE.
+ */
+
+#include <inttypes.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+
+#include <bsp.h>
+#include <rtems/bspIo.h>
+#include <pci.h>
+
+#include <grlib/ambapp.h>
+#include <grlib/grlib.h>
+#include <drvmgr/drvmgr.h>
+#include <grlib/ambapp_bus.h>
+#include <drvmgr/pci_bus.h>
+#include <grlib/bspcommon.h>
+#include <grlib/genirq.h>
+
+#include <grlib/gr_rasta_tmtc.h>
+
+#include <grlib/grlib_impl.h>
+
+/* Determines which PCI address the AHB masters will access, it should be
+ * set so that the masters can access the CPU RAM. Default is base of CPU RAM,
+ * CPU RAM is mapped 1:1 to PCI space.
+ */
+extern unsigned int _RAM_START;
+#define AHBMST2PCIADR (((unsigned int)&_RAM_START) & 0xf0000000)
+
+#define GAISLER_GPIO         0x01a
+#define AHB1_BASE_ADDR 0x80000000
+#define AHB1_IOAREA_BASE_ADDR 0x80200000
+#define AHB1_IOAREA_OFS (AHB1_IOAREA_BASE_ADDR - AHB1_BASE_ADDR)
+
+/* Second revision constants (GRPCI2) */
+#define GRPCI2_BAR0_TO_AHB_MAP 0x04  /* Fixme */
+#define GRPCI2_BAR1_TO_AHB_MAP 0x08  /* Fixme */
+#define GRPCI2_PCI_CONFIG      0x20  /* Fixme */
+
+/* #define DEBUG 1 */
+
+#ifdef DEBUG
+#define DBG(x...) printk(x)
+#else
+#define DBG(x...) 
+#endif
+
+int gr_rasta_tmtc_init1(struct drvmgr_dev *dev);
+int gr_rasta_tmtc_init2(struct drvmgr_dev *dev);
+void gr_rasta_tmtc_isr (void *arg);
+
+struct grpci_regs {
+	volatile unsigned int cfg_stat;
+	volatile unsigned int bar0;
+	volatile unsigned int page0;
+	volatile unsigned int bar1;
+	volatile unsigned int page1;
+	volatile unsigned int iomap;
+	volatile unsigned int stat_cmd;
+};
+
+struct grpci2_regs {
+	volatile unsigned int ctrl;
+	volatile unsigned int statcap;
+	volatile unsigned int pcimstprefetch;
+	volatile unsigned int ahbtopciiomap;
+	volatile unsigned int dmactrl;
+	volatile unsigned int dmadesc;
+	volatile unsigned int dmachanact;
+	volatile unsigned int reserved;
+	volatile unsigned int pcibartoahb[6];
+	volatile unsigned int reserved2[2];
+	volatile unsigned int ahbtopcimemmap[16];
+	volatile unsigned int trcctrl;
+	volatile unsigned int trccntmode;
+	volatile unsigned int trcadpat;
+	volatile unsigned int trcadmask;
+	volatile unsigned int trcctrlsigpat;
+	volatile unsigned int trcctrlsigmask;
+	volatile unsigned int trcadstate;
+	volatile unsigned int trcctrlsigstate;
+};
+
+struct gr_rasta_tmtc_ver {
+	const unsigned int	amba_freq_hz;	/* The frequency */
+	const unsigned int	amba_ioarea;	/* The address where the PnP IOAREA starts at */
+};
+
+/* Private data structure for driver */
+struct gr_rasta_tmtc_priv {
+	/* Driver management */
+	struct drvmgr_dev		*dev;
+	char				prefix[20];
+	SPIN_DECLARE(devlock);
+
+	/* PCI */
+	pci_dev_t			pcidev;
+	struct pci_dev_info		*devinfo;
+	uint32_t			ahbmst2pci_map;
+
+	/* IRQ */
+	genirq_t			genirq;
+
+	/* GR-RASTA-TMTC */
+	struct gr_rasta_tmtc_ver	*version;
+	struct irqmp_regs		*irq;
+	struct grpci_regs		*grpci;
+	struct grpci2_regs		*grpci2;
+	struct grgpio_regs		*gpio;
+	struct drvmgr_map_entry		bus_maps_down[3];
+	struct drvmgr_map_entry		bus_maps_up[2];
+
+	/* AMBA Plug&Play information on GR-RASTA-TMTC */
+	struct ambapp_bus		abus;
+	struct ambapp_mmap		amba_maps[4];
+        struct ambapp_config		config;
+};
+
+struct gr_rasta_tmtc_ver gr_rasta_tmtc_ver0 = {
+	.amba_freq_hz		= 30000000,
+	.amba_ioarea		= AHB1_IOAREA_BASE_ADDR,
+};
+
+int ambapp_rasta_tmtc_int_register(
+	struct drvmgr_dev *dev,
+	int irq,
+	const char *info,
+	drvmgr_isr handler,
+	void *arg);
+int ambapp_rasta_tmtc_int_unregister(
+	struct drvmgr_dev *dev,
+	int irq,
+	drvmgr_isr handler,
+	void *arg);
+int ambapp_rasta_tmtc_int_unmask(
+	struct drvmgr_dev *dev,
+	int irq);
+int ambapp_rasta_tmtc_int_mask(
+	struct drvmgr_dev *dev,
+	int irq);
+int ambapp_rasta_tmtc_int_clear(
+	struct drvmgr_dev *dev,
+	int irq);
+int ambapp_rasta_tmtc_get_params(
+	struct drvmgr_dev *dev,
+	struct drvmgr_bus_params *params);
+
+struct ambapp_ops ambapp_rasta_tmtc_ops = {
+	.int_register = ambapp_rasta_tmtc_int_register,
+	.int_unregister = ambapp_rasta_tmtc_int_unregister,
+	.int_unmask = ambapp_rasta_tmtc_int_unmask,
+	.int_mask = ambapp_rasta_tmtc_int_mask,
+	.int_clear = ambapp_rasta_tmtc_int_clear,
+	.get_params = ambapp_rasta_tmtc_get_params
+};
+
+struct drvmgr_drv_ops gr_rasta_tmtc_ops = 
+{
+	.init = {gr_rasta_tmtc_init1, gr_rasta_tmtc_init2, NULL, NULL},
+	.remove = NULL,
+	.info = NULL,
+};
+
+struct pci_dev_id_match gr_rasta_tmtc_ids[] =
+{
+	PCIID_DEVVEND(PCIID_VENDOR_GAISLER, PCIID_DEVICE_GR_RASTA_TMTC),
+	PCIID_END_TABLE /* Mark end of table */
+};
+
+struct pci_drv_info gr_rasta_tmtc_info =
+{
+	{
+		DRVMGR_OBJ_DRV,			/* Driver */
+		NULL,				/* Next driver */
+		NULL,				/* Device list */
+		DRIVER_PCI_GAISLER_RASTATMTC_ID,/* Driver ID */
+		"GR-RASTA-TMTC_DRV",		/* Driver Name */
+		DRVMGR_BUS_TYPE_PCI,		/* Bus Type */
+		&gr_rasta_tmtc_ops,
+		NULL,				/* Funcs */
+		0,				/* No devices yet */
+		sizeof(struct gr_rasta_tmtc_priv) /* Let drvmgr alloc private */
+	},
+	&gr_rasta_tmtc_ids[0]
+};
+
+/* Driver resources configuration for the AMBA bus on the GR-RASTA-TMTC board.
+ * It is declared weak so that the user may override it from the project file,
+ * if the default settings are not enough.
+ *
+ * The configuration consists of an array of configuration pointers, each
+ * pointer determine the configuration of one GR-RASTA-TMTC board. Pointer
+ * zero is for board0, pointer 1 for board1 and so on.
+ *
+ * The array must end with a NULL pointer.
+ */
+struct drvmgr_bus_res *gr_rasta_tmtc_resources[] __attribute__((weak)) =
+{
+	NULL,
+};
+
+void gr_rasta_tmtc_register_drv(void)
+{
+	DBG("Registering GR-RASTA-TMTC PCI driver\n");
+	drvmgr_drv_register(&gr_rasta_tmtc_info.general);
+}
+
+void gr_rasta_tmtc_isr (void *arg)
+{
+	struct gr_rasta_tmtc_priv *priv = arg;
+	unsigned int status, tmp;
+	int irq;
+	SPIN_ISR_IRQFLAGS(irqflags);
+
+	tmp = status = priv->irq->ipend;
+
+	/* printk("GR-RASTA-TMTC: IRQ 0x%x\n",status); */
+
+	SPIN_LOCK(&priv->devlock, irqflags);
+	for(irq=0; irq<32; irq++) {
+		if ( status & (1<<irq) ) {
+			genirq_doirq(priv->genirq, irq);
+			priv->irq->iclear = (1<<irq);
+			status &= ~(1<<irq);
+			if ( status == 0 )
+				break;
+		}
+	}
+	SPIN_UNLOCK(&priv->devlock, irqflags);
+
+	/* ACK interrupt, this is because PCI is Level, so the IRQ Controller still drives the IRQ. */
+	if ( tmp )
+		drvmgr_interrupt_clear(priv->dev, 0);
+
+	DBG("RASTA-TMTC-IRQ: 0x%x\n", tmp);
+}
+
+/* Init AMBA bus frequency, IRQ controller, GPIO register, bus maps and other
+ * common stuff between rev0 and rev1.
+ */
+static int gr_rasta_tmtc_hw_init_common(struct gr_rasta_tmtc_priv *priv)
+{
+	struct ambapp_dev *tmp;
+	unsigned int pci_freq_hz;
+
+	/* Initialize Frequency of AMBA bus. The AMBA bus runs at same
+	 * frequency as PCI bus
+	 */
+	drvmgr_freq_get(priv->dev, 0, &pci_freq_hz);
+	ambapp_freq_init(&priv->abus, NULL, pci_freq_hz);
+
+	/* Find IRQ controller, Clear all current IRQs */
+	tmp = (struct ambapp_dev *)ambapp_for_each(&priv->abus,
+					(OPTIONS_ALL|OPTIONS_APB_SLVS),
+					VENDOR_GAISLER, GAISLER_IRQMP,
+					ambapp_find_by_idx, NULL);
+	if ( !tmp ) {
+		return -4;
+	}
+	priv->irq = (struct irqmp_regs *)DEV_TO_APB(tmp)->start;
+	/* Set up GR-RASTA-TMTC irq controller */
+	priv->irq->mask[0] = 0;
+	priv->irq->iclear = 0xffffffff;
+	priv->irq->ilevel = 0;
+
+	/* Find First GPIO controller */
+	tmp = (struct ambapp_dev *)ambapp_for_each(&priv->abus,
+					(OPTIONS_ALL|OPTIONS_APB_SLVS),
+					VENDOR_GAISLER, GAISLER_GPIO,
+					ambapp_find_by_idx, NULL);
+	if ( !tmp ) {
+		return -5;
+	}
+	priv->gpio = (struct grgpio_regs *) (((struct ambapp_apb_info *)tmp->devinfo)->start);
+	/* Clear GR-RASTA-TMTC GPIO controller */
+	priv->gpio->imask = 0;
+	priv->gpio->ipol = 0;
+	priv->gpio->iedge = 0;
+	priv->gpio->bypass = 0;
+	/* Set up GR-RASTA-TMTC GPIO controller to select GRTM and GRTC */
+	priv->gpio->output = (GR_TMTC_GPIO_GRTM_SEL|GR_TMTC_GPIO_TRANSP_CLK) | (GR_TMTC_GPIO_TC_BIT_LOCK|GR_TMTC_GPIO_TC_RF_AVAIL|GR_TMTC_GPIO_TC_ACTIVE_HIGH|GR_TMTC_GPIO_TC_RISING_CLK);
+	priv->gpio->dir = 0xffffffff;
+	DBG("GR-TMTC GPIO: 0x%x\n", (unsigned int)priv->gpio);
+
+	/* DOWN streams translation table */
+	priv->bus_maps_down[0].name = "PCI BAR0 -> AMBA";
+	priv->bus_maps_down[0].size = priv->amba_maps[0].size;
+	priv->bus_maps_down[0].from_adr = (void *)priv->amba_maps[0].local_adr;
+	priv->bus_maps_down[0].to_adr = (void *)priv->amba_maps[0].remote_adr;
+
+	priv->bus_maps_down[1].name = "PCI BAR1 -> AMBA";
+	priv->bus_maps_down[1].size = priv->amba_maps[1].size;
+	priv->bus_maps_down[1].from_adr = (void *)priv->amba_maps[1].local_adr;
+	priv->bus_maps_down[1].to_adr = (void *)priv->amba_maps[1].remote_adr;
+
+	/* Mark end of translation table */
+	priv->bus_maps_down[2].size = 0;
+
+	return 0;
+}
+
+/* PCI Hardware (Revision 0) initialization */
+static int gr_rasta_tmtc0_hw_init(struct gr_rasta_tmtc_priv *priv)
+{
+	unsigned int *page0 = NULL;
+	struct ambapp_dev *tmp;
+	struct ambapp_ahb_info *ahb;
+	int status;
+	pci_dev_t pcidev = priv->pcidev;
+	struct pci_dev_info *devinfo = priv->devinfo;
+	uint32_t bar0, bar0_size;
+
+	/* Select version of GR-RASTA-TMTC board */
+	switch (devinfo->rev) {
+		case 0:
+			priv->version = &gr_rasta_tmtc_ver0;
+			break;
+		default:
+			return -2;
+	}
+
+	bar0 = devinfo->resources[0].address;
+	bar0_size = devinfo->resources[0].size;
+	page0 = (unsigned int *)(bar0 + bar0_size/2); 
+
+	/* Point PAGE0 to start of Plug and Play information */
+	*page0 = priv->version->amba_ioarea & 0xf0000000;
+
+#if 0
+	{
+		uint32_t data;
+		/* set parity error response */
+		pci_cfg_r32(pcidev, PCIR_COMMAND, &data);
+		pci_cfg_w32(pcidev, PCIR_COMMAND, (data|PCIM_CMD_PERRESPEN));
+	}
+#endif
+
+	/* Setup cache line size. Default cache line size will result in
+	 * poor performance (256 word fetches), 0xff will set it according
+	 * to the max size of the PCI FIFO.
+	 */
+	pci_cfg_w8(pcidev, PCIR_CACHELNSZ, 0xff);
+
+	/* Scan AMBA Plug&Play */
+
+	/* AMBA MAP bar0 (in CPU) ==> 0x80000000(remote amba address) */
+	priv->amba_maps[0].size = 0x10000000;
+	priv->amba_maps[0].local_adr = bar0;
+	priv->amba_maps[0].remote_adr = AHB1_BASE_ADDR;
+
+	/* AMBA MAP bar1 (in CPU) ==> 0x40000000(remote amba address) */
+	priv->amba_maps[1].size = devinfo->resources[1].size;
+	priv->amba_maps[1].local_adr = devinfo->resources[1].address;
+	priv->amba_maps[1].remote_adr = 0x40000000;
+
+	/* Addresses not matching with map be untouched */
+	priv->amba_maps[2].size = 0xfffffff0;
+	priv->amba_maps[2].local_adr = 0;
+	priv->amba_maps[2].remote_adr = 0;
+
+	/* Mark end of table */
+	priv->amba_maps[3].size=0;
+	priv->amba_maps[3].local_adr = 0;
+	priv->amba_maps[3].remote_adr = 0;
+
+	/* Start AMBA PnP scan at first AHB bus */
+	ambapp_scan(&priv->abus,
+		bar0 + (priv->version->amba_ioarea & ~0xf0000000),
+		NULL, &priv->amba_maps[0]);
+
+	/* Point PAGE0 to start of APB area */
+	*page0 = AHB1_BASE_ADDR;
+
+	/* Find GRPCI controller */
+	tmp = (struct ambapp_dev *)ambapp_for_each(&priv->abus,
+					(OPTIONS_ALL|OPTIONS_APB_SLVS),
+					VENDOR_GAISLER, GAISLER_PCIFBRG,
+					ambapp_find_by_idx, NULL);
+	if ( !tmp ) {
+		return -3;
+	}
+	priv->grpci = (struct grpci_regs *)((struct ambapp_apb_info *)tmp->devinfo)->start;
+
+	/* Set GRPCI mmap so that AMBA masters can access CPU-RAM over
+	 * the PCI window.
+	 */
+	priv->grpci->cfg_stat = (priv->grpci->cfg_stat & 0x0fffffff) |
+				(priv->ahbmst2pci_map & 0xf0000000);
+	priv->grpci->page1 = 0x40000000;
+
+	/* init AMBA bus, IRQCtrl, GPIO, bus down-maps */
+	status = gr_rasta_tmtc_hw_init_common(priv);
+	if (status)
+		return status;
+
+	/* Find GRPCI controller AHB Slave interface */
+	tmp = (struct ambapp_dev *)ambapp_for_each(&priv->abus,
+				(OPTIONS_ALL|OPTIONS_AHB_SLVS),
+				VENDOR_GAISLER, GAISLER_PCIFBRG,
+				ambapp_find_by_idx, NULL);
+	if ( !tmp ) {
+		return -6;
+	}
+	ahb = (struct ambapp_ahb_info *)tmp->devinfo;
+
+	/* UP streams translation table */
+	priv->bus_maps_up[0].name = "AMBA GRPCI Window";
+	priv->bus_maps_up[0].size = ahb->mask[0]; /* AMBA->PCI Window on GR-RASTA-TMTC board */
+	priv->bus_maps_up[0].from_adr = (void *)ahb->start[0];
+	priv->bus_maps_up[0].to_adr = (void *)
+					(priv->ahbmst2pci_map & 0xf0000000);
+
+	/* Mark end of translation table */
+	priv->bus_maps_up[1].size = 0;
+
+	/* Successfully registered the RASTA board */
+	return 0;
+}
+
+/* PCI Hardware (Revision 1) initialization */
+static int gr_rasta_tmtc1_hw_init(struct gr_rasta_tmtc_priv *priv)
+{
+	int i;
+	uint32_t data;
+	unsigned int ctrl;
+	uint8_t tmp2;
+	struct ambapp_dev *tmp;
+	int status;
+	struct ambapp_ahb_info *ahb;
+	uint8_t cap_ptr;
+	pci_dev_t pcidev = priv->pcidev;
+	struct pci_dev_info *devinfo = priv->devinfo;
+
+	/* Check capabilities list bit */
+	pci_cfg_r8(pcidev, PCIR_STATUS, &tmp2);
+
+	if (!((tmp2 >> 4) & 1)) {
+		/* Capabilities list not available which it should be in the
+		 * GRPCI2
+		 */
+		return -3;
+	}
+
+	/* Read capabilities pointer */
+	pci_cfg_r8(pcidev, PCIR_CAP_PTR, &cap_ptr);
+
+	/* Set AHB address mappings for target PCI bars
+	 * BAR0: 16MB  : Mapped to I/O at 0x80000000
+	 * BAR1: 256MB : Mapped to MEM at 0x40000000
+	 */
+	pci_cfg_w32(pcidev, cap_ptr+GRPCI2_BAR0_TO_AHB_MAP, AHB1_BASE_ADDR);
+	pci_cfg_w32(pcidev, cap_ptr+GRPCI2_BAR1_TO_AHB_MAP, 0x40000000);
+
+	/* Set PCI bus to be same endianess as PCI system */
+	pci_cfg_r32(pcidev, cap_ptr+GRPCI2_PCI_CONFIG, &data);
+	if (pci_endian == PCI_BIG_ENDIAN)
+		data = data & 0xFFFFFFFE;
+	else
+		data = data | 0x00000001;
+	pci_cfg_w32(pcidev, cap_ptr+GRPCI2_PCI_CONFIG, data);
+
+#if 0
+	/* set parity error response */
+	pci_cfg_r32(pcidev, PCIR_COMMAND, &data);
+	pci_cfg_w32(pcidev, PCIR_COMMAND, (data|PCIM_CMD_PERRESPEN));
+#endif
+
+	/* Scan AMBA Plug&Play */
+
+	/* AMBA MAP bar0 (in PCI) ==> 0x40000000 (remote amba address) */
+	priv->amba_maps[0].size = devinfo->resources[0].size;
+	priv->amba_maps[0].local_adr = devinfo->resources[0].address;
+	priv->amba_maps[0].remote_adr = AHB1_BASE_ADDR;
+
+	/* AMBA MAP bar0 (in PCI) ==> 0x80000000 (remote amba address) */
+	priv->amba_maps[1].size = devinfo->resources[1].size;
+	priv->amba_maps[1].local_adr = devinfo->resources[1].address;
+	priv->amba_maps[1].remote_adr = 0x40000000;
+
+	/* Addresses not matching with map be untouched */
+	priv->amba_maps[2].size = 0xfffffff0;
+	priv->amba_maps[2].local_adr = 0;
+	priv->amba_maps[2].remote_adr = 0;
+
+	/* Mark end of table */
+	priv->amba_maps[3].size=0;
+
+	/* Start AMBA PnP scan at first AHB bus */
+	ambapp_scan(
+		&priv->abus,
+		devinfo->resources[0].address + AHB1_IOAREA_OFS,
+		NULL,
+		&priv->amba_maps[0]);
+
+	/* init AMBA bus, IRQCtrl, GPIO, bus down-maps */
+	status = gr_rasta_tmtc_hw_init_common(priv);
+	if (status)
+		return status;
+
+	/* Find GRPCI2 controller AHB Slave interface */
+	tmp = (struct ambapp_dev *)ambapp_for_each(&priv->abus,
+					(OPTIONS_ALL|OPTIONS_AHB_SLVS),
+					VENDOR_GAISLER, GAISLER_GRPCI2,
+					ambapp_find_by_idx, NULL);
+	if ( !tmp ) {
+		return -6;
+	}
+	ahb = (struct ambapp_ahb_info *)tmp->devinfo;
+	priv->bus_maps_up[0].name = "AMBA GRPCI2 Window";
+	priv->bus_maps_up[0].size = ahb->mask[0]; /* AMBA->PCI Window on GR-RASTA-SPW-ROUTER board */
+	priv->bus_maps_up[0].from_adr = (void *)ahb->start[0];
+	priv->bus_maps_up[0].to_adr = (void *)
+				(priv->ahbmst2pci_map & ~(ahb->mask[0]-1));
+	priv->bus_maps_up[1].size = 0;
+
+	/* Find GRPCI2 controller APB Slave interface */
+	tmp = (struct ambapp_dev *)ambapp_for_each(&priv->abus,
+					(OPTIONS_ALL|OPTIONS_APB_SLVS),
+					VENDOR_GAISLER, GAISLER_GRPCI2,
+					ambapp_find_by_idx, NULL);
+	if ( !tmp ) {
+		return -7;
+	}
+	priv->grpci2 = (struct grpci2_regs *)
+		((struct ambapp_apb_info *)tmp->devinfo)->start;
+
+	/* Set AHB to PCI mapping for all AMBA AHB masters */
+	for(i = 0; i < 16; i++) {
+		priv->grpci2->ahbtopcimemmap[i] = priv->ahbmst2pci_map &
+							~(ahb->mask[0]-1);
+	}
+
+	/* Make sure dirq(0) sampling is enabled */
+	ctrl = priv->grpci2->ctrl;
+	ctrl = (ctrl & 0xFFFFFF0F) | (1 << 4);
+	priv->grpci2->ctrl = ctrl;
+
+	/* Successfully registered the RASTA-SPW-ROUTER board */
+	return 0;
+}
+
+static void gr_rasta_tmtc_hw_init2(struct gr_rasta_tmtc_priv *priv)
+{
+	/* Enable DMA by enabling PCI target as master */
+	pci_master_enable(priv->pcidev);
+}
+
+/* Called when a PCI target is found with the PCI device and vendor ID 
+ * given in gr_rasta_tmtc_ids[].
+ */
+int gr_rasta_tmtc_init1(struct drvmgr_dev *dev)
+{
+	struct gr_rasta_tmtc_priv *priv;
+	struct pci_dev_info *devinfo;
+	int status;
+	uint32_t bar0, bar1, bar0_size, bar1_size;
+	union drvmgr_key_value *value;
+	int resources_cnt;
+
+	priv = dev->priv;
+	if (!priv)
+		return DRVMGR_NOMEM;
+	priv->dev = dev;
+
+	/* Determine number of configurations */
+	resources_cnt = get_resarray_count(gr_rasta_tmtc_resources);
+
+	/* Generate Device prefix */
+
+	strcpy(priv->prefix, "/dev/rastatmtc0");
+	priv->prefix[14] += dev->minor_drv;
+	mkdir(priv->prefix, S_IRWXU | S_IRWXG | S_IRWXO);
+	priv->prefix[15] = '/';
+	priv->prefix[16] = '\0';
+
+	priv->devinfo = devinfo = (struct pci_dev_info *)dev->businfo;
+	priv->pcidev = devinfo->pcidev;
+	bar0 = devinfo->resources[0].address;
+	bar0_size = devinfo->resources[0].size;
+	bar1 = devinfo->resources[1].address;
+	bar1_size = devinfo->resources[1].size;
+	printk("\n\n--- GR-RASTA-TMTC[%d] ---\n", dev->minor_drv);
+	printk(" PCI BUS: 0x%x, SLOT: 0x%x, FUNCTION: 0x%x\n",
+		PCI_DEV_EXPAND(priv->pcidev));
+	printk(" PCI VENDOR: 0x%04x, DEVICE: 0x%04x\n",
+		devinfo->id.vendor, devinfo->id.device);
+	printk(" PCI BAR[0]: 0x%" PRIx32 " - 0x%" PRIx32 "\n",
+		bar0, bar0 + bar0_size - 1);
+	printk(" PCI BAR[1]: 0x%" PRIx32 " - 0x%" PRIx32 "\n",
+		bar1, bar1 + bar1_size - 1);
+	printk(" IRQ: %d\n\n\n", devinfo->irq);
+
+	/* all neccessary space assigned to GR-RASTA-IO target? */
+	if ((bar0_size == 0) || (bar1_size == 0))
+		return DRVMGR_ENORES;
+
+	/* Initialize spin-lock for this PCI peripheral device. This is to
+	 * protect the Interrupt Controller Registers. The genirq layer is
+         * protecting its own internals and ISR dispatching.
+         */
+	SPIN_INIT(&priv->devlock, priv->prefix);
+
+	/* Let user override which PCI address the AHB masters of the
+	 * GR-RASTA-TMTC board access when doing DMA to CPU RAM. The AHB masters
+	 * access the PCI Window of the AMBA bus, the MSB 4-bits of that address
+	 * is translated according this config option before the address
+	 * goes out on the PCI bus.
+	 * Only the 4 MSB bits have an effect;
+	 */
+	value = drvmgr_dev_key_get(priv->dev, "ahbmst2pci", DRVMGR_KT_INT);
+	if (value)
+		priv->ahbmst2pci_map = value->i;
+	else
+		priv->ahbmst2pci_map = AHBMST2PCIADR; /* default */	
+
+	priv->genirq = genirq_init(32);
+	if ( priv->genirq == NULL )
+		return DRVMGR_FAIL;
+
+	/* Select version of GR-RASTA-IO board */
+	switch (devinfo->rev) {
+		case 0:
+			puts("GR-RASTA-TMTC: REVISION 0");
+			status = gr_rasta_tmtc0_hw_init(priv);
+			break;
+		case 1:
+			puts("GR-RASTA-TMTC: REVISION 1");
+			status = gr_rasta_tmtc1_hw_init(priv);
+			break;
+		default:
+			return DRVMGR_ENOSYS; /* HW not supported */
+	}
+
+	if ( status != 0 ) {
+		genirq_destroy(priv->genirq);
+		printk(" Failed to initialize GR-RASTA-TMTC HW: %d\n", status);
+		return DRVMGR_FAIL;
+	}
+
+	/* Init amba bus */
+	priv->config.abus = &priv->abus;
+	priv->config.ops = &ambapp_rasta_tmtc_ops;
+	priv->config.maps_up = &priv->bus_maps_up[0];
+	priv->config.maps_down = &priv->bus_maps_down[0];
+	if ( priv->dev->minor_drv < resources_cnt ) {
+		priv->config.resources = gr_rasta_tmtc_resources[priv->dev->minor_drv];
+	} else {
+		priv->config.resources = NULL;
+	}
+
+	return ambapp_bus_register(dev, &priv->config);
+}
+
+int gr_rasta_tmtc_init2(struct drvmgr_dev *dev)
+{
+	struct gr_rasta_tmtc_priv *priv = dev->priv;
+
+	/* Clear any old interrupt requests */
+	drvmgr_interrupt_clear(priv->dev, 0);
+
+	/* Enable System IRQ so that GR-RASTA-TMTC PCI target interrupt goes
+	 * through.
+	 *
+	 * It is important to enable it in stage init2. If interrupts were
+	 * enabled in init1 this might hang the system when more than one
+	 * PCI target is connected, this is because PCI interrupts might 
+	 * be shared and PCI board 2 have not initialized and
+	 * might therefore drive interrupt already when entering init1().
+	 */
+	drvmgr_interrupt_register(
+		priv->dev,
+		0,
+		"gr_rasta_tmtc",
+		gr_rasta_tmtc_isr,
+		(void *)priv);
+
+	gr_rasta_tmtc_hw_init2(priv);
+
+	return DRVMGR_OK;
+}
+
+int ambapp_rasta_tmtc_int_register(
+	struct drvmgr_dev *dev,
+	int irq,
+	const char *info,
+	drvmgr_isr handler,
+	void *arg)
+{
+	struct gr_rasta_tmtc_priv *priv = dev->parent->dev->priv;
+	SPIN_IRQFLAGS(irqflags);
+	int status;
+	void *h;
+
+	h = genirq_alloc_handler(handler, arg);
+	if ( h == NULL )
+		return DRVMGR_FAIL;
+
+	SPIN_LOCK_IRQ(&priv->devlock, irqflags);
+
+	status = genirq_register(priv->genirq, irq, h);
+	if ( status == 0 ) {
+		/* Disable and clear IRQ for first registered handler */
+		priv->irq->iclear = (1<<irq);
+	} else if ( status == 1 )
+		status = 0;
+
+	if (status != 0) {
+		SPIN_UNLOCK_IRQ(&priv->devlock, irqflags);
+		genirq_free_handler(h);
+		return DRVMGR_FAIL;
+	}
+
+	status = genirq_enable(priv->genirq, irq, handler, arg);
+	if ( status == 0 ) {
+		/* Enable IRQ for first enabled handler only */
+		priv->irq->mask[0] |= (1<<irq); /* unmask interrupt source */
+	} else if ( status == 1 )
+		status = 0;
+
+	SPIN_UNLOCK_IRQ(&priv->devlock, irqflags);
+
+	return status;
+}
+
+int ambapp_rasta_tmtc_int_unregister(
+	struct drvmgr_dev *dev,
+	int irq,
+	drvmgr_isr isr,
+	void *arg)
+{
+	struct gr_rasta_tmtc_priv *priv = dev->parent->dev->priv;
+	SPIN_IRQFLAGS(irqflags);
+	int status;
+	void *handler;
+
+	SPIN_LOCK_IRQ(&priv->devlock, irqflags);
+
+	status = genirq_disable(priv->genirq, irq, isr, arg);
+	if ( status == 0 ) {
+		/* Disable IRQ only when no enabled handler exists */
+		priv->irq->mask[0] &= ~(1<<irq); /* mask interrupt source */
+	} else if ( status == 1 )
+		status = 0;
+
+	handler = genirq_unregister(priv->genirq, irq, isr, arg);
+	if ( handler == NULL )
+		status = DRVMGR_FAIL;
+	else
+		status = DRVMGR_OK;
+
+	SPIN_UNLOCK_IRQ(&priv->devlock, irqflags);
+
+	if (handler)
+		genirq_free_handler(handler);
+
+	return status;
+}
+
+int ambapp_rasta_tmtc_int_unmask(
+	struct drvmgr_dev *dev,
+	int irq)
+{
+	struct gr_rasta_tmtc_priv *priv = dev->parent->dev->priv;
+	SPIN_IRQFLAGS(irqflags);
+
+	DBG("RASTA-TMTC IRQ %d: unmask\n", irq);
+
+	if ( genirq_check(priv->genirq, irq) )
+		return DRVMGR_EINVAL;
+
+	SPIN_LOCK_IRQ(&priv->devlock, irqflags);
+
+	/* Enable IRQ */
+	priv->irq->mask[0] |= (1<<irq); /* unmask interrupt source */
+
+	SPIN_UNLOCK_IRQ(&priv->devlock, irqflags);
+
+	return DRVMGR_OK;
+}
+
+int ambapp_rasta_tmtc_int_mask(
+	struct drvmgr_dev *dev,
+	int irq)
+{
+	struct gr_rasta_tmtc_priv *priv = dev->parent->dev->priv;
+	SPIN_IRQFLAGS(irqflags);
+
+	DBG("RASTA-TMTC IRQ %d: mask\n", irq);
+
+	if ( genirq_check(priv->genirq, irq) )
+		return DRVMGR_EINVAL;
+
+	SPIN_LOCK_IRQ(&priv->devlock, irqflags);
+
+	/* Disable IRQ */
+	priv->irq->mask[0] &= ~(1<<irq); /* mask interrupt source */
+
+	SPIN_UNLOCK_IRQ(&priv->devlock, irqflags);
+
+	return DRVMGR_OK;
+}
+
+int ambapp_rasta_tmtc_int_clear(
+	struct drvmgr_dev *dev,
+	int irq)
+{
+	struct gr_rasta_tmtc_priv *priv = dev->parent->dev->priv;
+
+	if ( genirq_check(priv->genirq, irq) )
+		return DRVMGR_FAIL;
+
+	priv->irq->iclear = (1<<irq);
+
+	return DRVMGR_OK;
+}
+
+int ambapp_rasta_tmtc_get_params(struct drvmgr_dev *dev, struct drvmgr_bus_params *params)
+{
+	struct gr_rasta_tmtc_priv *priv = dev->parent->dev->priv;
+
+	/* Device name prefix pointer, skip /dev */
+	params->dev_prefix = &priv->prefix[5];
+
+	return 0;
+}
+
+void gr_rasta_tmtc_print_dev(struct drvmgr_dev *dev, int options)
+{
+	struct gr_rasta_tmtc_priv *priv = dev->priv;
+	struct pci_dev_info *devinfo = priv->devinfo;
+	uint32_t bar0, bar1, bar0_size, bar1_size;
+
+	/* Print */
+	printf("--- GR-RASTA-TMTC [bus 0x%x, dev 0x%x, fun 0x%x] ---\n",
+		PCI_DEV_EXPAND(priv->pcidev));
+
+	bar0 = devinfo->resources[0].address;
+	bar0_size = devinfo->resources[0].size;
+	bar1 = devinfo->resources[1].address;
+	bar1_size = devinfo->resources[1].size;
+
+	printf(" PCI BAR[0]: 0x%" PRIx32 " - 0x%" PRIx32 "\n",
+		bar0, bar0 + bar0_size - 1);
+	printf(" PCI BAR[1]: 0x%" PRIx32 " - 0x%" PRIx32 "\n",
+		bar1, bar1 + bar1_size - 1);
+	printf(" IRQ:             %d\n", devinfo->irq);
+	printf(" PCI REVISION:    %d\n", devinfo->rev);
+	printf(" FREQ:            %d Hz\n", priv->version->amba_freq_hz);
+	printf(" IMASK:           0x%08x\n", priv->irq->mask[0]);
+	printf(" IPEND:           0x%08x\n", priv->irq->ipend);
+
+	/* Print amba config */
+	if ( options & RASTA_TMTC_OPTIONS_AMBA ) {
+		ambapp_print(&priv->abus, 10);
+	}
+
+#if 0
+	/* Print IRQ handlers and their arguments */
+	if ( options & RASTA_TMTC_OPTIONS_IRQ ) {
+		int i;
+		for(i=0; i<16; i++) {
+			printf(" IRQ[%02d]:         0x%x, arg: 0x%x\n", 
+				i, (unsigned int)priv->isrs[i].handler, (unsigned int)priv->isrs[i].arg);
+		}
+	}
+#endif
+}
+
+void gr_rasta_tmtc_print(int options)
+{
+	struct pci_drv_info *drv = &gr_rasta_tmtc_info;
+	struct drvmgr_dev *dev;
+
+	dev = drv->general.dev;
+	while(dev) {
+		gr_rasta_tmtc_print_dev(dev, options);
+		dev = dev->next_in_drv;
+	}
+}
diff --git a/bsps/shared/grlib/pci/gr_tmtc_1553.c b/bsps/shared/grlib/pci/gr_tmtc_1553.c
new file mode 100644
index 0000000000..ce02b8a256
--- /dev/null
+++ b/bsps/shared/grlib/pci/gr_tmtc_1553.c
@@ -0,0 +1,595 @@
+/*  GR-TMTC-1553 PCI Target driver.
+ * 
+ *  COPYRIGHT (c) 2008.
+ *  Cobham Gaisler AB.
+ *
+ *  Configures the GR-TMTC-1553 interface PCI board.
+ *  This driver provides a AMBA PnP bus by using the general part
+ *  of the AMBA PnP bus driver (ambapp_bus.c).
+ *
+ *  Driver resources for the AMBA PnP bus provided can be set using
+ *  gr_tmtc_1553_set_resources().
+ *
+ *  The license and distribution terms for this file may be
+ *  found in found in the file LICENSE in this distribution or at
+ *  http://www.rtems.org/license/LICENSE.
+ */
+
+#include <inttypes.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+
+#include <bsp.h>
+#include <rtems/bspIo.h>
+#include <pci.h>
+#include <pci/access.h>
+
+#include <grlib/ambapp.h>
+#include <grlib/grlib.h>
+#include <drvmgr/drvmgr.h>
+#include <grlib/ambapp_bus.h>
+#include <drvmgr/pci_bus.h>
+#include <grlib/bspcommon.h>
+#include <grlib/genirq.h>
+
+#include <grlib/gr_tmtc_1553.h>
+
+#include <grlib/grlib_impl.h>
+
+/*#define DEBUG 1 */
+
+#ifdef DEBUG
+#define DBG(x...) printk(x)
+#else
+#define DBG(x...) 
+#endif
+
+/* PCI ID */
+#define PCIID_VENDOR_GAISLER		0x1AC8
+
+int gr_tmtc_1553_init1(struct drvmgr_dev *dev);
+int gr_tmtc_1553_init2(struct drvmgr_dev *dev);
+void gr_tmtc_1553_isr (void *arg);
+
+struct gr_tmtc_1553_ver {
+	const unsigned int	amba_freq_hz;	/* The frequency */
+	const unsigned int	amba_ioarea;	/* The address where the PnP IOAREA starts at */
+};
+
+/* Private data structure for driver */
+struct gr_tmtc_1553_priv {
+	/* Driver management */
+	struct drvmgr_dev		*dev;
+	char				prefix[32];
+	SPIN_DECLARE(devlock);
+
+	/* PCI */
+	pci_dev_t			pcidev;
+	struct pci_dev_info		*devinfo;
+
+	/* IRQ */
+	genirq_t			genirq;
+
+	struct gr_tmtc_1553_ver         *version;
+	struct irqmp_regs		*irq;
+	struct drvmgr_map_entry		bus_maps_down[2];
+
+	struct ambapp_bus		abus;
+	struct ambapp_mmap		amba_maps[4];
+        struct ambapp_config		config;
+};
+
+struct gr_tmtc_1553_ver gr_tmtc_1553_ver0 = {
+	.amba_freq_hz		= 33333333,
+	.amba_ioarea		= 0xfff00000,
+};
+
+
+int ambapp_tmtc_1553_int_register(
+	struct drvmgr_dev *dev,
+	int irq,
+	const char *info,
+	drvmgr_isr handler,
+	void *arg);
+int ambapp_tmtc_1553_int_unregister(
+	struct drvmgr_dev *dev,
+	int irq,
+	drvmgr_isr handler,
+	void *arg);
+int ambapp_tmtc_1553_int_unmask(
+	struct drvmgr_dev *dev,
+	int irq);
+int ambapp_tmtc_1553_int_mask(
+	struct drvmgr_dev *dev,
+	int irq);
+int ambapp_tmtc_1553_int_clear(
+	struct drvmgr_dev *dev,
+	int irq);
+int ambapp_tmtc_1553_get_params(
+	struct drvmgr_dev *dev,
+	struct drvmgr_bus_params *params);
+
+struct ambapp_ops ambapp_tmtc_1553_ops = {
+	.int_register = ambapp_tmtc_1553_int_register,
+	.int_unregister = ambapp_tmtc_1553_int_unregister,
+	.int_unmask = ambapp_tmtc_1553_int_unmask,
+	.int_mask = ambapp_tmtc_1553_int_mask,
+	.int_clear = ambapp_tmtc_1553_int_clear,
+	.get_params = ambapp_tmtc_1553_get_params
+};
+
+struct drvmgr_drv_ops gr_tmtc_1553_ops = 
+{
+	{gr_tmtc_1553_init1, gr_tmtc_1553_init2, NULL, NULL},
+	NULL,
+	NULL
+};
+
+struct pci_dev_id_match gr_tmtc_1553_ids[] = 
+{
+	PCIID_DEVVEND(PCIID_VENDOR_GAISLER, PCIID_DEVICE_GR_TMTC_1553),
+	PCIID_END_TABLE /* Mark end of table */
+};
+
+struct pci_drv_info gr_tmtc_1553_info =
+{
+	{
+		DRVMGR_OBJ_DRV,			/* Driver */
+		NULL,				/* Next driver */
+		NULL,				/* Device list */
+		DRIVER_PCI_GAISLER_TMTC_1553_ID, /* Driver ID */
+		"GR-TMTC-1553_DRV",		/* Driver Name */
+		DRVMGR_BUS_TYPE_PCI,		/* Bus Type */
+		&gr_tmtc_1553_ops,
+		NULL,				/* Funcs */
+		0,				/* No devices yet */
+		0,
+	},
+	&gr_tmtc_1553_ids[0]
+};
+
+/* Driver resources configuration for the AMBA bus on the GR-RASTA-IO board.
+ * It is declared weak so that the user may override it from the project file,
+ * if the default settings are not enough.
+ *
+ * The configuration consists of an array of configuration pointers, each
+ * pointer determine the configuration of one GR-RASTA-IO board. Pointer
+ * zero is for board0, pointer 1 for board1 and so on.
+ *
+ * The array must end with a NULL pointer.
+ */
+struct drvmgr_bus_res *gr_tmtc_1553_resources[] __attribute__((weak)) =
+{
+	NULL
+};
+
+void gr_tmtc_1553_register_drv(void)
+{
+	DBG("Registering GR-TMTC-1553 PCI driver\n");
+	drvmgr_drv_register(&gr_tmtc_1553_info.general);
+}
+
+void gr_tmtc_1553_isr (void *arg)
+{
+	struct gr_tmtc_1553_priv *priv = arg;
+	unsigned int status, tmp;
+	int irq;
+	SPIN_ISR_IRQFLAGS(irqflags);
+
+	tmp = status = priv->irq->ipend;
+
+	/* DBG("GR-RASTA-IO: IRQ 0x%x\n",status); */
+
+	SPIN_LOCK(&priv->devlock, irqflags);
+	for(irq=0; irq<16; irq++) {
+		if ( status & (1<<irq) ) {
+			genirq_doirq(priv->genirq, irq);
+			priv->irq->iclear = (1<<irq);
+			status &= ~(1<<irq);
+			if ( status == 0 )
+				break;
+		}
+	}
+	SPIN_UNLOCK(&priv->devlock, irqflags);
+
+	/* ACK interrupt, this is because PCI is Level, so the IRQ Controller still drives the IRQ. */
+	if ( tmp ) 
+		drvmgr_interrupt_clear(priv->dev, 0);
+
+	DBG("GR-TMTC-1553-IRQ: 0x%x\n", tmp);
+}
+
+static int gr_tmtc_1553_hw_init(struct gr_tmtc_1553_priv *priv)
+{
+	unsigned int *page0 = NULL;
+	struct ambapp_dev *tmp;
+	unsigned int pci_freq_hz;
+	struct pci_dev_info *devinfo = priv->devinfo;
+	uint32_t bar0, bar0_size;
+
+	/* Select version of GR-TMTC-1553 board */
+	switch (devinfo->rev) {
+		case 0:
+			priv->version = &gr_tmtc_1553_ver0;
+			break;
+		default:
+			return -2;
+	}
+
+	bar0 = devinfo->resources[0].address;
+	bar0_size = devinfo->resources[0].size;
+	page0 = (unsigned int *)(bar0 + bar0_size/2); 
+
+	/* Point PAGE0 to start of board address map. RAM at 0xff000000, APB at 0xffc00000, IOAREA at 0xfff000000 */
+	/* XXX We assume little endian host with byte twisting enabled here */
+	*page0 = 0x010000ff;	/* Set little endian mode on peripheral. */
+
+	/* Scan AMBA Plug&Play */
+
+	/* AMBA MAP bar0 (in CPU) ==> 0x80000000(remote amba address) */
+	priv->amba_maps[0].size = 0x1000000;
+	priv->amba_maps[0].local_adr = bar0;
+	priv->amba_maps[0].remote_adr = 0xff000000;
+	
+	/* Addresses not matching with map be untouched */
+	priv->amba_maps[2].size = 0xfffffff0;
+	priv->amba_maps[2].local_adr = 0;
+	priv->amba_maps[2].remote_adr = 0;
+
+	/* Mark end of table */
+	priv->amba_maps[3].size=0;
+	priv->amba_maps[3].local_adr = 0;
+	priv->amba_maps[3].remote_adr = 0;
+
+	/* Start AMBA PnP scan at first AHB bus */
+	ambapp_scan(&priv->abus,
+		bar0 + (priv->version->amba_ioarea & ~0xff000000),
+		NULL, &priv->amba_maps[0]);
+
+	/* Frequency is the hsame as the PCI bus frequency */
+	drvmgr_freq_get(priv->dev, 0, &pci_freq_hz);
+
+	ambapp_freq_init(&priv->abus, NULL, pci_freq_hz);
+
+	/* Find IRQ controller */
+	tmp = (struct ambapp_dev *)ambapp_for_each(&priv->abus,
+					(OPTIONS_ALL|OPTIONS_APB_SLVS),
+					VENDOR_GAISLER, GAISLER_IRQMP,
+					ambapp_find_by_idx, NULL);
+	if ( !tmp ) {
+		return -4;
+	}
+	priv->irq = (struct irqmp_regs *)DEV_TO_APB(tmp)->start;
+	/* Set up irq controller */
+	priv->irq->mask[0] = 0;
+	priv->irq->iclear = 0xffff;
+	priv->irq->ilevel = 0;
+
+	/* DOWN streams translation table */
+	priv->bus_maps_down[0].name = "PCI BAR0 -> AMBA";
+	priv->bus_maps_down[0].size = priv->amba_maps[0].size;
+	priv->bus_maps_down[0].from_adr = (void *)priv->amba_maps[0].local_adr;
+	priv->bus_maps_down[0].to_adr = (void *)priv->amba_maps[0].remote_adr;
+	/* Mark end of translation table */
+	priv->bus_maps_down[1].size = 0;
+
+	/* Successfully registered the board */
+	return 0;
+}
+
+
+/* Called when a PCI target is found with the PCI device and vendor ID 
+ * given in gr_tmtc_1553_ids[].
+ */
+int gr_tmtc_1553_init1(struct drvmgr_dev *dev)
+{
+	struct gr_tmtc_1553_priv *priv;
+	struct pci_dev_info *devinfo;
+	int status;
+	uint32_t bar0, bar0_size;
+	int resources_cnt;
+
+	/* PCI device does not have the IRQ line register, when PCI autoconf configures it the configuration
+	 * is forgotten. We take the IRQ number from the PCI Host device (AMBA device), this works as long
+	 * as PCI-IRQs are ored together on the bus.
+	 *
+	 * Note that this only works on LEON.
+	 */
+	((struct pci_dev_info *)dev->businfo)->irq = ((struct amba_dev_info *)dev->parent->dev->businfo)->info.irq; 
+
+	priv = grlib_calloc(1, sizeof(*priv));
+	if ( !priv )
+		return DRVMGR_NOMEM;
+
+	dev->priv = priv;
+	priv->dev = dev;
+
+	/* Determine number of configurations */
+	resources_cnt = get_resarray_count(gr_tmtc_1553_resources);
+
+	/* Generate Device prefix */
+
+	strcpy(priv->prefix, "/dev/tmtc1553_0");
+	priv->prefix[14] += dev->minor_drv;
+	mkdir(priv->prefix, S_IRWXU | S_IRWXG | S_IRWXO);
+	priv->prefix[15] = '/';
+	priv->prefix[16] = '\0';
+
+	priv->devinfo = devinfo = (struct pci_dev_info *)dev->businfo;
+	priv->pcidev = devinfo->pcidev;
+	bar0 = devinfo->resources[0].address;
+	bar0_size = devinfo->resources[0].size;
+	printk("\n\n--- GR-TMTC-1553[%d] ---\n", dev->minor_drv);
+	printk(" PCI BUS: 0x%x, SLOT: 0x%x, FUNCTION: 0x%x\n",
+		PCI_DEV_EXPAND(priv->pcidev));
+	printk(" PCI VENDOR: 0x%04x, DEVICE: 0x%04x\n",
+		devinfo->id.vendor, devinfo->id.device);
+	printk(" PCI BAR[0]: 0x%" PRIx32 " - 0x%" PRIx32 "\n",
+		bar0, bar0 + bar0_size - 1);
+	printk(" IRQ: %d\n\n\n", devinfo->irq);
+
+	/* all neccessary space assigned to GR-TMTC-1553 target? */
+	if (bar0_size == 0)
+		return DRVMGR_ENORES;
+
+	/* Initialize spin-lock for this PCI peripheral device. This is to
+	 * protect the Interrupt Controller Registers. The genirq layer is
+         * protecting its own internals and ISR dispatching.
+         */
+	SPIN_INIT(&priv->devlock, priv->prefix);
+
+	priv->genirq = genirq_init(16);
+	if ( priv->genirq == NULL ) {
+		free(priv);
+		dev->priv = NULL;
+		return DRVMGR_FAIL;
+	}
+
+	status = gr_tmtc_1553_hw_init(priv);
+	if ( status != 0 ) {
+		genirq_destroy(priv->genirq);
+		free(priv);
+		dev->priv = NULL;
+		printk(" Failed to initialize GR-TMTC-1553 HW: %d\n", status);
+		return DRVMGR_FAIL;
+	}
+
+	/* Init amba bus */
+	priv->config.abus = &priv->abus;
+	priv->config.ops = &ambapp_tmtc_1553_ops;
+	priv->config.maps_down = &priv->bus_maps_down[0];
+	/* This PCI device has only target interface so DMA is not supported,
+	 * which means that translation from AMBA->PCI should fail if attempted.
+	 */
+	priv->config.maps_up = DRVMGR_TRANSLATE_NO_BRIDGE;
+	if ( priv->dev->minor_drv < resources_cnt ) {
+		priv->config.resources = gr_tmtc_1553_resources[priv->dev->minor_drv];
+	} else {
+		priv->config.resources = NULL;
+	}
+
+	/* Create And Register AMBA PnP Bus */
+	return ambapp_bus_register(dev, &priv->config);
+}
+
+int gr_tmtc_1553_init2(struct drvmgr_dev *dev)
+{
+	struct gr_tmtc_1553_priv *priv = dev->priv;
+
+	/* Clear any old interrupt requests */
+	drvmgr_interrupt_clear(dev, 0);
+
+	/* Enable System IRQ so that GR-TMTC-1553 PCI target interrupt goes through.
+	 *
+	 * It is important to enable it in stage init2. If interrupts were enabled in init1
+	 * this might hang the system when more than one PCI target is connected, this is
+	 * because PCI interrupts might be shared and PCI target 2 have not initialized and
+	 * might therefore drive interrupt already when entering init1().
+	 */
+	drvmgr_interrupt_register(
+		dev,
+		0,
+		"gr_tmtc_1553",
+		gr_tmtc_1553_isr,
+		(void *)priv);
+
+	return DRVMGR_OK;
+}
+
+int ambapp_tmtc_1553_int_register(
+	struct drvmgr_dev *dev,
+	int irq,
+	const char *info,
+	drvmgr_isr handler,
+	void *arg)
+{
+	struct gr_tmtc_1553_priv *priv = dev->parent->dev->priv;
+	SPIN_IRQFLAGS(irqflags);
+	int status;
+	void *h;
+
+	h = genirq_alloc_handler(handler, arg);
+	if ( h == NULL )
+		return DRVMGR_FAIL;
+
+	SPIN_LOCK_IRQ(&priv->devlock, irqflags);
+
+	status = genirq_register(priv->genirq, irq, h);
+	if ( status == 0 ) {
+		/* Disable and clear IRQ for first registered handler */
+		priv->irq->iclear = (1<<irq);
+		priv->irq->mask[0] &= ~(1<<irq); /* mask interrupt source */
+	} else if ( status == 1 )
+		status = 0;
+
+	if (status != 0) {
+		SPIN_UNLOCK_IRQ(&priv->devlock, irqflags);
+		genirq_free_handler(h);
+		return DRVMGR_FAIL;
+	}
+
+	status = genirq_enable(priv->genirq, irq, handler, arg);
+	if ( status == 0 ) {
+		/* Enable IRQ for first enabled handler only */
+		priv->irq->mask[0] |= (1<<irq); /* unmask interrupt source */
+	} else if ( status == 1 )
+		status = 0;
+
+	SPIN_UNLOCK_IRQ(&priv->devlock, irqflags);
+
+	return status;
+}
+
+int ambapp_tmtc_1553_int_unregister(
+	struct drvmgr_dev *dev,
+	int irq,
+	drvmgr_isr isr,
+	void *arg)
+{
+	struct gr_tmtc_1553_priv *priv = dev->parent->dev->priv;
+	SPIN_IRQFLAGS(irqflags);
+	int status;
+	void *handler;
+
+	SPIN_LOCK_IRQ(&priv->devlock, irqflags);
+
+	status = genirq_disable(priv->genirq, irq, isr, arg);
+	if ( status == 0 ) {
+		/* Disable IRQ only when no enabled handler exists */
+		priv->irq->mask[0] &= ~(1<<irq); /* mask interrupt source */
+	} else if ( status == 1 )
+		status = 0;
+
+	handler = genirq_unregister(priv->genirq, irq, isr, arg);
+	if ( handler == NULL )
+		status = DRVMGR_FAIL;
+	else
+		status = DRVMGR_OK;
+
+	SPIN_UNLOCK_IRQ(&priv->devlock, irqflags);
+
+	if (handler)
+		genirq_free_handler(handler);
+
+	return status;
+}
+
+int ambapp_tmtc_1553_int_unmask(
+	struct drvmgr_dev *dev,
+	int irq)
+{
+	struct gr_tmtc_1553_priv *priv = dev->parent->dev->priv;
+	SPIN_IRQFLAGS(irqflags);
+
+	DBG("TMTC-1553 IRQ %d: enable\n", irq);
+	
+	if ( genirq_check(priv->genirq, irq) )
+		return DRVMGR_FAIL;
+
+	SPIN_LOCK_IRQ(&priv->devlock, irqflags);
+
+	/* Enable IRQ */
+	priv->irq->mask[0] |= (1<<irq); /* unmask interrupt source */
+
+	SPIN_UNLOCK_IRQ(&priv->devlock, irqflags);
+
+	return DRVMGR_OK;
+}
+
+int ambapp_tmtc_1553_int_mask(
+	struct drvmgr_dev *dev,
+	int irq)
+{
+	struct gr_tmtc_1553_priv *priv = dev->parent->dev->priv;
+	SPIN_IRQFLAGS(irqflags);
+
+	DBG("TMTC-1553 IRQ %d: disable\n", irq);
+
+	if ( genirq_check(priv->genirq, irq) )
+		return DRVMGR_FAIL;
+
+	SPIN_LOCK_IRQ(&priv->devlock, irqflags);
+
+	/* Disable IRQ */
+	priv->irq->mask[0] &= ~(1<<irq); /* mask interrupt source */
+
+	SPIN_UNLOCK_IRQ(&priv->devlock, irqflags);
+
+	return DRVMGR_OK;
+}
+
+int ambapp_tmtc_1553_int_clear(
+	struct drvmgr_dev *dev,
+	int irq)
+{
+	struct gr_tmtc_1553_priv *priv = dev->parent->dev->priv;
+
+	if ( genirq_check(priv->genirq, irq) )
+		return DRVMGR_FAIL;
+
+	priv->irq->iclear = (1<<irq);
+
+	return DRVMGR_OK;
+}
+
+int ambapp_tmtc_1553_get_params(struct drvmgr_dev *dev, struct drvmgr_bus_params *params)
+{
+	struct gr_tmtc_1553_priv *priv = dev->parent->dev->priv;
+
+	/* Device name prefix pointer, skip /dev */
+	params->dev_prefix = &priv->prefix[5];
+
+	return 0;
+}
+
+void gr_tmtc_1553_print_dev(struct drvmgr_dev *dev, int options)
+{
+	struct gr_tmtc_1553_priv *priv = dev->priv;
+	struct pci_dev_info *devinfo = priv->devinfo;
+	uint32_t bar0, bar0_size;
+
+	/* Print */
+	printf("--- GR-TMTC-1553 [bus 0x%x, dev 0x%x, fun 0x%x] ---\n",
+		PCI_DEV_EXPAND(priv->pcidev));
+
+	bar0 = devinfo->resources[0].address;
+	bar0_size = devinfo->resources[0].size;
+
+	printf(" PCI BAR[0]: 0x%" PRIx32 " - 0x%" PRIx32 "\n",
+		bar0, bar0 + bar0_size - 1);
+	printf(" IRQ REGS:        0x%" PRIxPTR "\n", (uintptr_t)priv->irq);
+	printf(" IRQ:             %d\n", devinfo->irq);
+	printf(" FREQ:            %d Hz\n", priv->version->amba_freq_hz);
+	printf(" IMASK:           0x%08x\n", priv->irq->mask[0]);
+	printf(" IPEND:           0x%08x\n", priv->irq->ipend);
+
+	/* Print amba config */
+	if ( options & TMTC_1553_OPTIONS_AMBA ) {
+		ambapp_print(&priv->abus, 10);
+	}
+#if 0
+	/* Print IRQ handlers and their arguments */
+	if ( options & TMTC_1553_OPTIONS_IRQ ) {
+		int i;
+		for(i=0; i<16; i++) {
+			printf(" IRQ[%02d]:         0x%x, arg: 0x%x\n", 
+				i, (unsigned int)priv->isrs[i].handler, (unsigned int)priv->isrs[i].arg);
+		}
+	}
+#endif
+}
+
+void gr_tmtc_1553_print(int options)
+{
+	struct pci_drv_info *drv = &gr_tmtc_1553_info;
+	struct drvmgr_dev *dev;
+
+	dev = drv->general.dev;
+	while(dev) {
+		gr_tmtc_1553_print_dev(dev, options);
+		dev = dev->next_in_drv;
+	}
+}
diff --git a/bsps/shared/grlib/pci/grpci.c b/bsps/shared/grlib/pci/grpci.c
new file mode 100644
index 0000000000..fc2f06a063
--- /dev/null
+++ b/bsps/shared/grlib/pci/grpci.c
@@ -0,0 +1,722 @@
+/*  GRLIB GRPCI PCI HOST driver.
+ * 
+ *  COPYRIGHT (c) 2008.
+ *  Cobham Gaisler AB.
+ *
+ *  Configures the GRPCI core and initialize,
+ *   - the PCI Library (pci.c)
+ *   - the general part of the PCI Bus driver (pci_bus.c)
+ *  
+ *  System interrupt assigned to PCI interrupt (INTA#..INTD#) is by
+ *  default taken from Plug and Play, but may be overridden by the 
+ *  driver resources INTA#..INTD#.
+ *
+ *  The license and distribution terms for this file may be
+ *  found in found in the file LICENSE in this distribution or at
+ *  http://www.rtems.org/license/LICENSE.
+ */
+
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+#include <rtems/bspIo.h>
+#include <libcpu/byteorder.h>
+#include <libcpu/access.h>
+#include <pci.h>
+#include <pci/cfg.h>
+
+#include <drvmgr/drvmgr.h>
+#include <grlib/ambapp_bus.h>
+#include <grlib/ambapp.h>
+#include <drvmgr/pci_bus.h>
+#include <grlib/grpci.h>
+
+#define DMAPCI_ADDR 0x80000500
+
+/* Configuration options */
+#define SYSTEM_MAINMEM_START 0x40000000
+
+/* If defined to 1 - byte twisting is enabled by default */
+#define DEFAULT_BT_ENABLED 0
+
+/* Interrupt assignment. Set to other value than 0xff in order to 
+ * override defaults and plug&play information
+ */
+#ifndef GRPCI_INTA_SYSIRQ
+ #define GRPCI_INTA_SYSIRQ 0xff
+#endif
+#ifndef GRPCI_INTB_SYSIRQ
+ #define GRPCI_INTB_SYSIRQ 0xff
+#endif
+#ifndef GRPCI_INTC_SYSIRQ
+ #define GRPCI_INTC_SYSIRQ 0xff
+#endif
+#ifndef GRPCI_INTD_SYSIRQ
+ #define GRPCI_INTD_SYSIRQ 0xff
+#endif
+
+#define PAGE0_BTEN_BIT    0
+#define PAGE0_BTEN        (1<<PAGE0_BTEN_BIT)
+
+#define CFGSTAT_HOST_BIT  13
+#define CFGSTAT_HOST      (1<<CFGSTAT_HOST_BIT)
+
+/*#define DEBUG 1*/
+
+#ifdef DEBUG
+#define DBG(x...) printk(x)
+#else
+#define DBG(x...) 
+#endif
+
+/*
+ * Bit encode for PCI_CONFIG_HEADER_TYPE register
+ */
+struct grpci_regs {
+	volatile unsigned int cfg_stat;
+	volatile unsigned int bar0;
+	volatile unsigned int page0;
+	volatile unsigned int bar1;
+	volatile unsigned int page1;
+	volatile unsigned int iomap;
+	volatile unsigned int stat_cmd;
+	volatile unsigned int irq;
+};
+
+#define HOST_TGT PCI_DEV(0xff, 0, 0)
+
+struct grpci_priv *grpcipriv = NULL;
+static int grpci_minor = 0;
+static unsigned int *pcidma = (unsigned int *)DMAPCI_ADDR;
+
+/* PCI Interrupt assignment. Connects an PCI interrupt pin (INTA#..INTD#)
+ * to a system interrupt number.
+ */
+unsigned char grpci_pci_irq_table[4] =
+{
+	/* INTA# */	GRPCI_INTA_SYSIRQ,
+	/* INTB# */	GRPCI_INTB_SYSIRQ,
+	/* INTC# */	GRPCI_INTC_SYSIRQ,
+	/* INTD# */	GRPCI_INTD_SYSIRQ
+};
+
+/* Driver private data struture */
+struct grpci_priv {
+	struct drvmgr_dev	*dev;
+	struct grpci_regs		*regs;
+	int				irq;
+	int				minor;
+
+	uint32_t			bar1_pci_adr;
+	uint32_t			bar1_size;
+
+	int				bt_enabled;
+	unsigned int			pci_area;
+	unsigned int			pci_area_end;
+	unsigned int			pci_io;    
+	unsigned int			pci_conf;
+	unsigned int			pci_conf_end;
+
+	uint32_t			devVend; /* Host PCI Vendor/Device ID */
+
+	struct drvmgr_map_entry	maps_up[2];
+	struct drvmgr_map_entry	maps_down[2];
+	struct pcibus_config	config;
+};
+
+int grpci_init1(struct drvmgr_dev *dev);
+
+/* GRPCI DRIVER */
+
+struct drvmgr_drv_ops grpci_ops = 
+{
+	.init = {grpci_init1, NULL, NULL, NULL},
+	.remove = NULL,
+	.info = NULL
+};
+
+struct amba_dev_id grpci_ids[] = 
+{
+	{VENDOR_GAISLER, GAISLER_PCIFBRG},
+	{0, 0}		/* Mark end of table */
+};
+
+struct amba_drv_info grpci_info =
+{
+	{
+		DRVMGR_OBJ_DRV,			/* Driver */
+		NULL,				/* Next driver */
+		NULL,				/* Device list */
+		DRIVER_AMBAPP_GAISLER_GRPCI_ID,	/* Driver ID */
+		"GRPCI_DRV",			/* Driver Name */
+		DRVMGR_BUS_TYPE_AMBAPP,		/* Bus Type */
+		&grpci_ops,
+		NULL,				/* Funcs */
+		0,				/* No devices yet */
+		sizeof(struct grpci_priv),	/* Make drvmgr alloc private */
+	},
+	&grpci_ids[0]
+};
+
+void grpci_register_drv(void)
+{
+	DBG("Registering GRPCI driver\n");
+	drvmgr_drv_register(&grpci_info.general);
+}
+
+static int grpci_cfg_r32(pci_dev_t dev, int ofs, uint32_t *val)
+{
+	struct grpci_priv *priv = grpcipriv;
+	volatile uint32_t *pci_conf;
+	uint32_t devfn;
+	int retval;
+	int bus = PCI_DEV_BUS(dev);
+
+	if (ofs & 3)
+		return PCISTS_EINVAL;
+
+	if (PCI_DEV_SLOT(dev) > 15) {
+		*val = 0xffffffff;
+		return PCISTS_OK;
+	}
+
+	/* GRPCI can access "non-standard" devices on bus0 (on AD11.AD16), 
+	 * but we skip them.
+	 */
+	if (dev == HOST_TGT)
+		bus = devfn = 0;
+	else if (bus == 0)
+		devfn = PCI_DEV_DEVFUNC(dev) + PCI_DEV(0, 6, 0);
+	else
+		devfn = PCI_DEV_DEVFUNC(dev);
+
+	/* Select bus */
+	priv->regs->cfg_stat = (priv->regs->cfg_stat & ~(0xf<<23)) | (bus<<23);
+
+	pci_conf = (volatile uint32_t *)(priv->pci_conf | (devfn << 8) | ofs);
+
+	if (priv->bt_enabled) {
+		*val =  CPU_swap_u32(*pci_conf);
+	} else {
+		*val = *pci_conf;
+	}
+
+	if (priv->regs->cfg_stat & 0x100) {
+		*val = 0xffffffff;
+		retval = PCISTS_MSTABRT;
+	} else
+		retval = PCISTS_OK;
+
+	DBG("pci_read: [%x:%x:%x] reg: 0x%x => addr: 0x%x, val: 0x%x\n",
+		PCI_DEV_EXPAND(dev), ofs, pci_conf, *val);
+
+	return retval;
+}
+
+
+static int grpci_cfg_r16(pci_dev_t dev, int ofs, uint16_t *val)
+{
+	uint32_t v;
+	int retval;
+
+	if (ofs & 1)
+		return PCISTS_EINVAL;
+
+	retval = grpci_cfg_r32(dev, ofs & ~0x3, &v);
+	*val = 0xffff & (v >> (8*(ofs & 0x3)));
+
+	return retval;
+}
+
+static int grpci_cfg_r8(pci_dev_t dev, int ofs, uint8_t *val)
+{
+	uint32_t v;
+	int retval;
+
+	retval = grpci_cfg_r32(dev, ofs & ~0x3, &v);
+
+	*val = 0xff & (v >> (8*(ofs & 3)));
+
+	return retval;
+}
+
+static int grpci_cfg_w32(pci_dev_t dev, int ofs, uint32_t val)
+{
+	struct grpci_priv *priv = grpcipriv;
+	volatile uint32_t *pci_conf;
+	uint32_t value, devfn = PCI_DEV_DEVFUNC(dev);
+	int bus = PCI_DEV_BUS(dev);
+
+	if (ofs & 0x3)
+		return PCISTS_EINVAL;
+
+	if (PCI_DEV_SLOT(dev) > 15)
+		return PCISTS_MSTABRT;
+
+	/* GRPCI can access "non-standard" devices on bus0 (on AD11.AD16), 
+	 * but we skip them.
+	 */
+	if (dev == HOST_TGT)
+		bus = devfn = 0;
+	else if (bus == 0)
+		devfn = PCI_DEV_DEVFUNC(dev) + PCI_DEV(0, 6, 0);
+	else
+		devfn = PCI_DEV_DEVFUNC(dev);
+
+	/* Select bus */
+	priv->regs->cfg_stat = (priv->regs->cfg_stat & ~(0xf<<23)) | (bus<<23);
+
+	pci_conf = (volatile uint32_t *)(priv->pci_conf | (devfn << 8) | ofs);
+
+	if ( priv->bt_enabled ) {
+		value = CPU_swap_u32(val);
+	} else {
+		value = val;
+	}
+
+	*pci_conf = value;
+
+	DBG("pci_write - [%x:%x:%x] reg: 0x%x => addr: 0x%x, val: 0x%x\n",
+		PCI_DEV_EXPAND(dev), ofs, pci_conf, value);
+
+	return PCISTS_OK;
+}
+
+static int grpci_cfg_w16(pci_dev_t dev, int ofs, uint16_t val)
+{
+	uint32_t v;
+	int retval;
+
+	if (ofs & 1)
+		return PCISTS_EINVAL;
+
+	retval = grpci_cfg_r32(dev, ofs & ~0x3, &v);
+	if (retval != PCISTS_OK)
+		return retval;
+
+	v = (v & ~(0xffff << (8*(ofs&3)))) | ((0xffff&val) << (8*(ofs&3)));
+
+	return grpci_cfg_w32(dev, ofs & ~0x3, v);
+}
+
+static int grpci_cfg_w8(pci_dev_t dev, int ofs, uint8_t val)
+{
+	uint32_t v;
+	int retval;
+
+	retval = grpci_cfg_r32(dev, ofs & ~0x3, &v);
+	if (retval != PCISTS_OK)
+		return retval;
+
+	v = (v & ~(0xff << (8*(ofs&3)))) | ((0xff&val) << (8*(ofs&3)));
+
+	return grpci_cfg_w32(dev, ofs & ~0x3, v);
+}
+
+/* Return the assigned system IRQ number that corresponds to the PCI
+ * "Interrupt Pin" information from configuration space.
+ *
+ * The IRQ information is stored in the grpci_pci_irq_table configurable
+ * by the user.
+ *
+ * Returns the "system IRQ" for the PCI INTA#..INTD# pin in irq_pin. Returns
+ * 0xff if not assigned.
+ */
+static uint8_t grpci_bus0_irq_map(pci_dev_t dev, int irq_pin)
+{
+	uint8_t sysIrqNr = 0; /* not assigned */
+	int irq_group;
+
+	if ( (irq_pin >= 1) && (irq_pin <= 4) ) {
+		/* Use default IRQ decoding on PCI BUS0 according slot numbering */
+		irq_group = PCI_DEV_SLOT(dev) & 0x3;
+		irq_pin = ((irq_pin - 1) + irq_group) & 0x3;
+		/* Valid PCI "Interrupt Pin" number */
+		sysIrqNr = grpci_pci_irq_table[irq_pin];
+	}
+	return sysIrqNr;
+}
+
+static int grpci_translate(uint32_t *address, int type, int dir)
+{
+	uint32_t adr;
+	struct grpci_priv *priv = grpcipriv;
+
+	if (type == 1) {
+		/* I/O */
+		if (dir != 0) {
+			/* The PCI bus can not access the CPU bus from I/O
+			 * because GRPCI core does not support I/O BARs
+			 */
+			return -1;
+		}
+
+		/* We have got a PCI BAR address that the CPU want to access...
+		 * Check that it is within the PCI I/O window, I/O adresses
+		 * are mapped 1:1 with GRPCI driver... no translation needed.
+		 */
+		adr = *(uint32_t *)address;
+		if (adr < priv->pci_io || adr >= priv->pci_conf)
+			return -1;
+	} else {
+		/* MEMIO and MEM.
+		 * Memory space is mapped 1:1 so no translation is needed.
+		 * Check that address is within accessible windows.
+		 */
+		adr = *(uint32_t *)address;
+		if (dir == 0) {
+			/* PCI BAR to AMBA-CPU address.. check that it is
+			 * located within GRPCI PCI Memory Window
+			 * adr = PCI address.
+			 */
+			if (adr < priv->pci_area || adr >= priv->pci_area_end)
+				return -1;
+		} else {
+			/* We have a CPU address and want to get access to it
+			 * from PCI space, typically when doing DMA into CPU
+			 * RAM. The GRPCI core has two target BARs that PCI
+			 * masters can access, we check here that the address
+			 * is accessible from PCI.
+			 * adr = AMBA address.
+			 */
+			if (adr < priv->bar1_pci_adr ||
+			    adr >= (priv->bar1_pci_adr + priv->bar1_size))
+				return -1;
+		}
+	}
+
+	return 0;
+}
+
+extern struct pci_memreg_ops pci_memreg_sparc_le_ops;
+extern struct pci_memreg_ops pci_memreg_sparc_be_ops;
+
+/* GRPCI PCI access routines, default to Little-endian PCI Bus */
+struct pci_access_drv grpci_access_drv = {
+	.cfg =
+	{
+		grpci_cfg_r8,
+		grpci_cfg_r16,
+		grpci_cfg_r32,
+		grpci_cfg_w8,
+		grpci_cfg_w16,
+		grpci_cfg_w32,
+	},
+	.io =
+	{
+		_ld8,
+		_ld_le16,
+		_ld_le32,
+		_st8,
+		_st_le16,
+		_st_le32,
+	},
+	.memreg = &pci_memreg_sparc_le_ops,
+	.translate = grpci_translate,
+};
+
+struct pci_io_ops grpci_io_ops_be =
+{
+	_ld8,
+	_ld_be16,
+	_ld_be32,
+	_st8,
+	_st_be16,
+	_st_be32,
+};
+
+static int grpci_hw_init(struct grpci_priv *priv)
+{
+	volatile unsigned int *mbar0, *page0;
+	uint32_t data, addr, mbar0size;
+	pci_dev_t host = HOST_TGT;
+
+	mbar0 = (volatile unsigned int *)priv->pci_area;
+
+	if ( !priv->bt_enabled && ((priv->regs->page0 & PAGE0_BTEN) == PAGE0_BTEN) ) {
+		/* Byte twisting is on, turn it off */
+		grpci_cfg_w32(host, PCIR_BAR(0), 0xffffffff);
+		grpci_cfg_r32(host, PCIR_BAR(0), &addr);
+		/* Setup bar0 to nonzero value */
+		grpci_cfg_w32(host, PCIR_BAR(0),
+				CPU_swap_u32(0x80000000));
+		/* page0 is accessed through upper half of bar0 */
+		addr = (~CPU_swap_u32(addr)+1)>>1;
+		mbar0size = addr*2;
+		DBG("GRPCI: Size of MBAR0: 0x%x, MBAR0: 0x%x(lower) 0x%x(upper)\n",mbar0size,((unsigned int)mbar0),((unsigned int)mbar0)+mbar0size/2);
+		page0 = &mbar0[mbar0size/8];
+		DBG("GRPCI: PAGE0 reg address: 0x%x (0x%x)\n",((unsigned int)mbar0)+mbar0size/2,page0);
+		priv->regs->cfg_stat = (priv->regs->cfg_stat & (~0xf0000000)) | 0x80000000;    /* Setup mmap reg so we can reach bar0 */ 
+		*page0 = 0<<PAGE0_BTEN_BIT;                                         /* Disable bytetwisting ... */
+	}
+
+	/* Get the GRPCI Host PCI ID */
+	grpci_cfg_r32(host, PCIR_VENDOR, &priv->devVend);
+
+	/* set 1:1 mapping between AHB -> PCI memory */
+	priv->regs->cfg_stat = (priv->regs->cfg_stat & 0x0fffffff) | priv->pci_area;
+
+	/* determine size of target BAR1 */
+	grpci_cfg_w32(host, PCIR_BAR(1), 0xffffffff);
+	grpci_cfg_r32(host, PCIR_BAR(1), &addr);
+	priv->bar1_size = (~(addr & ~0xf)) + 1;
+
+	/* and map system RAM at pci address 0x40000000 */
+	priv->bar1_pci_adr &= ~(priv->bar1_size - 1); /* Fix alignment of BAR1 */
+	grpci_cfg_w32(host, PCIR_BAR(1), priv->bar1_pci_adr);
+	priv->regs->page1 = priv->bar1_pci_adr;
+
+	/* Translate I/O accesses 1:1 */
+	priv->regs->iomap = priv->pci_io & 0xffff0000;
+
+	/* Setup Latency Timer and cache line size. Default cache line
+	 * size will result in poor performance (256 word fetches), 0xff
+	 * will set it according to the max size of the PCI FIFO.
+	 */
+	grpci_cfg_w8(host, PCIR_CACHELNSZ, 0xff);
+	grpci_cfg_w8(host, PCIR_LATTIMER, 0x40);
+
+	/* set as bus master and enable pci memory responses */  
+	grpci_cfg_r32(host, PCIR_COMMAND, &data);
+	data |= (PCIM_CMD_MEMEN | PCIM_CMD_BUSMASTEREN);
+	grpci_cfg_w32(host, PCIR_COMMAND, data);
+
+	/* unmask all PCI interrupts at PCI Core, not all GRPCI cores support
+	 * this
+	 */
+	priv->regs->irq = 0xf0000;
+
+	/* Successful */
+	return 0;
+}
+
+/* Initializes the GRPCI core and driver, must be called before calling init_pci() 
+ *
+ * Return values
+ *  0             Successful initalization
+ *  -1            Error during initialization, for example "PCI core not found".
+ *  -2            Error PCI controller not HOST (targets not supported)
+ *  -3            Error due to GRPCI hardware initialization
+ *  -4            Error registering driver to PCI layer
+ */
+static int grpci_init(struct grpci_priv *priv)
+{
+	struct ambapp_apb_info *apb;
+	struct ambapp_ahb_info *ahb;
+	int pin;
+	union drvmgr_key_value *value;
+	char keyname[6];
+	struct amba_dev_info *ainfo = priv->dev->businfo;
+
+	/* Find PCI core from Plug&Play information */
+	apb = ainfo->info.apb_slv;
+	ahb = ainfo->info.ahb_slv;
+
+	/* Found PCI core, init private structure */
+	priv->irq = apb->irq;
+	priv->regs = (struct grpci_regs *)apb->start;
+	priv->bt_enabled = DEFAULT_BT_ENABLED;
+
+	/* Calculate the PCI windows 
+	 *  AMBA->PCI Window:                       AHB SLAVE AREA0
+	 *  AMBA->PCI I/O cycles Window:            AHB SLAVE AREA1 Lower half
+	 *  AMBA->PCI Configuration cycles Window:  AHB SLAVE AREA1 Upper half
+	 */
+	priv->pci_area     = ahb->start[0];
+	priv->pci_area_end = ahb->start[0] + ahb->mask[0];
+	priv->pci_io       = ahb->start[1];
+	priv->pci_conf     = ahb->start[1] + (ahb->mask[1] >> 1);
+	priv->pci_conf_end = ahb->start[1] + ahb->mask[1];
+
+	/* On systems where PCI I/O area and configuration area is apart of the "PCI Window" 
+	 * the PCI Window stops at the start of the PCI I/O area
+	 */
+	if ( (priv->pci_io > priv->pci_area) && (priv->pci_io < (priv->pci_area_end-1)) ) {
+		priv->pci_area_end = priv->pci_io;
+	}
+
+	/* Init PCI interrupt assignment table to all use the interrupt routed through
+	 * the GRPCI core.
+	 */
+	strcpy(keyname, "INTX#");
+	for (pin=1; pin<5; pin++) {
+		if ( grpci_pci_irq_table[pin-1] == 0xff ) {
+			grpci_pci_irq_table[pin-1] = priv->irq;
+
+			/* User may override Both hardcoded IRQ setup and Plug & Play IRQ */
+			keyname[3] = 'A' + (pin-1);
+			value = drvmgr_dev_key_get(priv->dev, keyname, DRVMGR_KT_INT);
+			if ( value )
+				grpci_pci_irq_table[pin-1] = value->i;
+		}
+	}
+
+	/* User may override DEFAULT_BT_ENABLED to enable/disable byte twisting */
+	value = drvmgr_dev_key_get(priv->dev, "byteTwisting", DRVMGR_KT_INT);
+	if ( value )
+		priv->bt_enabled = value->i;
+
+	/* Use GRPCI target BAR1 to map CPU RAM to PCI, this is to make it
+	 * possible for PCI peripherals to do DMA directly to CPU memory.
+	 */
+	value = drvmgr_dev_key_get(priv->dev, "tgtbar1", DRVMGR_KT_INT);
+	if (value)
+		priv->bar1_pci_adr = value->i;
+	else
+		priv->bar1_pci_adr = SYSTEM_MAINMEM_START; /* default */
+
+	/* This driver only support HOST systems, we check for HOST */
+	if ( !(priv->regs->cfg_stat & CFGSTAT_HOST) ) {
+		/* Target not supported */
+		return -2;
+	}
+
+	/* Init the PCI Core */
+	if ( grpci_hw_init(priv) ) {
+		return -3;
+	}
+
+	/* Down streams translation table */
+	priv->maps_down[0].name = "AMBA -> PCI MEM Window";
+	priv->maps_down[0].size = priv->pci_area_end - priv->pci_area;
+	priv->maps_down[0].from_adr = (void *)priv->pci_area;
+	priv->maps_down[0].to_adr = (void *)priv->pci_area;
+	/* End table */
+	priv->maps_down[1].size = 0;
+
+	/* Up streams translation table */
+	priv->maps_up[0].name = "Target BAR1 -> AMBA";
+	priv->maps_up[0].size = priv->bar1_size;
+	priv->maps_up[0].from_adr = (void *)priv->bar1_pci_adr;
+	priv->maps_up[0].to_adr = (void *)priv->bar1_pci_adr;
+	/* End table */
+	priv->maps_up[1].size = 0;
+
+	return 0;
+}
+
+/* Called when a core is found with the AMBA device and vendor ID 
+ * given in grpci_ids[]. IRQ, Console does not work here
+ */
+int grpci_init1(struct drvmgr_dev *dev)
+{
+	int status;
+	struct grpci_priv *priv;
+	struct pci_auto_setup grpci_auto_cfg;
+
+	DBG("GRPCI[%d] on bus %s\n", dev->minor_drv, dev->parent->dev->name);
+
+	if ( grpci_minor != 0 ) {
+		DBG("Driver only supports one PCI core\n");
+		return DRVMGR_FAIL;
+	}
+
+	if ( (strcmp(dev->parent->dev->drv->name, "AMBAPP_GRLIB_DRV") != 0) && 
+	     (strcmp(dev->parent->dev->drv->name, "AMBAPP_LEON2_DRV") != 0) ) {
+		/* We only support GRPCI driver on local bus */
+		return DRVMGR_FAIL;
+	}
+
+	priv = dev->priv;
+	if ( !priv )
+		return DRVMGR_NOMEM;
+
+	priv->dev = dev;
+	priv->minor = grpci_minor++;
+
+	grpcipriv = priv;
+	status = grpci_init(priv);
+	if (status) {
+		printk("Failed to initialize grpci driver %d\n", status);
+		return DRVMGR_FAIL;
+	}
+
+
+	/* Register the PCI core at the PCI layers */
+
+	if (priv->bt_enabled == 0) {
+		/* Host is Big-Endian */
+		pci_endian = PCI_BIG_ENDIAN;
+
+		memcpy(&grpci_access_drv.io, &grpci_io_ops_be,
+						sizeof(grpci_io_ops_be));
+		grpci_access_drv.memreg = &pci_memreg_sparc_be_ops;
+	}
+
+	if (pci_access_drv_register(&grpci_access_drv)) {
+		/* Access routines registration failed */
+		return DRVMGR_FAIL;
+	}
+
+	/* Prepare memory MAP */
+	grpci_auto_cfg.options = 0;
+	grpci_auto_cfg.mem_start = 0;
+	grpci_auto_cfg.mem_size = 0;
+	grpci_auto_cfg.memio_start = priv->pci_area;
+	grpci_auto_cfg.memio_size = priv->pci_area_end - priv->pci_area;
+	grpci_auto_cfg.io_start = priv->pci_io;
+	grpci_auto_cfg.io_size = priv->pci_conf - priv->pci_io;
+	grpci_auto_cfg.irq_map = grpci_bus0_irq_map;
+	grpci_auto_cfg.irq_route = NULL; /* use standard routing */
+	pci_config_register(&grpci_auto_cfg);
+
+	if (pci_config_init()) {
+		/* PCI configuration failed */
+		return DRVMGR_FAIL;
+	}
+
+	priv->config.maps_down = &priv->maps_down[0];
+	priv->config.maps_up = &priv->maps_up[0];
+	return pcibus_register(dev, &priv->config);
+}
+
+/* DMA functions which uses GRPCIs optional DMA controller (len in words) */
+int grpci_dma_to_pci(
+	unsigned int ahb_addr,
+	unsigned int pci_addr,
+	unsigned int len)
+{
+    int ret = 0;
+
+    pcidma[0] = 0x82;
+    pcidma[1] = ahb_addr;
+    pcidma[2] = pci_addr;
+    pcidma[3] = len;
+    pcidma[0] = 0x83;        
+
+    while ( (pcidma[0] & 0x4) == 0)
+        ;
+
+    if (pcidma[0] & 0x8) { /* error */ 
+        ret = -1;
+    }
+
+    pcidma[0] |= 0xC; 
+    return ret;
+
+}
+
+int grpci_dma_from_pci(
+	unsigned int ahb_addr,
+	unsigned int pci_addr,
+	unsigned int len)
+{
+    int ret = 0;
+
+    pcidma[0] = 0x80;
+    pcidma[1] = ahb_addr;
+    pcidma[2] = pci_addr;
+    pcidma[3] = len;
+    pcidma[0] = 0x81;        
+
+    while ( (pcidma[0] & 0x4) == 0)
+        ;
+
+    if (pcidma[0] & 0x8) { /* error */ 
+        ret = -1;
+    }
+
+    pcidma[0] |= 0xC; 
+    return ret;
+
+}
diff --git a/bsps/shared/grlib/pci/grpci2.c b/bsps/shared/grlib/pci/grpci2.c
new file mode 100644
index 0000000000..21c09f47a5
--- /dev/null
+++ b/bsps/shared/grlib/pci/grpci2.c
@@ -0,0 +1,970 @@
+/*  GRLIB GRPCI2 PCI HOST driver.
+ * 
+ *  COPYRIGHT (c) 2011
+ *  Cobham Gaisler AB.
+ *
+ *  The license and distribution terms for this file may be
+ *  found in found in the file LICENSE in this distribution or at
+ *  http://www.rtems.org/license/LICENSE.
+ */
+
+/* Configures the GRPCI2 core and initialize,
+ *  - the PCI Library (pci.c)
+ *  - the general part of the PCI Bus driver (pci_bus.c)
+ *  
+ * System interrupt assigned to PCI interrupt (INTA#..INTD#) is by
+ * default taken from Plug and Play, but may be overridden by the 
+ * driver resources INTA#..INTD#. GRPCI2 handles differently depending
+ * on the design (4 different ways).
+ *
+ * GRPCI2 IRQ implementation notes
+ * -------------------------------
+ * Since the Driver Manager pci_bus layer implements IRQ by calling
+ * pci_interrupt_* which translates into BSP_shared_interrupt_*, and the
+ * root-bus also relies on BSP_shared_interrupt_*, it is safe for the GRPCI2
+ * driver to use the drvmgr_interrupt_* routines since they will be
+ * accessing the same routines in the end. Otherwise the GRPCI2 driver must
+ * have used the pci_interrupt_* routines.
+ */
+
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+#include <rtems.h>
+#include <rtems/bspIo.h>
+#include <libcpu/byteorder.h>
+#include <libcpu/access.h>
+#include <pci.h>
+#include <pci/cfg.h>
+
+#include <drvmgr/drvmgr.h>
+#include <grlib/ambapp_bus.h>
+#include <grlib/ambapp.h>
+#include <drvmgr/pci_bus.h>
+#include <grlib/grpci2.h>
+
+#include <grlib/grlib_impl.h>
+
+/* If defined to 1 - byte twisting is enabled by default */
+#define DEFAULT_BT_ENABLED 0
+
+/* If defined to 64 - Latency timer is 64 by default */
+#define DEFAULT_LATENCY_TIMER 64
+
+/* Interrupt assignment. Set to other value than 0xff in order to 
+ * override defaults and plug&play information
+ */
+#ifndef GRPCI2_INTA_SYSIRQ
+ #define GRPCI2_INTA_SYSIRQ 0xff
+#endif
+#ifndef GRPCI2_INTB_SYSIRQ
+ #define GRPCI2_INTB_SYSIRQ 0xff
+#endif
+#ifndef GRPCI2_INTC_SYSIRQ
+ #define GRPCI2_INTC_SYSIRQ 0xff
+#endif
+#ifndef GRPCI2_INTD_SYSIRQ
+ #define GRPCI2_INTD_SYSIRQ 0xff
+#endif
+
+/*#define DEBUG 1*/
+
+#ifdef DEBUG
+#define DBG(x...) printk(x)
+#else
+#define DBG(x...) 
+#endif
+
+/*
+ * GRPCI2 APB Register MAP
+ */
+struct grpci2_regs {
+	volatile unsigned int ctrl;		/* 0x00 */
+	volatile unsigned int sts_cap;		/* 0x04 */
+	volatile unsigned int ppref;		/* 0x08 */
+	volatile unsigned int io_map;		/* 0x0C */
+	volatile unsigned int dma_ctrl;		/* 0x10 */
+	volatile unsigned int dma_bdbase;	/* 0x14 */
+	volatile unsigned int dma_chact;	/* 0x18 */
+	int res1;				/* 0x1C */
+	volatile unsigned int bars[6];		/* 0x20 */
+	int res2[2];				/* 0x38 */
+	volatile unsigned int ahbmst_map[16];	/* 0x40 */
+};
+
+#define CTRL_BUS_BIT 16
+
+#define CTRL_SI (1<<27)
+#define CTRL_PE (1<<26)
+#define CTRL_ER (1<<25)
+#define CTRL_EI (1<<24)
+#define CTRL_BUS (0xff<<CTRL_BUS_BIT)
+#define CTRL_HOSTINT 0xf
+
+#define STS_HOST_BIT	31
+#define STS_MST_BIT	30
+#define STS_TAR_BIT	29
+#define STS_DMA_BIT	28
+#define STS_DI_BIT	27
+#define STS_HI_BIT	26
+#define STS_IRQMODE_BIT	24
+#define STS_TRACE_BIT	23
+#define STS_CFGERRVALID_BIT 20
+#define STS_CFGERR_BIT	19
+#define STS_INTTYPE_BIT	12
+#define STS_INTSTS_BIT	8
+#define STS_FDEPTH_BIT	2
+#define STS_FNUM_BIT	0
+
+#define STS_HOST	(1<<STS_HOST_BIT)
+#define STS_MST		(1<<STS_MST_BIT)
+#define STS_TAR		(1<<STS_TAR_BIT)
+#define STS_DMA		(1<<STS_DMA_BIT)
+#define STS_DI		(1<<STS_DI_BIT)
+#define STS_HI		(1<<STS_HI_BIT)
+#define STS_IRQMODE	(0x3<<STS_IRQMODE_BIT)
+#define STS_TRACE	(1<<STS_TRACE_BIT)
+#define STS_CFGERRVALID	(1<<STS_CFGERRVALID_BIT)
+#define STS_CFGERR	(1<<STS_CFGERR_BIT)
+#define STS_INTTYPE	(0x7f<<STS_INTTYPE_BIT)
+#define STS_INTSTS	(0xf<<STS_INTSTS_BIT)
+#define STS_FDEPTH	(0x7<<STS_FDEPTH_BIT)
+#define STS_FNUM	(0x3<<STS_FNUM_BIT)
+
+#define STS_ITIMEOUT	(1<<18)
+#define STS_ISYSERR	(1<<17)
+#define STS_IDMA	(1<<16)
+#define STS_IDMAERR	(1<<15)
+#define STS_IMSTABRT	(1<<14)
+#define STS_ITGTABRT	(1<<13)
+#define STS_IPARERR	(1<<12)
+
+/* GRPCI2 Capability */
+struct grpci2_cap_first {
+	unsigned int ctrl;
+	unsigned int pci2ahb_map[6];
+	unsigned int ext2ahb_map;
+	unsigned int io_map;
+	unsigned int pcibar_size[6];
+	unsigned int ahb_pref;
+};
+#define CAP9_CTRL_OFS 0
+#define CAP9_BAR_OFS 0x4
+#define CAP9_IOMAP_OFS 0x20
+#define CAP9_BARSIZE_OFS 0x24
+#define CAP9_AHBPREF_OFS 0x3C
+
+/* Used internally for accessing the PCI bridge's configuration space itself */
+#define HOST_TGT PCI_DEV(0xff, 0, 0)
+
+struct grpci2_priv *grpci2priv = NULL;
+
+/* PCI Interrupt assignment. Connects an PCI interrupt pin (INTA#..INTD#)
+ * to a system interrupt number.
+ */
+unsigned char grpci2_pci_irq_table[4] =
+{
+	/* INTA# */	GRPCI2_INTA_SYSIRQ,
+	/* INTB# */	GRPCI2_INTB_SYSIRQ,
+	/* INTC# */	GRPCI2_INTC_SYSIRQ,
+	/* INTD# */	GRPCI2_INTD_SYSIRQ
+};
+
+/* Start of workspace/dynamical area */
+extern unsigned int _end;
+#define DMA_START ((unsigned int) &_end)
+
+/* Default BAR mapping, set BAR0 256MB 1:1 mapped base of CPU RAM */
+struct grpci2_pcibar_cfg grpci2_default_bar_mapping[6] = {
+	/* BAR0 */ {DMA_START, DMA_START, 0x10000000},
+	/* BAR1 */ {0, 0, 0},
+	/* BAR2 */ {0, 0, 0},
+	/* BAR3 */ {0, 0, 0},
+	/* BAR4 */ {0, 0, 0},
+	/* BAR5 */ {0, 0, 0},
+};
+
+/* Driver private data struture */
+struct grpci2_priv {
+	struct drvmgr_dev		*dev;
+	struct grpci2_regs		*regs;
+	unsigned char			ver;
+	char				irq;
+	char				irq_mode; /* IRQ Mode from CAPSTS REG */
+	char				irq_dma; /* IRQ Index for DMA */
+	char				bt_enabled;
+	unsigned int			irq_mask;
+	unsigned int			latency_timer;
+
+	struct grpci2_pcibar_cfg	*barcfg;
+
+	unsigned int			pci_area;
+	unsigned int			pci_area_end;
+	unsigned int			pci_io;    
+	unsigned int			pci_conf;
+	unsigned int			pci_conf_end;
+
+	uint32_t			devVend; /* Host PCI Device/Vendor ID */
+
+	struct drvmgr_map_entry		maps_up[7];
+	struct drvmgr_map_entry		maps_down[2];
+	struct pcibus_config		config;
+
+	/* DMA interrupts */
+	void (*dma_isr)(void *data);
+	void *dma_isr_arg;
+
+	SPIN_DECLARE(devlock)
+};
+
+int grpci2_init1(struct drvmgr_dev *dev);
+int grpci2_init3(struct drvmgr_dev *dev);
+void grpci2_err_isr(void *arg);
+void grpci2_dma_isr(void *arg);
+
+/* GRPCI2 DRIVER */
+
+struct drvmgr_drv_ops grpci2_ops = 
+{
+	.init = {grpci2_init1, NULL, grpci2_init3, NULL},
+	.remove = NULL,
+	.info = NULL
+};
+
+struct amba_dev_id grpci2_ids[] = 
+{
+	{VENDOR_GAISLER, GAISLER_GRPCI2},
+	{0, 0}		/* Mark end of table */
+};
+
+struct amba_drv_info grpci2_info =
+{
+	{
+		DRVMGR_OBJ_DRV,                 /* Driver */
+		NULL,				/* Next driver */
+		NULL,				/* Device list */
+		DRIVER_AMBAPP_GAISLER_GRPCI2_ID,/* Driver ID */
+		"GRPCI2_DRV",			/* Driver Name */
+		DRVMGR_BUS_TYPE_AMBAPP,		/* Bus Type */
+		&grpci2_ops,
+		NULL,				/* Funcs */
+		0,				/* No devices yet */
+		sizeof(struct grpci2_priv),	/* Make drvmgr alloc private */
+	},
+	&grpci2_ids[0]
+};
+
+/* Defaults to do nothing - user can override this function
+ * by including the DMA DRIVER.
+ */
+int __attribute__((weak)) grpci2dma_init(void * regs, void isr_register( void (*isr)(void *), void * arg));
+
+int grpci2dma_init(void * regs, void isr_register( void (*isr)(void *), void * arg))
+{
+	return 0;
+}
+
+/* Prototype of grpci2_dma_isr_register function */
+static void grpci2_dma_isr_register( void (*isr)(void *), void * arg);
+
+void grpci2_register_drv(void)
+{
+	DBG("Registering GRPCI2 driver\n");
+	drvmgr_drv_register(&grpci2_info.general);
+}
+
+static int grpci2_cfg_r32(pci_dev_t dev, int ofs, uint32_t *val)
+{
+	struct grpci2_priv *priv = grpci2priv;
+	volatile uint32_t *pci_conf;
+	unsigned int tmp, devfn;
+	int retval, bus = PCI_DEV_BUS(dev);
+	SPIN_IRQFLAGS(irqflags);
+
+	if ((unsigned int)ofs & 0xffffff03) {
+		retval = PCISTS_EINVAL;
+		goto out2;
+	}
+
+	if (PCI_DEV_SLOT(dev) > 15) {
+		retval = PCISTS_MSTABRT;
+		goto out;
+	}
+
+	/* GRPCI2 can access "non-standard" devices on bus0 (on AD11.AD16), 
+	 * we skip them.
+	 */
+	if (dev == HOST_TGT)
+		bus = devfn = 0;
+	else if (bus == 0)
+		devfn = PCI_DEV_DEVFUNC(dev) + PCI_DEV(0, 6, 0);
+	else
+		devfn = PCI_DEV_DEVFUNC(dev);
+
+	pci_conf = (volatile uint32_t *) (priv->pci_conf | (devfn << 8) | ofs);
+
+	SPIN_LOCK_IRQ(&priv->devlock, irqflags);
+
+	/* Select bus */
+	priv->regs->ctrl = (priv->regs->ctrl & ~(0xff<<16)) | (bus<<16);
+	/* clear old status */
+	priv->regs->sts_cap = (STS_CFGERR | STS_CFGERRVALID);
+
+	tmp = *pci_conf;
+
+	/* Wait until GRPCI2 signals that CFG access is done, it should be
+	 * done instantaneously unless a DMA operation is ongoing...
+	 */
+	while ((priv->regs->sts_cap & STS_CFGERRVALID) == 0)
+		;
+
+	if (priv->regs->sts_cap & STS_CFGERR) {
+		retval = PCISTS_MSTABRT;
+	} else {
+		/* Bus always little endian (unaffected by byte-swapping) */
+		*val = CPU_swap_u32(tmp);
+		retval = PCISTS_OK;
+	}
+
+	SPIN_UNLOCK_IRQ(&priv->devlock, irqflags);
+
+out:
+	if (retval != PCISTS_OK)
+		*val = 0xffffffff;
+
+	DBG("pci_read: [%x:%x:%x] reg: 0x%x => addr: 0x%x, val: 0x%x  (%d)\n",
+		PCI_DEV_EXPAND(dev), ofs, pci_conf, *val, retval);
+
+out2:
+	return retval;
+}
+
+static int grpci2_cfg_r16(pci_dev_t dev, int ofs, uint16_t *val)
+{
+	uint32_t v;
+	int retval;
+
+	if (ofs & 1)
+		return PCISTS_EINVAL;
+
+	retval = grpci2_cfg_r32(dev, ofs & ~0x3, &v);
+	*val = 0xffff & (v >> (8*(ofs & 0x3)));
+
+	return retval;
+}
+
+static int grpci2_cfg_r8(pci_dev_t dev, int ofs, uint8_t *val)
+{
+	uint32_t v;
+	int retval;
+
+	retval = grpci2_cfg_r32(dev, ofs & ~0x3, &v);
+
+	*val = 0xff & (v >> (8*(ofs & 3)));
+
+	return retval;
+}
+
+static int grpci2_cfg_w32(pci_dev_t dev, int ofs, uint32_t val)
+{
+	struct grpci2_priv *priv = grpci2priv;
+	volatile uint32_t *pci_conf;
+	uint32_t value, devfn;
+	int retval, bus = PCI_DEV_BUS(dev);
+	SPIN_IRQFLAGS(irqflags);
+
+	if ((unsigned int)ofs & 0xffffff03)
+		return PCISTS_EINVAL;
+
+	if (PCI_DEV_SLOT(dev) > 15)
+		return PCISTS_MSTABRT;
+
+	value = CPU_swap_u32(val);
+
+	/* GRPCI2 can access "non-standard" devices on bus0 (on AD11.AD16), 
+	 * we skip them.
+	 */
+	if (dev == HOST_TGT)
+		bus = devfn = 0;
+	else if (bus == 0)
+		devfn = PCI_DEV_DEVFUNC(dev) + PCI_DEV(0, 6, 0);
+	else
+		devfn = PCI_DEV_DEVFUNC(dev);
+
+	pci_conf = (volatile uint32_t *) (priv->pci_conf | (devfn << 8) | ofs);
+
+	SPIN_LOCK_IRQ(&priv->devlock, irqflags);
+
+	/* Select bus */
+	priv->regs->ctrl = (priv->regs->ctrl & ~(0xff<<16)) | (bus<<16);
+	/* clear old status */
+	priv->regs->sts_cap = (STS_CFGERR | STS_CFGERRVALID);
+
+	*pci_conf = value;
+
+	/* Wait until GRPCI2 signals that CFG access is done, it should be
+	 * done instantaneously unless a DMA operation is ongoing...
+	 */
+	while ((priv->regs->sts_cap & STS_CFGERRVALID) == 0)
+		;
+
+	if (priv->regs->sts_cap & STS_CFGERR)
+		retval = PCISTS_MSTABRT;
+	else
+		retval = PCISTS_OK;
+
+	SPIN_UNLOCK_IRQ(&priv->devlock, irqflags);
+
+	DBG("pci_write - [%x:%x:%x] reg: 0x%x => addr: 0x%x, val: 0x%x  (%d)\n",
+		PCI_DEV_EXPAND(dev), ofs, pci_conf, value, retval);
+
+	return retval;
+}
+
+static int grpci2_cfg_w16(pci_dev_t dev, int ofs, uint16_t val)
+{
+	uint32_t v;
+	int retval;
+
+	if (ofs & 1)
+		return PCISTS_EINVAL;
+
+	retval = grpci2_cfg_r32(dev, ofs & ~0x3, &v);
+	if (retval != PCISTS_OK)
+		return retval;
+
+	v = (v & ~(0xffff << (8*(ofs&3)))) | ((0xffff&val) << (8*(ofs&3)));
+
+	return grpci2_cfg_w32(dev, ofs & ~0x3, v);
+}
+
+static int grpci2_cfg_w8(pci_dev_t dev, int ofs, uint8_t val)
+{
+	uint32_t v;
+	int retval;
+
+	retval = grpci2_cfg_r32(dev, ofs & ~0x3, &v);
+	if (retval != PCISTS_OK)
+		return retval;
+
+	v = (v & ~(0xff << (8*(ofs&3)))) | ((0xff&val) << (8*(ofs&3)));
+
+	return grpci2_cfg_w32(dev, ofs & ~0x3, v);
+}
+
+/* Return the assigned system IRQ number that corresponds to the PCI
+ * "Interrupt Pin" information from configuration space.
+ *
+ * The IRQ information is stored in the grpci2_pci_irq_table configurable
+ * by the user.
+ *
+ * Returns the "system IRQ" for the PCI INTA#..INTD# pin in irq_pin. Returns
+ * 0xff if not assigned.
+ */
+static uint8_t grpci2_bus0_irq_map(pci_dev_t dev, int irq_pin)
+{
+	uint8_t sysIrqNr = 0; /* not assigned */
+	int irq_group;
+
+	if ( (irq_pin >= 1) && (irq_pin <= 4) ) {
+		/* Use default IRQ decoding on PCI BUS0 according slot numbering */
+		irq_group = PCI_DEV_SLOT(dev) & 0x3;
+		irq_pin = ((irq_pin - 1) + irq_group) & 0x3;
+		/* Valid PCI "Interrupt Pin" number */
+		sysIrqNr = grpci2_pci_irq_table[irq_pin];
+	}
+	return sysIrqNr;
+}
+
+static int grpci2_translate(uint32_t *address, int type, int dir)
+{
+	uint32_t adr, start, end;
+	struct grpci2_priv *priv = grpci2priv;
+	int i;
+
+	if (type == 1) {
+		/* I/O */
+		if (dir != 0) {
+			/* The PCI bus can not access the CPU bus from I/O
+			 * because GRPCI2 core does not support I/O BARs
+			 */
+			return -1;
+		}
+
+		/* We have got a PCI IO BAR address that the CPU want to access.
+		 * Check that it is within the PCI I/O window, I/O adresses
+		 * are NOT mapped 1:1 with GRPCI2 driver... translation needed.
+		 */
+		adr = *(uint32_t *)address;
+		if (adr < 0x100 || adr > 0x10000)
+			return -1;
+		*address = adr + priv->pci_io;
+	} else {
+		/* MEMIO and MEM.
+		 * Memory space is mapped 1:1 so no translation is needed.
+		 * Check that address is within accessible windows.
+		 */
+		adr = *(uint32_t *)address;
+		if (dir == 0) {
+			/* PCI BAR to AMBA-CPU address.. check that it is
+			 * located within GRPCI2 PCI Memory Window
+			 * adr = PCI address.
+			 */
+			if (adr < priv->pci_area || adr >= priv->pci_area_end)
+				return -1;
+		} else {
+			/* We have a CPU address and want to get access to it
+			 * from PCI space, typically when doing DMA into CPU
+			 * RAM. The GRPCI2 core may have multiple target BARs
+			 * that PCI masters can access, the BARs are user
+			 * configurable in the following ways:
+			 *  BAR_SIZE, PCI_BAR Address and MAPPING (AMBA ADR)
+			 *
+			 * The below code tries to find a BAR for which the
+			 * AMBA bar may have been mapped onto, and translate
+			 * the AMBA-CPU address into a PCI address using the
+			 * given mapping.
+			 *
+			 * adr = AMBA address.
+			 */
+			for(i=0; i<6; i++) {
+				start = priv->barcfg[i].ahbadr;
+				end = priv->barcfg[i].ahbadr +
+					priv->barcfg[i].barsize;
+				if (adr >= start && adr < end) {
+					/* BAR match: Translate address */
+					*address = (adr - start) +
+						priv->barcfg[i].pciadr;
+					return 0;
+				}
+			}
+			return -1;
+		}
+	}
+
+	return 0;
+}
+
+extern struct pci_memreg_ops pci_memreg_sparc_le_ops;
+extern struct pci_memreg_ops pci_memreg_sparc_be_ops;
+
+/* GRPCI2 PCI access routines, default to Little-endian PCI Bus */
+struct pci_access_drv grpci2_access_drv = {
+	.cfg =
+	{
+		grpci2_cfg_r8,
+		grpci2_cfg_r16,
+		grpci2_cfg_r32,
+		grpci2_cfg_w8,
+		grpci2_cfg_w16,
+		grpci2_cfg_w32,
+	},
+	.io =
+	{
+		_ld8,
+		_ld_le16,
+		_ld_le32,
+		_st8,
+		_st_le16,
+		_st_le32,
+	},
+	.memreg = &pci_memreg_sparc_le_ops,
+	.translate = grpci2_translate,
+};
+
+struct pci_io_ops grpci2_io_ops_be =
+{
+	_ld8,
+	_ld_be16,
+	_ld_be32,
+	_st8,
+	_st_be16,
+	_st_be32,
+};
+
+/* PCI Error Interrupt handler, called when there may be a PCI Target/Master
+ * Abort.
+ */
+void grpci2_err_isr(void *arg)
+{
+	struct grpci2_priv *priv = arg;
+	unsigned int sts = priv->regs->sts_cap;
+
+	if (sts & (STS_IMSTABRT | STS_ITGTABRT | STS_IPARERR | STS_ISYSERR | STS_ITIMEOUT)) {
+		/* A PCI error IRQ ... Error handler unimplemented
+		 * add your code here...
+		 */
+		if (sts & STS_IMSTABRT) {
+			printk("GRPCI2: unhandled Master Abort IRQ\n");
+		}
+		if (sts & STS_ITGTABRT) {
+			printk("GRPCI2: unhandled Target Abort IRQ\n");
+		}
+		if (sts & STS_IPARERR) {
+			printk("GRPCI2: unhandled Parity Error IRQ\n");
+		}
+		if (sts & STS_ISYSERR) {
+			printk("GRPCI2: unhandled System Error IRQ\n");
+		}
+		if (sts & STS_ITIMEOUT) {
+			printk("GRPCI2: unhandled PCI target access timeout IRQ\n");
+		}
+	}
+}
+
+/* PCI DMA Interrupt handler, called when there may be a PCI DMA interrupt.
+ */
+void grpci2_dma_isr(void *arg)
+{
+	struct grpci2_priv *priv = arg;
+	unsigned int sts = (priv->regs->sts_cap & (STS_IDMAERR | STS_IDMA));
+
+	/* Clear Interrupt if taken*/
+	if (sts != 0){
+		/* Clear IDMAERR and IDMA bits */
+		priv->regs->sts_cap = (STS_IDMAERR | STS_IDMA);
+		/* Clear DRVMGR interrupt */
+		drvmgr_interrupt_clear(priv->dev, priv->irq_dma);
+		/* Call DMA driver ISR */
+		(priv->dma_isr)(priv->dma_isr_arg);
+	}
+}
+
+static int grpci2_hw_init(struct grpci2_priv *priv)
+{
+	struct grpci2_regs *regs = priv->regs;
+	int i;
+	uint8_t capptr;
+	uint32_t data, io_map, ahbadr, pciadr, size;
+	pci_dev_t host = HOST_TGT;
+	struct grpci2_pcibar_cfg *barcfg = priv->barcfg;
+
+	/* Reset any earlier setup */
+	regs->ctrl = 0;
+	regs->sts_cap = ~0; /* Clear Status */
+	regs->dma_ctrl = 0;
+	regs->dma_bdbase = 0;
+
+	/* Translate I/O accesses 1:1, (will not work for PCI 2.3) */
+	regs->io_map = priv->pci_io & 0xffff0000;
+
+	/* set 1:1 mapping between AHB -> PCI memory space, for all Masters
+	 * Each AHB master has it's own mapping registers. Max 16 AHB masters.
+	 */
+	for (i=0; i<16; i++)
+		regs->ahbmst_map[i] = priv->pci_area;
+
+	/* Get the GRPCI2 Host PCI ID */
+	grpci2_cfg_r32(host, PCIR_VENDOR, &priv->devVend);
+
+	/* Get address to first (always defined) capability structure */
+	grpci2_cfg_r8(host, PCIR_CAP_PTR, &capptr);
+	if (capptr == 0)
+		return -1;
+
+	/* Limit the prefetch for GRPCI2 version 0. */
+	if (priv->ver == 0)
+		grpci2_cfg_w32(host, capptr+CAP9_AHBPREF_OFS, 0);
+
+	/* Enable/Disable Byte twisting */
+	grpci2_cfg_r32(host, capptr+CAP9_IOMAP_OFS, &io_map);
+	io_map = (io_map & ~0x1) | (priv->bt_enabled ? 1 : 0);
+	grpci2_cfg_w32(host, capptr+CAP9_IOMAP_OFS, io_map);
+
+	/* Setup the Host's PCI Target BARs for others to access (DMA) */
+	for (i=0; i<6; i++) {
+		/* Make sure address is properly aligned */
+		size = ~(barcfg[i].barsize-1);
+		barcfg[i].pciadr &= size;
+		barcfg[i].ahbadr &= size;
+
+		pciadr = barcfg[i].pciadr;
+		ahbadr = barcfg[i].ahbadr;
+		size |= PCIM_BAR_MEM_PREFETCH;
+
+		grpci2_cfg_w32(host, capptr+CAP9_BARSIZE_OFS+i*4, size);
+		grpci2_cfg_w32(host, capptr+CAP9_BAR_OFS+i*4, ahbadr);
+		grpci2_cfg_w32(host, PCIR_BAR(0)+i*4, pciadr);
+	}
+
+	/* set as bus master and enable pci memory responses */  
+	grpci2_cfg_r32(host, PCIR_COMMAND, &data);
+	data |= (PCIM_CMD_MEMEN | PCIM_CMD_BUSMASTEREN);
+	grpci2_cfg_w32(host, PCIR_COMMAND, data);
+
+	/* set latency timer */
+	grpci2_cfg_r32(host, PCIR_CACHELNSZ, &data);
+	data &= ~0xff00;
+	data |= ((priv->latency_timer & 0xff) << 8);
+	grpci2_cfg_w32(host, PCIR_CACHELNSZ, data);
+
+	/* Enable Error respone (CPU-TRAP) on illegal memory access */
+	regs->ctrl = CTRL_ER | CTRL_PE;
+
+	/* Successful */
+	return 0;
+}
+
+/* Initializes the GRPCI2 core and driver, must be called before calling
+ * init_pci()
+ *
+ * Return values
+ *  0             Successful initalization
+ *  -1            Error during initialization, for example "PCI core not found".
+ *  -2            Error PCI controller not HOST (targets not supported)
+ *  -3            Error due to GRPCI2 hardware initialization
+ */
+static int grpci2_init(struct grpci2_priv *priv)
+{
+	struct ambapp_apb_info *apb;
+	struct ambapp_ahb_info *ahb;
+	int pin, i, j;
+	union drvmgr_key_value *value;
+	char keyname[6];
+	struct amba_dev_info *ainfo = priv->dev->businfo;
+	struct grpci2_pcibar_cfg *barcfg;
+	unsigned int size;
+
+	/* Find PCI core from Plug&Play information */
+	apb = ainfo->info.apb_slv;
+	ahb = ainfo->info.ahb_slv;
+
+	/* Found PCI core, init private structure */
+	priv->irq = apb->irq;
+	priv->ver = apb->ver;
+	priv->regs = (struct grpci2_regs *)apb->start;
+	priv->bt_enabled = DEFAULT_BT_ENABLED;
+	priv->irq_mode = (priv->regs->sts_cap & STS_IRQMODE) >> STS_IRQMODE_BIT;
+	priv->latency_timer = DEFAULT_LATENCY_TIMER;
+
+	/* Initialize Spin-lock for GRPCI2 Device. */
+	SPIN_INIT(&priv->devlock, "grpci2");
+
+	/* Calculate the PCI windows 
+	 *  AMBA->PCI Window:                       AHB SLAVE AREA0
+	 *  AMBA->PCI I/O cycles Window:            AHB SLAVE AREA1 Lower half
+	 *  AMBA->PCI Configuration cycles Window:  AHB SLAVE AREA1 Upper half
+	 */
+	priv->pci_area     = ahb->start[0];
+	priv->pci_area_end = ahb->start[0] + ahb->mask[0];
+	priv->pci_io       = ahb->start[1];
+	priv->pci_conf     = ahb->start[1] + 0x10000;
+	priv->pci_conf_end = priv->pci_conf + 0x10000;
+
+	/* On systems where PCI I/O area and configuration area is apart of the
+	 * "PCI Window" the PCI Window stops at the start of the PCI I/O area
+	 */
+	if ((priv->pci_io > priv->pci_area) &&
+	    (priv->pci_io < (priv->pci_area_end-1))) {
+		priv->pci_area_end = priv->pci_io;
+	}
+
+	/* Init PCI interrupt assignment table to all use the interrupt routed
+	 * through the GRPCI2 core.
+	 */
+	strcpy(keyname, "INTX#");
+	for (pin=1; pin<5; pin++) {
+		if (grpci2_pci_irq_table[pin-1] == 0xff) {
+			if (priv->irq_mode < 2) {
+				/* PCI Interrupts are shared */
+				grpci2_pci_irq_table[pin-1] = priv->irq;
+			} else {
+				/* Unique IRQ per PCI INT Pin */
+				grpci2_pci_irq_table[pin-1] = priv->irq + pin-1;
+			}
+
+			/* User may override Both hardcoded IRQ setup and Plug & Play IRQ */
+			keyname[3] = 'A' + (pin-1);
+			value = drvmgr_dev_key_get(priv->dev, keyname, DRVMGR_KT_INT);
+			if (value)
+				grpci2_pci_irq_table[pin-1] = value->i;
+		}
+
+		/* Remember which IRQs are enabled */
+		if (grpci2_pci_irq_table[pin-1] != 0)
+			priv->irq_mask |= 1 << (pin-1);
+	}
+
+	/* User may override DEFAULT_BT_ENABLED to enable/disable byte twisting */
+	value = drvmgr_dev_key_get(priv->dev, "byteTwisting", DRVMGR_KT_INT);
+	if (value)
+		priv->bt_enabled = value->i;
+
+	/* Let user Configure the 6 target BARs */
+	value = drvmgr_dev_key_get(priv->dev, "tgtBarCfg", DRVMGR_KT_POINTER);
+	if (value)
+		priv->barcfg = value->ptr;
+	else
+		priv->barcfg = grpci2_default_bar_mapping;
+
+	/* User may override DEFAULT_LATENCY_TIMER */
+	value = drvmgr_dev_key_get(priv->dev, "latencyTimer", DRVMGR_KT_INT);
+	if (value)
+		priv->latency_timer = value->i;
+
+	/* This driver only support HOST systems, we check that it can act as a 
+	 * PCI Master and that it is in the Host slot. */
+	if ((priv->regs->sts_cap&STS_HOST) || !(priv->regs->sts_cap&STS_MST))
+		return -2; /* Target not supported */
+
+	/* Init the PCI Core */
+	if (grpci2_hw_init(priv))
+		return -3;
+
+	/* Down streams translation table */
+	priv->maps_down[0].name = "AMBA -> PCI MEM Window";
+	priv->maps_down[0].size = priv->pci_area_end - priv->pci_area;
+	priv->maps_down[0].from_adr = (void *)priv->pci_area;
+	priv->maps_down[0].to_adr = (void *)priv->pci_area;
+	/* End table */
+	priv->maps_down[1].size = 0;
+
+	/* Up streams translation table */
+	/* Setup the Host's PCI Target BARs for others to access (DMA) */
+	barcfg = priv->barcfg;
+	for (i=0,j=0; i<6; i++) {
+		size = barcfg[i].barsize;
+		if (size == 0)
+			continue;
+
+		/* Make sure address is properly aligned */
+		priv->maps_up[j].name = "Target BAR[I] -> AMBA";
+		priv->maps_up[j].size = size;
+		priv->maps_up[j].from_adr = (void *)
+					(barcfg[i].pciadr & ~(size - 1));
+		priv->maps_up[j].to_adr = (void *)
+					(barcfg[i].ahbadr & ~(size - 1));
+		j++;
+	}
+
+	/* End table */
+	priv->maps_up[j].size = 0;
+
+	return 0;
+}
+
+/* Called when a core is found with the AMBA device and vendor ID 
+ * given in grpci2_ids[]. IRQ, Console does not work here
+ */
+int grpci2_init1(struct drvmgr_dev *dev)
+{
+	int status;
+	struct grpci2_priv *priv;
+	struct pci_auto_setup grpci2_auto_cfg;
+
+	DBG("GRPCI2[%d] on bus %s\n", dev->minor_drv, dev->parent->dev->name);
+
+	if (grpci2priv) {
+		DBG("Driver only supports one PCI core\n");
+		return DRVMGR_FAIL;
+	}
+
+	if ((strcmp(dev->parent->dev->drv->name, "AMBAPP_GRLIB_DRV") != 0) &&
+	    (strcmp(dev->parent->dev->drv->name, "AMBAPP_LEON2_DRV") != 0)) {
+		/* We only support GRPCI2 driver on local bus */
+		return DRVMGR_FAIL;
+	}
+
+	priv = dev->priv;
+	if (!priv)
+		return DRVMGR_NOMEM;
+
+	priv->dev = dev;
+	grpci2priv = priv;
+
+	/* Initialize GRPCI2 Hardware */
+	status = grpci2_init(priv);
+	if (status) {
+		printk("Failed to initialize grpci2 driver %d\n", status);
+		return -1;
+	}
+
+	/* Register the PCI core at the PCI layers */
+
+	if (priv->bt_enabled == 0) {
+		/* Host is Big-Endian */
+		pci_endian = PCI_BIG_ENDIAN;
+
+		memcpy(&grpci2_access_drv.io, &grpci2_io_ops_be,
+						sizeof(grpci2_io_ops_be));
+		grpci2_access_drv.memreg = &pci_memreg_sparc_be_ops;
+	}
+
+	if (pci_access_drv_register(&grpci2_access_drv)) {
+		/* Access routines registration failed */
+		return DRVMGR_FAIL;
+	}
+
+	/* Prepare memory MAP */
+	grpci2_auto_cfg.options = 0;
+	grpci2_auto_cfg.mem_start = 0;
+	grpci2_auto_cfg.mem_size = 0;
+	grpci2_auto_cfg.memio_start = priv->pci_area;
+	grpci2_auto_cfg.memio_size = priv->pci_area_end - priv->pci_area;
+	grpci2_auto_cfg.io_start = 0x100; /* avoid PCI address 0 */
+	grpci2_auto_cfg.io_size = 0x10000 - 0x100; /* lower 64kB I/O 16 */
+	grpci2_auto_cfg.irq_map = grpci2_bus0_irq_map;
+	grpci2_auto_cfg.irq_route = NULL; /* use standard routing */
+	pci_config_register(&grpci2_auto_cfg);
+
+	if (pci_config_init()) {
+		/* PCI configuration failed */
+		return DRVMGR_FAIL;
+	}
+
+	/* Initialize/Register Driver Manager PCI Bus */
+	priv->config.maps_down = &priv->maps_down[0];
+	priv->config.maps_up = &priv->maps_up[0];
+	return pcibus_register(dev, &priv->config);
+}
+
+int grpci2_init3(struct drvmgr_dev *dev)
+{
+	struct grpci2_priv *priv = dev->priv;
+
+	/* Install and Enable PCI Error interrupt handler */
+	drvmgr_interrupt_register(dev, 0, "grpci2", grpci2_err_isr, priv);
+
+	/* Initialize DMA driver (if supported) */
+	if (priv->regs->sts_cap & STS_DMA){
+		grpci2dma_init((void *) &(priv->regs->dma_ctrl), grpci2_dma_isr_register);
+	}
+
+	/* Unmask Error IRQ and all PCI interrupts at PCI Core. For this to be
+	 * safe every PCI board have to be resetted (no IRQ generation) before
+	 * Global IRQs are enabled (Init is reached or similar)
+	 */
+	priv->regs->ctrl |= (CTRL_EI | priv->irq_mask);
+
+	return DRVMGR_OK;
+}
+
+static void grpci2_dma_isr_register( void (*isr)(void *), void * arg)
+{
+	struct grpci2_priv *priv = grpci2priv;
+
+	/* Handle unregistration */
+	if (priv->dma_isr != NULL) {
+		drvmgr_interrupt_unregister(priv->dev, priv->irq_dma, grpci2_dma_isr, priv);
+		/* Uninstall user ISR */
+		priv->dma_isr = NULL;
+		priv->dma_isr_arg = NULL;
+	}
+
+	if (isr == NULL)
+		return;
+
+	/* Install user ISR */
+	priv->dma_isr_arg = arg;
+	priv->dma_isr = isr;
+
+	/* Install and Enable PCI DMA interrupt handler */
+	if (priv->irq_mode == 1) {
+		priv->irq_dma = 1;
+	} else if (priv->irq_mode == 3) {
+		priv->irq_dma = 4;
+	} else {
+		priv->irq_dma = 0;
+	}
+	drvmgr_interrupt_register(priv->dev, priv->irq_dma, "grpci2dma", grpci2_dma_isr, priv);
+}
diff --git a/bsps/shared/grlib/pci/grpci2dma.c b/bsps/shared/grlib/pci/grpci2dma.c
new file mode 100644
index 0000000000..7e39ca691d
--- /dev/null
+++ b/bsps/shared/grlib/pci/grpci2dma.c
@@ -0,0 +1,2026 @@
+/*
+ *  GRPCI2 DMA Driver
+ *
+ *  COPYRIGHT (c) 2017
+ *  Cobham Gaisler AB
+ *
+ *  The license and distribution terms for this file may be
+ *  found in the file LICENSE in this distribution or at
+ *  http://www.rtems.org/license/LICENSE.
+ */
+
+#include <stdlib.h>
+#include <string.h>
+#include <stddef.h>
+#include <drvmgr/drvmgr.h>
+#include <rtems.h>
+#include <rtems/bspIo.h>  /* for printk */
+#include <bsp.h>
+#include <grlib/grpci2dma.h>
+
+#include <grlib/grlib_impl.h>
+
+/* This driver has been prepared for SMP operation
+ */
+
+/*#define STATIC*/
+#define STATIC static
+
+/*#define INLINE*/
+#define INLINE inline
+
+/*#define UNUSED*/
+#define UNUSED __attribute__((unused))
+
+/*#define DEBUG 1*/
+
+#ifdef DEBUG
+#define DBG(x...) printf(x)
+#else
+#define DBG(x...) 
+#endif
+
+#define BD_CHAN_EN (1<<BD_CHAN_EN_BIT)
+#define BD_CHAN_ID (0x3<<BD_CHAN_ID_BIT)
+#define BD_CHAN_TYPE (0x3<<BD_CHAN_TYPE_BIT)
+#define BD_CHAN_TYPE_DMA (0x1<<BD_CHAN_TYPE_BIT)
+#define BD_CHAN_BDCNT (0xffff<<BD_CHAN_BDCNT_BIT)
+#define BD_CHAN_EN_BIT 31
+#define BD_CHAN_ID_BIT 22
+#define BD_CHAN_TYPE_BIT 20
+#define BD_CHAN_BDCNT_BIT 0
+
+#define BD_DATA_EN (0x1<<BD_DATA_EN_BIT)
+#define BD_DATA_IE (0x1<<BD_DATA_IE_BIT)
+#define BD_DATA_DR (0x1<<BD_DATA_DR_BIT)
+#define BD_DATA_BE (0x1<<BD_DATA_BE_BIT)
+#define BD_DATA_TYPE (0x3<<BD_DATA_TYPE_BIT)
+#define BD_DATA_TYPE_DATA (0x0<<BD_DATA_TYPE_BIT)
+#define BD_DATA_ER (0x1<<BD_DATA_ER_BIT)
+#define BD_DATA_LEN (0xffff<<BD_DATA_LEN_BIT)
+#define BD_DATA_EN_BIT 31
+#define BD_DATA_IE_BIT 30
+#define BD_DATA_DR_BIT 29
+#define BD_DATA_BE_BIT 28
+#define BD_DATA_TYPE_BIT 20
+#define BD_DATA_ER_BIT 19
+#define BD_DATA_LEN_BIT 0
+
+#define DMACTRL_SAFE (0x1<<DMACTRL_SAFE_BIT)
+#define DMACTRL_WCLEAR (0x1fff<<DMACTRL_ERR_BIT)
+#define DMACTRL_ERR (0x1f<<DMACTRL_ERR_BIT)
+#define DMACTRL_CHIRQ (0xff<<DMACTRL_CHIRQ_BIT)
+#define DMACTRL_ERR (0x1f<<DMACTRL_ERR_BIT)
+#define DMACTRL_NUMCH (0x7<<DMACTRL_NUMCH_BIT)
+#define DMACTRL_DIS (0x1<<DMACTRL_DIS_BIT)
+#define DMACTRL_IE (0x1<<DMACTRL_IE_BIT)
+#define DMACTRL_ACT (0x1<<DMACTRL_ACT_BIT)
+#define DMACTRL_EN (0x1<<DMACTRL_EN_BIT)
+
+#define DMACTRL_SAFE_BIT 31
+#define DMACTRL_CHIRQ_BIT 12
+#define DMACTRL_ERR_BIT 7
+#define DMACTRL_NUMCH_BIT 4
+#define DMACTRL_DIS_BIT 2
+#define DMACTRL_IE_BIT 1
+#define DMACTRL_ACT_BIT 3
+#define DMACTRL_EN_BIT 0
+
+/* GRPCI2 DMA does not allow more than 8 DMA chans */
+#define MAX_DMA_CHANS 8
+
+/* GRPCI2 DMA does not allow transfer of more than 0x10000 words */
+#define MAX_DMA_TRANSFER_SIZE (0x10000*4)
+
+/* We use the following limits as default */ 
+#define MAX_DMA_DATA 128
+
+/* Memory and HW Registers Access routines. All 32-bit access routines */
+#define BD_WRITE(addr, val) (*(volatile unsigned int *)(addr) = (unsigned int)(val))
+/*#define BD_READ(addr) (*(volatile unsigned int *)(addr))*/
+#define BD_READ(addr) leon_r32_no_cache((unsigned long)(addr))
+#define REG_WRITE(addr, val) (*(volatile unsigned int *)(addr) = (unsigned int)(val))
+#define REG_READ(addr) (*(volatile unsigned int *)(addr))
+
+/*
+ * GRPCI2 DMA Channel descriptor
+ */
+struct grpci2_bd_chan {
+	volatile unsigned int ctrl;	/* 0x00 DMA Control */
+	volatile unsigned int nchan;	/* 0x04 Next DMA Channel Address */
+	volatile unsigned int nbd;	/* 0x08 Next Data Descriptor in channel */
+	volatile unsigned int res;	/* 0x0C Reserved */
+};
+
+/*
+ * GRPCI2 DMA Data descriptor
+ */
+struct grpci2_bd_data {
+	volatile unsigned int ctrl;	/* 0x00 DMA Data Control */
+	volatile unsigned int pci_adr;	/* 0x04 PCI Start Address */
+	volatile unsigned int ahb_adr;	/* 0x08 AHB Start address */
+	volatile unsigned int next;	/* 0x0C Next Data Descriptor in channel */
+};
+
+
+/*
+ * GRPCI2 DMA APB Register MAP
+ */
+struct grpci2dma_regs {
+	volatile unsigned int dma_ctrl;		/* 0x00 */
+	volatile unsigned int dma_bdbase;	/* 0x04 */
+	volatile unsigned int dma_chact;	/* 0x08 */
+};
+
+#define DEVNAME_LEN 11
+/*
+ * GRPCI2 DMA Driver private data struture
+ */
+struct grpci2dma_priv {
+	/* DMA control registers */
+	struct grpci2dma_regs	*regs;
+	char devname[DEVNAME_LEN];
+
+	/* Channel info */
+	struct {
+		/* Channel pointer. Indicates the assigned channel 
+		 * for a given cid (used as index). NULL if not assigned.
+		 */
+		struct grpci2_bd_chan * ptr;
+		/* Is this channel allocated by the driver */
+		int allocated;
+		/* Last added data descriptor for each channel.
+		 * This simplifies/speeds up adding data descriptors
+		 * to the channel*/
+		struct grpci2_bd_data * lastdata;
+		/* Is this channel active */
+		int active;
+		/* Interrupt-code Handling 
+		 * - isr: Holds the ISR for each channel
+		 * - isr_arg: Holds the ISR arg for each channel
+		 */
+		grpci2dma_isr_t isr;
+		void * isr_arg;
+
+		/* DMA Channel Semaphore */
+		rtems_id sem;
+	} channel[MAX_DMA_CHANS];
+
+	/* Indicates the number of channels. */
+	int nchans;
+
+	/* Indicates the number of active channels. */
+	int nactive;
+
+	/* Indicates if the number of DMA ISR that have been registered 
+	 * into the GRPCI2 DRIVER */
+	int isr_registered;
+
+	/* Callback to register the DMA ISR into the GRPCI2 DRIVER */
+	void (*isr_register)( void (*isr)(void*), void * arg);
+
+	/* Spin-lock ISR protection */
+	SPIN_DECLARE(devlock);
+};
+
+/* The GRPCI2 DMA semaphore */
+rtems_id grpci2dma_sem;
+
+/*
+ * GRPCI2 DMA internal prototypes 
+ */
+/* -Descriptor linked-list functions*/
+STATIC int grpci2dma_channel_list_add(struct grpci2_bd_chan * list, 
+		struct grpci2_bd_chan * chan);
+STATIC int grpci2dma_channel_list_remove(struct grpci2_bd_chan * chan);
+STATIC int grpci2dma_data_list_add(struct grpci2_bd_chan * chan, 
+		struct grpci2_bd_data * data, struct grpci2_bd_data * last_chan_data);
+STATIC int grpci2dma_data_list_remove(struct grpci2_bd_chan * chan, 
+		struct grpci2_bd_data * data);
+STATIC int grpci2dma_channel_list_foreach(struct grpci2_bd_chan * chan, 
+		int func( struct grpci2_bd_chan * chan), int maxindex);
+STATIC int grpci2dma_data_list_foreach(struct grpci2_bd_data * data, 
+		int func( struct grpci2_bd_data * data), int maxindex);
+
+/* -DMA ctrl access functions */
+STATIC INLINE int grpci2dma_ctrl_init(void);
+STATIC INLINE int grpci2dma_ctrl_start(struct grpci2_bd_chan * chan);
+STATIC INLINE int grpci2dma_ctrl_stop(void);
+STATIC INLINE int grpci2dma_ctrl_resume(void);
+STATIC INLINE unsigned int grpci2dma_ctrl_status(void);
+STATIC INLINE unsigned int grpci2dma_ctrl_base(void);
+STATIC INLINE unsigned int grpci2dma_ctrl_active(void);
+STATIC INLINE int grpci2dma_ctrl_numch_set(int numch);
+STATIC INLINE int grpci2dma_ctrl_interrupt_status(void);
+STATIC INLINE int grpci2dma_ctrl_interrupt_enable(void);
+STATIC INLINE int grpci2dma_ctrl_interrupt_disable(void);
+STATIC INLINE int grpci2dma_ctrl_interrupt_clear(void);
+
+/* -Descriptor access functions */
+STATIC int grpci2dma_channel_bd_init(struct grpci2_bd_chan * chan);
+STATIC int grpci2dma_data_bd_init(struct grpci2_bd_data * data, 
+		uint32_t pci_adr, uint32_t ahb_adr, int dir, int endianness, 
+		int size, struct grpci2_bd_data * next);
+STATIC int grpci2dma_channel_bd_enable(struct grpci2_bd_chan * chan, 
+		unsigned int options);
+STATIC int grpci2dma_channel_bd_disable(struct grpci2_bd_chan * chan);
+STATIC void grpci2dma_channel_bd_set_cid(struct grpci2_bd_chan * chan, 
+		int cid);
+STATIC int grpci2dma_channel_bd_get_cid(struct grpci2_bd_chan * chan);
+STATIC int grpci2dma_data_bd_status(struct grpci2_bd_data *data);
+STATIC int grpci2dma_data_bd_disable(struct grpci2_bd_data *desc);
+STATIC int grpci2dma_data_bd_interrupt_enable(struct grpci2_bd_data * data);
+STATIC struct grpci2_bd_data * grpci2dma_channel_bd_get_data(
+		struct grpci2_bd_chan * chan);
+STATIC void grpci2dma_channel_bd_set_data(struct grpci2_bd_chan * chan, 
+		struct grpci2_bd_data * data);
+STATIC struct grpci2_bd_chan * grpci2dma_channel_bd_get_next(
+		struct grpci2_bd_chan * chan);
+STATIC struct grpci2_bd_data * grpci2dma_data_bd_get_next(
+		struct grpci2_bd_data * data);
+STATIC void grpci2dma_channel_bd_set_next(struct grpci2_bd_chan * chan, 
+		struct grpci2_bd_chan * next);
+STATIC void grpci2dma_data_bd_set_next(struct grpci2_bd_data * data, 
+		struct grpci2_bd_data * next);
+
+/* -Channel functions */
+STATIC int grpci2dma_channel_open(struct grpci2_bd_chan * chan, int cid);
+STATIC int grpci2dma_channel_free_id(void);
+STATIC struct grpci2_bd_chan * grpci2dma_channel_get_active_list(void);
+STATIC int grpci2dma_channel_start(int chan_no, int options);
+STATIC int grpci2dma_channel_stop(int chan_no);
+STATIC int grpci2dma_channel_push(int chan_no, void *dataptr, int index, 
+		int ndata);
+STATIC int grpci2dma_channel_close(int chan_no);
+STATIC int grpci2dma_channel_isr_unregister(int chan_no);
+
+/* -ISR functions*/
+STATIC void grpci2dma_isr(void *arg);
+
+/* -Init function called by GRPCI2*/
+int grpci2dma_init(void * regs, 
+		void isr_register( void (*isr)(void*), void * arg));
+
+
+#ifdef DEBUG
+STATIC int grpci2dma_channel_print(struct grpci2_bd_chan * chan);
+STATIC int grpci2dma_data_print(struct grpci2_bd_data * data);
+#endif
+
+static struct grpci2dma_priv *grpci2dmapriv = NULL;
+
+/* All data linked list must point to a disabled descriptor at the end.
+ * We use this DISABLED_DESCRIPTOR as a list end for all channels.
+ */
+#define ALIGNED __attribute__((aligned(GRPCI2DMA_BD_DATA_ALIGN)))
+static ALIGNED struct grpci2_bd_data disabled_data = {
+	/*.ctrl=*/0,
+	/*.pci_adr=*/0,
+	/*.ahb_adr=*/0,
+	/*.next=*/0
+};
+#define DISABLED_DESCRIPTOR (&disabled_data)
+
+/*** START OF DESCRIPTOR LINKED-LIST HELPER FUNCTIONS ***/
+
+/* This functions adds a channel descriptor to the DMA channel 
+ * linked list. It assumes that someone has check the input 
+ * parameters already.
+ */
+STATIC int grpci2dma_channel_list_add(struct grpci2_bd_chan * list, 
+		struct grpci2_bd_chan * chan)
+{
+	DBG("Adding channel (0x%08x) to GRPCI2 DMA driver\n", (unsigned int) chan);
+
+	/* Add channel to the linnked list */
+	if (list == chan) {
+		/* No previous channels. Finish. */
+		return GRPCI2DMA_ERR_OK;
+	} else {
+		/* Get next chan from list */
+		struct grpci2_bd_chan * nchan = grpci2dma_channel_bd_get_next(list);
+		/* Close the circular linked list */
+		grpci2dma_channel_bd_set_next(chan,nchan);
+		/* Attach the new channel in the middle */
+		grpci2dma_channel_bd_set_next(list, chan);
+		return GRPCI2DMA_ERR_OK;
+	}
+}
+
+/* This functions removes a channel descriptor from the DMA channel 
+ * linked list. It assumes that someone has check the input 
+ * parameters already.
+ * It returns 0 if successfull. Otherwise,
+ * it can return:
+ * - ERROR: Different causes:
+ *	  x Number of channels is corrupted.
+ */
+STATIC int grpci2dma_channel_list_remove(struct grpci2_bd_chan * chan)
+{
+	DBG("Removing channel (0x%08x) from GRPCI2 DMA driver\n",
+			(unsigned int) chan);
+
+	/* Remove channel from the linnked list */
+	struct grpci2_bd_chan * nchan = grpci2dma_channel_bd_get_next(chan);
+	if (nchan != chan){
+		/* There are more channels */
+		/* Since this is a circular linked list, we need to find last channel 
+		 * and update the pointer to the next element */
+		/* Use index to avoid having an infinite loop in case of corrupted 
+		 * channels */
+		struct grpci2_bd_chan * new_first_chan = nchan;
+		struct grpci2_bd_chan * curr_chan;
+		int i=1;
+		while((nchan != chan) && (i<MAX_DMA_CHANS)){
+			curr_chan = nchan;
+			nchan = grpci2dma_channel_bd_get_next(curr_chan);
+			i++;
+		}
+		if (nchan != chan) {
+			DBG("Maximum DMA channels exceeded. Maybe corrupted?\n");
+			return GRPCI2DMA_ERR_ERROR;
+		} else {
+			/* Update the pointer */
+			grpci2dma_channel_bd_set_next(curr_chan, new_first_chan);
+			return GRPCI2DMA_ERR_OK;
+		}
+	}else{
+		/* There are no more channels */
+		return GRPCI2DMA_ERR_OK;
+	}
+}
+
+/* This functions adds a data descriptor to the channel's data 
+ * linked list. The function assumes, that the data descriptor
+ * points to either a DISABLED_DESCRIPTOR or to linked list of
+ * data descriptors that ends with a DISABLED_DESCRIPTOR. 
+ * It returns the number of active data descriptors
+ * if successfull. Otherwise, it can return:
+ * - ERROR: Different causes:
+ *	  x Number of channels is corrupted.
+ *	  x Last linked list element is not pointing to the first.
+ */
+STATIC int grpci2dma_data_list_add(struct grpci2_bd_chan * chan, 
+		struct grpci2_bd_data * data, struct grpci2_bd_data * last_chan_data)
+{
+	DBG("Adding data (0x%08x) to channel (0x%08x)\n", 
+			(unsigned int) data, (unsigned int) chan);
+
+	/* Add data to the linnked list */
+	/* 1st- Get current data */
+	struct grpci2_bd_data * first_data = grpci2dma_channel_bd_get_data(chan);
+	if (first_data == NULL) {
+		/* Channel should always be pointing to a disabled descriptor */
+		DBG("Channel not pointing to disabled descpriptor\n");
+		return GRPCI2DMA_ERR_ERROR;
+	} else if (first_data == DISABLED_DESCRIPTOR){
+		/* No previous data. Assign this one and finish. */
+		grpci2dma_channel_bd_set_data(chan, data);
+		return GRPCI2DMA_ERR_OK;
+	} else {
+		/* Let's add the data to the last data pointer added to this channel */
+		/* Attach the new data */
+		grpci2dma_data_bd_set_next(last_chan_data, data);
+		/* 2nd- Let's check again to make sure that the DMA did not finished
+		 * while we were inserting the new data */
+		first_data = grpci2dma_channel_bd_get_data(chan);
+		if (first_data == DISABLED_DESCRIPTOR){
+			grpci2dma_channel_bd_set_data(chan, data);
+		}
+		return GRPCI2DMA_ERR_OK;
+	}
+}
+
+/* This functions removes a data descriptor from the channel's data 
+ * linked list. Note that in a normal execution, the DMA will remove 
+ * the data descriptors from the linked list, so there is no need to
+ * use this function. It returns 0 if successfull. Otherwise, 
+ * it can return:
+ * - WRONGPTR: The chan (or data) pointer is either NULL or not aligned to
+ *	 0x10.
+ * - STOPDMA: The DMA is running, cannot add channels while DMA is running.
+ * - TOOMANY: The max number of data is reached.
+ * - ERROR: Different causes:
+ *	  x There are no free channel id numbers.
+ *	  x Number of channels is corrupted.
+ *	  x Last linked list element is not pointing to the first.
+ */
+UNUSED STATIC int grpci2dma_data_list_remove(struct grpci2_bd_chan * chan, 
+		struct grpci2_bd_data * data)
+{
+	DBG("Removing data (0x%08x) from channel (0x%08x)\n", 
+			(unsigned int) data, (unsigned int) chan);
+
+	/* Remove data from the linked list */
+	/* 1st- Get current DMA data */
+	struct grpci2_bd_data * first_data = grpci2dma_channel_bd_get_data(chan);
+	if (first_data == NULL) {
+		/* Channel should always be pointing to a disabled descriptor */
+		DBG("Channel not pointing to disabled descpriptor\n");
+		return GRPCI2DMA_ERR_ERROR;
+	} else if (first_data == DISABLED_DESCRIPTOR){
+		/* No previous data. Cannot detach */
+		DBG("No data to detach.\n");
+		return GRPCI2DMA_ERR_NOTFOUND;
+	} else {
+		/* 2nd- Already available data, let's find the data */
+		if (first_data == data) {
+			/* 3rd- It is the first one. */
+			struct grpci2_bd_data *current = first_data;
+			struct grpci2_bd_data *next = grpci2dma_data_bd_get_next(current);
+			if (next != DISABLED_DESCRIPTOR){
+				/* There are more data */
+				/* Set channel next data descriptor to data*/
+				grpci2dma_channel_bd_set_data(chan, next);
+				/* Update the removed data */
+				grpci2dma_data_bd_set_next(data, DISABLED_DESCRIPTOR);
+				return GRPCI2DMA_ERR_OK;
+			}else{
+				/* No more data */
+				/* Clear DMA NBD */
+				grpci2dma_channel_bd_set_data(chan, DISABLED_DESCRIPTOR);
+				/* Update the removed data */
+				grpci2dma_data_bd_set_next(data, DISABLED_DESCRIPTOR);
+				return GRPCI2DMA_ERR_OK;
+			}
+		} else {
+			/* It is not the first data. Let's find it */
+			struct grpci2_bd_data * current = first_data;
+			struct grpci2_bd_data * next = grpci2dma_data_bd_get_next(current);
+			while( (next != data) && (next != DISABLED_DESCRIPTOR) && 
+					(next != NULL)){
+				current = next;
+				next = grpci2dma_data_bd_get_next(current);
+			}
+			if (next != data) {
+				DBG("Maximum DMA data exceeded. Maybe corrupted?\n");
+				return GRPCI2DMA_ERR_NOTFOUND;
+			} else {
+				/* Detach the data */
+				next = grpci2dma_data_bd_get_next(data);
+				grpci2dma_data_bd_set_next(current, next);
+				/* Update the removed data */
+				grpci2dma_data_bd_set_next(data, DISABLED_DESCRIPTOR);
+				return GRPCI2DMA_ERR_OK;
+			}
+		}
+	}
+}
+
+/* Iterate through all channel starting in FIRST_CHAN up to MAXINDEX 
+ * and execute FUNC*/
+UNUSED STATIC int grpci2dma_channel_list_foreach(
+		struct grpci2_bd_chan * first_chan, 
+		int func( struct grpci2_bd_chan * chan), int maxindex)
+{
+	if (maxindex <= 0) return 0;
+	if (first_chan == NULL) {
+		/* No previous channels */
+		return 0;
+	} else {
+		/* Available channels */
+		/* Iterate through next channels */
+		/* Use index to avoid having an infinite loop in case of corrupted
+		 * channels */
+		int i=0;
+		int ret;
+		struct grpci2_bd_chan * curr_chan = first_chan;
+		struct grpci2_bd_chan * nchan;
+		do{
+			if (curr_chan == NULL) return GRPCI2DMA_ERR_WRONGPTR;
+			ret = func(curr_chan);
+			if (ret < 0){
+				/* error */
+				return ret;
+			}
+			nchan = grpci2dma_channel_bd_get_next(curr_chan);
+			curr_chan = nchan;
+			i++;
+		}while((curr_chan != first_chan) && (i < maxindex));
+	}
+	return 0;
+}
+
+/* Iterate through all data starting in FIRST_DATA up to MAXINDEX 
+ * and execute FUNC*/
+STATIC int grpci2dma_data_list_foreach(struct grpci2_bd_data * first_data, 
+		int func( struct grpci2_bd_data * data), int maxindex)
+{
+	if (maxindex <= 0) return 0;
+	if (first_data == NULL) return GRPCI2DMA_ERR_WRONGPTR;
+	/* Available data */
+	/* Iterate through next data */
+	/* Use index to avoid having an infinite loop in case of corrupted 
+	 * channels */
+	int i=0;
+	int ret;
+	struct grpci2_bd_data * curr_data = first_data;
+	struct grpci2_bd_data * ndata;
+	while((curr_data != DISABLED_DESCRIPTOR) && (i < maxindex)){
+		if (curr_data == NULL) return GRPCI2DMA_ERR_WRONGPTR;
+		ret = func(curr_data);
+		if (ret < 0){
+			/* error */
+			return ret;
+		}
+		ndata = grpci2dma_data_bd_get_next(curr_data);
+		curr_data = ndata;
+		i++;
+	}
+	return 0;
+}
+
+
+/*** END OF DESCRIPTOR LINKED-LIST HELPER FUNCTIONS ***/
+
+/*** START OF DMACTRL ACCESS FUNCTIONS ***/
+
+/* Initialize the DMA Ctrl*/
+STATIC INLINE int grpci2dma_ctrl_init()
+{
+	struct grpci2dma_priv *priv = grpci2dmapriv;
+
+	/* Clear DMA Control: clear IRQ and ERR status */
+	REG_WRITE(&priv->regs->dma_ctrl, 0|DMACTRL_SAFE|DMACTRL_CHIRQ|DMACTRL_ERR);
+
+	/* Clear DMA BASE */
+	REG_WRITE(&priv->regs->dma_bdbase, 0);
+
+	/* Clear DMA Chan */
+	REG_WRITE(&priv->regs->dma_chact, 0);
+
+	return 0;
+}
+
+
+/* Stop the DMA */
+STATIC INLINE int grpci2dma_ctrl_stop( void )
+{
+	struct grpci2dma_priv *priv = grpci2dmapriv;
+
+	/* Stop DMA */
+	unsigned int ctrl = REG_READ(&priv->regs->dma_ctrl);
+	REG_WRITE(&priv->regs->dma_ctrl, (ctrl & ~(DMACTRL_WCLEAR | DMACTRL_EN)) |
+			DMACTRL_DIS);
+
+	return 0;
+}
+
+/* Start the DMA */
+STATIC INLINE int grpci2dma_ctrl_start( struct grpci2_bd_chan * chan)
+{
+	struct grpci2dma_priv *priv = grpci2dmapriv;
+
+	/* Set BDBASE to linked list of chans */
+	REG_WRITE(&priv->regs->dma_bdbase, (unsigned int) chan);
+
+	/* Start DMA */
+	unsigned int ctrl = REG_READ(&priv->regs->dma_ctrl);
+	REG_WRITE(&priv->regs->dma_ctrl, (ctrl & ~(DMACTRL_WCLEAR | DMACTRL_DIS)) |
+			DMACTRL_EN);
+
+	return 0;
+}
+
+/* Resume the DMA */
+STATIC INLINE int grpci2dma_ctrl_resume( void )
+{
+	struct grpci2dma_priv *priv = grpci2dmapriv;
+
+	/* Resume DMA */
+	unsigned int ctrl = REG_READ(&priv->regs->dma_ctrl);
+	REG_WRITE(&priv->regs->dma_ctrl, (ctrl & ~(DMACTRL_WCLEAR | DMACTRL_DIS)) |
+			DMACTRL_EN);
+
+	return 0;
+}
+
+/* Interrupt status*/
+STATIC INLINE int grpci2dma_ctrl_interrupt_status(void)
+{
+	struct grpci2dma_priv *priv = grpci2dmapriv;
+
+	unsigned int ctrl = REG_READ(&priv->regs->dma_ctrl);
+	return (ctrl & DMACTRL_IE);
+}
+
+/* Enable interrupts */
+STATIC INLINE int grpci2dma_ctrl_interrupt_enable(void)
+{
+	struct grpci2dma_priv *priv = grpci2dmapriv;
+
+	unsigned int ctrl = REG_READ(&priv->regs->dma_ctrl);
+	if (ctrl & DMACTRL_IE){
+		/* Nothing to do. Already enabled */
+		return 0;
+	}
+
+	/* Clear pending CHIRQ and errors */
+	ctrl = ctrl | (DMACTRL_CHIRQ | DMACTRL_ERR);
+
+	/* Enable interrupts */
+	ctrl = ctrl | DMACTRL_IE;
+
+	REG_WRITE(&priv->regs->dma_ctrl, ctrl );
+	return 0;
+}
+
+/* Disable interrupts */
+STATIC INLINE int grpci2dma_ctrl_interrupt_disable(void)
+{
+	struct grpci2dma_priv *priv = grpci2dmapriv;
+
+	unsigned int ctrl = REG_READ(&priv->regs->dma_ctrl);
+	if ((ctrl & DMACTRL_IE) == 0){
+		/* Nothing to do. Already disabled */
+		return 0;
+	}
+
+	/* Clear pending CHIRQ and errors */
+	ctrl = ctrl | (DMACTRL_CHIRQ | DMACTRL_ERR);
+
+	/* Disable interrupts */
+	ctrl = ctrl & ~(DMACTRL_IE);
+
+	REG_WRITE(&priv->regs->dma_ctrl, ctrl );
+	return 0;
+}
+
+/* Clear interrupts */
+STATIC INLINE int grpci2dma_ctrl_interrupt_clear(void)
+{
+	struct grpci2dma_priv *priv = grpci2dmapriv;
+
+	unsigned int ctrl = REG_READ(&priv->regs->dma_ctrl);
+	REG_WRITE(&priv->regs->dma_ctrl, (ctrl | DMACTRL_ERR | DMACTRL_CHIRQ));
+	return 0;
+}
+
+STATIC INLINE unsigned int grpci2dma_ctrl_status()
+{
+	struct grpci2dma_priv *priv = grpci2dmapriv;
+
+	/* Read DMA */
+	return (REG_READ(&priv->regs->dma_ctrl));
+}
+
+STATIC INLINE unsigned int grpci2dma_ctrl_base()
+{
+	struct grpci2dma_priv *priv = grpci2dmapriv;
+
+	/* Read DMA */
+	return (REG_READ(&priv->regs->dma_bdbase));
+}
+
+UNUSED STATIC INLINE unsigned int grpci2dma_ctrl_active()
+{
+	struct grpci2dma_priv *priv = grpci2dmapriv;
+
+	/* Read DMA */
+	return (REG_READ(&priv->regs->dma_chact));
+}
+
+/* Set the DMA CTRL register NUMCH field */
+STATIC INLINE int grpci2dma_ctrl_numch_set(int numch)
+{
+	struct grpci2dma_priv *priv = grpci2dmapriv;
+
+	unsigned int ctrl = REG_READ(&priv->regs->dma_ctrl);
+
+	/* Clear old value */
+	ctrl = (ctrl & ~(DMACTRL_NUMCH));
+
+	/* Put new value */
+	ctrl = (ctrl | ( (numch << DMACTRL_NUMCH_BIT) & DMACTRL_NUMCH));
+
+	REG_WRITE(&priv->regs->dma_ctrl, ctrl & ~(DMACTRL_WCLEAR));
+	return 0;
+}
+
+/*** END OF DMACTRL ACCESS FUNCTIONS ***/
+
+/*** START OF DESCRIPTOR ACCESS FUNCTIONS ***/
+
+STATIC int grpci2dma_data_bd_init(struct grpci2_bd_data * data, 
+		uint32_t pci_adr, uint32_t ahb_adr, int dir, int endianness, int size,
+		struct grpci2_bd_data * next)
+{
+	BD_WRITE(&data->ctrl, 0 |
+			(BD_DATA_EN) |
+			(BD_DATA_TYPE_DATA) |
+			(dir == GRPCI2DMA_AHBTOPCI? BD_DATA_DR:0) |
+			(endianness == GRPCI2DMA_LITTLEENDIAN? BD_DATA_BE:0) |
+			( (size << BD_DATA_LEN_BIT) & BD_DATA_LEN )
+			);
+	BD_WRITE(&data->pci_adr, pci_adr);
+	BD_WRITE(&data->ahb_adr, ahb_adr);
+	BD_WRITE(&data->next, (unsigned int) next);
+	return 0;
+}
+
+STATIC int grpci2dma_channel_bd_init(struct grpci2_bd_chan * chan)
+{
+	BD_WRITE(&chan->ctrl, 0 | BD_CHAN_TYPE_DMA | BD_CHAN_EN);
+	BD_WRITE(&chan->nchan, (unsigned int) chan);
+	BD_WRITE(&chan->nbd, (unsigned int) DISABLED_DESCRIPTOR);
+	return 0;
+}
+
+/* Enable a channel with options.
+ * options include:
+ * - options & 0xFFFF: Maximum data descriptor count before 
+ *	 moving to next DMA channel.
+ */
+STATIC int grpci2dma_channel_bd_enable(struct grpci2_bd_chan * chan, 
+		unsigned int options)
+{
+	unsigned int ctrl = BD_READ(&chan->ctrl);
+	ctrl = (ctrl & ~(BD_CHAN_BDCNT));
+	BD_WRITE(&chan->ctrl, (ctrl | BD_CHAN_EN | 
+				( (options << BD_CHAN_BDCNT_BIT) & BD_CHAN_BDCNT)));
+	return 0;
+}
+
+/* Disable channel.
+ */
+STATIC int grpci2dma_channel_bd_disable(struct grpci2_bd_chan * chan)
+{
+	unsigned int ctrl = BD_READ(&chan->ctrl);
+	BD_WRITE(&chan->ctrl, (ctrl & ~(BD_CHAN_EN)));
+	return 0;
+}
+
+/* Get the CID of a channel.
+ */
+UNUSED STATIC int grpci2dma_channel_bd_get_cid(struct grpci2_bd_chan * chan)
+{
+	/* Get cid from chan */
+	unsigned ctrl = BD_READ(&chan->ctrl);
+	unsigned cid  = (ctrl & (BD_CHAN_ID)) >> BD_CHAN_ID_BIT;
+	return cid;
+}
+
+/* Set the CID of a channel. */
+STATIC void grpci2dma_channel_bd_set_cid(struct grpci2_bd_chan * chan, int cid)
+{
+	/* Set cid from chan */
+	unsigned ctrl = BD_READ(&chan->ctrl);
+	ctrl = (ctrl & ~(BD_CHAN_ID)) | ((cid << BD_CHAN_ID_BIT) & BD_CHAN_ID);
+	BD_WRITE(&chan->ctrl,ctrl);
+	return;
+}
+
+/* Disable data descriptor*/
+UNUSED STATIC int grpci2dma_data_bd_disable(struct grpci2_bd_data *desc)
+{
+	BD_WRITE(&desc->ctrl,0);
+	return 0;
+}
+
+/* Return status of data descriptor*/
+STATIC int grpci2dma_data_bd_status(struct grpci2_bd_data *desc)
+{
+	int status = BD_READ(&desc->ctrl);
+	if (status & BD_DATA_ER) {
+		return GRPCI2DMA_BD_STATUS_ERR;
+	}else if (status & BD_DATA_EN) {
+		return GRPCI2DMA_BD_STATUS_ENABLED;
+	}else {
+		return GRPCI2DMA_BD_STATUS_DISABLED;
+	}
+	return GRPCI2DMA_BD_STATUS_ERR;
+}
+
+/* Enable interrupts in data descriptor*/
+STATIC int grpci2dma_data_bd_interrupt_enable(struct grpci2_bd_data * data)
+{
+	unsigned int ctrl = BD_READ(&data->ctrl);
+	BD_WRITE(&data->ctrl, ctrl | BD_DATA_IE);
+	return 0;
+}
+
+/* Get data descriptor */
+STATIC struct grpci2_bd_data * grpci2dma_channel_bd_get_data(
+		struct grpci2_bd_chan * chan)
+{
+	return	(struct grpci2_bd_data *) BD_READ(&chan->nbd);
+}
+
+/* Set data descriptorl */
+STATIC void grpci2dma_channel_bd_set_data(struct grpci2_bd_chan * chan, 
+		struct grpci2_bd_data * data)
+{
+	BD_WRITE(&chan->nbd, (unsigned int) data);
+}
+
+/* Get next channel */
+STATIC struct grpci2_bd_chan * grpci2dma_channel_bd_get_next(
+		struct grpci2_bd_chan * chan)
+{
+	return	(struct grpci2_bd_chan *) BD_READ(&chan->nchan);
+}
+
+/* Get next data */
+STATIC struct grpci2_bd_data * grpci2dma_data_bd_get_next(
+		struct grpci2_bd_data * data)
+{
+	return	(struct grpci2_bd_data *) BD_READ(&data->next);
+}
+
+/* Set next channel */
+STATIC void grpci2dma_channel_bd_set_next(struct grpci2_bd_chan * chan, 
+		struct grpci2_bd_chan * next)
+{
+	BD_WRITE(&chan->nchan,(unsigned int) next);
+}
+
+/* Set next data */
+STATIC void grpci2dma_data_bd_set_next(struct grpci2_bd_data * data, 
+		struct grpci2_bd_data * next)
+{
+	BD_WRITE(&data->next,(unsigned int) next);
+}
+
+/*** END OF DESCRIPTOR ACCESS FUNCTIONS ***/
+
+/*** START OF CHANNEL FUNCTIONS ***/
+
+STATIC int grpci2dma_channel_open(struct grpci2_bd_chan * chan, int cid)
+{
+	struct grpci2dma_priv *priv = grpci2dmapriv;
+	int allocated = 0;
+
+	/* Get pointer */
+	if (chan == NULL) {
+		/* User does not provide channel, let's create it */
+		chan = grpci2dma_channel_new(1);
+		allocated = 1;
+	}else{
+		/* Make sure the pointer is not already on the linked list */
+		int i;
+		for (i=0; i<MAX_DMA_CHANS; i++){
+			if (priv->channel[i].ptr == chan){
+				return GRPCI2DMA_ERR_WRONGPTR;
+			}
+		}
+	}
+
+	DBG("Opening channel %d (0x%08x)\n", cid, (unsigned int) chan);
+
+	/* Init channel descriptor */
+	grpci2dma_channel_bd_init(chan);
+
+	/* Assign cid to chan */
+	priv->channel[cid].ptr = chan;
+	grpci2dma_channel_bd_set_cid(chan, cid);
+
+	/* Increase number of channels */
+	priv->nchans++;
+
+	DBG("number of channels: %d\n", priv->nchans);
+
+	/* Initialize channel data */
+	priv->channel[cid].allocated = allocated;
+	priv->channel[cid].active = 0;
+
+	/* Initialize record of last added data */
+	priv->channel[cid].lastdata = DISABLED_DESCRIPTOR;
+
+	return cid;
+}
+
+/* Get first free CID.
+ */
+STATIC int grpci2dma_channel_free_id()
+{
+	struct grpci2dma_priv *priv = grpci2dmapriv;
+
+	/* Find the first free CID */
+	int i;
+	for (i=0; i<MAX_DMA_CHANS; i++){
+		if (priv->channel[i].ptr == NULL){
+			return i;
+		}
+	}
+	return GRPCI2DMA_ERR_TOOMANY;
+}
+
+/* Get the active channel circular linked list */
+STATIC struct grpci2_bd_chan * grpci2dma_channel_get_active_list()
+{
+	struct grpci2dma_priv *priv = grpci2dmapriv;
+	int i;
+	/* Just get the first non NULL associated cid */
+	for (i=0; i< MAX_DMA_CHANS; i++){
+		if ((priv->channel[i].ptr != NULL) && (priv->channel[i].active)){
+			return priv->channel[i].ptr;
+		}
+	}
+	return NULL;
+}
+
+/* Start a channel */
+STATIC int grpci2dma_channel_start(int chan_no, int options)
+{
+	struct grpci2dma_priv *priv = grpci2dmapriv;
+	struct grpci2_bd_chan *chan;
+	SPIN_IRQFLAGS(irqflags);
+
+	/* Get chan pointer */
+	chan = priv->channel[chan_no].ptr;
+
+	/* Check if channel is active */
+	if (priv->channel[chan_no].active){
+		/* nothing to do */
+		return GRPCI2DMA_ERR_OK;
+	}
+
+	/* Get the max descriptor count */
+	unsigned int desccnt;
+	if (options == 0){
+		/* Default */
+		desccnt = 0xffff;
+	}else{
+		desccnt = options & 0xffff;
+	}
+
+	/* Start the channel by enabling it.
+	 * HWNOTE: In GRPCI2 this bit does not work as it is supposed.
+	 * So we better add/remove the channel from the active linked 
+	 * list. */
+	grpci2dma_channel_bd_enable(chan, desccnt);
+	priv->channel[chan_no].active = 1;
+	priv->nactive++;
+	/* Get active linked list */
+	struct grpci2_bd_chan * list = grpci2dma_channel_get_active_list();
+	if (list == NULL){
+		/* No previous channels. New list */
+		list = chan;
+	}
+	/* Add channel from the linked list */
+	if (grpci2dma_channel_list_add(list, chan) < 0){
+		return GRPCI2DMA_ERR_ERROR;
+	}
+
+	/* Increase NUMCH in DMA ctrl */
+	SPIN_LOCK_IRQ(&priv->devlock, irqflags);
+	grpci2dma_ctrl_numch_set( (priv->nactive? priv->nactive -1:0));
+
+	/* Check if DMA is active */
+	if (!grpci2dma_active()){
+		/* Start DMA */
+		grpci2dma_ctrl_start(chan);
+	}
+	SPIN_UNLOCK_IRQ(&priv->devlock, irqflags);
+
+	DBG("Channel %d started (0x%08x)\n", chan_no, (unsigned int) chan);
+
+	return GRPCI2DMA_ERR_OK;
+}
+
+/* Stop a channel */
+STATIC int grpci2dma_channel_stop(int chan_no)
+{
+	struct grpci2dma_priv *priv = grpci2dmapriv;
+	struct grpci2_bd_chan *chan;
+	SPIN_IRQFLAGS(irqflags);
+	int resume;
+
+	/* Get chan pointer */
+	chan = priv->channel[chan_no].ptr;
+
+	/* Check if channel is active */
+	if (!priv->channel[chan_no].active){
+		/* nothing to do */
+		return GRPCI2DMA_ERR_OK;
+	}
+
+	/* First remove channel from the linked list */
+	if (grpci2dma_channel_list_remove(chan) < 0){
+		return GRPCI2DMA_ERR_ERROR;
+	}
+
+	/* Update driver struct */
+	priv->channel[chan_no].active = 0;
+	priv->nactive--;
+
+	/* Check if DMA is active and it the removed
+	 * channel is the active */
+	resume = 0;
+	SPIN_LOCK_IRQ(&priv->devlock, irqflags);
+	if (grpci2dma_active() && (grpci2dma_ctrl_active() == (unsigned int)chan)){
+		/* We need to stop the DMA */
+		grpci2dma_ctrl_stop();
+		SPIN_UNLOCK_IRQ(&priv->devlock, irqflags);
+		/* Wait until DMA stops */
+		while (grpci2dma_active()){}
+		/* We need to check later to resume the DMA */
+		resume = 1;
+	}else{
+		SPIN_UNLOCK_IRQ(&priv->devlock, irqflags);
+	}
+
+
+	/* Now either the DMA is stopped, or it is processing 
+	 * a different channel and the removed channel is no
+	 * longer in the linked list */
+
+	/* Now is safe to update the removed channel */
+	grpci2dma_channel_bd_set_next(chan, chan);
+
+	/* Stop the channel by disabling it. 
+	 * HWNOTE: In GRPCI2 this bit does not work as it is supposed.
+	 * So we better remove the channel from the active linked 
+	 * list. */
+	grpci2dma_channel_bd_disable(chan);
+
+	/* Point channel to disabled descriptor */
+	grpci2dma_channel_bd_set_data(chan, DISABLED_DESCRIPTOR);
+
+	DBG("Channel %d stoped (0x%08x)\n", chan_no, (unsigned int) chan);
+
+	/* Decrease NUMCH in DMA ctrl */
+	SPIN_LOCK_IRQ(&priv->devlock, irqflags);
+	grpci2dma_ctrl_numch_set( (priv->nactive? priv->nactive -1:0));
+
+	/* Reactivate DMA only if we stopped */
+	if (resume){
+		/* We have two options, either we stopped when the active
+		 * channel was still the active one, or we stopped when
+		 * the active channel was a different one */
+		if (grpci2dma_ctrl_active() == (unsigned int) chan){
+			/* In this case, we need to start the DMA with
+			 * any active channel on the list */
+			int i;
+			for (i=0; i<MAX_DMA_CHANS; i++){
+				if (priv->channel[i].active){
+					grpci2dma_ctrl_start(priv->channel[i].ptr);
+					break;
+				}
+			}
+		}else{
+			/* In this case, we need to resume the DMA operation */
+			/* HWNOTE: The GRPCI2 core does not update the channel next 
+			 * data descriptor if we stopped a channel. This means that
+			 * we need to resume the DMA from the descriptor is was,
+			 * by only setting the enable bit, and not changing the 
+			 * base register */
+			grpci2dma_ctrl_resume();
+		}
+	}
+	SPIN_UNLOCK_IRQ(&priv->devlock, irqflags);
+
+	return GRPCI2DMA_ERR_OK;
+}
+
+STATIC int grpci2dma_channel_push(int chan_no, void *dataptr, int index,
+		int ndata)
+{
+	struct grpci2dma_priv *priv = grpci2dmapriv;
+	struct grpci2_bd_chan * chan;
+	struct grpci2_bd_data * data = dataptr;
+
+	/* Get channel */
+	chan = priv->channel[chan_no].ptr;
+
+	DBG("Pushing %d data (starting at 0x%08x) to channel %d (0x%08x)\n",
+			ndata, (unsigned int) &data[index], chan_no, (unsigned int) chan);
+
+	/* Get last added data */
+	struct grpci2_bd_data * last_added = priv->channel[chan_no].lastdata;
+
+	/* Add data to channel */
+	grpci2dma_data_list_add(chan, &data[index], last_added);
+
+	/* Update last added */
+	priv->channel[chan_no].lastdata = &data[index + ndata-1];
+
+	return GRPCI2DMA_ERR_OK;
+}
+
+STATIC int grpci2dma_channel_close(int chan_no)
+{
+	struct grpci2dma_priv *priv = grpci2dmapriv;
+	struct grpci2_bd_chan * chan;
+
+	/* Get channel */
+	chan = priv->channel[chan_no].ptr;
+
+	DBG("Closing channel %d (0x%08x)\n", chan_no, (unsigned int) chan);
+
+	/* Stop channel */
+	if (grpci2dma_channel_stop(chan_no) != GRPCI2DMA_ERR_OK ){
+		DBG("Cannot stop channel!.\n");
+		return GRPCI2DMA_ERR_STOPDMA;
+	}
+
+	/* Unregister channel ISR */
+	grpci2dma_channel_isr_unregister(chan_no);
+
+	/* Free the cid */
+	priv->channel[chan_no].ptr = NULL;
+
+	/* Remove the ISR */
+	priv->channel[chan_no].isr = NULL;
+	priv->channel[chan_no].isr_arg = NULL;
+
+	/* Deallocate channel if needed */
+	if (priv->channel[chan_no].allocated){
+		grpci2dma_channel_delete((void *)chan);
+	}
+
+	/* Decrease number of channels */
+	priv->nchans--;
+
+	DBG("number of channels: %d\n", priv->nchans);
+
+	/* Everything OK */
+	return GRPCI2DMA_ERR_OK;
+}
+
+/* Register channel ISR */
+STATIC int grpci2dma_channel_isr_unregister(int chan_no)
+{
+	struct grpci2dma_priv *priv = grpci2dmapriv;
+	SPIN_IRQFLAGS(irqflags);
+
+	/* Unregister channel ISR */
+	priv->channel[chan_no].isr = NULL;
+	priv->channel[chan_no].isr_arg = NULL;
+
+	/* Unregister DMA ISR in GRPCI2 if needed */
+	priv->isr_registered--;
+	if(priv->isr_registered == 0){
+		/* Disable DMA Interrupts */
+		SPIN_LOCK_IRQ(&priv->devlock, irqflags);
+		grpci2dma_ctrl_interrupt_disable();
+		SPIN_UNLOCK_IRQ(&priv->devlock, irqflags);
+		(priv->isr_register)( NULL, NULL);
+	}
+
+	/* Everything OK */
+	return GRPCI2DMA_ERR_OK;
+}
+
+/*** END OF CHANNEL FUNCTIONS ***/
+
+/*** START OF ISR FUNCTIONS ***/
+
+/* PCI DMA Interrupt handler, called when there is a PCI DMA interrupt */
+STATIC void grpci2dma_isr(void *arg)
+{
+	struct grpci2dma_priv *priv = arg;
+	SPIN_ISR_IRQFLAGS(irqflags);
+	unsigned int ctrl = grpci2dma_ctrl_status();
+	/* Clear Interrupts */
+	SPIN_LOCK(&priv->devlock, irqflags);
+	grpci2dma_ctrl_interrupt_clear();
+	SPIN_UNLOCK(&priv->devlock, irqflags);
+	unsigned int sts = (ctrl & DMACTRL_CHIRQ) >> DMACTRL_CHIRQ_BIT;
+	unsigned int errsts = (ctrl & DMACTRL_ERR);
+
+	/* Error interrupt */
+	if(errsts){
+		/* Find which channels had the error.
+		 * The GRPCI2DMA core does not indicate which channel
+		 * had the error, so we need to get 1st the base descriptor register
+		 * and see if it a channel. If is not a channel, then the active 
+		 * channel register tells us which channel is.
+		 * After having the channel we need to find out which channel was. */
+		struct grpci2_bd_chan * chan =
+			(struct grpci2_bd_chan *) grpci2dma_ctrl_base();
+		/* Check if the base is a channel descriptor */
+		if ((BD_READ(&chan->ctrl) & BD_CHAN_TYPE) != BD_CHAN_TYPE_DMA){
+			/* Is not a channel, so the channel is in the channel active 
+			 * register */
+			chan = (struct grpci2_bd_chan *) grpci2dma_ctrl_active();
+		}
+		int i;
+		for (i=0; i<MAX_DMA_CHANS; i++){
+			if (chan == priv->channel[i].ptr){
+				/* Found */
+				if (priv->channel[i].isr != NULL){
+					(priv->channel[i].isr)(priv->channel[i].isr_arg,i,errsts);
+				}else{
+					printk("Unhandled GRPCI2 DMA error interrupt, sts:0x%02x\n", errsts);
+				}
+				break;
+			}
+		}
+		if (i == MAX_DMA_CHANS){
+			printk("Unhandled GRPCI2 DMA error interrupt , sts:0x%02x\n", errsts);
+		}
+	}
+
+	/* Normal packet interrupt */
+	int cid=0;
+	/* Find which channels have interrupts */
+	while(sts){
+		/* Find if current channel has an interrupt*/
+		if(sts & 0x1){
+			/* Find if current channel has an ISR */
+			if (priv->channel[cid].isr != NULL){
+				(priv->channel[cid].isr)(
+						priv->channel[cid].isr_arg, cid, errsts);
+			}else{
+				printk("Unhandled GRPCI2 DMA interrupt in channel %d, sts:0x%02x\n", cid, 0);
+			}
+		}
+		/* Next channel */
+		sts = sts >> 1;
+		cid++;
+	}
+}
+
+/*** END OF ISR FUNCTIONS ***/
+
+/*** START OF DEBUG HELPERS ***/
+#ifdef DEBUG
+STATIC int grpci2dma_channel_print(struct grpci2_bd_chan * chan)
+{
+	printf("  GRPCI2 DMA channel descriptor\n");
+	printf("	  0x%08x  DMA channel control					  0x%08x\n", (unsigned int) chan, chan->ctrl);
+	printf("	  31	 en				   0x%01x		  Channel descriptor enable.\n", (chan->ctrl >> 31) & (0x1));
+	printf("	  24:22  cid			   0x%01x		  Channel ID.\n", (chan->ctrl >> 22) & (0x7));
+	printf("	  21:20  type			   0x%01x		  Descriptor type. 01=DMA channel descriptor.\n", (chan->ctrl >> 20) & (0x3)); 
+	printf("	  15:0	 dlen			0x%04x		   Data descriptor count.\n", (chan->ctrl >> 0) & (0xffff));
+	printf("\n");
+	printf("	  0x%08x  Next DMA channel						  0x%08x\n", (unsigned int) &(chan->nchan), chan->nchan);
+	printf("	  31:0	 nc			0x%08x		   Next DMA channel.\n", chan->nchan);
+	printf("\n");
+	printf("	  0x%08x  Next data descriptor					  0x%08x\n" , (unsigned int) &(chan->nbd), chan->nbd);
+	printf("	  31:0	 nd			0x%08x		   Next data descriptor.\n", chan->nbd);
+	printf("\n");
+	return 0;
+}
+
+STATIC int grpci2dma_data_print(struct grpci2_bd_data * data)
+{
+	printf("  GRPCI2 DMA data descriptor\n");
+	printf("	  0x%08x  DMA data control						  0x%08x\n", (unsigned int) data, data->ctrl);
+	printf("	  31	 en				   0x%01x		  Data descriptor enable.\n" , (data->ctrl >> 31) & (0x1));
+	printf("	  30	 ie				   0x%01x		  Interrupt generation enable.\n" , (data->ctrl >> 30) & (0x1));
+	printf("	  29	 dr				   0x%01x		  Tranfer direction.\n" , (data->ctrl >> 29) & (0x1));
+	printf("	  28	 be				   0x%01x		  Bus endianess.\n" , (data->ctrl >> 28) & (0x1));
+	printf("	  21:20  type			   0x%01x		  Descriptor type. 00=DMA data descriptor.\n" , (data->ctrl >> 20) & (0x3));
+	printf("	  19	 er				   0x%01x		  Error status.\n" , (data->ctrl >> 19) & (0x1));
+	printf("	  15:0	 len			0x%04x		   Transfer lenght (in words) - 1.\n" ,  (data->ctrl >> 0) & (0xffff));
+	printf("\n");
+	printf("	  0x%08x  32-bit PCI start address				  0x%08x\n" , (unsigned int) &(data->pci_adr), data->pci_adr);
+	printf("	  31:0	 pa			0x%08x		   PCI address.\n" , data->pci_adr);
+	printf("\n");
+	printf("	  0x%08x  32-bit AHB start address				  0x%08x\n" , (unsigned int) &(data->ahb_adr), data->ahb_adr);
+	printf("	  31:0	 aa			0x%08x		   AHB address.\n" , data->ahb_adr);
+	printf("\n");
+	printf("	  0x%08x  Next data descriptor					  0x%08x\n" , (unsigned int) &(data->next), data->next);
+	printf("	  31:0	 nd			0x%08x		   Next data descriptor.\n" , data->next);
+	printf("\n");
+	return 0;
+}
+#endif
+/*** END OF DEBUG HELPERS ***/
+
+/*** START OF MEMORY ALLOCATION FUNCTIONS ***/
+
+void * grpci2dma_channel_new(int number)
+{
+	/* Allocate memory */
+	unsigned int * orig_ptr = (unsigned int *) grlib_malloc(
+			(GRPCI2DMA_BD_CHAN_SIZE)*number + GRPCI2DMA_BD_CHAN_ALIGN);
+	if (orig_ptr == NULL) return NULL;
+
+	/* Get the aligned pointer */
+	unsigned int aligned_ptr = (
+		((unsigned int) orig_ptr + GRPCI2DMA_BD_CHAN_ALIGN) &
+		 ~(GRPCI2DMA_BD_CHAN_ALIGN - 1));
+
+	/* Save the original pointer just before the aligned pointer */
+	unsigned int ** tmp_ptr =
+		(unsigned int **) (aligned_ptr - sizeof(orig_ptr));
+	*tmp_ptr= orig_ptr;
+
+	/* Return aligned pointer */
+	return (void *) aligned_ptr;
+}
+
+void grpci2dma_channel_delete(void * chan)
+{
+	/* Recover orignal pointer placed just before the aligned pointer */
+	unsigned int * orig_ptr;
+	unsigned int ** tmp_ptr =  (unsigned int **) (chan - sizeof(orig_ptr));
+	orig_ptr = *tmp_ptr;
+
+	/* Deallocate memory */
+	free(orig_ptr);
+}
+
+void * grpci2dma_data_new(int number)
+{
+	/* Allocate memory */
+	unsigned int * orig_ptr = (unsigned int *) grlib_malloc(
+			(GRPCI2DMA_BD_DATA_SIZE)*number + GRPCI2DMA_BD_DATA_ALIGN);
+	if (orig_ptr == NULL) return NULL;
+
+	/* Get the aligned pointer */
+	unsigned int aligned_ptr = (
+		((unsigned int) orig_ptr + GRPCI2DMA_BD_DATA_ALIGN) & 
+		~(GRPCI2DMA_BD_DATA_ALIGN - 1));
+
+	/* Save the original pointer before the aligned pointer */
+	unsigned int ** tmp_ptr = 
+		(unsigned int **) (aligned_ptr - sizeof(orig_ptr));
+	*tmp_ptr= orig_ptr;
+
+	/* Return aligned pointer */
+	return (void *) aligned_ptr;
+}
+
+void grpci2dma_data_delete(void * data)
+{
+	/* Recover orignal pointer placed just before the aligned pointer */
+	unsigned int * orig_ptr;
+	unsigned int ** tmp_ptr =  (unsigned int **) (data - sizeof(orig_ptr));
+	orig_ptr = *tmp_ptr;
+
+	/* Deallocate memory */
+	free(orig_ptr);
+}
+
+/*** END OF MEMORY ALLOCATION FUNCTIONS ***/
+
+/*** START OF USER API ***/
+
+/* Initialize GRPCI2 DMA: GRPCI2 DRIVER calls this
+ * using a weak function definition */
+int grpci2dma_init(
+		void * regs, void isr_register( void (*isr)(void*), void * arg))
+{
+	struct grpci2dma_priv *priv;
+	int i;
+
+	DBG("Registering GRPCI2 DMA driver with arg: 0x%08x\n",
+			(unsigned int) regs);
+
+	/* We only allow one GRPCI2 DMA */
+	if (grpci2dmapriv) {
+		DBG("Driver only supports one PCI DMA core\n");
+		return DRVMGR_FAIL;
+	}
+
+	/* Device Semaphore created with count = 1 */
+	if (rtems_semaphore_create(rtems_build_name('G', 'P', '2', 'D'), 1,
+		RTEMS_FIFO | RTEMS_SIMPLE_BINARY_SEMAPHORE | \
+		RTEMS_NO_INHERIT_PRIORITY | RTEMS_LOCAL | \
+		RTEMS_NO_PRIORITY_CEILING, 0, &grpci2dma_sem) != RTEMS_SUCCESSFUL)
+		return -1;
+
+	/* Allocate and init Memory for DMA */
+	priv = grlib_calloc(1, sizeof(*priv));
+	if (priv == NULL)
+		return DRVMGR_NOMEM;
+
+	priv->regs = regs;
+	strncpy(&priv->devname[0], "grpci2dma0", DEVNAME_LEN);
+
+	/* Initialize Spin-lock for GRPCI2dma Device. */
+	SPIN_INIT(&priv->devlock, priv->devname);
+
+	/* Channel Sempahores */
+	for (i=0; i<MAX_DMA_CHANS; i++){
+		/* set to NULL, they are created when openning channels */
+		priv->channel[i].sem = RTEMS_ID_NONE;
+	}
+
+	/* Register device */
+	grpci2dmapriv = priv;
+
+	/* Initialize Ctrl regs */
+	grpci2dma_ctrl_init();
+
+	/* Install DMA ISR */
+	priv->isr_register = isr_register;
+
+	/* Startup actions:
+	 * - stop DMA
+	 */
+	grpci2dma_ctrl_stop();
+
+	return DRVMGR_OK;
+}
+
+/* Assign ISR Function to DMA IRQ */
+int grpci2dma_isr_register(int chan_no, grpci2dma_isr_t dmaisr, void *data)
+{
+	struct grpci2dma_priv *priv = grpci2dmapriv;
+	SPIN_IRQFLAGS(irqflags);
+
+	if (!priv){
+		/* DMA not initialized */
+		return GRPCI2DMA_ERR_NOINIT;
+	}
+
+	/* Check isr */
+	if (dmaisr == NULL){
+		/* No ISR */
+		return GRPCI2DMA_ERR_WRONGPTR;
+	}
+
+	/* Get chan pointer */
+	if ((chan_no < 0 ) || (chan_no >= MAX_DMA_CHANS)) {
+		/* Wrong channel id */
+		return GRPCI2DMA_ERR_WRONGPTR;
+	}
+
+	/* Check chan is open */
+	if (priv->channel[chan_no].ptr == NULL){
+		/* No channel */
+		return GRPCI2DMA_ERR_NOTFOUND;
+	}
+
+	/* Take driver lock - Wait until we get semaphore */
+	if (rtems_semaphore_obtain(grpci2dma_sem, RTEMS_WAIT, RTEMS_NO_TIMEOUT)
+		!= RTEMS_SUCCESSFUL){
+		return GRPCI2DMA_ERR_ERROR;
+	}
+
+	/* Take channel lock - Wait until we get semaphore */
+	if (rtems_semaphore_obtain(priv->channel[chan_no].sem, RTEMS_WAIT, RTEMS_NO_TIMEOUT)
+		!= RTEMS_SUCCESSFUL){
+		rtems_semaphore_release(grpci2dma_sem);
+		return GRPCI2DMA_ERR_ERROR;
+	}
+
+	/* Register channel ISR */
+	priv->channel[chan_no].isr_arg = data;
+	priv->channel[chan_no].isr = dmaisr;
+
+	/* Register DMA ISR in GRPCI2 if not done yet */
+	if(priv->isr_registered == 0){
+		(priv->isr_register)( grpci2dma_isr, (void *) priv);
+		/* Enable DMA Interrupts */
+		SPIN_LOCK_IRQ(&priv->devlock, irqflags);
+		grpci2dma_ctrl_interrupt_enable();
+		SPIN_UNLOCK_IRQ(&priv->devlock, irqflags);
+	}
+	priv->isr_registered++;
+
+	/* Release channel sempahore */
+	rtems_semaphore_release(priv->channel[chan_no].sem);
+
+	/* Release driver sempahore */
+	rtems_semaphore_release(grpci2dma_sem);
+
+	return GRPCI2DMA_ERR_OK;
+}
+
+/* Assign ISR Function to DMA IRQ */
+int grpci2dma_isr_unregister(int chan_no)
+{
+	struct grpci2dma_priv *priv = grpci2dmapriv;
+	int ret;
+
+	if (!priv){
+		/* DMA not initialized */
+		return GRPCI2DMA_ERR_NOINIT;
+	}
+
+	/* Get chan pointer */
+	if ((chan_no < 0 ) || (chan_no >= MAX_DMA_CHANS)) {
+		/* Wrong channel id */
+		return GRPCI2DMA_ERR_WRONGPTR;
+	}
+
+	/* Check chan is open */
+	if (priv->channel[chan_no].ptr == NULL){
+		/* No channel */
+		return GRPCI2DMA_ERR_NOTFOUND;
+	}
+
+	/* Get chan ISR */
+	if (priv->channel[chan_no].isr == NULL){
+		/* Nothing to do */
+		return GRPCI2DMA_ERR_OK;
+	}
+
+	/* Take driver lock - Wait until we get semaphore */
+	if (rtems_semaphore_obtain(grpci2dma_sem, RTEMS_WAIT, RTEMS_NO_TIMEOUT)
+		!= RTEMS_SUCCESSFUL){
+		return GRPCI2DMA_ERR_ERROR;
+	}
+
+	/* Take channel lock - Wait until we get semaphore */
+	if (rtems_semaphore_obtain(priv->channel[chan_no].sem, RTEMS_WAIT, RTEMS_NO_TIMEOUT)
+		!= RTEMS_SUCCESSFUL){
+		rtems_semaphore_release(grpci2dma_sem);
+		return GRPCI2DMA_ERR_ERROR;
+	}
+
+	/* Unregister channel ISR */
+	ret = grpci2dma_channel_isr_unregister(chan_no);
+
+	/* Release channel sempahore */
+	rtems_semaphore_release(priv->channel[chan_no].sem);
+
+	/* Release driver sempahore */
+	rtems_semaphore_release(grpci2dma_sem);
+
+	return ret;
+}
+
+int grpci2dma_open(void * chanptr)
+{
+	struct grpci2dma_priv *priv = grpci2dmapriv;
+	int cid;
+	int ret;
+
+	if (!priv){
+		/* DMA not initialized */
+		return GRPCI2DMA_ERR_NOINIT;
+	}
+
+	/* Check alignment */
+	if (((unsigned int ) chanptr) & (GRPCI2DMA_BD_CHAN_ALIGN-1)) {
+		/* Channel is not properly aligned */
+		return GRPCI2DMA_ERR_WRONGPTR;
+	}
+
+	/* Take driver lock - Wait until we get semaphore */
+	if (rtems_semaphore_obtain(grpci2dma_sem, RTEMS_WAIT, RTEMS_NO_TIMEOUT)
+		!= RTEMS_SUCCESSFUL){
+		return GRPCI2DMA_ERR_ERROR;
+	}
+
+	/* Get free channel id */
+	cid = grpci2dma_channel_free_id();
+	if (cid < 0 ){
+		rtems_semaphore_release(grpci2dma_sem);
+		return GRPCI2DMA_ERR_TOOMANY;
+	}
+
+	/* Open channel */
+	ret = grpci2dma_channel_open((struct grpci2_bd_chan *) chanptr, cid);
+
+	/* Create channel semaphore with count = 1 */
+	if (ret >= 0){
+		if (rtems_semaphore_create(
+					rtems_build_name('P', 'D', '0', '0' + cid), 1,
+					RTEMS_FIFO | RTEMS_SIMPLE_BINARY_SEMAPHORE | \
+					RTEMS_NO_INHERIT_PRIORITY | RTEMS_LOCAL | \
+					RTEMS_NO_PRIORITY_CEILING, 0, &priv->channel[cid].sem
+					) != RTEMS_SUCCESSFUL) {
+			priv->channel[cid].sem = RTEMS_ID_NONE;
+			rtems_semaphore_release(grpci2dma_sem);
+			return GRPCI2DMA_ERR_ERROR;
+		}
+	}
+
+	/* Release driver semaphore */
+	rtems_semaphore_release(grpci2dma_sem);
+
+	/* Return channel id */
+	return ret;
+}
+
+int grpci2dma_close(int chan_no)
+{
+	struct grpci2dma_priv *priv = grpci2dmapriv;
+	int ret;
+
+	if (!priv){
+		/* DMA not initialized */
+		return GRPCI2DMA_ERR_NOINIT;
+	}
+
+	/* Get chan pointer */
+	if ((chan_no < 0) || (chan_no >= MAX_DMA_CHANS)){
+		/* Wrong channel id */
+		return GRPCI2DMA_ERR_WRONGPTR;
+	}
+
+	/* Check chan is open */
+	if (priv->channel[chan_no].ptr == NULL){
+		/* No channel */
+		return GRPCI2DMA_ERR_NOTFOUND;
+	}
+
+	/* Take driver lock - Wait until we get semaphore */
+	if (rtems_semaphore_obtain(grpci2dma_sem, RTEMS_WAIT, RTEMS_NO_TIMEOUT)
+		!= RTEMS_SUCCESSFUL){
+		return GRPCI2DMA_ERR_ERROR;
+	}
+
+	/* Take channel lock - Wait until we get semaphore */
+	if (rtems_semaphore_obtain(priv->channel[chan_no].sem, RTEMS_WAIT, RTEMS_NO_TIMEOUT)
+		!= RTEMS_SUCCESSFUL){
+		rtems_semaphore_release(grpci2dma_sem);
+		return GRPCI2DMA_ERR_ERROR;
+	}
+
+	/* Close channel */
+	ret = grpci2dma_channel_close(chan_no);
+
+	/* Release channel sempahore */
+	rtems_semaphore_release(priv->channel[chan_no].sem);
+
+	/* Delete channel semaphore */
+	if (ret == GRPCI2DMA_ERR_OK){
+		if (rtems_semaphore_delete(priv->channel[chan_no].sem) 
+				!= RTEMS_SUCCESSFUL){
+			/* Release driver semaphore */
+			rtems_semaphore_release(grpci2dma_sem);
+			return GRPCI2DMA_ERR_ERROR;
+		}
+	}
+
+	/* Release driver semaphore */
+	rtems_semaphore_release(grpci2dma_sem);
+
+	return ret;
+}
+
+/* Transfer_size =0 means maximum */
+int grpci2dma_prepare(
+		uint32_t pci_start, uint32_t ahb_start, int dir, int endianness,
+		int size, void * dataptr, int index, int ndata, int transfer_size)
+{
+	struct grpci2_bd_data * data = dataptr;
+
+	/* Check data pointer */
+	if ((data == NULL) || 
+			(((unsigned int ) data) & (GRPCI2DMA_BD_DATA_ALIGN-1))){
+		return GRPCI2DMA_ERR_WRONGPTR;
+	}
+
+	/* Check indexes */
+	int maxdata = ndata - index;
+	if ((maxdata < 1) || (index < 0)){
+		/* No data descriptors to use */
+		return GRPCI2DMA_ERR_WRONGPTR;
+	}
+
+	/* Check PCI transfer size */
+	if ( (transfer_size < 0) || 
+			(transfer_size > MAX_DMA_TRANSFER_SIZE) || 
+			(transfer_size%4 != 0) ) {
+		return GRPCI2DMA_ERR_WRONGPTR;
+	}
+	if (transfer_size == 0){
+		transfer_size = MAX_DMA_TRANSFER_SIZE;
+	}
+
+	/* Check total size */
+	if ( (size <=0) || (size % 4 != 0)){
+		return GRPCI2DMA_ERR_WRONGPTR;
+	}
+
+	/* Calculate number of data descriptors needed */
+	int words = size/4;
+	int blocksize = transfer_size/4;
+	int datacnt = words/blocksize + (words%blocksize != 0? 1: 0);
+	/* Check that we can transfer the data */
+	if (datacnt > maxdata) {
+		return GRPCI2DMA_ERR_TOOMANY;
+	}
+
+	/* Prepare data descriptors */
+	int i;
+	uint32_t pci_adr;
+	uint32_t ahb_adr;
+	int remaining=words;
+	int datasize;
+	struct grpci2_bd_data * next;
+	for (i=0; i<datacnt; i++){
+		/* Get PCI and AHB start addresses */
+		pci_adr = pci_start + i*blocksize;
+		ahb_adr = ahb_start + i*blocksize;
+		/* Get current data size */
+		if (remaining >= blocksize){
+			datasize = blocksize - 1;
+			remaining -= blocksize;
+		} else {
+			datasize = remaining -1;
+			remaining = 0;
+		}
+		/* Get linked list pointers */
+		if (i == datacnt - 1){
+			/* Last transfer */
+			next = DISABLED_DESCRIPTOR;
+		}else{
+			next = &data[i+index+1];
+		}
+		/* Set Data descriptor */
+		grpci2dma_data_bd_init(&data[i+index], pci_adr, ahb_adr, dir, endianness, datasize, next);
+	}
+	/* Return number of transfers used */
+	return datacnt;
+}
+
+int grpci2dma_status(void *dataptr, int index, int ndata)
+{
+	struct grpci2_bd_data * data = dataptr;
+	int i;
+
+	/* Check data pointer */
+	if ((data == NULL) || 
+			(((unsigned int ) data) & (GRPCI2DMA_BD_DATA_ALIGN-1))){
+		return GRPCI2DMA_ERR_WRONGPTR;
+	}
+
+	/* Check maxdata */
+	int maxdata = ndata - index;
+	if ((maxdata < 1) || (index < 0)){
+		/* No data descriptors to use */
+		return GRPCI2DMA_ERR_WRONGPTR;
+	}
+
+	/* Check status of all packets in transfer */
+	int status;
+	for (i=0; i< maxdata; i++){
+		status = grpci2dma_data_bd_status(&data[i+index]); 
+		if (status == GRPCI2DMA_BD_STATUS_ERR){
+			/* Error in one packet, means error in transfer */
+			return status;
+		} else if (status == GRPCI2DMA_BD_STATUS_ENABLED){
+			/* If one packet is enabled, means transfer is not done */
+			return status;
+		}
+	}
+
+	/* If we reach here it means they are all disabled */
+	return status;
+}
+
+int grpci2dma_print(int chan_no)
+{
+	struct grpci2dma_priv *priv = grpci2dmapriv;
+	struct grpci2_bd_chan * chan;
+
+	if (!priv){
+		/* DMA not initialized */
+		DBG("DMA not initialized.\n");
+		return GRPCI2DMA_ERR_NOINIT;
+	}
+
+	if ( (chan_no < 0) || (chan_no >= MAX_DMA_CHANS )){
+		/* Wrong chan no*/
+		return GRPCI2DMA_ERR_WRONGPTR;
+	}
+
+	chan = priv->channel[chan_no].ptr;
+	if (chan == NULL) {
+		return GRPCI2DMA_ERR_WRONGPTR;
+	}
+
+	#ifdef DEBUG
+	/* Print channel state */
+	grpci2dma_channel_print(chan);
+
+	/* Get current DATA desc */
+	struct grpci2_bd_data * first_data = (struct grpci2_bd_data *) BD_READ(&chan->nbd);
+
+	/* Print data state */
+	grpci2dma_data_list_foreach(first_data, grpci2dma_data_print, MAX_DMA_DATA);
+	#endif
+	return GRPCI2DMA_ERR_OK;
+}
+
+int grpci2dma_print_bd(void * dataptr)
+{
+	struct grpci2dma_priv *priv = grpci2dmapriv;
+	struct grpci2_bd_data * data = (struct grpci2_bd_data *) dataptr;
+
+	if (!priv){
+		/* DMA not initialized */
+		DBG("DMA not initialized.\n");
+		return GRPCI2DMA_ERR_NOINIT;
+	}
+
+	if ( data == NULL ){
+		/* Wrong chan no*/
+		return GRPCI2DMA_ERR_WRONGPTR;
+	}
+
+	#ifdef DEBUG
+	/* Print data state */
+	grpci2dma_data_list_foreach(data, grpci2dma_data_print, MAX_DMA_DATA);
+	#endif
+	return GRPCI2DMA_ERR_OK;
+}
+
+int grpci2dma_interrupt_enable(
+		void *dataptr, int index, int maxindex, int options)
+{
+	struct grpci2_bd_data * data = dataptr;
+	struct grpci2dma_priv *priv = grpci2dmapriv;
+	SPIN_IRQFLAGS(irqflags);
+
+	if (!priv){
+		/* DMA not initialized */
+		return GRPCI2DMA_ERR_NOINIT;
+	}
+
+	/* Check data pointer */
+	if ((data == NULL) || 
+			(((unsigned int ) data) & (GRPCI2DMA_BD_DATA_ALIGN-1))){
+		return GRPCI2DMA_ERR_WRONGPTR;
+	}
+
+	/* Check index */
+	if ((index < 0) || (maxindex < 1) || (index >= maxindex)){
+		/* No data descriptors to use */
+		return GRPCI2DMA_ERR_WRONGPTR;
+	}
+
+	if (options & GRPCI2DMA_OPTIONS_ALL){
+		/* Enable all interrupts */
+		if (grpci2dma_data_list_foreach(
+					&data[index],
+					grpci2dma_data_bd_interrupt_enable, maxindex -index)){
+			return GRPCI2DMA_ERR_ERROR;
+		}
+	}else{
+		/* Enable one packet interrupts */
+		grpci2dma_data_bd_interrupt_enable(&data[index]);
+	}
+
+	/* Finally enable DMA interrupts if they are not already enabled */
+	if (grpci2dma_ctrl_interrupt_status()==0){
+		SPIN_LOCK_IRQ(&priv->devlock, irqflags);
+		grpci2dma_ctrl_interrupt_enable();
+		SPIN_UNLOCK_IRQ(&priv->devlock, irqflags);
+	}
+
+	DBG("Interrupts enabled for data (0x%08x), index:%d, maxindex:%d, %s.\n", 
+			(unsigned int) data, index, maxindex, 
+			(options & GRPCI2DMA_OPTIONS_ALL)? "ALL":"ONE" );
+
+	return GRPCI2DMA_ERR_OK;
+}
+
+int grpci2dma_push(int chan_no, void *dataptr, int index, int ndata)
+{
+	struct grpci2dma_priv *priv = grpci2dmapriv;
+	SPIN_IRQFLAGS(irqflags);
+	int ret;
+
+	if (!priv){
+		/* DMA not initialized */
+		return GRPCI2DMA_ERR_NOINIT;
+	}
+
+	/* Check data pointer */
+	if ((dataptr == NULL) || 
+			(((unsigned int ) dataptr) & (GRPCI2DMA_BD_DATA_ALIGN-1))){
+		return GRPCI2DMA_ERR_WRONGPTR;
+	}
+
+	/* Check index */
+	if ((ndata < 1) || (index < 0)){
+		/* No data descriptors to use */
+		return GRPCI2DMA_ERR_WRONGPTR;
+	}
+
+	/* Check chan_no */
+	if ( (chan_no < 0) || (chan_no >= MAX_DMA_CHANS )){
+		/* Wrong chan no*/
+		return GRPCI2DMA_ERR_WRONGPTR;
+	}
+
+	/* Check chan is open */
+	if (priv->channel[chan_no].ptr == NULL){
+		/* No channel */
+		return GRPCI2DMA_ERR_NOTFOUND;
+	}
+
+	/* Take channel lock - Wait until we get semaphore */
+	if (rtems_semaphore_obtain(priv->channel[chan_no].sem, RTEMS_WAIT, RTEMS_NO_TIMEOUT)
+		!= RTEMS_SUCCESSFUL){
+		return GRPCI2DMA_ERR_ERROR;
+	}
+
+	/* push data to channel */
+	ret = grpci2dma_channel_push(chan_no, dataptr, index, ndata);
+
+	if (ret != GRPCI2DMA_ERR_OK){
+		/* Release channel lock */
+		rtems_semaphore_release(priv->channel[chan_no].sem);
+		return ret;
+	}
+
+	/* Start DMA if it is not active and channel is active*/
+	SPIN_LOCK_IRQ(&priv->devlock, irqflags);
+	if ((!grpci2dma_active()) && (priv->channel[chan_no].active)){
+		grpci2dma_ctrl_start(priv->channel[chan_no].ptr);
+	}
+	SPIN_UNLOCK_IRQ(&priv->devlock, irqflags);
+
+	/* Release channel lock */
+	rtems_semaphore_release(priv->channel[chan_no].sem);
+
+	return ret;
+}
+
+/* Start the channel */
+int grpci2dma_start(int chan_no, int options)
+{
+	struct grpci2dma_priv *priv = grpci2dmapriv;
+	int ret;
+
+	if (!priv){
+		/* DMA not initialized */
+		return GRPCI2DMA_ERR_NOINIT;
+	}
+
+	if ((chan_no < 0 ) || (chan_no >= MAX_DMA_CHANS )) {
+		/* Wrong channel id */
+		return GRPCI2DMA_ERR_WRONGPTR;
+	}
+
+	if ( options < 0 ) {
+		/* Wrong options */
+		return GRPCI2DMA_ERR_WRONGPTR;
+	}
+
+	/* Check chan is open */
+	if (priv->channel[chan_no].ptr == NULL){
+		/* No channel */
+		return GRPCI2DMA_ERR_NOTFOUND;
+	}
+
+	/* Take driver lock - Wait until we get semaphore */
+	if (rtems_semaphore_obtain(grpci2dma_sem, RTEMS_WAIT, RTEMS_NO_TIMEOUT)
+		!= RTEMS_SUCCESSFUL){
+		return GRPCI2DMA_ERR_ERROR;
+	}
+
+	/* Take channel lock - Wait until we get semaphore */
+	if (rtems_semaphore_obtain(priv->channel[chan_no].sem, RTEMS_WAIT, RTEMS_NO_TIMEOUT)
+		!= RTEMS_SUCCESSFUL){
+		rtems_semaphore_release(grpci2dma_sem);
+		return GRPCI2DMA_ERR_ERROR;
+	}
+
+	/* Start the channel */
+	ret = grpci2dma_channel_start(chan_no, options);
+
+	/* Release channel lock */
+	rtems_semaphore_release(priv->channel[chan_no].sem);
+
+	/* Release driver lock */
+	rtems_semaphore_release(grpci2dma_sem);
+
+	return ret;
+}
+
+/* Stop the channel, but don't stop ongoing transfers! */
+int grpci2dma_stop(int chan_no)
+{
+	struct grpci2dma_priv *priv = grpci2dmapriv;
+	int ret;
+
+	if (!priv){
+		/* DMA not initialized */
+		return GRPCI2DMA_ERR_NOINIT;
+	}
+
+	if ((chan_no < 0 ) || (chan_no >= MAX_DMA_CHANS)) {
+		/* Wrong channel id */
+		return GRPCI2DMA_ERR_WRONGPTR;
+	}
+
+	/* Check chan is open */
+	if (priv->channel[chan_no].ptr == NULL){
+		/* No channel */
+		return GRPCI2DMA_ERR_NOTFOUND;
+	}
+
+	/* Take driver lock - Wait until we get semaphore */
+	if (rtems_semaphore_obtain(grpci2dma_sem, RTEMS_WAIT, RTEMS_NO_TIMEOUT)
+		!= RTEMS_SUCCESSFUL){
+		return GRPCI2DMA_ERR_ERROR;
+	}
+
+	/* Take channel lock - Wait until we get semaphore */
+	if (rtems_semaphore_obtain(priv->channel[chan_no].sem, RTEMS_WAIT, RTEMS_NO_TIMEOUT)
+		!= RTEMS_SUCCESSFUL){
+		rtems_semaphore_release(grpci2dma_sem);
+		return GRPCI2DMA_ERR_ERROR;
+	}
+
+	/* Stop the channel */
+	ret = grpci2dma_channel_stop(chan_no);
+
+	/* Release channel lock */
+	rtems_semaphore_release(priv->channel[chan_no].sem);
+
+	/* Release driver lock */
+	rtems_semaphore_release(grpci2dma_sem);
+
+	return ret;
+}
+
+int grpci2dma_active()
+{
+	return ((grpci2dma_ctrl_status()) & DMACTRL_ACT) >> DMACTRL_ACT_BIT;
+}
+
diff --git a/bsps/shared/grlib/pci/pcif.c b/bsps/shared/grlib/pci/pcif.c
new file mode 100644
index 0000000000..17708a9c0b
--- /dev/null
+++ b/bsps/shared/grlib/pci/pcif.c
@@ -0,0 +1,586 @@
+/*  GRLIB PCIF PCI HOST driver.
+ * 
+ *  COPYRIGHT (c) 2008.
+ *  Cobham Gaisler AB.
+ *
+ *  Configures the PCIF core and initialize,
+ *   - the PCI Library (pci.c)
+ *   - the general part of the PCI Bus driver (pci_bus.c)
+ *  
+ *  System interrupt assigned to PCI interrupt (INTA#..INTD#) is by
+ *  default taken from Plug and Play, but may be overridden by the 
+ *  driver resources INTA#..INTD#.
+ *
+ *  The license and distribution terms for this file may be
+ *  found in found in the file LICENSE in this distribution or at
+ *  http://www.rtems.org/license/LICENSE.
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <libcpu/byteorder.h>
+#include <libcpu/access.h>
+#include <rtems/bspIo.h>
+#include <pci.h>
+#include <pci/cfg.h>
+
+#include <drvmgr/drvmgr.h>
+#include <grlib/ambapp_bus.h>
+#include <grlib/ambapp.h>
+#include <drvmgr/pci_bus.h>
+#include <grlib/pcif.h>
+
+
+/* Configuration options */
+#define SYSTEM_MAINMEM_START 0x40000000
+
+/* Interrupt assignment. Set to other value than 0xff in order to 
+ * override defaults and plug&play information
+ */
+#ifndef PCIF_INTA_SYSIRQ
+ #define PCIF_INTA_SYSIRQ 0xff
+#endif
+#ifndef PCIF_INTB_SYSIRQ
+ #define PCIF_INTB_SYSIRQ 0xff
+#endif
+#ifndef PCIF_INTC_SYSIRQ
+ #define PCIF_INTC_SYSIRQ 0xff
+#endif
+#ifndef PCIF_INTD_SYSIRQ
+ #define PCIF_INTD_SYSIRQ 0xff
+#endif
+
+/*#define DEBUG 1  */
+
+#ifdef DEBUG
+#define DBG(x...) printk(x)
+#else
+#define DBG(x...) 
+#endif
+
+/*
+ * Bit encode for PCI_CONFIG_HEADER_TYPE register
+ */
+struct pcif_regs {
+	volatile unsigned int bars[4];  /* 0x00-0x10 */
+	volatile unsigned int bus;      /* 0x10 */
+	volatile unsigned int map_io;   /* 0x14 */
+	volatile unsigned int status;   /* 0x18 */
+	volatile unsigned int intr;     /* 0x1c */
+	int unused[(0x40-0x20)/4];      /* 0x20-0x40 */
+	volatile unsigned int maps[(0x80-0x40)/4];   /* 0x40-0x80*/
+};
+
+/* Used internally for accessing the PCI bridge's configuration space itself */
+#define HOST_TGT PCI_DEV(0xff, 0, 0)
+
+struct pcif_priv *pcifpriv = NULL;
+static int pcif_minor = 0;
+
+/* PCI Interrupt assignment. Connects an PCI interrupt pin (INTA#..INTD#)
+ * to a system interrupt number.
+ */
+unsigned char pcif_pci_irq_table[4] =
+{
+	/* INTA# */	PCIF_INTA_SYSIRQ,
+	/* INTB# */	PCIF_INTB_SYSIRQ,
+	/* INTC# */	PCIF_INTC_SYSIRQ,
+	/* INTD# */	PCIF_INTD_SYSIRQ
+};
+
+/* Driver private data struture */
+struct pcif_priv {
+	struct drvmgr_dev	*dev;
+	struct pcif_regs		*regs;
+	int				irq;
+	int				minor;
+	int				irq_mask;
+
+	unsigned int			pci_area;
+	unsigned int			pci_area_end;
+	unsigned int			pci_io;    
+	unsigned int			pci_conf;
+	unsigned int			pci_conf_end;
+
+	uint32_t			devVend; /* Host PCI Vendor/Device ID */
+	uint32_t			bar1_size;
+
+	struct drvmgr_map_entry		maps_up[2];
+	struct drvmgr_map_entry		maps_down[2];
+	struct pcibus_config		config;
+};
+
+int pcif_init1(struct drvmgr_dev *dev);
+int pcif_init3(struct drvmgr_dev *dev);
+
+/* PCIF DRIVER */
+
+struct drvmgr_drv_ops pcif_ops = 
+{
+	.init = {pcif_init1, NULL, pcif_init3, NULL},
+	.remove = NULL,
+	.info = NULL
+};
+
+struct amba_dev_id pcif_ids[] = 
+{
+	{VENDOR_GAISLER, GAISLER_PCIF},
+	{0, 0}		/* Mark end of table */
+};
+
+struct amba_drv_info pcif_info =
+{
+	{
+		DRVMGR_OBJ_DRV,			/* Driver */
+		NULL,				/* Next driver */
+		NULL,				/* Device list */
+		DRIVER_AMBAPP_GAISLER_PCIF_ID,	/* Driver ID */
+		"PCIF_DRV",			/* Driver Name */
+		DRVMGR_BUS_TYPE_AMBAPP,		/* Bus Type */
+		&pcif_ops,
+		NULL,				/* Funcs */
+		0,				/* No devices yet */
+		sizeof(struct pcif_priv),	/* Let drvmgr alloc private */
+	},
+	&pcif_ids[0]
+};
+
+void pcif_register_drv(void)
+{
+	DBG("Registering PCIF driver\n");
+	drvmgr_drv_register(&pcif_info.general);
+}
+
+static int pcif_cfg_r32(pci_dev_t dev, int ofs, uint32_t *val)
+{
+	struct pcif_priv *priv = pcifpriv;
+	volatile uint32_t *pci_conf;
+	uint32_t devfn;
+	int retval;
+	int bus = PCI_DEV_BUS(dev);
+
+	if (ofs & 3)
+		return PCISTS_EINVAL;
+
+	if (PCI_DEV_SLOT(dev) > 15) {
+		*val = 0xffffffff;
+		return PCISTS_OK;
+	}
+
+	/* PCIF can access "non-standard" devices on bus0 (on AD11.AD16), 
+	 * but we skip them.
+	 */
+	if (dev == HOST_TGT)
+		bus = devfn = 0;
+	else if (bus == 0)
+		devfn = PCI_DEV_DEVFUNC(dev) + PCI_DEV(0, 6, 0);
+	else
+		devfn = PCI_DEV_DEVFUNC(dev);
+
+	/* Select bus */
+	priv->regs->bus = bus << 16;
+
+	pci_conf = (volatile uint32_t *)(priv->pci_conf | (devfn << 8) | ofs);
+
+	*val = *pci_conf;
+
+	if (priv->regs->status & 0x30000000) {
+		*val = 0xffffffff;
+		retval = PCISTS_MSTABRT;
+	} else
+		retval = PCISTS_OK;
+
+	DBG("pci_read: [%x:%x:%x] reg: 0x%x => addr: 0x%x, val: 0x%x\n",
+		PCI_DEV_EXPAND(dev), ofs, pci_conf, *val);
+
+	return retval;
+}
+
+static int pcif_cfg_r16(pci_dev_t dev, int ofs, uint16_t *val)
+{
+	uint32_t v;
+	int retval;
+
+	if (ofs & 1)
+		return PCISTS_EINVAL;
+
+	retval = pcif_cfg_r32(dev, ofs & ~0x3, &v);
+	*val = 0xffff & (v >> (8*(ofs & 0x3)));
+
+	return retval;
+}
+
+static int pcif_cfg_r8(pci_dev_t dev, int ofs, uint8_t *val)
+{
+	uint32_t v;
+	int retval;
+
+	retval = pcif_cfg_r32(dev, ofs & ~0x3, &v);
+
+	*val = 0xff & (v >> (8*(ofs & 3)));
+
+	return retval;
+}
+
+static int pcif_cfg_w32(pci_dev_t dev, int ofs, uint32_t val)
+{
+	struct pcif_priv *priv = pcifpriv;
+	volatile uint32_t *pci_conf;
+	uint32_t devfn;
+	int bus = PCI_DEV_BUS(dev);
+
+	if (ofs & ~0xfc)
+		return PCISTS_EINVAL;
+
+	if (PCI_DEV_SLOT(dev) > 15)
+		return PCISTS_MSTABRT;
+
+	/* PCIF can access "non-standard" devices on bus0 (on AD11.AD16), 
+	 * but we skip them.
+	 */
+	if (dev == HOST_TGT)
+		bus = devfn = 0;
+	else if (bus == 0)
+		devfn = PCI_DEV_DEVFUNC(dev) + PCI_DEV(0, 6, 0);
+	else
+		devfn = PCI_DEV_DEVFUNC(dev);
+
+	/* Select bus */
+	priv->regs->bus = bus << 16;
+
+	pci_conf = (volatile uint32_t *)(priv->pci_conf | (devfn << 8) | ofs);
+
+	*pci_conf = val;
+
+	DBG("pci_write - [%x:%x:%x] reg: 0x%x => addr: 0x%x, val: 0x%x\n",
+		PCI_DEV_EXPAND(dev), ofs, pci_conf, value);
+
+	return PCISTS_OK;
+}
+
+static int pcif_cfg_w16(pci_dev_t dev, int ofs, uint16_t val)
+{
+	uint32_t v;
+	int retval;
+
+	if (ofs & 1)
+		return PCISTS_EINVAL;
+
+	retval = pcif_cfg_r32(dev, ofs & ~0x3, &v);
+	if (retval != PCISTS_OK)
+		return retval;
+
+	v = (v & ~(0xffff << (8*(ofs&3)))) | ((0xffff&val) << (8*(ofs&3)));
+
+	return pcif_cfg_w32(dev, ofs & ~0x3, v);
+}
+
+static int pcif_cfg_w8(pci_dev_t dev, int ofs, uint8_t val)
+{
+	uint32_t v;
+	int retval;
+
+	retval = pcif_cfg_r32(dev, ofs & ~0x3, &v);
+	if (retval != PCISTS_OK)
+		return retval;
+
+	v = (v & ~(0xff << (8*(ofs&3)))) | ((0xff&val) << (8*(ofs&3)));
+
+	return pcif_cfg_w32(dev, ofs & ~0x3, v);
+}
+
+
+/* Return the assigned system IRQ number that corresponds to the PCI
+ * "Interrupt Pin" information from configuration space.
+ *
+ * The IRQ information is stored in the pcif_pci_irq_table configurable
+ * by the user.
+ *
+ * Returns the "system IRQ" for the PCI INTA#..INTD# pin in irq_pin. Returns
+ * 0xff if not assigned.
+ */
+static uint8_t pcif_bus0_irq_map(pci_dev_t dev, int irq_pin)
+{
+	uint8_t sysIrqNr = 0; /* not assigned */
+	int irq_group;
+
+	if ( (irq_pin >= 1) && (irq_pin <= 4) ) {
+		/* Use default IRQ decoding on PCI BUS0 according slot numbering */
+		irq_group = PCI_DEV_SLOT(dev) & 0x3;
+		irq_pin = ((irq_pin - 1) + irq_group) & 0x3;
+		/* Valid PCI "Interrupt Pin" number */
+		sysIrqNr = pcif_pci_irq_table[irq_pin];
+	}
+	return sysIrqNr;
+}
+
+static int pcif_translate(uint32_t *address, int type, int dir)
+{
+	/* No address translation implmented at this point */
+	return 0;
+}
+
+extern struct pci_memreg_ops pci_memreg_sparc_be_ops;
+
+/* PCIF Big-Endian PCI access routines */
+struct pci_access_drv pcif_access_drv = {
+	.cfg =
+	{
+		pcif_cfg_r8,
+		pcif_cfg_r16,
+		pcif_cfg_r32,
+		pcif_cfg_w8,
+		pcif_cfg_w16,
+		pcif_cfg_w32,
+	},
+	.io =	/* PCIF only supports Big-endian */
+	{
+		_ld8,
+		_ld_be16,
+		_ld_be32,
+		_st8,
+		_st_be16,
+		_st_be32,
+	},
+	.memreg = &pci_memreg_sparc_be_ops,
+	.translate = pcif_translate,
+};
+
+/* Initializes the PCIF core hardware
+ *
+ */
+static int pcif_hw_init(struct pcif_priv *priv)
+{
+	struct pcif_regs *regs;
+	uint32_t data, size;
+	int mst;
+	pci_dev_t host = HOST_TGT;
+
+	regs = priv->regs;
+
+	/* Mask PCI interrupts */
+	regs->intr = 0;
+
+	/* Get the PCIF Host PCI ID */
+	pcif_cfg_r32(host, PCIR_VENDOR, &priv->devVend);
+
+	/* set 1:1 mapping between AHB -> PCI memory space, for all Master cores */
+	for ( mst=0; mst<16; mst++) {
+		regs->maps[mst] = priv->pci_area;
+
+		/* Check if this register is implemented */
+		if ( regs->maps[mst] != priv->pci_area )
+			break;
+	}
+
+	/* and map system RAM at pci address SYSTEM_MAINMEM_START. This way
+	 * PCI targets can do DMA directly into CPU main memory.
+	 */
+	regs->bars[0] = SYSTEM_MAINMEM_START;
+	regs->bars[1] = 0;
+	regs->bars[2] = 0;
+	regs->bars[3] = 0;
+
+	/* determine size of target BAR1 */
+	pcif_cfg_w32(host, PCIR_BAR(1), 0xffffffff);
+	pcif_cfg_r32(host, PCIR_BAR(1), &size);
+	priv->bar1_size = (~(size & ~0xf)) + 1;
+
+	pcif_cfg_w32(host, PCIR_BAR(0), 0);
+	pcif_cfg_w32(host, PCIR_BAR(1), SYSTEM_MAINMEM_START);
+	pcif_cfg_w32(host, PCIR_BAR(2), 0);
+	pcif_cfg_w32(host, PCIR_BAR(3), 0);
+	pcif_cfg_w32(host, PCIR_BAR(4), 0);
+	pcif_cfg_w32(host, PCIR_BAR(5), 0);
+
+	/* set as bus master and enable pci memory responses */  
+	pcif_cfg_r32(host, PCIR_COMMAND, &data);
+	data |= (PCIM_CMD_MEMEN | PCIM_CMD_BUSMASTEREN);
+	pcif_cfg_w32(host, PCIR_COMMAND, data);
+
+	/* Successful */
+	return 0;
+}
+
+/* Initializes the PCIF core and driver, must be called before calling init_pci() 
+ *
+ * Return values
+ *  0             Successful initalization
+ *  -1            Error during initialization, for example "PCI core not found".
+ *  -2            Error PCI controller not HOST (targets not supported)
+ *  -3            Error due to PCIF hardware initialization
+ *  -4            Error registering driver to PCI layer
+ */
+static int pcif_init(struct pcif_priv *priv)
+{
+	struct ambapp_apb_info *apb;
+	struct ambapp_ahb_info *ahb;
+	int pin;
+	union drvmgr_key_value *value;
+	char keyname[6];
+	struct amba_dev_info *ainfo = priv->dev->businfo;
+
+	/* Find PCI core from Plug&Play information */
+	apb = ainfo->info.apb_slv;
+	ahb = ainfo->info.ahb_slv;
+
+	/* Found PCI core, init private structure */
+	priv->irq = apb->irq;
+	priv->regs = (struct pcif_regs *)apb->start;
+
+	/* Calculate the PCI windows 
+	 *  AMBA->PCI Window:                       AHB SLAVE AREA0
+	 *  AMBA->PCI I/O cycles Window:            AHB SLAVE AREA1 Lower half
+	 *  AMBA->PCI Configuration cycles Window:  AHB SLAVE AREA1 Upper half
+	 */
+	priv->pci_area     = ahb->start[0];
+	priv->pci_area_end = ahb->start[0] + ahb->mask[0];
+	priv->pci_io       = ahb->start[1];
+	priv->pci_conf     = ahb->start[1] + (ahb->mask[1] >> 1);
+	priv->pci_conf_end = ahb->start[1] + ahb->mask[1];
+
+	/* On systems where PCI I/O area and configuration area is apart of the "PCI Window" 
+	 * the PCI Window stops at the start of the PCI I/O area
+	 */
+	if ( (priv->pci_io > priv->pci_area) && (priv->pci_io < (priv->pci_area_end-1)) ) {
+		priv->pci_area_end = priv->pci_io;
+	}
+
+	/* Init PCI interrupt assignment table to all use the interrupt routed through
+	 * the PCIF core.
+	 */
+	strcpy(keyname, "INTX#");
+	for (pin=1; pin<5; pin++) {
+		if ( pcif_pci_irq_table[pin-1] == 0xff ) {
+			pcif_pci_irq_table[pin-1] = priv->irq;
+
+			/* User may override Plug & Play IRQ */
+			keyname[3] = 'A' + (pin-1);
+			value = drvmgr_dev_key_get(priv->dev, keyname, DRVMGR_KT_INT);
+			if ( value )
+				pcif_pci_irq_table[pin-1] = value->i;
+		}
+	}
+
+	priv->irq_mask = 0xf;
+	value = drvmgr_dev_key_get(priv->dev, "", DRVMGR_KT_INT);
+	if ( value )
+		priv->irq_mask = value->i & 0xf;
+
+	/* This driver only support HOST systems, we check for HOST */
+	if ( priv->regs->status & 0x00000001 ) {
+		/* Target not supported */
+		return -2;
+	}
+
+	/* Init the PCI Core */
+	if ( pcif_hw_init(priv) ) {
+		return -3;
+	}
+
+	/* Down streams translation table */
+	priv->maps_down[0].name = "AMBA -> PCI MEM Window";
+	priv->maps_down[0].size = priv->pci_area_end - priv->pci_area;
+	priv->maps_down[0].from_adr = (void *)priv->pci_area;
+	priv->maps_down[0].to_adr = (void *)priv->pci_area;
+	/* End table */
+	priv->maps_down[1].size = 0;
+
+	/* Up streams translation table */
+	priv->maps_up[0].name = "Target BAR1 -> AMBA";
+	priv->maps_up[0].size = priv->bar1_size;
+	priv->maps_up[0].from_adr = (void *)SYSTEM_MAINMEM_START;
+	priv->maps_up[0].to_adr = (void *)SYSTEM_MAINMEM_START;
+	/* End table */
+	priv->maps_up[1].size = 0;
+
+	return 0;
+}
+
+/* Called when a core is found with the AMBA device and vendor ID 
+ * given in pcif_ids[].
+ */
+int pcif_init1(struct drvmgr_dev *dev)
+{
+	struct pcif_priv *priv;
+	struct pci_auto_setup pcif_auto_cfg;
+
+	DBG("PCIF[%d] on bus %s\n", dev->minor_drv, dev->parent->dev->name);
+
+	if ( pcif_minor != 0 ) {
+		printk("Driver only supports one PCI core\n");
+		return DRVMGR_FAIL;
+	}
+
+	priv = dev->priv;
+	if ( !priv )
+		return DRVMGR_NOMEM;
+
+	dev->priv = priv;
+	priv->dev = dev;
+	priv->minor = pcif_minor++;
+
+	pcifpriv = priv;
+	if ( pcif_init(priv) ) {
+		printk("Failed to initialize PCIF driver\n");
+		free(priv);
+		dev->priv = NULL;
+		return DRVMGR_FAIL;
+	}
+
+	/* Host is always Big-Endian */
+	pci_endian = PCI_BIG_ENDIAN;
+
+	/* Register the PCI core at the PCI layer */
+
+	if (pci_access_drv_register(&pcif_access_drv)) {
+		/* Access routines registration failed */
+		return DRVMGR_FAIL;
+	}
+
+	/* Prepare memory MAP */
+	pcif_auto_cfg.options = 0;
+	pcif_auto_cfg.mem_start = 0;
+	pcif_auto_cfg.mem_size = 0;
+	pcif_auto_cfg.memio_start = priv->pci_area;
+	pcif_auto_cfg.memio_size = priv->pci_area_end - priv->pci_area;
+	pcif_auto_cfg.io_start = priv->pci_io;
+	pcif_auto_cfg.io_size = priv->pci_conf - priv->pci_io;
+	pcif_auto_cfg.irq_map = pcif_bus0_irq_map;
+	pcif_auto_cfg.irq_route = NULL; /* use standard routing */
+	pci_config_register(&pcif_auto_cfg);
+
+	if (pci_config_init()) {
+		/* PCI configuration failed */
+		return DRVMGR_FAIL;
+	}
+
+	priv->config.maps_down = &priv->maps_down[0];
+	priv->config.maps_up = &priv->maps_up[0];
+	return pcibus_register(dev, &priv->config);
+}
+
+int pcif_init3(struct drvmgr_dev *dev)
+{
+	struct pcif_priv *priv = dev->priv;
+
+	/* Unmask all interrupts, on some sytems this 
+	 * might be problematic because all PCI IRQs are
+	 * not connected on the PCB or used for something
+	 * else. The irqMask driver resource can be used to 
+	 * control which PCI IRQs are used to generate the
+	 * PCI system IRQ, example:
+	 *
+	 * 0xf - enable all  (DEFAULT)
+	 * 0x8 - enable one PCI irq
+	 *
+	 * Before unmasking PCI IRQ, all PCI boards must
+	 * have been initialized and IRQ turned off to avoid
+	 * system hang.
+	 */
+
+	priv->regs->intr = priv->irq_mask;
+
+	return DRVMGR_OK;
+}