LEON: updated shared drivers to Driver Manger framework

Some bugfixes at the same time. After this patch the drivers
may be used on RASTA systems having a big-endian PCI layout.

Removed not up to date changelogs, rely on git log instead.

10 files changed, 4391 insertions, 3843 deletions

diff --git a/c/src/lib/libbsp/sparc/shared/1553/b1553brm.c b/c/src/lib/libbsp/sparc/shared/1553/b1553brm.c
index 045146a58b..19ad36ca5f 100644
--- a/c/src/lib/libbsp/sparc/shared/1553/b1553brm.c
+++ b/c/src/lib/libbsp/sparc/shared/1553/b1553brm.c
@@ -2,52 +2,19 @@
  *  BRM driver
  *
  *  COPYRIGHT (c) 2006.
- *  Gaisler Research.
+ *  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.
- *
  */
 
 /********** Set defaults **********/
 
-/* basic bus/interface of device,
- * Default to direct accessed AMBA bus.
- */
-#ifndef B1553BRM_NO_AMBA
- #define B1553BRM_AMBA
- #undef B1553BRM_PCI
-#endif
-
-/* default name to /dev/brm */
-#if !defined(B1553BRM_DEVNAME) || !defined(B1553BRM_DEVNAME_NO)
- #undef B1553BRM_DEVNAME
- #undef B1553BRM_DEVNAME_NO
- #define B1553BRM_DEVNAME "/dev/brm0"
- #define B1553BRM_DEVNAME_NO(devstr,no) ((devstr)[8]='0'+(no))
-#endif
-
-#ifndef B1553BRM_PREFIX
- #define B1553BRM_PREFIX(name) b1553brm##name
-#else
- #define B1553BRM_REGISTER_STATIC
-#endif
-
-/* default to no translation */
-#ifndef B1553BRM_ADR_TO
- #define memarea_to_hw(x) ((unsigned int)(x))
-#endif
-
-#ifndef B1553BRM_REG_INT
- #define B1553BRM_REG_INT(handler,irqno,arg) set_vector(handler,(irqno)+0x10,1)
-  #undef  B1553BRM_DEFINE_INTHANDLER
-  #define B1553BRM_DEFINE_INTHANDLER
-#endif
-
-/* default to 128K memory layout */
-#if !defined(DMA_MEM_16K)
- #define DMA_MEM_128K
+/* default to 16K memory layout */
+#define DMA_MEM_128K
+#if !defined(DMA_MEM_128K)
+ #define DMA_MEM_16K
 #endif
 
 #include <bsp.h>
@@ -59,8 +26,10 @@
 #include <ctype.h>
 #include <rtems/bspIo.h>
 
+#include <drvmgr/drvmgr.h>
 #include <b1553brm.h>
 #include <ambapp.h>
+#include <drvmgr/ambapp_bus.h>
 
 /* Uncomment for debug output */
 /*#define DEBUG 1
@@ -70,24 +39,24 @@
 
 /* EVENT_QUEUE_SIZE sets the size of the event queue
  */
-#define EVENT_QUEUE_SIZE           1024
+#define EVENT_QUEUE_SIZE           1024  
 
 
 #define INDEX(x) ( x&(EVENT_QUEUE_SIZE-1) )
 
-#ifdef DEBUG
+#if 0
 #define DBG(x...) printk(x)
 #else
-#define DBG(x...)
+#define DBG(x...) 
 #endif
 
 #ifdef FUNCDEBUG
 #define FUNCDBG(x...) printk(x)
 #else
-#define FUNCDBG(x...)
+#define FUNCDBG(x...) 
 #endif
 
-#define READ_REG(address) _BRM_REG_READ16((unsigned int)address)
+#define READ_REG(address) (*(volatile unsigned int *)address)
 #define READ_DMA(address) _BRM_REG_READ16((unsigned int)address)
 static __inline__ unsigned short _BRM_REG_READ16(unsigned int addr) {
 	unsigned short tmp;
@@ -107,7 +76,7 @@ static rtems_device_driver brm_control(rtems_device_major_number major, rtems_de
 
 #define BRM_DRIVER_TABLE_ENTRY { brm_initialize, brm_open, brm_close, brm_read, brm_write, brm_control }
 
-static rtems_driver_address_table brm_driver = BRM_DRIVER_TABLE_ENTRY;
+static rtems_driver_address_table b1553brm_driver = BRM_DRIVER_TABLE_ENTRY;
 
 struct msg {
 	unsigned short miw;
@@ -134,165 +103,464 @@ struct irq_log_list {
         volatile unsigned short iaw;
 };
 
-typedef struct {
-
-    unsigned int memarea_base;
-    struct brm_reg *regs;
-
-    /* BRM descriptors */
-    struct desc_table {
-
-        volatile unsigned short ctrl;
-        volatile unsigned short top;
-        volatile unsigned short cur;
-        volatile unsigned short bot;
-
-    } *desc;
-
-    volatile unsigned short *mem;
-		/* bc mem struct */
+typedef struct { 
+
+	struct drvmgr_dev *dev; /* Driver manager device */
+	char devName[32]; /* Device Name */
+	struct brm_reg *regs;
+
+	unsigned int memarea_base;
+	unsigned int memarea_base_remote;
+	unsigned int cfg_clksel;
+	unsigned int cfg_clkdiv;
+	unsigned int cfg_freq;
+
+	/* BRM descriptors */
+	struct desc_table {
+		volatile unsigned short ctrl;        
+		volatile unsigned short top;
+		volatile unsigned short cur;
+		volatile unsigned short bot;
+	} *desc;
+
+	volatile unsigned short *mem;
+	/* bc mem struct */
+	struct {
+		/* BC Descriptors */
 		struct {
-			/* BC Descriptors */
-			struct {
-				unsigned short ctrl; /* control */
-				unsigned short cw1;  /* Command word 1*/
-				unsigned short cw2;  /* Command word 1*/
-				unsigned short dptr; /* Data pointer in halfword offset from bcmem */
-				unsigned short tsw[2]; /* status word 1 & 2 */
-				unsigned short ba;     /* branch address */
-				unsigned short timer;  /* timer value */
-			} descs[128]; /* 2k (1024 half words) */
-
-			/* message data */
-			struct {
-				unsigned short data[32]; /* 1 message's data */
-			} msg_data[128]; /* 8k */
+			unsigned short ctrl; /* control */
+			unsigned short cw1;  /* Command word 1*/
+			unsigned short cw2;  /* Command word 1*/
+			unsigned short dptr; /* Data pointer in halfword offset from bcmem */
+			unsigned short tsw[2]; /* status word 1 & 2 */
+			unsigned short ba;     /* branch address */
+			unsigned short timer;  /* timer value */
+		} descs[128]; /* 2k (1024 half words) */
+
+		/* message data */
+		struct {
+			unsigned short data[32]; /* 1 message's data */
+		} msg_data[128]; /* 8k */
 
 #if defined(DMA_MEM_128K)
-			/* offset to last 64bytes of 128k */
-			unsigned short unused[(64*1024-(128*8+128*32))-16*2];
+		/* offset to last 64bytes of 128k */
+		unsigned short unused[(64*1024-(128*8+128*32))-16*2];
 #elif defined(DMA_MEM_16K)
-			unsigned short unused[(8*1024-(128*8+128*32))-16*2];
+		unsigned short unused[(8*1024-(128*8+128*32))-16*2];
 #endif
-			/* interrupt log at 64 bytes from end */
-			struct irq_log_list irq_logs[16];
-		} *bcmem;
+		/* interrupt log at 64 bytes from end */
+		struct irq_log_list irq_logs[16];
+	} *bcmem;
 
 #if defined(DMA_MEM_128K)
-		/* Memory structure of a RT being inited, just used
-		 * for RT initialization.
-		 *
-		 * *mesgs[32] fit each minimally 8 messages per sub address.
-		 */
-		struct {
-			/* RX Sub Address descriptors */
-			struct desc_table rxsubs[32];
-			/* TX Sub Address descriptors */
-			struct desc_table txsubs[32];
-			/* RX mode code descriptors */
-			struct desc_table rxmodes[32];
-			/* TX mode code descriptors */
-			struct desc_table txmodes[32];
-
-			/* RX Sub Address messages */
-			struct circ_buf rxsuba_msgs[32];
-			/* TX Sub Address messages */
-			struct circ_buf txsuba_msgs[32];
-			/* RX Mode Code messages */
-			struct circ_buf rxmode_msgs[32];
-			/* RX Mode Code messages */
-			struct circ_buf txmode_msgs[32];
-
-
-			/* offset to last 64bytes of 128k: tot-used-needed */
-			unsigned short unused[(64*1024-(4*32*4+4*32*9*34))-16*2];
-
-			/* interrupt log at 64 bytes from end */
-			struct irq_log_list irq_logs[16];
-		} *rtmem;
+	/* Memory structure of a RT being inited, just used
+	 * for RT initialization.
+	 *
+	 * *mesgs[32] fit each minimally 8 messages per sub address.
+	 */
+	struct {
+		/* RX Sub Address descriptors */
+		struct desc_table rxsubs[32];
+		/* TX Sub Address descriptors */
+		struct desc_table txsubs[32];
+		/* RX mode code descriptors */
+		struct desc_table rxmodes[32];
+		/* TX mode code descriptors */
+		struct desc_table txmodes[32];
+
+		/* RX Sub Address messages */
+		struct circ_buf rxsuba_msgs[32];
+		/* TX Sub Address messages */
+		struct circ_buf txsuba_msgs[32];
+		/* RX Mode Code messages */
+		struct circ_buf rxmode_msgs[32];
+		/* RX Mode Code messages */
+		struct circ_buf txmode_msgs[32];
+
+		/* offset to last 64bytes of 128k: tot-used-needed */
+		unsigned short unused[(64*1024-(4*32*4+4*32*9*34))-16*2];
+
+		/* interrupt log at 64 bytes from end */
+		struct irq_log_list irq_logs[16];
+	} *rtmem;
 #elif defined(DMA_MEM_16K)
-		/* Memory structure of a RT being inited, just used
-		 * for RT initialization.
-		 *
-		 * circ_buf_2 *mesgs[32] fit each minimally 2 messages per queue.
-		 * circ_buf_1 *mesgs[32] fit each minimally 1 messages per queue.
-		 */
-		struct {
-			/* RX Sub Address descriptors */
-			struct desc_table rxsubs[32];
-			/* TX Sub Address descriptors */
-			struct desc_table txsubs[32];
-			/* RX mode code descriptors */
-			struct desc_table rxmodes[32];
-			/* TX mode code descriptors */
-			struct desc_table txmodes[32];
-
-			/* RX Sub Address messages */
-			struct circ_buf_2 rxsuba_msgs[32];
-			/* TX Sub Address messages */
-			struct circ_buf_2 txsuba_msgs[32];
-			/* RX Mode Code messages */
-			struct circ_buf_2 rxmode_msgs[32];
-			/* RX Mode Code messages */
-			struct circ_buf_1 txmode_msgs[32];
-
-
-			/* offset to last 64bytes of 16k: tot-used-needed */
-			unsigned short unused[8*1024 -(4*32*4 +3*32*2*34 +1*32*1*34) -16*2];
-
-			/* interrupt log at 64 bytes from end */
-			struct irq_log_list irq_logs[16];
-		} *rtmem;
+	/* Memory structure of a RT being inited, just used
+	 * for RT initialization.
+	 *
+	 * circ_buf_2 *mesgs[32] fit each minimally 2 messages per queue.
+	 * circ_buf_1 *mesgs[32] fit each minimally 1 messages per queue.
+	 */
+	struct {
+		/* RX Sub Address descriptors */
+		struct desc_table rxsubs[32];
+		/* TX Sub Address descriptors */
+		struct desc_table txsubs[32];
+		/* RX mode code descriptors */
+		struct desc_table rxmodes[32];
+		/* TX mode code descriptors */
+		struct desc_table txmodes[32];
+
+		/* RX Sub Address messages */
+		struct circ_buf_2 rxsuba_msgs[32];
+		/* TX Sub Address messages */
+		struct circ_buf_2 txsuba_msgs[32];
+		/* RX Mode Code messages */
+		struct circ_buf_2 rxmode_msgs[32];
+		/* RX Mode Code messages */
+		struct circ_buf_1 txmode_msgs[32];
+
+		/* offset to last 64bytes of 16k: tot-used-needed */
+		unsigned short unused[8*1024 -(4*32*4 +3*32*2*34 +1*32*1*34) -16*2];
+
+		/* interrupt log at 64 bytes from end */
+		struct irq_log_list irq_logs[16];
+	} *rtmem;
 #else
 	#error You must define one DMA_MEM_???K
 #endif
 
-    /* Interrupt log list */
-    struct irq_log_list *irq_log;
-    unsigned int irq;
+	/* Interrupt log list */
+	struct irq_log_list *irq_log;
+	unsigned int irq;
 
-    /* Received events waiting to be read */
-    struct rt_msg *rt_event;
-    struct bm_msg *bm_event;
+	/* Received events waiting to be read */
+	struct rt_msg *rt_event;
+	struct bm_msg *bm_event;
 
-    unsigned int head, tail;
+	unsigned int head, tail;
 
-    unsigned int last_read[128];
-    unsigned int written[32];
+	unsigned int last_read[128];
+	unsigned int written[32];
 
-    struct bc_msg *cur_list;
+	struct bc_msg *cur_list;
 
-    int tx_blocking, rx_blocking;
+	int tx_blocking, rx_blocking;
 
-    rtems_id rx_sem, tx_sem, dev_sem;
-		int minor;
-		int irqno;
-		unsigned int mode;
-#ifdef DEBUG
-		unsigned int log[EVENT_QUEUE_SIZE*4];
-		unsigned int log_i;
+	rtems_id rx_sem, tx_sem, dev_sem;
+	int minor;
+	int irqno;
+	unsigned int mode;
+#ifdef DEBUG    		
+	unsigned int log[EVENT_QUEUE_SIZE*4];
+	unsigned int log_i;
 #endif
 
-		rtems_id event_id; /* event that may be signalled upon errors, needs to be set through ioctl command BRM_SET_EVENTID */
-		unsigned int status;
-		int bc_list_fail;
+	rtems_id event_id; /* event that may be signalled upon errors, needs to be set through ioctl command BRM_SET_EVENTID */
+	unsigned int status;
+	int bc_list_fail;
 } brm_priv;
 
-static int brm_cores;
-static unsigned int allbrm_memarea;
-static brm_priv *brms;
-static struct ambapp_bus *amba_bus;
-static unsigned int	allbrm_cfg_clksel;
-static unsigned int allbrm_cfg_clkdiv;
-static unsigned int allbrm_cfg_freq;
-
-static void brm_interrupt(brm_priv *brm);
-#ifdef B1553BRM_DEFINE_INTHANDLER
-static void b1553brm_interrupt_handler(rtems_vector_number v);
-#endif
+static void b1553brm_interrupt(void *arg);
+static rtems_device_driver rt_init(brm_priv *brm);
 
 #define OFS(ofs) (((unsigned int)&ofs & 0x1ffff)>>1)
 
+static int b1553brm_driver_io_registered = 0;
+static rtems_device_major_number b1553brm_driver_io_major = 0;
+
+/******************* Driver manager interface ***********************/
+
+/* Driver prototypes */
+int b1553brm_register_io(rtems_device_major_number *m);
+int b1553brm_device_init(brm_priv *pDev);
+
+int b1553brm_init2(struct drvmgr_dev *dev);
+int b1553brm_init3(struct drvmgr_dev *dev);
+int b1553brm_remove(struct drvmgr_dev *dev);
+
+struct drvmgr_drv_ops b1553brm_ops = 
+{
+	.init = {NULL, b1553brm_init2, b1553brm_init3, NULL},
+	.remove = b1553brm_remove,
+	.info = NULL
+};
+
+struct amba_dev_id b1553brm_ids[] = 
+{
+	{VENDOR_GAISLER, GAISLER_B1553BRM},
+	{0, 0}		/* Mark end of table */
+};
+
+struct amba_drv_info b1553brm_drv_info =
+{
+	{
+		DRVMGR_OBJ_DRV,				/* Driver */
+		NULL,					/* Next driver */
+		NULL,					/* Device list */
+		DRIVER_AMBAPP_GAISLER_B1553BRM_ID,	/* Driver ID */
+		"B1553BRM_DRV",				/* Driver Name */
+		DRVMGR_BUS_TYPE_AMBAPP,			/* Bus Type */
+		&b1553brm_ops,
+		NULL,					/* Funcs */
+		0,					/* No devices yet */
+		0,
+	},
+	&b1553brm_ids[0]
+};
+
+void b1553brm_register_drv (void)
+{
+	DBG("Registering B1553BRM driver\n");
+	drvmgr_drv_register(&b1553brm_drv_info.general);
+}
+
+int b1553brm_init2(struct drvmgr_dev *dev)
+{
+	brm_priv *priv;
+
+	DBG("B1553BRM[%d] on bus %s\n", dev->minor_drv, dev->parent->dev->name);
+	priv = dev->priv = malloc(sizeof(brm_priv));
+	if ( !priv )
+		return DRVMGR_NOMEM;
+	memset(priv, 0, sizeof(*priv));
+	priv->dev = dev;
+
+	/* This core will not find other cores, so we wait for init2() */
+
+	return DRVMGR_OK;
+}
+
+int b1553brm_init3(struct drvmgr_dev *dev)
+{
+	brm_priv *priv;
+	char prefix[32];
+	rtems_status_code status;
+
+	priv = dev->priv;
+
+	/* Do initialization */
+
+	if ( b1553brm_driver_io_registered == 0) {
+		/* Register the I/O driver only once for all cores */
+		if ( b1553brm_register_io(&b1553brm_driver_io_major) ) {
+			/* Failed to register I/O driver */
+			dev->priv = NULL;
+			return DRVMGR_FAIL;
+		}
+
+		b1553brm_driver_io_registered = 1;
+	}
+
+	/* I/O system registered and initialized 
+	 * Now we take care of device initialization.
+	 */
+
+	if ( b1553brm_device_init(priv) ) {
+		return DRVMGR_FAIL;
+	}
+
+	/* Get Filesystem name prefix */
+	prefix[0] = '\0';
+	if ( drvmgr_get_dev_prefix(dev, prefix) ) {
+		/* Failed to get prefix, make sure of a unique FS name
+		 * by using the driver minor.
+		 */
+		sprintf(priv->devName, "/dev/b1553brm%d", dev->minor_drv);
+	} else {
+		/* Got special prefix, this means we have a bus prefix
+		 * And we should use our "bus minor"
+		 */
+		sprintf(priv->devName, "/dev/%sb1553brm%d", prefix, dev->minor_bus);
+	}
+
+	/* Register Device */
+	status = rtems_io_register_name(priv->devName, b1553brm_driver_io_major, dev->minor_drv);
+	if (status != RTEMS_SUCCESSFUL) {
+		return DRVMGR_FAIL;
+	}
+
+	return DRVMGR_OK;
+}
+
+int b1553brm_remove(struct drvmgr_dev *dev)
+{
+	/* Stop more tasks to open driver */
+
+	/* Throw out all tasks using this driver */
+
+	/* Unregister I/O node */
+
+	/* Unregister and disable Interrupt */
+
+	/* Free device memory */
+
+	/* Return sucessfully */
+
+	return DRVMGR_OK;
+}
+
+/******************* Driver Implementation ***********************/
+
+int b1553brm_register_io(rtems_device_major_number *m)
+{
+	rtems_status_code r;
+
+	if ((r = rtems_io_register_driver(0, &b1553brm_driver, m)) == RTEMS_SUCCESSFUL) {
+		DBG("B1553BRM driver successfully registered, major: %d\n", *m);
+	} else {
+		switch(r) {
+		case RTEMS_TOO_MANY:
+			printk("B1553BRM rtems_io_register_driver failed: RTEMS_TOO_MANY\n");
+			return -1;
+		case RTEMS_INVALID_NUMBER:  
+			printk("B1553BRM rtems_io_register_driver failed: RTEMS_INVALID_NUMBER\n");
+			return -1;
+		case RTEMS_RESOURCE_IN_USE:
+			printk("B1553BRM rtems_io_register_driver failed: RTEMS_RESOURCE_IN_USE\n");
+			return -1;
+		default:
+			printk("B1553BRM rtems_io_register_driver failed\n");
+			return -1;
+		}
+	}
+	return 0;
+}
+
+int b1553brm_device_init(brm_priv *pDev)
+{
+	struct amba_dev_info *ambadev;
+	struct ambapp_core *pnpinfo;
+	union drvmgr_key_value *value;
+	unsigned int mem;
+	int size;
+
+	/* Get device information from AMBA PnP information */
+	ambadev = (struct amba_dev_info *)pDev->dev->businfo;
+	if ( ambadev == NULL ) {
+		return -1;
+	}
+	pnpinfo = &ambadev->info;
+	pDev->irqno = pnpinfo->irq;
+	/* Two versions of the BRM core. One where the registers are accessed using the AHB bus 
+	 * and one where the APB bus is used
+	 */
+	if ( pnpinfo->ahb_slv ) {
+		/* Registers accessed over AHB */
+		pDev->regs = (struct brm_reg *)pnpinfo->ahb_slv->start[0];
+	} else {
+		/* Registers accessed over APB */
+		pDev->regs = (struct brm_reg *)pnpinfo->apb_slv->start;
+	}
+	pDev->minor = pDev->dev->minor_drv;
+#ifdef DEBUG
+	pDev->log_i = 0;	
+	memset(pDev->log,0,sizeof(pDev->log));
+#endif
+
+#ifdef DMA_MEM_128K
+	size = 128 * 1024;
+#else
+	size = 16 * 1024;
+#endif
+
+	/* Get memory configuration from bus resources */
+	value = drvmgr_dev_key_get(pDev->dev, "dmaBaseAdr", KEY_TYPE_POINTER);
+	if (value)
+		mem = (unsigned int)value->ptr;
+
+	if (value && (mem & 1)) {
+		/* Remote address, address as BRM looks at it. */
+
+		/* Translate the base address into an address that the the CPU can understand */
+		pDev->memarea_base_remote = mem & ~1;
+		drvmgr_translate_check(pDev->dev, DMAMEM_TO_CPU,
+					(void *)pDev->memarea_base_remote,
+					(void **)&pDev->memarea_base,
+					size);
+	} else {
+		if (!value) {
+			/* Use dynamically allocated memory + 128k for
+			 * alignment
+			 */
+			mem = (unsigned int)malloc(size + 128 * 1024);
+			if (!mem){
+				printk("BRM: Failed to allocate HW memory\n\r");
+				return -1;
+			}
+			/* align memory to 128k boundary */
+			pDev->memarea_base = (mem + 0x1ffff) & ~0x1ffff;
+		} else {
+			pDev->memarea_base = mem;
+		}
+
+		/* Translate the base address into an address that the BRM core can understand */
+		drvmgr_translate_check(pDev->dev, CPUMEM_TO_DMA,
+					(void *)pDev->memarea_base,
+					(void **)&pDev->memarea_base_remote,
+					size);
+	}
+
+	/* clear the used memory */
+	memset((char *)pDev->memarea_base, 0, size);
+
+	/* Set base address of all descriptors */
+	pDev->desc = (struct desc_table *) pDev->memarea_base;
+	pDev->mem = (volatile unsigned short *) pDev->memarea_base;
+	pDev->irq_log	= (struct irq_log_list *)(pDev->memarea_base + (0xFFE0<<1)); /* last 64byte */
+
+	pDev->bm_event = NULL;
+	pDev->rt_event = NULL;
+
+	pDev->cfg_clksel = 0;
+	pDev->cfg_clkdiv = 0;
+	pDev->cfg_freq = BRM_FREQ_24MHZ;
+
+	value = drvmgr_dev_key_get(pDev->dev, "clkSel", KEY_TYPE_INT);
+	if ( value ) {
+		pDev->cfg_clksel = value->i & CLKSEL_MASK;
+	}
+
+	value = drvmgr_dev_key_get(pDev->dev, "clkDiv", KEY_TYPE_INT);
+	if ( value ) {
+		pDev->cfg_clkdiv = value->i & CLKDIV_MASK;
+	}
+
+	value = drvmgr_dev_key_get(pDev->dev, "coreFreq", KEY_TYPE_INT);
+	if ( value ) {
+		pDev->cfg_freq = value->i & BRM_FREQ_MASK;
+	}
+
+	/* Sel clock so that we can write to BRM's registers */
+	pDev->regs->w_ctrl = (pDev->cfg_clksel<<9) | (pDev->cfg_clkdiv<<5);
+	/* Reset BRM core */
+	pDev->regs->w_ctrl = 1<<10 | READ_REG(&pDev->regs->w_ctrl);
+
+	/* RX Semaphore created with count = 0 */
+	if ( rtems_semaphore_create(rtems_build_name('B', 'M', 'R', '0' + pDev->minor),
+		0,
+		RTEMS_FIFO|RTEMS_SIMPLE_BINARY_SEMAPHORE|RTEMS_NO_INHERIT_PRIORITY|RTEMS_LOCAL|RTEMS_NO_PRIORITY_CEILING, 
+		0,
+		&pDev->rx_sem) != RTEMS_SUCCESSFUL ) {
+		printk("BRM: Failed to create rx semaphore\n");
+		return RTEMS_INTERNAL_ERROR;
+	}
+
+	/* TX Semaphore created with count = 1 */
+	if ( rtems_semaphore_create(rtems_build_name('B', 'M', 'T', '0' + pDev->minor),
+		1,
+		RTEMS_FIFO|RTEMS_SIMPLE_BINARY_SEMAPHORE|RTEMS_NO_INHERIT_PRIORITY|RTEMS_LOCAL|RTEMS_NO_PRIORITY_CEILING, 
+		0,
+		&pDev->tx_sem) != RTEMS_SUCCESSFUL ){
+		printk("BRM: Failed to create tx semaphore\n");
+		return RTEMS_INTERNAL_ERROR;
+	}
+
+	/* Device Semaphore created with count = 1 */
+	if ( rtems_semaphore_create(rtems_build_name('B', 'M', 'D', '0' + pDev->minor),
+		1,
+		RTEMS_FIFO|RTEMS_SIMPLE_BINARY_SEMAPHORE|RTEMS_NO_INHERIT_PRIORITY|RTEMS_LOCAL|RTEMS_NO_PRIORITY_CEILING, 
+		0,
+		&pDev->dev_sem) != RTEMS_SUCCESSFUL ){
+		printk("BRM: Failed to create device semaphore\n");
+		return RTEMS_INTERNAL_ERROR;
+	}
+
+	/* Default to RT-mode */
+	rt_init(pDev);
+
+	return 0;
+}
+
 static int odd_parity(unsigned int data) {
     unsigned int i=0;
 
@@ -300,12 +568,11 @@ static int odd_parity(unsigned int data) {
     {
         i++;
         data &= (data - 1);
-    }
+    } 
 
     return !(i&1);
 }
 
-
 static void start_operation(brm_priv *brm) {
 	unsigned int ctrl = READ_REG(&brm->regs->ctrl);
 	brm->regs->ctrl = ctrl | 0x8000;
@@ -315,80 +582,12 @@ static void stop_operation(brm_priv *brm) {
 	unsigned int ctrl = READ_REG(&brm->regs->ctrl);
   brm->regs->ctrl = ctrl & ~0x8000;
 }
+
 static int is_executing(brm_priv *brm) {
 	unsigned int ctrl = READ_REG(&brm->regs->ctrl);
 	return ((ctrl>>15) & 1);
 }
 
-#ifdef LEON3
-#ifndef DONT_DEF_RAMON
-int brm_register_leon3_ramon_fpga(void){
-	/* Clock div & Clock sel is NOT available.
-	 * The BRM is always clocked with 24MHz.
-	 * 3 in BRM enhanced register will select 24MHz
-	 */
-	return b1553brm_register(&ambapp_plb, 0, 0, 3);
-}
-
-int brm_register_leon3_ramon_asic(void){
-	/* Clock div & Clock sel is available.
-	 * Clkdiv only matter when clksel is 1.
-	 * clksel=2, clkdiv=don't care, brm_frq=24MHz
-	 *
-	 * 3 in BRM enhanced register will select 24MHz
-	 */
-	return b1553brm_register(&ambapp_plb, 2, 0, 3);
-}
-#endif
-#endif
-
-#ifdef B1553BRM_REGISTER_STATIC
-static
-#endif
-int B1553BRM_PREFIX(_register)(struct ambapp_bus *bus, unsigned int clksel, unsigned int clkdiv, unsigned int brm_freq)
-{
-    rtems_status_code r;
-    rtems_device_major_number m;
-
-    FUNCDBG("brm_register:\n\r");
-
-		/* save amba bus pointer */
-		amba_bus = bus;
-		if ( !bus ){
-			printk("brm_register: bus is NULL\n\r");
-			return 1;
-		}
-
-#ifdef B1553BRM_LOCAL_MEM
-		allbrm_memarea = B1553BRM_LOCAL_MEM_ADR;
-#else
-		allbrm_memarea = 0;
-#endif
-
-		/* Save clksel, clkdiv and brm_freq for later use */
-		allbrm_cfg_clksel = clksel & CLKSEL_MASK;
-		allbrm_cfg_clkdiv = clkdiv & CLKDIV_MASK;
-		allbrm_cfg_freq = brm_freq & BRM_FREQ_MASK;
-
-    if ((r = rtems_io_register_driver(0, &brm_driver, &m)) == RTEMS_SUCCESSFUL) {
-        DBG("BRM: driver successfully registered, major: %d\n",m);
-
-    } else {
-        switch(r) {
-        case RTEMS_TOO_MANY:
-            printk("BRM rtems_io_register_driver failed: RTEMS_TOO_MANY\n"); break;
-        case RTEMS_INVALID_NUMBER:
-            printk("BRM rtems_io_register_driver failed: RTEMS_INVALID_NUMBER\n"); break;
-        case RTEMS_RESOURCE_IN_USE:
-            printk("BRM rtems_io_register_driver failed: RTEMS_RESOURCE_IN_USE\n"); break;
-        default:
-           printk("BRM rtems_io_register_driver failed\n");
-        }
-				return 1;
-    }
-		return 0;
-}
-
 static void clr_int_logs(struct irq_log_list *logs){
 	int i;
 	for(i=0; i<16; i++){
@@ -403,18 +602,18 @@ static rtems_device_driver rt_init(brm_priv *brm) {
 	brm->head = brm->tail = 0;
 	brm->rx_blocking = brm->tx_blocking = 1;
 
-  if ( brm->bm_event )
+	if ( brm->bm_event )
 		free(brm->bm_event);
 	brm->bm_event = NULL;
 
-  if ( brm->rt_event )
+	if ( brm->rt_event )
 		free(brm->rt_event);
-
+	
 	brm->bcmem = NULL;
 	brm->rtmem = (void *)brm->mem;
 
 	brm->rt_event = (struct rt_msg *) malloc(EVENT_QUEUE_SIZE*sizeof(struct rt_msg));
-
+  
 	if (brm->rt_event == NULL) {
 		DBG("BRM driver failed to allocated memory.");
 		return RTEMS_NO_MEMORY;
@@ -427,11 +626,11 @@ static rtems_device_driver rt_init(brm_priv *brm) {
 	brm->regs->imask	 = BRM_RT_ILLCMD_IRQ|BRM_SUBAD_IRQ|BRM_TAPF_IRQ|BRM_DMAF_IRQ|BRM_WRAPF_IRQ|BRM_MERR_IRQ;
 	brm->regs->dpoint	= 0;
 	brm->regs->ipoint	= OFS(brm->rtmem->irq_logs[0]);
-	brm->regs->enhanced  = 0x0000 | allbrm_cfg_freq;  /* BRM clocked with freq = 12,16,20 or 24MHz */
-	brm->regs->w_ctrl	= (allbrm_cfg_clksel<<9) | (allbrm_cfg_clkdiv<<5) | 1;
+	brm->regs->enhanced  = 0x0000 | brm->cfg_freq;  /* BRM clocked with freq = 12,16,20 or 24MHz */
+	brm->regs->w_ctrl	= (brm->cfg_clksel<<9) | (brm->cfg_clkdiv<<5) | 1;
 	brm->regs->w_irqctrl = 6;
-	brm->regs->w_ahbaddr = (unsigned int) memarea_to_hw(brm->memarea_base);
-
+	brm->regs->w_ahbaddr = brm->memarea_base_remote;
+		
 	clr_int_logs(brm->irq_log);
 
 	/* Legalize all commands */
@@ -439,19 +638,19 @@ static rtems_device_driver rt_init(brm_priv *brm) {
 		brm->regs->rt_cmd_leg[i] = 0;
 	}
 
-	/* Init descriptor table
-	 *
+	/* Init descriptor table 
+	 * 
 	 * Each circular buffer has room for 8 messages with up to 34 (32 data + miw + time) words (16b) in each.
 	 * The buffers must separated by 34 words.
 	 */
 
-
+ 
 	/* RX Sub-address 0 - 31 */
 	for (i = 0; i < 32; i++) {
 		brm->rtmem->rxsubs[i].ctrl = 0x00E0;				/* Interrupt: INTX, IWA, and IBRD */
 		brm->rtmem->rxsubs[i].top  = OFS(brm->rtmem->rxsuba_msgs[i]);		     /* Top address */
 		brm->rtmem->rxsubs[i].cur  = OFS(brm->rtmem->rxsuba_msgs[i]);		     /* Current address */
-		brm->rtmem->rxsubs[i].bot  = OFS(brm->rtmem->rxsuba_msgs[i+1]) - sizeof(struct msg)/2; /* Bottom address */
+		brm->rtmem->rxsubs[i].bot  = OFS(brm->rtmem->rxsuba_msgs[i+1]) - sizeof(struct msg)/2; /* Bottom address */	
 		brm->last_read[i] = OFS(brm->rtmem->rxsuba_msgs[i]);
 	}
 	/* TX Sub-address 0 - 31 */
@@ -470,7 +669,7 @@ static rtems_device_driver rt_init(brm_priv *brm) {
 		brm->rtmem->rxmodes[i].cur  = OFS(brm->rtmem->rxmode_msgs[i]);			  /* Current address */
 		brm->rtmem->rxmodes[i].bot  = OFS(brm->rtmem->rxmode_msgs[i+1]) - sizeof(struct msg)/2;	/* Bottom address */
 		brm->last_read[i+64] = OFS(brm->rtmem->rxmode_msgs[i]);
-	}
+	}   
 	/* TX mode code 0 - 31 */
 	for (i = 0; i < 32; i++) {
 		brm->rtmem->txmodes[i].ctrl = 0x0060;					/* Interrupt: IWA and IBRD */
@@ -480,6 +679,12 @@ static rtems_device_driver rt_init(brm_priv *brm) {
 		brm->last_read[i+96] = OFS(brm->rtmem->txmode_msgs[i]);
 	}
 
+#ifdef DEBUG
+	printk("b1553BRM DMA_AREA: 0x%x\n", (unsigned int)brm->rtmem);
+	printk("LOG: 0x%x\n", &brm->log[0]);
+	printk("LOG_I: 0x%x\n", &brm->log_i);
+#endif
+
 	brm->mode = BRM_MODE_RT;
 
 	return RTEMS_SUCCESSFUL;
@@ -487,36 +692,35 @@ static rtems_device_driver rt_init(brm_priv *brm) {
 
 static rtems_device_driver bc_init(brm_priv *brm){
 
-  if ( brm->bm_event )
+	if ( brm->bm_event )
 		free(brm->bm_event);
 	brm->bm_event = NULL;
 
-  if ( brm->rt_event )
+	if ( brm->rt_event )
 		free(brm->rt_event);
 	brm->rt_event = NULL;
-
+	
 	brm->bcmem = (void *)brm->mem;
 	brm->rtmem = NULL;
 	brm->irq_log = (struct irq_log_list *)&brm->bcmem->irq_logs[0];
-
+	
 	brm->head = brm->tail = 0;
 	brm->rx_blocking = brm->tx_blocking = 1;
-
+	
 	brm->regs->ctrl	  = 0x0006;  /* ping pong enable and enable interrupt log */
 	brm->regs->oper	  = 0x0800;  /* configure as BC */
 	brm->regs->imask	 = BRM_EOL_IRQ|BRM_BC_ILLCMD_IRQ|BRM_ILLOP_IRQ|BRM_DMAF_IRQ|BRM_WRAPF_IRQ|BRM_MERR_IRQ;
 	brm->regs->dpoint	= 0;
-	printk("Set BC interrupt log: 0x%lx, 0x%lx, 0x%lx\n",OFS(brm->bcmem->irq_logs[0]),&brm->bcmem->irq_logs[0],brm->bcmem);
 	brm->regs->ipoint	= OFS(brm->bcmem->irq_logs[0]);
-	brm->regs->enhanced  = 0x0000 | (allbrm_cfg_freq&0x3);  /* freq = 24 */
-	brm->regs->w_ctrl	= (allbrm_cfg_clksel<<9) | (allbrm_cfg_clkdiv<<5) | 1;
+	brm->regs->enhanced  = 0x0000 | (brm->cfg_freq&BRM_FREQ_MASK);  /* freq = 24 */
+	brm->regs->w_ctrl	= (brm->cfg_clksel<<9) | (brm->cfg_clkdiv<<5) | 1;
 	brm->regs->w_irqctrl = 6;
-	brm->regs->w_ahbaddr = (unsigned int) memarea_to_hw(brm->memarea_base);
-
+	brm->regs->w_ahbaddr = brm->memarea_base_remote;
+	
 	clr_int_logs(brm->irq_log);
-
+	
 	brm->mode = BRM_MODE_BC;
-
+	
 	return RTEMS_SUCCESSFUL;
 }
 
@@ -526,18 +730,18 @@ static rtems_device_driver bm_init(brm_priv *brm) {
 	brm->head = brm->tail = 0;
 	brm->rx_blocking = brm->tx_blocking = 1;
 
-  if ( brm->rt_event )
+	if ( brm->rt_event )
 		free(brm->rt_event);
 	brm->rt_event = NULL;
 
-  if ( brm->bm_event )
+	if ( brm->bm_event )
 		free(brm->bm_event);
-
+	
 	brm->bcmem = NULL;
 	brm->rtmem = NULL;
-
+	
 	brm->bm_event	 = (struct bm_msg *) malloc(EVENT_QUEUE_SIZE*sizeof(struct bm_msg));
-
+ 
 	if (brm->bm_event == NULL) {
 		DBG("BRM driver failed to allocated memory.");
 		return RTEMS_NO_MEMORY;
@@ -548,761 +752,634 @@ static rtems_device_driver bm_init(brm_priv *brm) {
 
 	brm->regs->ctrl	  = 0x0006;  /* ping pong enable and enable interrupt log */
 	brm->regs->oper	  = 0x0A00;  /* configure as BM */
-	brm->regs->imask	= BRM_MBC_IRQ|BRM_MERR_IRQ|BRM_DMAF_IRQ|BRM_MERR_IRQ;
+	brm->regs->imask	= BRM_MBC_IRQ|BRM_MERR_IRQ|BRM_DMAF_IRQ;
 	brm->regs->dpoint	= 0;
 	brm->regs->ipoint	= OFS(brm->mem[8*1024-16*2]);
 	brm->regs->mcpoint   = 0;	   /* Command pointer */
 	brm->regs->mdpoint   = 0x100;   /* Data pointer */
 	brm->regs->mbc	   = 1;	   /* Block count */
-	brm->regs->enhanced  = 0x0000 | (allbrm_cfg_freq&0x3);  /* freq = 24 */
-	brm->regs->w_ctrl	= (allbrm_cfg_clksel<<9) | (allbrm_cfg_clkdiv<<5) | 1;
+	brm->regs->enhanced  = 0x0000 | (brm->cfg_freq&BRM_FREQ_MASK);  /* freq = 24 */
+	brm->regs->w_ctrl	= (brm->cfg_clksel<<9) | (brm->cfg_clkdiv<<5) | 1;
 	brm->regs->w_irqctrl = 6;
-	brm->regs->w_ahbaddr = (unsigned int) memarea_to_hw(brm->memarea_base);
-
+	brm->regs->w_ahbaddr = brm->memarea_base_remote;
+	
 	clr_int_logs(brm->irq_log);
-
+	
 	brm->mode = BRM_MODE_BM;
-
+	
 	return RTEMS_SUCCESSFUL;
 }
 
 
 static rtems_device_driver brm_initialize(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
 {
-	rtems_status_code status;
-	int dev_cnt;
-	char fs_name[20];
-	brm_priv *brm;
-	struct ambapp_ahb_info ambadev;
-	char *mem;
-
-	FUNCDBG("brm_initialize\n");
+	return RTEMS_SUCCESSFUL;
+}
 
-	brm_cores = 0;
-	strcpy(fs_name,B1553BRM_DEVNAME);
+static rtems_device_driver brm_open(rtems_device_major_number major, rtems_device_minor_number minor, void *arg) {
+	brm_priv *brm;
+	struct drvmgr_dev *dev;
 
-	/* Find all BRM devices */
-	dev_cnt = ambapp_get_number_ahbslv_devices(amba_bus, VENDOR_GAISLER, GAISLER_B1553BRM);
-	if ( dev_cnt < 1 ){
-		/* Failed to find any CAN cores! */
-		printk("BRM: Failed to find any BRM cores\n\r");
-		return -1;
-	}
+	FUNCDBG("brm_open\n");
 
-	/* allocate & zero memory for the brm devices */
-	brms = (brm_priv *)malloc(sizeof(*brms)*dev_cnt);
-	if ( !brms ){
-		printk("BRM: Failed to allocate SW memory\n\r");
-		return -1;
+	if ( drvmgr_get_dev(&b1553brm_drv_info.general, minor, &dev) ) {
+		DBG("Wrong minor %d\n", minor);
+		return RTEMS_UNSATISFIED;
 	}
-	memset(brms,0,sizeof(*brms)*dev_cnt);
-
-	/* Allocate memory for all device's descriptors,
-	 * they must be aligned to a XXX byte boundary.
-	 */
-	#define BRM_DESCS_PER_CTRL 128
-	if ( allbrm_memarea ){
-		mem = (char *)allbrm_memarea;
-	}else{
- 		/* sizeof(struct desc_table) * BRM_DESCS_PER_CTRL * dev_cnt */
-		mem = (char *)malloc( (128*1024) * (dev_cnt+1)); /* 128k per core + 128k for alignment */
-		if ( !mem ){
-			free(brms);
-			printk("BRM: Failed to allocate HW memory\n\r");
-			return -1;
-		}
+	brm = (brm_priv *)dev->priv;
 
-		/* align memory to 128k boundary */
-		mem = (char *)(((unsigned int)mem+0x1ffff) & ~0x1ffff);
+	if (rtems_semaphore_obtain(brm->dev_sem, RTEMS_NO_WAIT, RTEMS_NO_TIMEOUT) != RTEMS_SUCCESSFUL) {
+		DBG("brm_open: resource in use\n");
+		return RTEMS_RESOURCE_IN_USE; /* EBUSY */
 	}
 
-	/* clear the used memory */
-	memset(mem,0,(128*1024) * dev_cnt);
-
-	/* initialize each brm device, one at a time */
-	for(minor=0; minor<dev_cnt; minor++){
-		brm = &brms[minor];
-
-		/* Get AMBA AHB device info from Plug&Play */
-		ambapp_find_ahbslv_next(amba_bus, VENDOR_GAISLER,
-		                        GAISLER_B1553BRM, &ambadev, minor);
+	/* Set defaults */
+	brm->event_id = 0;
 
-		/* Copy Basic HW info */
-		brm->regs = (void *)ambadev.start[0];
-		brm->irqno = ambadev.irq;
-		brm->minor = minor;
-		brm->irq = 0;
-#ifdef DEBUG
-		brm->log_i = 0;
-		memset(brm->log,0,sizeof(brm->log));
-#endif
+	start_operation(brm);
 
-		/* Set unique name */
-		B1553BRM_DEVNAME_NO(fs_name,minor);
-
-    DBG("Registering BRM core at [0x%x] irq %d, minor %d as %s\n",brm->regs,brm->irqno,minor,fs_name);
-
-		/* Bind filesystem name to device number (minor) */
-		status = rtems_io_register_name(fs_name, major, minor);
-    if (status != RTEMS_SUCCESSFUL)
-			rtems_fatal_error_occurred(status);
-
-		/* RX Semaphore created with count = 0 */
-		if ( rtems_semaphore_create(rtems_build_name('B', 'M', 'R', '0'+minor),
-		         0,
-		         RTEMS_FIFO|RTEMS_SIMPLE_BINARY_SEMAPHORE|RTEMS_NO_INHERIT_PRIORITY|RTEMS_LOCAL|RTEMS_NO_PRIORITY_CEILING,
-		         0,
-		         &brm->rx_sem) != RTEMS_SUCCESSFUL ){
-      printk("BRM: Failed to create rx semaphore\n");
-	  	return RTEMS_INTERNAL_ERROR;
-    }
-
-			/* TX Semaphore created with count = 1 */
-		if ( rtems_semaphore_create(rtems_build_name('B', 'M', 'T', '0'+minor),
-		         1,
-		         RTEMS_FIFO|RTEMS_SIMPLE_BINARY_SEMAPHORE|RTEMS_NO_INHERIT_PRIORITY|RTEMS_LOCAL|RTEMS_NO_PRIORITY_CEILING,
-		         0,
-		         &brm->tx_sem) != RTEMS_SUCCESSFUL ){
-      printk("BRM: Failed to create tx semaphore\n");
-	  	return RTEMS_INTERNAL_ERROR;
-    }
-
-		/* Device Semaphore created with count = 1 */
-		if ( rtems_semaphore_create(rtems_build_name('B', 'M', 'D', '0'+minor),
-		         1,
-		         RTEMS_FIFO|RTEMS_SIMPLE_BINARY_SEMAPHORE|RTEMS_NO_INHERIT_PRIORITY|RTEMS_LOCAL|RTEMS_NO_PRIORITY_CEILING,
-		         0,
-		         &brm->dev_sem) != RTEMS_SUCCESSFUL ){
-      printk("BRM: Failed to create device semaphore\n");
-	  	return RTEMS_INTERNAL_ERROR;
-    }
-
-
-		/* Set base address of all descriptors */
-		brm->memarea_base = (unsigned int)&mem[(128*1024) * minor];
-		brm->desc = (struct desc_table *) brm->memarea_base;
-		brm->mem = (volatile unsigned short *) brm->memarea_base;
- 	  brm->irq_log	= (struct irq_log_list *)(brm->memarea_base + (0xFFE0<<1)); /* last 64byte */
-
-		brm->bm_event = NULL;
-		brm->rt_event = NULL;
-
-		/* Sel clock so that we can write to BRM's registers */
-		brm->regs->w_ctrl = (allbrm_cfg_clksel<<9) | (allbrm_cfg_clkdiv<<5);
-		/* Reset BRM core */
-		brm->regs->w_ctrl = 1<<10 | READ_REG(&brm->regs->w_ctrl);
-
-		/* Register interrupt handler */
-		B1553BRM_REG_INT(B1553BRM_PREFIX(_interrupt_handler), brm->irqno, brm);
-
-		rt_init(brm);
-
-		DBG("BRM: LOG: 0x%lx, 0x%lx\n\r",brm->log,brm);
+	/* Register interrupt routine */
+	if ( drvmgr_interrupt_register(brm->dev, 0, "b1553brm", b1553brm_interrupt, brm) ) {
+		rtems_semaphore_release(brm->dev_sem);
+		return RTEMS_UNSATISFIED;
 	}
 
-	/* save number of BRM cores found */
-	brm_cores = dev_cnt;
-
-	DBG("BRM initialisation done.\n");
-
 	return RTEMS_SUCCESSFUL;
 }
-
-static rtems_device_driver brm_open(rtems_device_major_number major, rtems_device_minor_number minor, void *arg) {
-		brm_priv *brm;
-
-    FUNCDBG("brm_open\n");
-
-    if (minor >= brm_cores) {
-        DBG("Wrong minor %d\n", minor);
-        return RTEMS_UNSATISFIED; /* ENODEV */
-    }
-
-		brm = &brms[minor];
-
-    if (rtems_semaphore_obtain(brm->dev_sem, RTEMS_NO_WAIT, RTEMS_NO_TIMEOUT) != RTEMS_SUCCESSFUL) {
-        DBG("brm_open: resource in use\n");
-        return RTEMS_RESOURCE_IN_USE; /* EBUSY */
-    }
-
-		/* Set defaults */
-		brm->event_id = 0;
-
-    start_operation(brm);
-
-    return RTEMS_SUCCESSFUL;
-}
-
+ 
 static rtems_device_driver brm_close(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
 {
-	brm_priv *brm = &brms[minor];
+	brm_priv *brm;
+	struct drvmgr_dev *dev;
+
 	FUNCDBG("brm_close");
+	
+	if ( drvmgr_get_dev(&b1553brm_drv_info.general, minor, &dev) ) {
+		return RTEMS_UNSATISFIED;
+	}
+	brm = (brm_priv *)dev->priv;
+
+	drvmgr_interrupt_unregister(brm->dev, 0, b1553brm_interrupt, brm);
 
 	stop_operation(brm);
 	rtems_semaphore_release(brm->dev_sem);
 
 	return RTEMS_SUCCESSFUL;
 }
+ 
+static int get_rt_messages(brm_priv *brm, void *buf, unsigned int msg_count)
+{
+	struct rt_msg *dest = (struct rt_msg *) buf;
+	int count = 0;
 
-static int get_rt_messages(brm_priv *brm, void *buf, unsigned int msg_count) {
-
-    struct rt_msg *dest = (struct rt_msg *) buf;
-    int count = 0;
-
-    if (brm->head == brm->tail) {
-        return 0;
-    }
-
-    do {
-
-        DBG("rt read - head: %d, tail: %d\n", brm->head, brm->tail);
-        dest[count++] = brm->rt_event[INDEX(brm->tail++)];
+	if (brm->head == brm->tail) {
+		return 0;
+	}
 
-    } while (brm->head != brm->tail && count < msg_count);
+	do {
 
-    return count;
+		DBG("rt read - head: %d, tail: %d\n", brm->head, brm->tail);
+		dest[count++] = brm->rt_event[INDEX(brm->tail++)];
+	} while (brm->head != brm->tail && count < msg_count);
 
+	return count;
 }
 
-static int get_bm_messages(brm_priv *brm, void *buf, unsigned int msg_count) {
-
-    struct bm_msg *dest = (struct bm_msg *) buf;
-    int count = 0;
-
-    if (brm->head == brm->tail) {
-        return 0;
-    }
+static int get_bm_messages(brm_priv *brm, void *buf, unsigned int msg_count)
+{
+	struct bm_msg *dest = (struct bm_msg *) buf;
+	int count = 0;
 
-    do {
+	if (brm->head == brm->tail) {
+		return 0;
+	}
 
-        DBG("bm read - head: %d, tail: %d\n", brm->head, brm->tail);
-        dest[count++] = brm->bm_event[INDEX(brm->tail++)];
+	do {
 
-    } while (brm->head != brm->tail && count < msg_count);
+		DBG("bm read - head: %d, tail: %d\n", brm->head, brm->tail);
+		dest[count++] = brm->bm_event[INDEX(brm->tail++)];
 
-    return count;
+	} while (brm->head != brm->tail && count < msg_count);
 
+	return count;
 }
 
 static rtems_device_driver brm_read(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
 {
-    rtems_libio_rw_args_t *rw_args;
-    int count = 0;
-		brm_priv *brm = &brms[minor];
-    int (*get_messages)(brm_priv *brm, void *buf, unsigned int count);
-
-		if ( ! (brm->mode & (BRM_MODE_RT | BRM_MODE_BM)) ){
-			return RTEMS_INVALID_NAME;
-		}
-
-    rw_args = (rtems_libio_rw_args_t *) arg;
-
-    if ( ((READ_REG(&brm->regs->oper)>>8) & 3) == 1 ) { /* RT */
-        get_messages = get_rt_messages;
-    }
-    else { /* BM */
-        get_messages = get_bm_messages;
-    }
+	rtems_libio_rw_args_t *rw_args;
+	int count = 0;
+	brm_priv *brm;
+	struct drvmgr_dev *dev;
+	int (*get_messages)(brm_priv *brm, void *buf, unsigned int count);
 
+	if ( drvmgr_get_dev(&b1553brm_drv_info.general, minor, &dev) ) {
+		return RTEMS_UNSATISFIED;
+	}
+	brm = (brm_priv *)dev->priv;
 
-    FUNCDBG("brm_read [%i,%i]: buf: 0x%x len: %i\n",major, minor, (unsigned int)rw_args->buffer,rw_args->count);
+	if ( ! (brm->mode & (BRM_MODE_RT | BRM_MODE_BM)) ){
+		return RTEMS_INVALID_NAME;
+	}
 
-    while ( (count=get_messages(brm,rw_args->buffer, rw_args->count)) == 0 ) {
+	rw_args = (rtems_libio_rw_args_t *) arg;
 
-        if (brm->rx_blocking) {
-            rtems_semaphore_obtain(brm->rx_sem, RTEMS_WAIT, RTEMS_NO_TIMEOUT);
-        }
-        else {
-            /* Translates to EBUSY */
-            return RTEMS_RESOURCE_IN_USE;
-        }
+	if ( ((READ_REG(&brm->regs->oper)>>8) & 3) == 1 ) { /* RT */
+		get_messages = get_rt_messages;
+	} else { /* BM */
+		get_messages = get_bm_messages;
+	}
 
-    }
+	FUNCDBG("brm_read [%i,%i]: buf: 0x%x len: %i\n",major, minor, (unsigned int)rw_args->buffer,rw_args->count);
 
-    rw_args->bytes_moved = count;
-    return RTEMS_SUCCESSFUL;
+	while ( (count=get_messages(brm,rw_args->buffer, rw_args->count)) == 0 ) {
+		if (brm->rx_blocking) {
+			rtems_semaphore_obtain(brm->rx_sem, RTEMS_WAIT, RTEMS_NO_TIMEOUT);
+		} else {
+			/* Translates to EBUSY */
+			return RTEMS_RESOURCE_IN_USE;
+		}
+	}
 
+	rw_args->bytes_moved = count;
+	return RTEMS_SUCCESSFUL;
 }
 
-
 static rtems_device_driver brm_write(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
 {
-    rtems_libio_rw_args_t *rw_args;
-    struct rt_msg *source;
-    unsigned int count=0, current, next, descriptor, wc, suba;
-		brm_priv *brm = &brms[minor];
-
-		if ( ! (brm->mode & BRM_MODE_RT) ){
-			return RTEMS_INVALID_NAME;
-		}
-
-    rw_args = (rtems_libio_rw_args_t *) arg;
-    source = (struct rt_msg *) rw_args->buffer;
-
-    FUNCDBG("brm_write [%i,%i]: buf: 0x%x len: %i\n",major, minor, (unsigned int)rw_args->buffer,rw_args->count);
+	rtems_libio_rw_args_t *rw_args;
+	struct rt_msg *source;
+	unsigned int count=0, current, next, descriptor, wc, suba;
+	brm_priv *brm;
+	struct drvmgr_dev *dev;
+  
+	if ( drvmgr_get_dev(&b1553brm_drv_info.general, minor, &dev) ) {
+		return RTEMS_UNSATISFIED;
+	}
+	brm = (brm_priv *)dev->priv;
+	
+	if ( ! (brm->mode & BRM_MODE_RT) ){
+		return RTEMS_INVALID_NAME;
+	}
+	
+	rw_args = (rtems_libio_rw_args_t *) arg;
+	source = (struct rt_msg *) rw_args->buffer;
 
-    do {
+	FUNCDBG("brm_write [%i,%i]: buf: 0x%x len: %i\n",major, minor, (unsigned int)rw_args->buffer,rw_args->count);
 
-        descriptor = source[count].desc & 0x7F;
-        suba       = descriptor-32;
-        wc         = source[count].miw >> 11;
-        wc = wc ? wc : 32;
+	do {
 
-        /* Only subaddress transmission is allowed with write */
-        if (descriptor < 32 || descriptor >= 64)
-            return RTEMS_INVALID_NAME;
+		descriptor = source[count].desc & 0x7F;
+		suba       = descriptor-32;
+		wc         = source[count].miw >> 11;
+		wc = wc ? wc : 32;
 
-        current = brm->desc[descriptor].cur;
-        next = brm->written[suba] + 2 + wc;
+		/* Only subaddress transmission is allowed with write */
+		if (descriptor < 32 || descriptor >= 64)
+			return RTEMS_INVALID_NAME;
 
-        if (brm->written[suba] < current) {
+		current = brm->desc[descriptor].cur; 
+		next = brm->written[suba] + 2 + wc;
 
-            if (next > current) {
+		if (brm->written[suba] < current) {
 
-                /* No room in transmission buffer */
+			if (next > current) {
 
-                if (brm->tx_blocking && count == 0) {
-                    rtems_semaphore_obtain(brm->tx_sem, RTEMS_WAIT, RTEMS_NO_TIMEOUT);
-                }
-								else if ( brm->tx_blocking && (count != 0) ){
-									/* return the number of messages sent so far */
-									break;
-								}
-                else {
-                    /* Translates to posix EBUSY */
-                    return RTEMS_RESOURCE_IN_USE;
-                }
-            }
-        }
+				/* No room in transmission buffer */ 
+				if (brm->tx_blocking && count == 0) {
+					rtems_semaphore_obtain(brm->tx_sem, RTEMS_WAIT, RTEMS_NO_TIMEOUT);
+				} else if ( count > 0 ) {
+					/* return the number of messages sent so far */
+					break;
+				} else {	
+					/* Translates to posix EBUSY */
+					return RTEMS_RESOURCE_IN_USE;
+				}
+			}
+		}
 
-        memcpy((void *)&brm->mem[brm->written[suba]], &source[count], (2+wc)*2);
+		memcpy((void *)&brm->mem[brm->written[suba]], &source[count], (2+wc)*2);
 
-        count++;
+		count++;
 
-        if (next >= brm->desc[descriptor].bot) {
-            next = brm->desc[descriptor].top;
-        }
-        brm->written[suba] = next;
+		if (next >= brm->desc[descriptor].bot) {
+			next = brm->desc[descriptor].top;
+		}
+		brm->written[suba] = next;
 
-    }  while (count < rw_args->count);
+	}  while (count < rw_args->count);
 
-    rw_args->bytes_moved = count;
+	rw_args->bytes_moved = count; 
 
-    if (count >= 0) {
-        return RTEMS_SUCCESSFUL;
-    }
-    return RTEMS_UNSATISFIED;
+	if (count >= 0) {
+		return RTEMS_SUCCESSFUL;
+	}
+	return RTEMS_UNSATISFIED;
 }
 
 static rtems_device_driver brm_control(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
 {
+    
+	unsigned int i=0;
+	unsigned short ctrl, oper, cw1, cw2;
+	rtems_libio_ioctl_args_t *ioarg = (rtems_libio_ioctl_args_t *) arg;
+	unsigned int *data = ioarg->buffer;
+	struct bc_msg *cmd_list = (struct bc_msg *) ioarg->buffer;
+  	brm_priv *brm;
+	struct drvmgr_dev *dev;
+	rtems_device_driver ret;
+	int len, msglen;
+
+	FUNCDBG("brm_control[%d]: [%i,%i]\n", minor, major, minor);
+
+	if ( drvmgr_get_dev(&b1553brm_drv_info.general, minor, &dev) ) {
+		return RTEMS_UNSATISFIED;
+	}
+	brm = (brm_priv *)dev->priv;
 
-    unsigned int i=0;
-    unsigned short ctrl, oper, cw1, cw2;
-    rtems_libio_ioctl_args_t *ioarg = (rtems_libio_ioctl_args_t *) arg;
-    unsigned int *data = ioarg->buffer;
-    struct bc_msg *cmd_list = (struct bc_msg *) ioarg->buffer;
-		brm_priv *brm = &brms[minor];
-		rtems_device_driver ret;
-		int len;
-
-    FUNCDBG("brm_control[%d]: [%i,%i]\n",minor,major, minor);
-
-    if (!ioarg) {
-        DBG("brm_control: invalid argument\n");
-        return RTEMS_INVALID_NAME;
-    }
-
-    ioarg->ioctl_return = 0;
-    switch(ioarg->command) {
-
-    case BRM_SET_MODE:
-			if ( data[0] > 2 )
-				return RTEMS_INVALID_NAME;
-      stop_operation(brm);
-        if (data[0] == 0) {
-            ret = bc_init(brm);
-        }
-        else if (data[0] == 1) {
-            ret = rt_init(brm);
-        }
-        else if (data[0] == 2) {
-            ret = bm_init(brm);
-        }else
-					ret = RTEMS_INVALID_NAME;
-
-				if ( ret != RTEMS_SUCCESSFUL)
-							return ret;
-
-        if ( brm->mode & (BRM_MODE_RT | BRM_MODE_BM ) )
-					start_operation(brm);
-        break;
-
-    case BRM_SET_BUS:
-        stop_operation(brm);
-				ctrl = READ_REG(&brm->regs->ctrl);
-        ctrl &= 0xE7FF;                               /* Clear bit 12-11 ...      */
-        ctrl |= (data[0]&0x3)<<11;                    /* ... OR in new bus status */
-				brm->regs->ctrl = ctrl;
-        start_operation(brm);
-        break;
-
-    case BRM_SET_MSGTO:
-        stop_operation(brm);
-        ctrl = READ_REG(&brm->regs->ctrl);
-				ctrl &= 0xFDFF;                               /* Clear bit 9 ...          */
-        ctrl |= (data[0]&1)<<9;                       /* ... OR in new MSGTO      */
-				brm->regs->ctrl = ctrl;
-        start_operation(brm);
-        break;
-
-    case BRM_SET_RT_ADDR:
-        stop_operation(brm);
-				oper = READ_REG(&brm->regs->oper);
-        oper &= 0x03FF;                               /* Clear bit 15-10 ...      */
-        oper |= (data[0]&0x1f)<<11;                   /* ... OR in new address    */
-        oper |= odd_parity(data[0]&0x1f)<<10;         /* ... OR in parity         */
-				brm->regs->oper = oper;
-        start_operation(brm);
-        break;
-
-    case BRM_SET_STD:
-        stop_operation(brm);
-				ctrl = READ_REG(&brm->regs->ctrl);
-        ctrl &= 0xFF7F;                               /* Clear bit 7 ...           */
-        ctrl |= (data[0]&1)<<7;                       /* ... OR in new ABSTD (1=A) */
-				brm->regs->ctrl = ctrl;
-        start_operation(brm);
-        break;
-
-    case BRM_SET_BCE:
-        stop_operation(brm);
-				ctrl = READ_REG(&brm->regs->ctrl);
-        ctrl &= 0xFFEF;                               /* Clear bit 4 ...           */
-        ctrl |= (data[0]&1)<<4;                       /* ... OR in new BCE         */
-				brm->regs->ctrl = ctrl;
-        start_operation(brm);
-     break;
-
-    case BRM_TX_BLOCK:
-        brm->tx_blocking     = data[0];
-        break;
-
-    case BRM_RX_BLOCK:
-        brm->rx_blocking     = data[0];
-        break;
-
-    case BRM_DO_LIST:
-
-        if ( brm->mode != BRM_MODE_BC ){
-          return RTEMS_INVALID_NAME;
-				}
-
-        /* Check if we are bus controller */
-        if ( ((READ_REG(&brm->regs->oper)>>8) & 3) != 0 ) {
-            return RTEMS_INVALID_NAME;
-        }
-
-        /* Already processing list? */
-        if (is_executing(brm)) {
-            return RTEMS_RESOURCE_IN_USE;
-        }
-
-				/* clear any earlier releases */
-        rtems_semaphore_obtain(brm->tx_sem, RTEMS_NO_WAIT, RTEMS_NO_TIMEOUT);
-
-				brm->bc_list_fail = 0;
-        brm->cur_list = cmd_list;
-				brm->regs->dpoint = 0;
-
-        i = 0;
-        while ( (cmd_list[i].ctrl & BC_EOL) == 0) {
+	if (!ioarg) {
+		DBG("brm_control: invalid argument\n");
+		return RTEMS_INVALID_NAME;
+	}
 
-            ctrl = (4<<12) | (((cmd_list[i].ctrl&BC_BUSA)==BC_BUSA)<<9) | (((cmd_list[i].ctrl&BC_RTRT)==BC_RTRT)<<8);
+	ioarg->ioctl_return = 0;
+	switch (ioarg->command) {
 
-            if (cmd_list[i].ctrl&BC_RTRT) {
-                cw1 = (cmd_list[i].rtaddr[0]<<11) | (0<<10) | (cmd_list[i].subaddr[0]<<5) | (cmd_list[i].wc & 0x1f); /* receive cw */
-                cw2 = (cmd_list[i].rtaddr[1]<<11) | (1<<10) | (cmd_list[i].subaddr[1]<<5) | (cmd_list[i].wc & 0x1f); /* transmit cw */
-            }
-            else {
-                cw1 = (cmd_list[i].rtaddr[0]<<11) | (((cmd_list[i].ctrl&BC_TR)==BC_TR)<<10) | (cmd_list[i].subaddr[0]<<5) | (cmd_list[i].wc&0x1f);
-								cw2 = 0;
-            }
+		case BRM_SET_MODE:
+		if ( data[0] > 2 )
+			return RTEMS_INVALID_NAME;
+		stop_operation(brm);
+		if (data[0] == 0) {
+			ret = bc_init(brm);
+		} else if (data[0] == 1) { 
+			ret = rt_init(brm);				
+		} else if (data[0] == 2) { 
+			ret = bm_init(brm);						
+		} else {
+			ret = RTEMS_INVALID_NAME;
+		}
+		if ( ret != RTEMS_SUCCESSFUL)
+			return ret;
+
+		if ( brm->mode & (BRM_MODE_RT | BRM_MODE_BM ) )
+			start_operation(brm);
+		break;
+
+		case BRM_SET_BUS:
+		stop_operation(brm);
+		ctrl = READ_REG(&brm->regs->ctrl);
+		ctrl &= 0xE7FF;                               /* Clear bit 12-11 ...      */
+		ctrl |= (data[0]&0x3)<<11;                    /* ... OR in new bus status */
+		brm->regs->ctrl = ctrl;
+		start_operation(brm);
+		break;
+
+		case BRM_SET_MSGTO:
+		stop_operation(brm);
+		ctrl = READ_REG(&brm->regs->ctrl);
+		ctrl &= 0xFDFF;                               /* Clear bit 9 ...          */
+		ctrl |= (data[0]&1)<<9;                       /* ... OR in new MSGTO      */
+		brm->regs->ctrl = ctrl;
+		start_operation(brm);
+		break;
+
+		case BRM_SET_RT_ADDR:   
+		stop_operation(brm);
+		oper = READ_REG(&brm->regs->oper);
+		oper &= 0x03FF;                               /* Clear bit 15-10 ...      */
+		oper |= (data[0]&0x1f)<<11;                   /* ... OR in new address    */
+		oper |= odd_parity(data[0]&0x1f)<<10;         /* ... OR in parity         */
+		brm->regs->oper = oper;
+		start_operation(brm);
+		break;
+
+		case BRM_SET_STD:   
+		stop_operation(brm);
+		ctrl = READ_REG(&brm->regs->ctrl);
+		ctrl &= 0xFF7F;                               /* Clear bit 7 ...           */
+		ctrl |= (data[0]&1)<<7;                       /* ... OR in new ABSTD (1=A) */
+		brm->regs->ctrl = ctrl;
+		start_operation(brm);
+		break;
+
+		case BRM_SET_BCE:
+		stop_operation(brm);
+		ctrl = READ_REG(&brm->regs->ctrl);
+		ctrl &= 0xFFEF;                               /* Clear bit 4 ...           */
+		ctrl |= (data[0]&1)<<4;                       /* ... OR in new BCE         */
+		brm->regs->ctrl = ctrl;
+		start_operation(brm);
+		break;
+
+		case BRM_TX_BLOCK:
+		brm->tx_blocking     = data[0];      
+		break;
+
+		case BRM_RX_BLOCK: 
+		brm->rx_blocking     = data[0];   
+		break;
+
+		case BRM_DO_LIST:
+		if ( brm->mode != BRM_MODE_BC ){
+			return RTEMS_INVALID_NAME;
+		}
 
+		/* Check if we are bus controller */
+		if ( ((READ_REG(&brm->regs->oper)>>8) & 3) != 0 ) {
+			return RTEMS_INVALID_NAME;
+		}
 
-            /* Set up command block */
-            brm->bcmem->descs[i].ctrl = ctrl;
-            brm->bcmem->descs[i].cw1 = cw1;
-            brm->bcmem->descs[i].cw2 = cw2;
-						/* data pointer:
-						 * (&brm->bcmem->msg_data[i].data[0] & 0x1ffff) / 2
-						 */
-            brm->bcmem->descs[i].dptr = 1024+i*32;  /* data pointer */
-						brm->bcmem->descs[i].tsw[0] = 0;
-						brm->bcmem->descs[i].tsw[1] = 0;
-						brm->bcmem->descs[i].ba = 0;
-						brm->bcmem->descs[i].timer = 0;
+		/* Already processing list? */
+		if (is_executing(brm)) {
+			return RTEMS_RESOURCE_IN_USE;
+		}
 
-            memcpy((void *)&brm->bcmem->msg_data[i].data[0], &cmd_list[i].data[0], cmd_list[i].wc*2);
+		/* clear any earlier releases */
+		rtems_semaphore_obtain(brm->tx_sem, RTEMS_NO_WAIT, RTEMS_NO_TIMEOUT);
+
+		brm->bc_list_fail = 0;
+		brm->cur_list = cmd_list;
+		brm->regs->dpoint = 0;
+
+		i = 0;
+		while ( (cmd_list[i].ctrl & BC_EOL) == 0) {
+
+			ctrl = (4<<12) | (((cmd_list[i].ctrl&BC_BUSA)==BC_BUSA)<<9) | (((cmd_list[i].ctrl&BC_RTRT)==BC_RTRT)<<8);
+
+			if (cmd_list[i].ctrl&BC_RTRT) {
+				cw1 = (cmd_list[i].rtaddr[0]<<11) | (0<<10) | (cmd_list[i].subaddr[0]<<5) | (cmd_list[i].wc & 0x1f); /* receive cw */
+				cw2 = (cmd_list[i].rtaddr[1]<<11) | (1<<10) | (cmd_list[i].subaddr[1]<<5) | (cmd_list[i].wc & 0x1f); /* transmit cw */
+			} else {
+				cw1 = (cmd_list[i].rtaddr[0]<<11) | (((cmd_list[i].ctrl&BC_TR)==BC_TR)<<10) | (cmd_list[i].subaddr[0]<<5) | (cmd_list[i].wc&0x1f);
+				cw2 = 0;
+			}
+
+			/* Set up command block */
+			brm->bcmem->descs[i].ctrl = ctrl;
+			brm->bcmem->descs[i].cw1 = cw1;
+			brm->bcmem->descs[i].cw2 = cw2;
+			/* data pointer:
+			 * (&brm->bcmem->msg_data[i].data[0] & 0x1ffff) / 2 
+			 */
+			brm->bcmem->descs[i].dptr = 1024+i*32;  /* data pointer */
+			brm->bcmem->descs[i].tsw[0] = 0;
+			brm->bcmem->descs[i].tsw[1] = 0;
+			brm->bcmem->descs[i].ba = 0;
+			brm->bcmem->descs[i].timer = 0;
+
+			msglen = cmd_list[i].wc;
+			if ( msglen == 0 ) 
+				msglen = 32;
+			memcpy((void *)&brm->bcmem->msg_data[i].data[0], &cmd_list[i].data[0], msglen*2);
+
+			i++;
+		}
 
-            i++;
-        }
+		brm->bcmem->descs[i].ctrl = 0; /* end of list */
 
-        brm->bcmem->descs[i].ctrl = 0; /* end of list */
+		start_operation(brm);        
+		break;  
 
-        start_operation(brm);
+		case BRM_LIST_DONE:
 
-        break;
+		if ( brm->mode != BRM_MODE_BC ){
+			return RTEMS_INVALID_NAME;
+		}
+				
+		/* Check if we are bus controller */
+		if ( ((READ_REG(&brm->regs->oper)>>8) & 3) != 0 ) {
+			return RTEMS_INVALID_NAME;
+		}
 
-    case BRM_LIST_DONE:
+		if (is_executing(brm)) {
 
-        if ( brm->mode != BRM_MODE_BC ){
-          return RTEMS_INVALID_NAME;
+			data[0] = 0;
+			if (brm->tx_blocking) {
+				rtems_semaphore_obtain(brm->tx_sem, RTEMS_WAIT, RTEMS_NO_TIMEOUT);
+				data[0] = 1;
+				if ( brm->bc_list_fail ){
+					return RTEMS_INVALID_NAME;
 				}
+			} else {
+				return RTEMS_RESOURCE_IN_USE;
+			}
+		} else {
+			data[0] = 1; /* done */
+		}
 
-				/* Check if we are bus controller */
-        if ( ((READ_REG(&brm->regs->oper)>>8) & 3) != 0 ) {
-            return RTEMS_INVALID_NAME;
-        }
-
-        if (is_executing(brm)) {
-
-            data[0] = 0;
-            if (brm->tx_blocking) {
-                rtems_semaphore_obtain(brm->tx_sem, RTEMS_WAIT, RTEMS_NO_TIMEOUT);
-                data[0] = 1;
-								if ( brm->bc_list_fail ){
-									return RTEMS_INVALID_NAME;
-								}
-            }else{
-							return RTEMS_RESOURCE_IN_USE;
-						}
-
-
-        }
-        else {
-            data[0] = 1; /* done */
-        }
-
-        /* copy finished list results back into bc_msg array */
-        i = 0;
-        while ( (brm->cur_list[i].ctrl & BC_EOL) == 0) {
-
-            if (READ_DMA(&brm->bcmem->descs[i].ctrl) & 1) {
-                brm->cur_list[i].ctrl |= 0x8000; /* Set BAME */
-            }
-            if (brm->cur_list[i].ctrl & BC_TR) {
-                /* RT Transmit command, copy received data */
-								len = brm->cur_list[i].wc;
-								while( len-- > 0){
-									brm->cur_list[i].data[len] = READ_DMA(&brm->bcmem->msg_data[i].data[len]);
-								}
-            }
-            brm->cur_list[i].tsw[0] = READ_DMA(&brm->bcmem->descs[i].tsw[0]);
-            brm->cur_list[i].tsw[1] = READ_DMA(&brm->bcmem->descs[i].tsw[1]);
-
-            i++;
-        }
-        break;
-
-
-    case BRM_CLR_STATUS:
-			brm->status = 0;
-			break;
+		/* copy finished list results back into bc_msg array */
+		i = 0;
+		while ( (brm->cur_list[i].ctrl & BC_EOL) == 0) {
+			if (READ_DMA(&brm->bcmem->descs[i].ctrl) & 1) {
+				brm->cur_list[i].ctrl |= 0x8000; /* Set BAME */ 
+			}
+			if (brm->cur_list[i].ctrl & BC_TR) {
+				/* RT Transmit command, copy received data */
+				len = brm->cur_list[i].wc;
+				if ( len == 0 )
+					len = 32;
+				while ( len-- > 0) {
+					brm->cur_list[i].data[len] = READ_DMA(&brm->bcmem->msg_data[i].data[len]);
+				}
+			}
+			brm->cur_list[i].tsw[0] = READ_DMA(&brm->bcmem->descs[i].tsw[0]);
+			brm->cur_list[i].tsw[1] = READ_DMA(&brm->bcmem->descs[i].tsw[1]);
 
-    case BRM_GET_STATUS: /* copy status */
+			i++;
+		}
+		break;
 
-			if ( !ioarg->buffer )
-				return RTEMS_INVALID_NAME;
+		case BRM_CLR_STATUS:
+		brm->status = 0;
+		break;
 
-			*(unsigned int *)ioarg->buffer = brm->status;
-			break;
+		case BRM_GET_STATUS: /* copy status */
+		if ( !ioarg->buffer )
+			return RTEMS_INVALID_NAME;
 
+		*(unsigned int *)ioarg->buffer = brm->status;
+		break;
+    
 		case BRM_SET_EVENTID:
-			brm->event_id = (rtems_id)ioarg->buffer;
-			break;
+		brm->event_id = (rtems_id)ioarg->buffer;
+		break;
 
-    default:
-        return RTEMS_NOT_DEFINED;
-    }
-    return RTEMS_SUCCESSFUL;
-}
-
-#ifdef B1553BRM_DEFINE_INTHANDLER
-static void b1553brm_interrupt_handler(rtems_vector_number v){
-	int i;
-	/* find minor */
-	for(i=0; i<brm_cores; i++){
-		if ( (brms[i].irqno+0x10) == v ){
-			brm_interrupt(&brms[i]);
-			return;
-		}
+		default:
+		return RTEMS_NOT_DEFINED;
 	}
+	return RTEMS_SUCCESSFUL;
 }
-#endif
 
-static void brm_interrupt(brm_priv *brm) {
-  unsigned short descriptor, current, pending, miw, wc, tmp;
+static void b1553brm_interrupt(void *arg)
+{
+	brm_priv *brm = arg;
+	unsigned short descriptor, current, pending, miw, wc, tmp, ctrl;
 	unsigned short msgadr, iaw, iiw;
 	int len;
-	int signal_event=0;
+	int signal_event=0, wake_rx_task=0, wake_tx_task=0;
 	unsigned int event_status=0;
+	int accessed;
 	#define SET_ERROR_DESCRIPTOR(descriptor) (event_status = (event_status & 0x0000ffff) | descriptor<<16)
-
-	while( (iiw=READ_REG(&brm->irq_log[brm->irq].iiw)) != 0xffff ){
-		iaw=READ_REG(&brm->irq_log[brm->irq].iaw);
-
+ 		
+	while( (iiw=READ_DMA(&brm->irq_log[brm->irq].iiw)) != 0xffff ){
+		iaw=READ_DMA(&brm->irq_log[brm->irq].iaw);
+		
 		/* indicate that the interrupt log entry has been processed */
 		brm->irq_log[brm->irq].iiw = 0xffff;
 
 		/* Interpret interrupt log entry  */
-    descriptor = iaw >> 2;
-    pending    = iiw;
-    brm->irq = (brm->irq + 1) % 16;
-
+		descriptor = iaw >> 2;
+		pending    = iiw;
+		brm->irq = (brm->irq + 1) % 16;
+		
 		/* Clear the log so that we */
 
 
-    /* Subaddress accessed irq (RT only)
-     *
-     * Can be either a receive or transmit command
-     * as well as a mode code.
-     */
-    if (pending & BRM_SUBAD_IRQ) {
-
-        /* Pointer to next free message in circular buffer */
-        current = READ_DMA(&brm->desc[descriptor].cur);
-
-        while  ( (msgadr=brm->last_read[descriptor]) != current) {
-
-            /* Get word count */
-            miw = READ_DMA(&brm->mem[msgadr]);
-            wc  = miw >> 11;
-
-            /* Data received */
-            if (descriptor < 32) {
-                wc = wc ? wc : 32;
-            }
-            /* Data transmitted */
-            else if (descriptor < 64) {
-                wc = wc ? wc : 32;
-                rtems_semaphore_release(brm->tx_sem);
-            }
-            /* RX Mode code */
-            else if (descriptor < 96) {
-                wc = (wc>>4);
-            }
-            /* TX Mode code */
-            else if (descriptor < 128) {
-                wc = (wc>>4);
-            }
-
+		/* Subaddress accessed irq (RT only) 
+		 *
+		 * Can be either a receive or transmit command
+		 * as well as a mode code.
+		*/
+		if (pending & BRM_SUBAD_IRQ) {
+
+			/* Pointer to next free message in circular buffer */
+			current = READ_DMA(&brm->desc[descriptor].cur);
+			ctrl = READ_DMA(&brm->desc[descriptor].ctrl);
 #ifdef DEBUG
-            brm->log[brm->log_i++ % EVENT_QUEUE_SIZE] = (descriptor << 16) | wc;
-            brm->log[brm->log_i++ % EVENT_QUEUE_SIZE] = current;
-            brm->log[brm->log_i++ % EVENT_QUEUE_SIZE] = msgadr;
+			brm->log[brm->log_i++ % EVENT_QUEUE_SIZE] = (0xff<<16);
+			brm->log[brm->log_i++ % EVENT_QUEUE_SIZE] = current;
+			brm->log[brm->log_i++ % EVENT_QUEUE_SIZE] = ctrl;
+			brm->log[brm->log_i++ % EVENT_QUEUE_SIZE] = 0;
 #endif
+			accessed = ctrl & 0x10;
+			/* Note that current may be equal to bot and top when 
+			 * circular buffer one can handle one message.
+			 */
+			if ( accessed )
+			  do {
+			  	msgadr = brm->last_read[descriptor];
+
+				/* Get word count */
+				miw = READ_DMA(&brm->mem[msgadr]);
+				wc  = miw >> 11;
+
+				/* Data received */
+				if (descriptor < 32) {
+					wc = wc ? wc : 32;
+				}
+				/* Data transmitted */ 
+				else if (descriptor < 64) {
+					wc = wc ? wc : 32;  
+					wake_tx_task=1;
+				}
+				/* RX Mode code */
+				else if (descriptor < 96) {
+					wc = (wc>>4);
+				}
+				/* TX Mode code */
+				else if (descriptor < 128) {
+					wc = (wc>>4);
+				}
 
-            /* If there is room in the event queue, copy the event there */
-            if (brm->head - brm->tail != EVENT_QUEUE_SIZE) {
-
-                /* Copy to event queue */
-								brm->rt_event[INDEX(brm->head)].miw = READ_DMA(&brm->mem[msgadr]);
-								brm->rt_event[INDEX(brm->head)].time = READ_DMA(&brm->mem[msgadr+1]);
-								len = wc;
-								while( len-- > 0){
-									brm->rt_event[INDEX(brm->head)].data[len] = READ_DMA(&brm->mem[msgadr+2+len]);
-								}
-                brm->rt_event[INDEX(brm->head)].desc = descriptor;
-                brm->head++;
+#ifdef DEBUG            
+				brm->log[brm->log_i++ % EVENT_QUEUE_SIZE] = (descriptor << 16) | wc; 
+				brm->log[brm->log_i++ % EVENT_QUEUE_SIZE] = current;
+				brm->log[brm->log_i++ % EVENT_QUEUE_SIZE] = msgadr;
+#endif
 
-            }
-            else {
-                /* Indicate overrun */
-                brm->rt_event[INDEX(brm->head)].desc |= 0x8000;
-            }
+				/* If there is room in the event queue, copy the event there */
+				if (brm->head - brm->tail != EVENT_QUEUE_SIZE) {
+
+					/* Copy to event queue */
+					brm->rt_event[INDEX(brm->head)].miw = READ_DMA(&brm->mem[msgadr]);
+					brm->rt_event[INDEX(brm->head)].time = READ_DMA(&brm->mem[msgadr+1]);
+					len = wc;
+					while( len-- > 0){
+						brm->rt_event[INDEX(brm->head)].data[len] = READ_DMA(&brm->mem[msgadr+2+len]);
+					}
+					brm->rt_event[INDEX(brm->head)].desc = descriptor;
+					brm->head++;
+				}
+				else {
+					/* Indicate overrun */
+					brm->rt_event[INDEX(brm->head)].desc |= 0x8000;
+				}
 
-            msgadr += (2+wc);
+				msgadr += (2+wc);
 
-            if (msgadr >= brm->desc[descriptor].bot) {
-                msgadr = brm->desc[descriptor].top;
-            }
-						brm->last_read[descriptor] = msgadr;
+				if (msgadr >= READ_DMA(&brm->desc[descriptor].bot)) {
+					msgadr = READ_DMA(&brm->desc[descriptor].top);
+				}
+				brm->last_read[descriptor] = msgadr;
 
 #ifdef DEBUG
-            brm->log[brm->log_i++ % EVENT_QUEUE_SIZE] = msgadr;
+				brm->log[brm->log_i++ % EVENT_QUEUE_SIZE] = msgadr;
 #endif
+				wake_rx_task = 1;
+			  } while ( (msgadr=brm->last_read[descriptor]) != current );
+		}
 
-            /* Wake any blocked rx thread */
-            rtems_semaphore_release(brm->rx_sem);
-
-        }
-
-    }
-
-    if (pending & BRM_EOL_IRQ) {
-        rtems_semaphore_release(brm->tx_sem);
-    }
-
-    if (pending & BRM_BC_ILLCMD_IRQ) {
-				brm->bc_list_fail = 1;
-        rtems_semaphore_release(brm->tx_sem);
-				SET_ERROR_DESCRIPTOR(descriptor);
-				FUNCDBG("BRM: ILLCMD IRQ\n\r");
-    }
-
-    /* Monitor irq */
-    if (pending & BRM_MBC_IRQ) {
-
-        stop_operation(brm);
-        brm->regs->mbc = 1;
-        start_operation(brm);
-
-        /* If there is room in the event queue, copy the event there */
-        if (brm->head - brm->tail != EVENT_QUEUE_SIZE) {
-
-            /* Copy to event queue */
+		if (pending & BRM_EOL_IRQ) {  
+			wake_tx_task = 1;
+		}
 
-            brm->bm_event[INDEX(brm->head)].miw  =  READ_DMA(&brm->mem[0]);
-            brm->bm_event[INDEX(brm->head)].cw1  =  READ_DMA(&brm->mem[1]);
-            brm->bm_event[INDEX(brm->head)].cw2  =  READ_DMA(&brm->mem[2]);
-            brm->bm_event[INDEX(brm->head)].sw1  =  READ_DMA(&brm->mem[4]);
-            brm->bm_event[INDEX(brm->head)].sw2  =  READ_DMA(&brm->mem[5]);
-            brm->bm_event[INDEX(brm->head)].time =  READ_DMA(&brm->mem[6]);
+		if (pending & BRM_BC_ILLCMD_IRQ) {
+			brm->bc_list_fail = 1;
+			wake_tx_task = 1;
+			SET_ERROR_DESCRIPTOR(descriptor);
+			FUNCDBG("BRM: ILLCMD IRQ\n\r");
+		}
 
-            len = 32;
-            while ( len-- ){
-              brm->bm_event[INDEX(brm->head)].data[len] =  READ_DMA(&brm->mem[0x100+len]);
-              len--;
-              brm->bm_event[INDEX(brm->head)].data[len] =  READ_DMA(&brm->mem[0x100+len]);
-              len--;
-              brm->bm_event[INDEX(brm->head)].data[len] =  READ_DMA(&brm->mem[0x100+len]);
-              len--;
-              brm->bm_event[INDEX(brm->head)].data[len] =  READ_DMA(&brm->mem[0x100+len]);
-            }
-/*            memcpy((void *)brm->bm_event[INDEX(brm->head)].data, &brm->mem[0x100], 32);*/
+		/* Monitor irq */
+		if (pending & BRM_MBC_IRQ) { 
+
+			stop_operation(brm);
+			brm->regs->mbc = 1;
+			start_operation(brm);
+
+			/* If there is room in the event queue, copy the event there */
+			if (brm->head - brm->tail != EVENT_QUEUE_SIZE) {
+
+				/* Copy to event queue */
+
+				brm->bm_event[INDEX(brm->head)].miw  =  READ_DMA(&brm->mem[0]);
+				brm->bm_event[INDEX(brm->head)].cw1  =  READ_DMA(&brm->mem[1]);
+				brm->bm_event[INDEX(brm->head)].cw2  =  READ_DMA(&brm->mem[2]);
+				brm->bm_event[INDEX(brm->head)].sw1  =  READ_DMA(&brm->mem[4]);
+				brm->bm_event[INDEX(brm->head)].sw2  =  READ_DMA(&brm->mem[5]);
+				brm->bm_event[INDEX(brm->head)].time =  READ_DMA(&brm->mem[6]);
+
+				len = 32;
+				while ( len-- ){
+					brm->bm_event[INDEX(brm->head)].data[len] =  READ_DMA(&brm->mem[0x100+len]);
+					len--;
+					brm->bm_event[INDEX(brm->head)].data[len] =  READ_DMA(&brm->mem[0x100+len]);
+					len--;
+					brm->bm_event[INDEX(brm->head)].data[len] =  READ_DMA(&brm->mem[0x100+len]);
+					len--;
+					brm->bm_event[INDEX(brm->head)].data[len] =  READ_DMA(&brm->mem[0x100+len]);
+				}
+/*				memcpy((void *)brm->bm_event[INDEX(brm->head)].data, &brm->mem[0x100], 32);*/
 
 #ifdef DEBUG
-            brm->log[brm->log_i++ % EVENT_QUEUE_SIZE] = READ_REG(&brm->regs->mbc);
-            brm->log[brm->log_i++ % EVENT_QUEUE_SIZE] = READ_DMA(&brm->mem[0]);
-            brm->log[brm->log_i++ % EVENT_QUEUE_SIZE] = READ_DMA(&brm->mem[1]);
-            brm->log[brm->log_i++ % EVENT_QUEUE_SIZE] = READ_DMA(&brm->mem[4]);
+				brm->log[brm->log_i++ % EVENT_QUEUE_SIZE] = READ_REG(&brm->regs->mbc) & 0xffff;
+				brm->log[brm->log_i++ % EVENT_QUEUE_SIZE] = READ_DMA(&brm->mem[0]);
+				brm->log[brm->log_i++ % EVENT_QUEUE_SIZE] = READ_DMA(&brm->mem[1]);
+				brm->log[brm->log_i++ % EVENT_QUEUE_SIZE] = READ_DMA(&brm->mem[4]);
 #endif
 
-            brm->head++;
-
-        }
-        else {
-            /* Indicate overrun */
-            brm->rt_event[INDEX(brm->head)].miw |= 0x8000;
-        }
+				brm->head++;
 
-        /* Wake any blocking thread */
-        rtems_semaphore_release(brm->rx_sem);
+			}
+			else {
+				/* Indicate overrun */
+				brm->bm_event[INDEX(brm->head)].miw |= 0x8000;
+			}
 
-    }
+			/* Wake any blocking thread */
+			wake_rx_task = 1;
+		}
 
-		/* The reset of the interrupts
+		/* The reset of the interrupts 
 		 * cause a event to be signalled
 		 * so that user can handle error.
 		 */
@@ -1317,9 +1394,9 @@ static void brm_interrupt(brm_priv *brm) {
 		if ( pending & BRM_ILLOP_IRQ){
 			FUNCDBG("BRM: BRM_ILLOP_IRQ\n\r");
 			brm->bc_list_fail = 1;
-			rtems_semaphore_release(brm->tx_sem);
+			wake_tx_task = 1;
 			event_status |= BRM_ILLOP_IRQ;
-			SET_ERROR_DESCRIPTOR(descriptor);
+			SET_ERROR_DESCRIPTOR(descriptor);			
 			signal_event=1;
 		}
 
@@ -1329,10 +1406,15 @@ static void brm_interrupt(brm_priv *brm) {
 			SET_ERROR_DESCRIPTOR(descriptor);
 			signal_event=1;
 		}
-	    /* Clear Block Accessed Bit */
-    tmp = READ_REG(&brm->desc[descriptor].ctrl);
-    brm->desc[descriptor].ctrl = tmp & ~0x10;
-
+		/* Clear Block Accessed Bit */ 
+		tmp = READ_DMA(&brm->desc[descriptor].ctrl);
+		brm->desc[descriptor].ctrl = tmp & ~0x10;
+#ifdef DEBUG
+		brm->log[brm->log_i++ % EVENT_QUEUE_SIZE] = (0xfe<<16);
+		brm->log[brm->log_i++ % EVENT_QUEUE_SIZE] = 0;
+		brm->log[brm->log_i++ % EVENT_QUEUE_SIZE] = tmp & ~0x10;
+		brm->log[brm->log_i++ % EVENT_QUEUE_SIZE] = tmp;
+#endif
 	} /* While */
 
 	/* clear interrupt flags & handle Hardware errors */
@@ -1363,9 +1445,88 @@ static void brm_interrupt(brm_priv *brm) {
 		brm->status |= event_status;
 	}
 
+	/* Wake any blocked rx thread only on receive interrupts */
+	if ( wake_rx_task ) {
+		rtems_semaphore_release(brm->rx_sem);
+	}
+
+	/* Wake any blocked tx thread only on transmit interrupts */
+	if ( wake_tx_task ) {
+		rtems_semaphore_release(brm->tx_sem);
+	}        
+
 	/* signal event once */
 	if ( signal_event && (brm->event_id!=0) ){
 		rtems_event_send(brm->event_id, event_status);
 	}
 
 }
+
+void b1553brm_print_dev(struct drvmgr_dev *dev, int options)
+{
+	brm_priv *pDev = dev->priv;
+	struct amba_dev_info *devinfo;
+	struct brm_reg *regs = pDev->regs;
+
+	devinfo = (struct amba_dev_info *)pDev->dev->businfo;
+
+	/* Print */
+	printf("--- B1553BRM[%d] %s ---\n", pDev->minor, pDev->devName);
+	printf(" REGS:            0x%x\n", (unsigned int)pDev->regs);
+	printf(" IRQ:             %d\n", pDev->irqno);
+	switch (pDev->mode) {
+		case BRM_MODE_BC:
+			printf(" MODE:            BC\n");
+			printf(" DESCS:           0x%x\n", (unsigned int)&pDev->bcmem->descs[0]);
+			printf(" DATA:            0x%x\n", (unsigned int)&pDev->bcmem->msg_data[0].data[0]);
+			printf(" IRQLOG:          0x%x\n", (unsigned int)&pDev->bcmem->irq_logs[0]);
+			break;
+		case BRM_MODE_BM:
+			printf(" MODE:            BM\n");
+			break;
+		case BRM_MODE_RT:
+			printf(" MODE:            RT\n");
+			printf(" RXSUBS:          0x%x\n", (unsigned int)&pDev->rtmem->rxsubs[0]);
+			printf(" TXSUBS:          0x%x\n", (unsigned int)&pDev->rtmem->txsubs[0]);
+			printf(" RXMODES:         0x%x\n", (unsigned int)&pDev->rtmem->rxmodes[0]);
+			printf(" TXOMODES:        0x%x\n", (unsigned int)&pDev->rtmem->txmodes[0]);
+			printf(" RXSUBS MSGS:     0x%x\n", (unsigned int)&pDev->rtmem->rxsuba_msgs[0]);
+			printf(" TXSUBS MSGS:     0x%x\n", (unsigned int)&pDev->rtmem->txsuba_msgs[0]);
+			printf(" RXMODES MSGS:    0x%x\n", (unsigned int)&pDev->rtmem->rxmode_msgs[0]);
+			printf(" TXMODES MSGS:    0x%x\n", (unsigned int)&pDev->rtmem->txmode_msgs[0]);
+			printf(" IRQLOG:          0x%x\n", (unsigned int)&pDev->rtmem->irq_logs[0]);
+			break;
+	}
+	printf(" CTRL:            0x%x\n", regs->ctrl);
+	printf(" OPER:            0x%x\n", regs->oper);
+	printf(" CUR_CMD:         0x%x\n", regs->cur_cmd);
+	printf(" IMASK:           0x%x\n", regs->imask);
+	printf(" IPEND:           0x%x\n", regs->ipend);
+	printf(" IPOINT:          0x%x\n", regs->ipoint);
+	printf(" BIT_REG:         0x%x\n", regs->bit_reg);
+	printf(" TTAG:            0x%x\n", regs->ttag);
+	printf(" DPOINT:          0x%x\n", regs->dpoint);
+	printf(" SW:              0x%x\n", regs->sw);
+	printf(" INITCOUNT:       0x%x\n", regs->initcount);
+	printf(" MCPOINT:         0x%x\n", regs->mcpoint);
+	printf(" MDPOINT:         0x%x\n", regs->mdpoint);
+	printf(" MBC:             0x%x\n", regs->mbc);
+	printf(" MFILTA:          0x%x\n", regs->mfilta);
+	printf(" MFILTB:          0x%x\n", regs->mfiltb);
+	printf(" ENHANCED:        0x%x\n", regs->enhanced);
+	printf(" W_CTRL:          0x%x\n", regs->w_ctrl);
+	printf(" W_IRQCTRL:       0x%x\n", regs->w_irqctrl);
+	printf(" W_AHBADDR:       0x%x\n", regs->w_ahbaddr);
+}
+
+void b1553brm_print(int options)
+{
+	struct amba_drv_info *drv = &b1553brm_drv_info;
+	struct drvmgr_dev *dev;
+
+	dev = drv->general.dev;
+	while(dev) {
+		b1553brm_print_dev(dev, options);
+		dev = dev->next_in_drv;
+	}
+}
diff --git a/c/src/lib/libbsp/sparc/shared/can/grcan.c b/c/src/lib/libbsp/sparc/shared/can/grcan.c
index 7833f8c1d6..7ad53e1fb6 100644
--- a/c/src/lib/libbsp/sparc/shared/can/grcan.c
+++ b/c/src/lib/libbsp/sparc/shared/can/grcan.c
@@ -1,19 +1,12 @@
 /*
  *  GRCAN driver
- */
-
-/*
+ *
  *  COPYRIGHT (c) 2007.
- *  Gaisler Research.
+ *  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.
- *
- *
- *  2007-06-13, Daniel Hellstrom <daniel@gaisler.com>
- *    New driver in sparc shared directory. Parts taken
- *    from rasta grhcan driver.
  */
 
 #include <bsp.h>
@@ -26,22 +19,26 @@
 #include <rtems/bspIo.h>
 
 #include <grcan.h>
+#include <drvmgr/drvmgr.h>
+#include <drvmgr/ambapp_bus.h>
 #include <ambapp.h>
-#include <grlib.h>
 
 #define WRAP_AROUND_TX_MSGS 1
 #define WRAP_AROUND_RX_MSGS 2
 #define GRCAN_MSG_SIZE sizeof(struct grcan_msg)
 #define BLOCK_SIZE (16*4)
 
+/* grcan needs to have it buffers aligned to 1k boundaries */
+#define BUFFER_ALIGNMENT_NEEDS 1024
+
 /* Default Maximium buffer size for statically allocated buffers */
 #ifndef TX_BUF_SIZE
-#define TX_BUF_SIZE (BLOCK_SIZE*16)
+ #define TX_BUF_SIZE (BLOCK_SIZE*16)
 #endif
 
 /* Make receiver buffers bigger than transmitt */
 #ifndef RX_BUF_SIZE
-#define RX_BUF_SIZE ((3*BLOCK_SIZE)*16)
+ #define RX_BUF_SIZE ((3*BLOCK_SIZE)*16)
 #endif
 
 #ifndef IRQ_GLOBAL_PREPARE
@@ -68,35 +65,13 @@
  #define IRQ_MASK(irqno)
 #endif
 
-#ifndef GRCAN_PREFIX
- #define GRCAN_PREFIX(name) grcan##name
-#endif
-
-#ifndef MEMAREA_TO_HW
-  #define MEMAREA_TO_HW(x) (x)
-#endif
-
-/* default name to /dev/grcan0 */
-#if !defined(GRCAN_DEVNAME) || !defined(GRCAN_DEVNAME_NO)
- #undef GRCAN_DEVNAME
- #undef GRCAN_DEVNAME_NO
- #define GRCAN_DEVNAME "/dev/grcan0"
- #define GRCAN_DEVNAME_NO(devstr,no) ((devstr)[10]='0'+(no))
-#endif
-
-#ifndef GRCAN_REG_INT
- #define GRCAN_REG_INT(handler,irqno,arg) set_vector(handler,irqno+0x10,1)
- #undef  GRCAN_DEFINE_INTHANDLER
- #define GRCAN_DEFINE_INTHANDLER
-#endif
-
 #ifndef GRCAN_DEFAULT_BAUD
-  /* default to 500kbits/s */
-  #define GRCAN_DEFAULT_BAUD 500000
+ /* default to 500kbits/s */
+ #define GRCAN_DEFAULT_BAUD 500000
 #endif
 
 #ifndef GRCAN_SAMPLING_POINT
-#define GRCAN_SAMPLING_POINT 80
+ #define GRCAN_SAMPLING_POINT 80
 #endif
 
 /* Uncomment for debug output */
@@ -114,63 +89,9 @@
 
 /*********************************************************/
 
-/* grcan needs to have it buffers aligned to 1k boundaries */
-#define BUFFER_ALIGNMENT_NEEDS 1024
-
-#ifdef STATICALLY_ALLOCATED_TX_BUFFER
-static unsigned int tx_circbuf[GRCAN_MAX_CORES][TX_BUF_SIZE]
-	__attribute__ ((aligned(BUFFER_ALIGNMENT_NEEDS)));
-#define STATIC_TX_BUF_SIZE TX_BUF_SIZE
-#define STATIC_TX_BUF_ADDR(core) (&tx_circbuf[(core)][0])
-#endif
-
-#ifdef STATICALLY_ALLOCATED_RX_BUFFER
-static unsigned int rx_circbuf[GRCAN_MAX_CORES][RX_BUF_SIZE]
-	__attribute__ ((aligned(BUFFER_ALIGNMENT_NEEDS)));
-#define STATIC_RX_BUF_SIZE RX_BUF_SIZE
-#define STATIC_RX_BUF_ADDR(core) (&rx_circbuf[(core)][0])
-#endif
-
-/*
- * If USE_AT697_RAM is defined the RAM on the AT697 board will be used for DMA buffers (but rx message queue is always in AT697 ram).
- * USE_AT697_DMA specifies whether the messages will be fetched using DMA or PIO.
- *
- * RASTA_PCI_BASE is the base address of the GRPCI AHB slave
- *
- * GRCAN_BUF_SIZE must be set to the size (in bytes) of the GRCAN DMA buffers.
- *
- * RX_QUEUE_SIZE defines the number of messages that fits in the  RX message queue. On RX interrupts the messages in the DMA buffer
- * are copied into the message queue (using dma if the rx buf is not in the AT697 ram).
- */
-
-/*#define USE_AT697_RAM              1      */
-#define USE_AT697_DMA              1
-#define RASTA_PCI_BASE             0xe0000000
-#define GRCAN_BUF_SIZE            4096
-#define RX_QUEUE_SIZE              1024
-
-#define INDEX(x) ( x&(RX_QUEUE_SIZE-1) )
-
-/* pa(x)
- *
- * x: address in AT697 address space
- *
- * returns the address in the RASTA address space that can be used to access x with dma.
- *
-*/
-#ifdef USE_AT697_RAM
-static inline unsigned int pa(unsigned int addr) {
-    return ((addr & 0x0fffffff) | RASTA_PCI_BASE);
-}
-#else
-static inline unsigned int pa(unsigned int addr) {
-    return ((addr & 0x0fffffff) | 0x40000000);
-}
-#endif
-
 struct grcan_msg {
-    unsigned int head[2];
-    unsigned char data[8];
+	unsigned int head[2];
+	unsigned char data[8];
 };
 
 struct grcan_config {
@@ -181,43 +102,41 @@ struct grcan_config {
 };
 
 struct grcan_priv {
-  unsigned int baseaddr, ram_base;
-  struct grcan_regs *regs;
-  int irq;
-  int minor;
-  int open;
-  int started;
-  unsigned int channel;
-  int flushing;
-  unsigned int corefreq_hz;
-
-  /* Circular DMA buffers */
-  void *_rx;
-  void *_tx;
-  struct grcan_msg *rx;
-  struct grcan_msg *tx;
-  unsigned int rxbuf_size;    /* requested RX buf size in bytes */
-  unsigned int txbuf_size;    /* requested TX buf size in bytes */
-
-  int txblock, rxblock;
-  int txcomplete, rxcomplete;
-  int txerror, rxerror;
-
-  struct grcan_filter sfilter;
-  struct grcan_filter afilter;
-  int config_changed; /* 0=no changes, 1=changes ==> a Core reset is needed */
-  struct grcan_config config;
-  struct grcan_stats stats;
-
-  rtems_id rx_sem, tx_sem, txempty_sem, dev_sem;
+	struct drvmgr_dev *dev;	/* Driver manager device */
+	char devName[32];	/* Device Name */
+	unsigned int baseaddr, ram_base;
+	struct grcan_regs *regs;
+	int irq;
+	int minor;
+	int open;
+	int started;
+	unsigned int channel;
+	int flushing;
+	unsigned int corefreq_hz;
+
+	/* Circular DMA buffers */
+	void *_rx, *_rx_hw;
+	void *_tx, *_tx_hw;
+	void *txbuf_adr;
+	void *rxbuf_adr;
+	struct grcan_msg *rx;
+	struct grcan_msg *tx;
+	unsigned int rxbuf_size;    /* requested RX buf size in bytes */
+	unsigned int txbuf_size;    /* requested TX buf size in bytes */
+
+	int txblock, rxblock;
+	int txcomplete, rxcomplete;
+	int txerror, rxerror;
+
+	struct grcan_filter sfilter;
+	struct grcan_filter afilter;
+	int config_changed; /* 0=no changes, 1=changes ==> a Core reset is needed */
+	struct grcan_config config;
+	struct grcan_stats stats;
+
+	rtems_id rx_sem, tx_sem, txempty_sem, dev_sem;
 };
 
-static int grcan_core_cnt;
-struct grcan_priv *grcans;
-static struct ambapp_bus *amba_bus;
-struct grcan_device_info *grcan_cores;
-static int grcan_core_cnt;
-
 static rtems_device_driver grcan_initialize(rtems_device_major_number  major, rtems_device_minor_number  minor, void *arg);
 static rtems_device_driver grcan_open(rtems_device_major_number major, rtems_device_minor_number minor, void *arg);
 static rtems_device_driver grcan_close(rtems_device_major_number major, rtems_device_minor_number minor, void *arg);
@@ -227,6 +146,8 @@ static rtems_device_driver grcan_ioctl(rtems_device_major_number major, rtems_de
 
 #define GRCAN_DRIVER_TABLE_ENTRY { grcan_initialize, grcan_open, grcan_close, grcan_read, grcan_write, grcan_ioctl }
 
+static void __inline__ grcan_hw_reset(struct grcan_regs *regs);
+
 static unsigned int grcan_hw_read_try(
   struct grcan_priv *pDev,
   struct grcan_regs *regs,
@@ -251,11 +172,7 @@ static void grcan_hw_sync(
   struct grcan_regs *regs,
   struct grcan_filter *sfilter);
 
-#ifndef GRCAN_DONT_DECLARE_IRQ_HANDLER
-static rtems_isr grcan_interrupt_handler(rtems_vector_number v);
-#endif
-
-static void grcan_interrupt(struct grcan_priv *pDev);
+static void grcan_interrupt(void *arg);
 
 #ifdef GRCAN_REG_BYPASS_CACHE
 #define READ_REG(address) _grcan_read_nocache((unsigned int)(address))
@@ -268,12 +185,12 @@ static void grcan_interrupt(struct grcan_priv *pDev);
 #define READ_DMA_BYTE(address) _grcan_read_nocache_byte((unsigned int)(address))
 static unsigned char __inline__ _grcan_read_nocache_byte(unsigned int address)
 {
-  unsigned char tmp;
-  __asm__ (" lduba [%1]1, %0 "
-    : "=r"(tmp)
-    : "r"(address)
-  );
-  return tmp;
+	unsigned char tmp;
+	__asm__ (" lduba [%1]1, %0 "
+	    : "=r"(tmp)
+	    : "r"(address)
+	);
+	return tmp;
 }
 #else
 #define READ_DMA_WORD(address) (*(volatile unsigned int *)(address))
@@ -283,17 +200,228 @@ static unsigned char __inline__ _grcan_read_nocache_byte(unsigned int address)
 #if defined(GRCAN_REG_BYPASS_CACHE) || defined(GRCAN_DMA_BYPASS_CACHE)
 static unsigned int __inline__ _grcan_read_nocache(unsigned int address)
 {
-  unsigned int tmp;
-  __asm__ (" lda [%1]1, %0 "
-    : "=r"(tmp)
-    : "r"(address)
-  );
-  return tmp;
+	unsigned int tmp;
+	__asm__ (" lda [%1]1, %0 "
+	        : "=r"(tmp)
+	        : "r"(address)
+	);
+	return tmp;
 }
 #endif
 
 
 static rtems_driver_address_table grcan_driver = GRCAN_DRIVER_TABLE_ENTRY;
+static int grcan_driver_io_registered = 0;
+static rtems_device_major_number grcan_driver_io_major = 0;
+
+/******************* Driver manager interface ***********************/
+
+/* Driver prototypes */
+int grcan_register_io(rtems_device_major_number *m);
+int grcan_device_init(struct grcan_priv *pDev);
+
+int grcan_init2(struct drvmgr_dev *dev);
+int grcan_init3(struct drvmgr_dev *dev);
+
+struct drvmgr_drv_ops grcan_ops = 
+{
+	.init = {NULL, grcan_init2, grcan_init3, NULL},
+	.remove = NULL,
+	.info = NULL
+};
+
+struct amba_dev_id grcan_ids[] = 
+{
+	{VENDOR_GAISLER, GAISLER_GRCAN},
+	{VENDOR_GAISLER, GAISLER_GRHCAN},
+	{0, 0}		/* Mark end of table */
+};
+
+struct amba_drv_info grcan_drv_info =
+{
+	{
+		DRVMGR_OBJ_DRV,			/* Driver */
+		NULL,				/* Next driver */
+		NULL,				/* Device list */
+		DRIVER_AMBAPP_GAISLER_GRCAN_ID,	/* Driver ID */
+		"GRCAN_DRV",			/* Driver Name */
+		DRVMGR_BUS_TYPE_AMBAPP,		/* Bus Type */
+		&grcan_ops,
+		NULL,				/* Funcs */
+		0,				/* No devices yet */
+		0,
+	},
+	&grcan_ids[0]
+};
+
+void grcan_register_drv (void)
+{
+	DBG("Registering GRCAN driver\n");
+	drvmgr_drv_register(&grcan_drv_info.general);
+}
+
+int grcan_init2(struct drvmgr_dev *dev)
+{
+	struct grcan_priv *priv;
+
+	DBG("GRCAN[%d] on bus %s\n", dev->minor_drv, dev->parent->dev->name);
+	priv = dev->priv = malloc(sizeof(struct grcan_priv));
+	if ( !priv )
+		return DRVMGR_NOMEM;
+	memset(priv, 0, sizeof(*priv));
+	priv->dev = dev;
+
+	/* This core will not find other cores, so we wait for init2() */
+
+	return DRVMGR_OK;
+}
+
+int grcan_init3(struct drvmgr_dev *dev)
+{
+	struct grcan_priv *priv;
+	char prefix[32];
+	rtems_status_code status;
+
+	priv = dev->priv;
+
+	/* Do initialization */
+
+	if ( grcan_driver_io_registered == 0) {
+		/* Register the I/O driver only once for all cores */
+		if ( grcan_register_io(&grcan_driver_io_major) ) {
+			/* Failed to register I/O driver */
+			dev->priv = NULL;
+			return DRVMGR_FAIL;
+		}
+
+		grcan_driver_io_registered = 1;
+	}
+
+	/* I/O system registered and initialized 
+	 * Now we take care of device initialization.
+	 */
+
+	if ( grcan_device_init(priv) ) {
+		return DRVMGR_FAIL;
+	}
+
+	/* Get Filesystem name prefix */
+	prefix[0] = '\0';
+	if ( drvmgr_get_dev_prefix(dev, prefix) ) {
+		/* Failed to get prefix, make sure of a unique FS name
+		 * by using the driver minor.
+		 */
+		sprintf(priv->devName, "/dev/grcan%d", dev->minor_drv);
+	} else {
+		/* Got special prefix, this means we have a bus prefix
+		 * And we should use our "bus minor"
+		 */
+		sprintf(priv->devName, "/dev/%sgrcan%d", prefix, dev->minor_bus);
+	}
+
+	/* Register Device */
+	status = rtems_io_register_name(priv->devName, grcan_driver_io_major, dev->minor_drv);
+	if (status != RTEMS_SUCCESSFUL) {
+		return DRVMGR_FAIL;
+	}
+
+	return DRVMGR_OK;
+}
+
+/******************* Driver Implementation ***********************/
+
+int grcan_register_io(rtems_device_major_number *m)
+{
+	rtems_status_code r;
+
+	if ((r = rtems_io_register_driver(0, &grcan_driver, m)) == RTEMS_SUCCESSFUL) {
+		DBG("GRCAN driver successfully registered, major: %d\n", *m);
+	} else {
+		switch(r) {
+		case RTEMS_TOO_MANY:
+			printk("GRCAN rtems_io_register_driver failed: RTEMS_TOO_MANY\n");
+			return -1;
+		case RTEMS_INVALID_NUMBER:  
+			printk("GRCAN rtems_io_register_driver failed: RTEMS_INVALID_NUMBER\n");
+			return -1;
+		case RTEMS_RESOURCE_IN_USE:
+			printk("GRCAN rtems_io_register_driver failed: RTEMS_RESOURCE_IN_USE\n");
+			return -1;
+		default:
+			printk("GRCAN rtems_io_register_driver failed\n");
+			return -1;
+		}
+	}
+	return 0;
+}
+
+int grcan_device_init(struct grcan_priv *pDev)
+{
+	struct amba_dev_info *ambadev;
+	struct ambapp_core *pnpinfo;
+
+	/* Get device information from AMBA PnP information */
+	ambadev = (struct amba_dev_info *)pDev->dev->businfo;
+	if ( ambadev == NULL ) {
+		return -1;
+	}
+	pnpinfo = &ambadev->info;
+	pDev->irq = pnpinfo->irq;
+	pDev->regs = (struct grcan_regs *)pnpinfo->apb_slv->start;
+	pDev->minor = pDev->dev->minor_drv;
+
+	/* Get frequency in Hz */
+	if ( drvmgr_freq_get(pDev->dev, DEV_APB_SLV, &pDev->corefreq_hz) ) {
+		return -1;
+	}
+
+	DBG("GRCAN frequency: %d Hz\n", pDev->corefreq_hz);
+
+	/* Reset Hardware before attaching IRQ handler */
+	grcan_hw_reset(pDev->regs);
+
+	/* RX Semaphore created with count = 0 */
+	if ( rtems_semaphore_create(rtems_build_name('G', 'C', 'R', '0' + pDev->minor),
+		0,
+		RTEMS_FIFO|RTEMS_SIMPLE_BINARY_SEMAPHORE|RTEMS_NO_INHERIT_PRIORITY|\
+		RTEMS_LOCAL|RTEMS_NO_PRIORITY_CEILING, 
+		0,
+		&pDev->rx_sem) != RTEMS_SUCCESSFUL ) {
+		return RTEMS_INTERNAL_ERROR;
+	}
+
+	/* TX Semaphore created with count = 0 */
+	if ( rtems_semaphore_create(rtems_build_name('G', 'C', 'T', '0' + pDev->minor),
+		0,
+		RTEMS_FIFO|RTEMS_SIMPLE_BINARY_SEMAPHORE|RTEMS_NO_INHERIT_PRIORITY|\
+		RTEMS_LOCAL|RTEMS_NO_PRIORITY_CEILING, 
+		0,
+		&pDev->tx_sem) != RTEMS_SUCCESSFUL ) {
+		return RTEMS_INTERNAL_ERROR;
+	}
+
+	/* TX Empty Semaphore created with count = 0 */
+	if ( rtems_semaphore_create(rtems_build_name('G', 'C', 'E', '0' + pDev->minor),
+		0,
+		RTEMS_FIFO|RTEMS_SIMPLE_BINARY_SEMAPHORE|RTEMS_NO_INHERIT_PRIORITY|\
+		RTEMS_LOCAL|RTEMS_NO_PRIORITY_CEILING, 
+		0,
+		&pDev->txempty_sem) != RTEMS_SUCCESSFUL ) {
+		return RTEMS_INTERNAL_ERROR;
+	}
+
+	/* Device Semaphore created with count = 1 */
+	if ( rtems_semaphore_create(rtems_build_name('G', 'C', 'A', '0' + pDev->minor),
+		1,
+		RTEMS_FIFO|RTEMS_SIMPLE_BINARY_SEMAPHORE|RTEMS_NO_INHERIT_PRIORITY|\
+		RTEMS_LOCAL|RTEMS_NO_PRIORITY_CEILING,
+		0,
+		&pDev->dev_sem) != RTEMS_SUCCESSFUL ) {
+		return RTEMS_INTERNAL_ERROR;
+	}
+
+	return 0;
+}
 
 static void __inline__ grcan_hw_reset(struct grcan_regs *regs)
 {
@@ -325,11 +453,11 @@ static rtems_device_driver grcan_start(struct grcan_priv *pDev)
   }
 
   /* Setup receiver */
-  pDev->regs->rx0addr = MEMAREA_TO_HW((unsigned int)pDev->rx);
+  pDev->regs->rx0addr = (unsigned int)pDev->_rx_hw;
   pDev->regs->rx0size = pDev->rxbuf_size;
 
   /* Setup Transmitter */
-  pDev->regs->tx0addr = MEMAREA_TO_HW((unsigned int)pDev->tx);
+  pDev->regs->tx0addr = (unsigned int)pDev->_tx_hw;
   pDev->regs->tx0size = pDev->txbuf_size;
 
   /* Setup acceptance filters */
@@ -875,7 +1003,7 @@ static int grcan_wait_txspace(
 
   IRQ_GLOBAL_DISABLE(oldLevel);
 
-  /*pDev->regs->tx0ctrl = GRCAN_TXCTRL_ENABLE;*/
+  pDev->regs->tx0ctrl = GRCAN_TXCTRL_ENABLE;
 
   size = READ_REG(&pDev->regs->tx0size);
   wp = READ_REG(&pDev->regs->tx0wr);
@@ -972,851 +1100,720 @@ static int grcan_tx_flush(struct grcan_priv *pDev)
 
 static int grcan_alloc_buffers(struct grcan_priv *pDev, int rx, int tx)
 {
-  FUNCDBG();
-
-  if ( tx ) {
-#ifdef STATIC_TX_BUF_ADDR
-    pDev->_tx = STATIC_TX_BUF_ADDR(pDev->minor);
-    if ( pDev->txbuf_size > STATIC_TX_BUF_SIZE ){
-      pDev->txbuf_size = STATIC_TX_BUF_SIZE;
-      return -1;
-    }
-    /* Assume aligned buffer */
-    pDev->tx = (struct grcan_msg *)pDev->_tx;
-#else
-    pDev->_tx = malloc(pDev->txbuf_size + BUFFER_ALIGNMENT_NEEDS);
-    if ( !pDev->_tx )
-      return -1;
-
-    /* Align TX buffer */
-    pDev->tx = (struct grcan_msg *)
-               (((unsigned int)pDev->_tx + (BUFFER_ALIGNMENT_NEEDS-1)) &
-               ~(BUFFER_ALIGNMENT_NEEDS-1));
-#endif
-  }
-
-  if ( rx ) {
-#ifdef STATIC_RX_BUF_ADDR
-    pDev->_rx = STATIC_RX_BUF_ADDR(pDev->minor);
-    if ( pDev->rxbuf_size > STATIC_RX_BUF_SIZE ){
-      pDev->rxbuf_size = STATIC_RX_BUF_SIZE;
-      return -1;
-    }
-    /* Assume aligned buffer */
-    pDev->rx = (struct grcan_msg *)pDev->_rx;
-#else
-    pDev->_rx = malloc(pDev->rxbuf_size + BUFFER_ALIGNMENT_NEEDS);
-    if ( !pDev->_rx )
-      return -1;
+	unsigned int adr;
+	FUNCDBG();
+
+	if ( tx ) {
+		adr = (unsigned int)pDev->txbuf_adr;
+		if (adr & 0x1) {
+			/* User defined "remote" address. Translate it into
+			 * a CPU accessible address
+			 */
+			pDev->_tx_hw = (void *)(adr & ~0x1);
+			drvmgr_translate_check(
+				pDev->dev,
+				DMAMEM_TO_CPU,
+				(void *)pDev->_tx_hw,
+				(void **)&pDev->_tx,
+				pDev->txbuf_size);
+			pDev->tx = (struct grcan_msg *)pDev->_tx;
+		} else {
+			if (adr == 0) {
+				pDev->_tx = malloc(pDev->txbuf_size +
+				                   BUFFER_ALIGNMENT_NEEDS);
+				if (!pDev->_tx)
+					return -1;
+			} else {
+				/* User defined "cou-local" address. Translate
+				 * it into a CPU accessible address
+				 */
+				pDev->_tx = (void *)adr;
+			}
+			/* Align TX buffer */
+			pDev->tx = (struct grcan_msg *)
+			           (((unsigned int)pDev->_tx +
+				   (BUFFER_ALIGNMENT_NEEDS-1)) &
+			           ~(BUFFER_ALIGNMENT_NEEDS-1));
+
+			/* Translate address into an hardware accessible
+			 * address
+			 */
+			drvmgr_translate_check(
+				pDev->dev,
+				CPUMEM_TO_DMA,
+				(void *)pDev->tx,
+				(void **)&pDev->_tx_hw,
+				pDev->txbuf_size);
+		}
+	}
 
-    /* Align TX buffer */
-    pDev->rx = (struct grcan_msg *)
-               (((unsigned int)pDev->_rx + (BUFFER_ALIGNMENT_NEEDS-1)) &
-               ~(BUFFER_ALIGNMENT_NEEDS-1));
-#endif
-  }
-  return 0;
+	if ( rx ) {
+		adr = (unsigned int)pDev->rxbuf_adr;
+		if (adr & 0x1) {
+			/* User defined "remote" address. Translate it into
+			 * a CPU accessible address
+			 */
+			pDev->_rx_hw = (void *)(adr & ~0x1);
+			drvmgr_translate_check(
+				pDev->dev,
+				DMAMEM_TO_CPU,
+				(void *)pDev->_rx_hw,
+				(void **)&pDev->_rx,
+				pDev->rxbuf_size);
+			pDev->rx = (struct grcan_msg *)pDev->_rx;
+		} else {
+			if (adr == 0) {
+				pDev->_rx = malloc(pDev->rxbuf_size +
+				                   BUFFER_ALIGNMENT_NEEDS);
+				if (!pDev->_rx)
+					return -1;
+			} else {
+				/* User defined "cou-local" address. Translate
+				 * it into a CPU accessible address
+				 */
+				pDev->_rx = (void *)adr;
+			}
+			/* Align RX buffer */
+			pDev->rx = (struct grcan_msg *)
+			           (((unsigned int)pDev->_rx +
+				   (BUFFER_ALIGNMENT_NEEDS-1)) &
+			           ~(BUFFER_ALIGNMENT_NEEDS-1));
+
+			/* Translate address into an hardware accessible
+			 * address
+			 */
+			drvmgr_translate_check(
+				pDev->dev,
+				CPUMEM_TO_DMA,
+				(void *)pDev->rx,
+				(void **)&pDev->_rx_hw,
+				pDev->rxbuf_size);
+		}
+	}
+	return 0;
 }
 
 static void grcan_free_buffers(struct grcan_priv *pDev, int rx, int tx)
 {
   FUNCDBG();
 
-#ifndef STATIC_TX_BUF_ADDR
   if ( tx && pDev->_tx ){
     free(pDev->_tx);
     pDev->_tx = NULL;
     pDev->tx = NULL;
   }
-#endif
-#ifndef STATIC_RX_BUF_ADDR
+
   if ( rx && pDev->_rx ){
     free(pDev->_rx);
     pDev->_rx = NULL;
     pDev->rx = NULL;
   }
-#endif
 }
 
-#if 0
-static char *almalloc(int sz)
-{
-  char *tmp;
-  tmp = calloc(1,2*sz);
-  tmp = (char *) (((int)tmp+sz) & ~(sz -1));
-  return(tmp);
-}
-#endif
-
 static rtems_device_driver grcan_initialize(
   rtems_device_major_number major,
   rtems_device_minor_number unused,
   void *arg
   )
 {
-  int minor;
-  struct grcan_priv *pDev;
-  struct ambapp_apb_info dev;
-  rtems_status_code status;
-  char fs_name[20];
-  unsigned int sys_freq_hz;
-  unsigned int deviceid = GAISLER_GRHCAN;
-
-  printk("grcan_initialize()\n\r");
-
-  FUNCDBG();
-
-  /* find GRCAN cores */
-  if ( !grcan_cores ) {
-    grcan_core_cnt = ambapp_get_number_apbslv_devices(amba_bus, VENDOR_GAISLER,
-                                                      deviceid);
-    if ( grcan_core_cnt < 1 ){
-      deviceid = GAISLER_GRCAN;
-      grcan_core_cnt = ambapp_get_number_apbslv_devices(amba_bus, VENDOR_GAISLER,
-                                                        deviceid);
-      if ( grcan_core_cnt < 1 ) {
-        DBG("GRCAN: Using AMBA Plug&Play, found %d cores\n",grcan_core_cnt);
-        return RTEMS_UNSATISFIED;
-      }
-    }
-    DBG("GRCAN: Using AMBA Plug&Play, found %d cores\n",grcan_core_cnt);
-  }
-
-#ifdef GRCAN_MAX_CORENR
-  /* limit number of cores */
-  if ( grcan_core_cnt > GRCAN_MAX_CORENR )
-    grcan_core_cnt = GRCAN_MAX_CORENR;
-#endif
-
-  /* Allocate memory for cores */
-  grcans = malloc(grcan_core_cnt * sizeof(struct grcan_priv));
-  if ( !grcans )
-    return RTEMS_NO_MEMORY;
-  memset(grcans,0,grcan_core_cnt * sizeof(struct grcan_priv));
-
-  /* make a local copy of device name */
-  strcpy(fs_name,GRCAN_DEVNAME);
-
-  /* Detect System Frequency from initialized timer */
-#ifndef SYS_FREQ_HZ
-#if defined(LEON3)
-  /* LEON3: find timer address via AMBA Plug&Play info */
-  {
-    struct ambapp_apb_info gptimer;
-    struct gptimer_regs *tregs;
-
-    if (ambapp_find_apbslv (&ambapp_plb, VENDOR_GAISLER, GAISLER_GPTIMER, &gptimer)
-        == 1) {
-      tregs = (struct gptimer_regs *) gptimer.start;
-      sys_freq_hz = (tregs->scaler_reload + 1) * 1000 * 1000;
-      DBG("GRCAN: detected %dHZ system frequency\n\r", sys_freq_hz);
-    } else {
-      sys_freq_hz = 40000000;   /* Default to 40MHz */
-      printk("GRCAN: Failed to detect system frequency\n\r");
-    }
-  }
-#elif defined(LEON2)
-  /* LEON2: use hardcoded address to get to timer */
-  {
-    LEON_Register_Map *regs = (LEON_Register_Map *) 0x80000000;
-
-    sys_freq_hz = (regs->Scaler_Reload + 1) * 1000 * 1000;
-  }
-#else
-#error CPU not supported by driver
-#endif
-#else
-  /* Use hardcoded frequency */
-  sys_freq_hz = SYS_FREQ_HZ;
-#endif
-
-  for(minor=0; minor<grcan_core_cnt; minor++){
-
-    pDev = &grcans[minor];
-    pDev->minor = minor;
-    pDev->open = 0;
-    pDev->corefreq_hz = sys_freq_hz;
-    GRCAN_DEVNAME_NO(fs_name,minor);
-
-    /* Find core address & IRQ */
-    if ( !grcan_cores ) {
-      ambapp_find_apbslv_next(amba_bus, VENDOR_GAISLER, deviceid, &dev, minor);
-      pDev->irq = dev.irq;
-      pDev->regs = (struct grcan_regs *)dev.start;
-    }else{
-      pDev->irq = grcan_cores[minor].irq;
-      pDev->regs = (struct grcan_regs *)grcan_cores[minor].base_address;
-    }
-
-    printk("Registering GRCAN core at [0x%x] irq %d, minor %d as %s\n\r",pDev->regs,pDev->irq,minor,fs_name);
-
-    status = rtems_io_register_name(fs_name, major, 0);
-    if (status != RTEMS_SUCCESSFUL)
-      rtems_fatal_error_occurred(status);
-
-		/* Reset Hardware before attaching IRQ handler */
-    grcan_hw_reset(pDev->regs);
-
-    /* Register interrupt handler */
-    GRCAN_REG_INT(GRCAN_PREFIX(_interrupt_handler), pDev->irq+GRCAN_IRQ_IRQ, pDev);
-    /*
-    GRCAN_REG_INT(grcan_interrupt_handler, pDev->irq+GRCAN_IRQ_TXSYNC, pDev);
-    GRCAN_REG_INT(grcan_interrupt_handler, pDev->irq+GRCAN_IRQ_RXSYNC, pDev);
-    */
-
-    /* RX Semaphore created with count = 0 */
-    if ( rtems_semaphore_create(rtems_build_name('G', 'C', 'R', '0'+minor),
-        0,
-        RTEMS_FIFO|RTEMS_SIMPLE_BINARY_SEMAPHORE|RTEMS_NO_INHERIT_PRIORITY|\
-        RTEMS_LOCAL|RTEMS_NO_PRIORITY_CEILING,
-        0,
-        &pDev->rx_sem) != RTEMS_SUCCESSFUL )
-      return RTEMS_INTERNAL_ERROR;
-
-    /* TX Semaphore created with count = 0 */
-    if ( rtems_semaphore_create(rtems_build_name('G', 'C', 'T', '0'+minor),
-        0,
-        RTEMS_FIFO|RTEMS_SIMPLE_BINARY_SEMAPHORE|RTEMS_NO_INHERIT_PRIORITY|\
-        RTEMS_LOCAL|RTEMS_NO_PRIORITY_CEILING,
-        0,
-        &pDev->tx_sem) != RTEMS_SUCCESSFUL )
-      return RTEMS_INTERNAL_ERROR;
-
-    /* TX Empty Semaphore created with count = 0 */
-    if ( rtems_semaphore_create(rtems_build_name('G', 'C', 'E', '0'+minor),
-        0,
-        RTEMS_FIFO|RTEMS_SIMPLE_BINARY_SEMAPHORE|RTEMS_NO_INHERIT_PRIORITY|\
-        RTEMS_LOCAL|RTEMS_NO_PRIORITY_CEILING,
-        0,
-        &pDev->txempty_sem) != RTEMS_SUCCESSFUL )
-      return RTEMS_INTERNAL_ERROR;
-
-    /* Device Semaphore created with count = 1 */
-    if ( rtems_semaphore_create(rtems_build_name('G', 'C', 'A', '0'+minor),
-        1,
-        RTEMS_FIFO|RTEMS_SIMPLE_BINARY_SEMAPHORE|RTEMS_NO_INHERIT_PRIORITY|\
-        RTEMS_LOCAL|RTEMS_NO_PRIORITY_CEILING,
-        0,
-        &pDev->dev_sem) != RTEMS_SUCCESSFUL )
-      return RTEMS_INTERNAL_ERROR;
-  }
-
-  return RTEMS_SUCCESSFUL;
+	return RTEMS_SUCCESSFUL;
 }
 
-static rtems_device_driver grcan_open(rtems_device_major_number major, rtems_device_minor_number minor, void *arg) {
-  struct grcan_priv *pDev;
-  rtems_device_driver ret;
+static rtems_device_driver grcan_open(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
+{
+	struct grcan_priv *pDev;
+	rtems_device_driver ret;
+	struct drvmgr_dev *dev;
+	union drvmgr_key_value *value;
 
-  FUNCDBG();
+	FUNCDBG();
 
-  if ( (minor < 0) || (minor>=grcan_core_cnt) ) {
-    DBG("Wrong minor %d\n", minor);
-    return RTEMS_INVALID_NUMBER;
-  }
-
-  pDev = &grcans[minor];
+	if ( drvmgr_get_dev(&grcan_drv_info.general, minor, &dev) ) {
+		DBG("Wrong minor %d\n", minor);
+		return RTEMS_INVALID_NAME;
+	}
+	pDev = (struct grcan_priv *)dev->priv;
 
-  /* Wait until we get semaphore */
-  if ( rtems_semaphore_obtain(pDev->dev_sem, RTEMS_WAIT, RTEMS_NO_TIMEOUT) !=
-       RTEMS_SUCCESSFUL ){
-    return RTEMS_INTERNAL_ERROR;
-  }
+	/* Wait until we get semaphore */
+	if (rtems_semaphore_obtain(pDev->dev_sem, RTEMS_WAIT, RTEMS_NO_TIMEOUT)
+	    != RTEMS_SUCCESSFUL) {
+		return RTEMS_INTERNAL_ERROR;
+	}
 
-  /* is device busy/taken? */
-  if  ( pDev->open ) {
-    ret=RTEMS_RESOURCE_IN_USE;
-    goto out;
-  }
+	/* is device busy/taken? */
+	if ( pDev->open ) {
+		ret=RTEMS_RESOURCE_IN_USE;
+		goto out;
+	}
 
-  /* Mark device taken */
-  pDev->open = 1;
-
-  pDev->txblock = pDev->rxblock = 1;
-  pDev->txcomplete = pDev->rxcomplete = 0;
-  pDev->started = 0;
-  pDev->config_changed = 1;
-  pDev->config.silent = 0;
-  pDev->config.abort = 0;
-  pDev->config.selection.selection = 0;
-  pDev->config.selection.enable0 = 0;
-  pDev->config.selection.enable1 = 1;
-  pDev->flushing = 0;
-  pDev->rx = pDev->_rx = NULL;
-  pDev->tx = pDev->_tx = NULL;
-  pDev->txbuf_size = TX_BUF_SIZE;
-  pDev->rxbuf_size = RX_BUF_SIZE;
-  printk("Defaulting to rxbufsize: %d, txbufsize: %d\n",RX_BUF_SIZE,TX_BUF_SIZE);
-
-  /* Default to accept all messages */
-  pDev->afilter.mask = 0x00000000;
-  pDev->afilter.code = 0x00000000;
-
-  /* Default to disable sync messages (only trigger when id is set to all ones) */
-  pDev->sfilter.mask = 0xffffffff;
-  pDev->sfilter.code = 0x00000000;
-
-  /* Calculate default timing register values */
-  grcan_calc_timing(GRCAN_DEFAULT_BAUD,pDev->corefreq_hz,GRCAN_SAMPLING_POINT,&pDev->config.timing);
-
-  if ( grcan_alloc_buffers(pDev,1,1) ) {
-    ret=RTEMS_NO_MEMORY;
-    goto out;
-  }
+	/* Mark device taken */
+	pDev->open = 1;
+
+	pDev->txblock = pDev->rxblock = 1;
+	pDev->txcomplete = pDev->rxcomplete = 0;
+	pDev->started = 0;
+	pDev->config_changed = 1;
+	pDev->config.silent = 0;
+	pDev->config.abort = 0;
+	pDev->config.selection.selection = 0;
+	pDev->config.selection.enable0 = 0;
+	pDev->config.selection.enable1 = 1;
+	pDev->flushing = 0;
+	pDev->rx = pDev->_rx = NULL;
+	pDev->tx = pDev->_tx = NULL;
+	pDev->txbuf_adr = 0;
+	pDev->rxbuf_adr = 0;
+	pDev->txbuf_size = TX_BUF_SIZE;
+	pDev->rxbuf_size = RX_BUF_SIZE;
+
+	/* Override default buffer sizes if available from bus resource */
+	value = drvmgr_dev_key_get(pDev->dev, "txBufSize", KEY_TYPE_INT);
+	if ( value )
+		pDev->txbuf_size = value->i;
+
+	value = drvmgr_dev_key_get(pDev->dev, "rxBufSize", KEY_TYPE_INT);
+	if ( value )
+		pDev->rxbuf_size = value->i;
+
+	value = drvmgr_dev_key_get(pDev->dev, "txBufAdr", KEY_TYPE_POINTER);
+	if ( value )
+		pDev->txbuf_adr = value->ptr;
+
+	value = drvmgr_dev_key_get(pDev->dev, "rxBufAdr", KEY_TYPE_POINTER);
+	if ( value )
+		pDev->rxbuf_adr = value->ptr;
+
+	DBG("Defaulting to rxbufsize: %d, txbufsize: %d\n",RX_BUF_SIZE,TX_BUF_SIZE);
+
+	/* Default to accept all messages */
+	pDev->afilter.mask = 0x00000000;
+	pDev->afilter.code = 0x00000000;
+
+	/* Default to disable sync messages (only trigger when id is set to all ones) */
+	pDev->sfilter.mask = 0xffffffff;
+	pDev->sfilter.code = 0x00000000;
+
+	/* Calculate default timing register values */
+	grcan_calc_timing(GRCAN_DEFAULT_BAUD,pDev->corefreq_hz,GRCAN_SAMPLING_POINT,&pDev->config.timing);
+
+	if ( grcan_alloc_buffers(pDev,1,1) ) {
+		ret=RTEMS_NO_MEMORY;
+		goto out;
+	}
 
-  /* Clear statistics */
-  memset(&pDev->stats,0,sizeof(struct grcan_stats));
+	/* Clear statistics */
+	memset(&pDev->stats,0,sizeof(struct grcan_stats));
 
-  ret = RTEMS_SUCCESSFUL;
+	ret = RTEMS_SUCCESSFUL;
 out:
-  rtems_semaphore_release(pDev->dev_sem);
-  return ret;
+	rtems_semaphore_release(pDev->dev_sem);
+	return ret;
 }
 
 static rtems_device_driver grcan_close(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
 {
-  struct grcan_priv *pDev = &grcans[minor];
+	struct grcan_priv *pDev;
+	struct drvmgr_dev *dev;
 
-  FUNCDBG();
+	FUNCDBG();
 
-  if ( pDev->started )
-    grcan_stop(pDev);
+	if ( drvmgr_get_dev(&grcan_drv_info.general, minor, &dev) ) {
+		return RTEMS_INVALID_NAME;
+	}
+	pDev = (struct grcan_priv *)dev->priv;
 
-  grcan_hw_reset(pDev->regs);
+	if ( pDev->started )
+		grcan_stop(pDev);
 
-  grcan_free_buffers(pDev,1,1);
+	grcan_hw_reset(pDev->regs);
 
-  /* Mark Device as closed */
-  pDev->open = 0;
+	grcan_free_buffers(pDev,1,1);
 
-  return RTEMS_SUCCESSFUL;
+	/* Mark Device as closed */
+	pDev->open = 0;
+
+	return RTEMS_SUCCESSFUL;
 }
 
 static rtems_device_driver grcan_read(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
 {
-  struct grcan_priv *pDev = &grcans[minor];
-  rtems_libio_rw_args_t *rw_args;
-  CANMsg *dest;
-  unsigned int count, left;
-  int req_cnt;
+	struct grcan_priv *pDev;
+	struct drvmgr_dev *dev;
+	rtems_libio_rw_args_t *rw_args;  
+	CANMsg *dest;
+	unsigned int count, left;
+	int req_cnt;
 
-  rw_args = (rtems_libio_rw_args_t *) arg;
-  dest = (CANMsg *) rw_args->buffer;
-  req_cnt = rw_args->count / sizeof(CANMsg);
+	FUNCDBG();
 
-  FUNCDBG();
+	if ( drvmgr_get_dev(&grcan_drv_info.general, minor, &dev) ) {
+		return RTEMS_INVALID_NAME;
+	}
+	pDev = (struct grcan_priv *)dev->priv;
 
-  if ( (!dest) || (req_cnt<1) )
-    return RTEMS_INVALID_NAME;
+	rw_args = (rtems_libio_rw_args_t *) arg;
+	dest = (CANMsg *) rw_args->buffer;
+	req_cnt = rw_args->count / sizeof(CANMsg);
 
-  if ( !pDev->started )
-    return RTEMS_RESOURCE_IN_USE;
+	if ( (!dest) || (req_cnt<1) )
+		return RTEMS_INVALID_NAME;
 
-/*  FUNCDBG("grcan_read [%i,%i]: buf: 0x%x len: %i\n",major, minor, (unsigned int)rw_args->buffer,rw_args->count);*/
+	if ( !pDev->started )
+		return RTEMS_RESOURCE_IN_USE;
 
-  count = grcan_hw_read_try(pDev,pDev->regs,dest,req_cnt);
-  if ( !( pDev->rxblock && pDev->rxcomplete && (count!=req_cnt) ) ){
-    if ( count > 0 ) {
-      /* Successfully received messages (at least one) */
-      rw_args->bytes_moved = count * sizeof(CANMsg);
-      return RTEMS_SUCCESSFUL;
-    }
+	/*FUNCDBG("grcan_read [%i,%i]: buf: 0x%x len: %i\n",major, minor, (unsigned int)rw_args->buffer,rw_args->count);*/
 
-    /* nothing read, shall we block? */
-    if ( !pDev->rxblock ) {
-      /* non-blocking mode */
-      rw_args->bytes_moved = 0;
-      return RTEMS_TIMEOUT;
-    }
-  }
+	count = grcan_hw_read_try(pDev,pDev->regs,dest,req_cnt);
+	if ( !( pDev->rxblock && pDev->rxcomplete && (count!=req_cnt) ) ){
+		if ( count > 0 ) {
+			/* Successfully received messages (at least one) */
+			rw_args->bytes_moved = count * sizeof(CANMsg);
+			return RTEMS_SUCCESSFUL;
+		}
 
-  while(count == 0 || (pDev->rxcomplete && (count!=req_cnt)) ){
+		/* nothing read, shall we block? */
+		if ( !pDev->rxblock ) {
+			/* non-blocking mode */
+			rw_args->bytes_moved = 0;
+			return RTEMS_TIMEOUT;
+		}
+	}
 
-    if ( !pDev->rxcomplete ){
-      left = 1; /* return as soon as there is one message available */
-    }else{
-      left = req_cnt - count;     /* return as soon as all data are available */
-
-      /* never wait for more than the half the maximum size of the receive buffer
-       * Why? We need some time to copy buffer before to catch up with hw, otherwise
-       * we would have to copy everything when the data has been received.
-       */
-      if ( left > ((pDev->rxbuf_size/GRCAN_MSG_SIZE)/2) ){
-        left = (pDev->rxbuf_size/GRCAN_MSG_SIZE)/2;
-      }
-    }
+	while(count == 0 || (pDev->rxcomplete && (count!=req_cnt)) ){
 
-    if ( grcan_wait_rxdata(pDev,left) ) {
-      /* The wait has been aborted, probably due to
-       * the device driver has been closed by another
-       * thread.
-       */
-      rw_args->bytes_moved = count * sizeof(CANMsg);
-      return RTEMS_UNSATISFIED;
-    }
+	if ( !pDev->rxcomplete ){
+		left = 1; /* return as soon as there is one message available */
+	}else{
+		left = req_cnt - count;     /* return as soon as all data are available */
 
-    /* Try read bytes from circular buffer */
-    count += grcan_hw_read_try(
-      pDev,
-      pDev->regs,
-      dest+count,
-      req_cnt-count);
-  }
-  /* no need to unmask IRQ as IRQ Handler do that for us. */
-  rw_args->bytes_moved = count * sizeof(CANMsg);
-  return RTEMS_SUCCESSFUL;
+		/* never wait for more than the half the maximum size of the receive buffer 
+		 * Why? We need some time to copy buffer before to catch up with hw,
+		 * otherwise we would have to copy everything when the data has been
+		 * received.
+		 */
+		if ( left > ((pDev->rxbuf_size/GRCAN_MSG_SIZE)/2) ){
+			left = (pDev->rxbuf_size/GRCAN_MSG_SIZE)/2;
+		}
+	}
+
+	if ( grcan_wait_rxdata(pDev,left) ) {
+		/* The wait has been aborted, probably due to 
+		 * the device driver has been closed by another
+		 * thread.
+		 */
+		rw_args->bytes_moved = count * sizeof(CANMsg);
+		return RTEMS_UNSATISFIED;
+	}
+
+	/* Try read bytes from circular buffer */
+	count += grcan_hw_read_try(
+			pDev,
+			pDev->regs,
+			dest+count,
+			req_cnt-count);
+	}
+	/* no need to unmask IRQ as IRQ Handler do that for us. */
+	rw_args->bytes_moved = count * sizeof(CANMsg);
+	return RTEMS_SUCCESSFUL;
 }
 
 static rtems_device_driver grcan_write(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
 {
-  struct grcan_priv *pDev = &grcans[minor];
-  rtems_libio_rw_args_t *rw_args;
-  CANMsg *source;
-  unsigned int count, left;
-  int req_cnt;
+	struct grcan_priv *pDev;
+	struct drvmgr_dev *dev;
+	rtems_libio_rw_args_t *rw_args;
+	CANMsg *source;
+	unsigned int count, left;
+	int req_cnt;
 
-  DBGC(DBG_TX,"\n");
-  /*FUNCDBG();*/
+	DBGC(DBG_TX,"\n");
 
-  if ( !pDev->started || pDev->config.silent || pDev->flushing )
-    return RTEMS_RESOURCE_IN_USE;
+	if ( drvmgr_get_dev(&grcan_drv_info.general, minor, &dev) ) {
+		return RTEMS_INVALID_NAME;
+	}
+	pDev = (struct grcan_priv *)dev->priv;
 
-  rw_args = (rtems_libio_rw_args_t *) arg;
-  req_cnt = rw_args->count / sizeof(CANMsg);
-  source = (CANMsg *) rw_args->buffer;
+	if ( !pDev->started || pDev->config.silent || pDev->flushing )
+		return RTEMS_RESOURCE_IN_USE;
 
-  /* check proper length and buffer pointer */
-  if (( req_cnt < 1) || (source == NULL) ){
-    return RTEMS_INVALID_NAME;
-  }
+	rw_args = (rtems_libio_rw_args_t *) arg;
+	req_cnt = rw_args->count / sizeof(CANMsg);
+	source = (CANMsg *) rw_args->buffer;
 
-  count = grcan_hw_write_try(pDev,pDev->regs,source,req_cnt);
-  if ( !(pDev->txblock && pDev->txcomplete && (count!=req_cnt)) ) {
-    if ( count > 0 ) {
-      /* Successfully transmitted chars (at least one char) */
-      rw_args->bytes_moved = count * sizeof(CANMsg);
-      return RTEMS_SUCCESSFUL;
-    }
+	/* check proper length and buffer pointer */
+	if (( req_cnt < 1) || (source == NULL) ){
+		return RTEMS_INVALID_NAME;
+	}
 
-    /* nothing written, shall we block? */
-    if ( !pDev->txblock ) {
-      /* non-blocking mode */
-      rw_args->bytes_moved = 0;
-      return RTEMS_TIMEOUT;
-    }
-  }
+	count = grcan_hw_write_try(pDev,pDev->regs,source,req_cnt);
+	if ( !(pDev->txblock && pDev->txcomplete && (count!=req_cnt)) ) {
+		if ( count > 0 ) {
+			/* Successfully transmitted chars (at least one char) */
+			rw_args->bytes_moved = count * sizeof(CANMsg);
+			return RTEMS_SUCCESSFUL;
+		}
 
-  /* if in txcomplete mode we need to transmit all chars */
-  while((count == 0) || (pDev->txcomplete && (count!=req_cnt)) ){
-    /*** block until room to fit all or as much of transmit buffer as possible IRQ comes
-     * Set up a valid IRQ point so that an IRQ is received
-     * when we can put a chunk of data into transmit fifo
-     */
-    if ( !pDev->txcomplete ){
-      left = 1; /* wait for anything to fit buffer */
-    }else{
-      left = req_cnt - count; /* wait for all data to fit in buffer */
+		/* nothing written, shall we block? */
+		if ( !pDev->txblock ) {
+			/* non-blocking mode */
+			rw_args->bytes_moved = 0;
+			return RTEMS_TIMEOUT;
+		}
+	}
 
-      /* never wait for more than the half the maximum size of the transmitt buffer
-       * Why? We need some time to fill buffer before hw catches up.
-       */
-      if ( left > ((pDev->txbuf_size/GRCAN_MSG_SIZE)/2) ){
-        left = (pDev->txbuf_size/GRCAN_MSG_SIZE)/2;
-      }
-    }
+	/* if in txcomplete mode we need to transmit all chars */
+	while((count == 0) || (pDev->txcomplete && (count!=req_cnt)) ){
+		/*** block until room to fit all or as much of transmit buffer as possible
+		 * IRQ comes. Set up a valid IRQ point so that an IRQ is received 
+		 * when we can put a chunk of data into transmit fifo
+		 */
+		if ( !pDev->txcomplete ){
+			left = 1; /* wait for anything to fit buffer */
+		}else{
+			left = req_cnt - count; /* wait for all data to fit in buffer */
+
+			/* never wait for more than the half the maximum size of the transmit
+			 * buffer 
+			 * Why? We need some time to fill buffer before hw catches up.
+			 */
+			if ( left > ((pDev->txbuf_size/GRCAN_MSG_SIZE)/2) ){
+				left = (pDev->txbuf_size/GRCAN_MSG_SIZE)/2;
+			}
+		}
 
-    /* Wait until more room in transmit buffer */
-    if ( grcan_wait_txspace(pDev,left) ){
-      /* The wait has been aborted, probably due to
-       * the device driver has been closed by another
-       * thread. To avoid deadlock we return directly
-       * with error status.
-       */
-      rw_args->bytes_moved = count * sizeof(CANMsg);
-      return RTEMS_UNSATISFIED;
-    }
+		/* Wait until more room in transmit buffer */
+		if ( grcan_wait_txspace(pDev,left) ){
+			/* The wait has been aborted, probably due to 
+			 * the device driver has been closed by another
+			 * thread. To avoid deadlock we return directly
+			 * with error status.
+			 */
+			rw_args->bytes_moved = count * sizeof(CANMsg);
+			return RTEMS_UNSATISFIED;
+		}
 
-    if ( pDev->txerror ){
-      /* Return number of bytes sent, compare write pointers */
-      pDev->txerror = 0;
-#if 0
+		if ( pDev->txerror ){
+			/* Return number of bytes sent, compare write pointers */
+			pDev->txerror = 0;
+#if 0 
 #error HANDLE AMBA error
 #endif
-    }
+		}
 
-    /* Try read bytes from circular buffer */
-    count += grcan_hw_write_try(
-      pDev,
-      pDev->regs,
-      source+count,
-      req_cnt-count);
-  }
-  /* no need to unmask IRQ as IRQ Handler do that for us. */
+		/* Try read bytes from circular buffer */
+		count += grcan_hw_write_try(
+			pDev,
+			pDev->regs,
+			source+count,
+			req_cnt-count);
+	}
+	/* no need to unmask IRQ as IRQ Handler do that for us. */
 
-  rw_args->bytes_moved = count * sizeof(CANMsg);
-  return RTEMS_SUCCESSFUL;
+	rw_args->bytes_moved = count * sizeof(CANMsg);
+	return RTEMS_SUCCESSFUL;
 }
 
 static rtems_device_driver grcan_ioctl(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
 {
-  struct grcan_priv *pDev = &grcans[minor];
-  rtems_libio_ioctl_args_t *ioarg = (rtems_libio_ioctl_args_t *)arg;
-  unsigned int *data = ioarg->buffer;
-  struct grcan_timing timing;
-  unsigned int speed;
-  struct grcan_selection *selection;
-  int tmp,ret;
-  rtems_device_driver status;
-  struct grcan_stats *stats;
-  struct grcan_filter *filter;
-  IRQ_GLOBAL_PREPARE(oldLevel);
+	struct grcan_priv *pDev;
+	struct drvmgr_dev *dev;
+	rtems_libio_ioctl_args_t *ioarg = (rtems_libio_ioctl_args_t *)arg;
+	unsigned int *data = ioarg->buffer;
+	struct grcan_timing timing;
+	unsigned int speed;
+	struct grcan_selection *selection;
+	int tmp,ret;
+	rtems_device_driver status;
+	struct grcan_stats *stats;
+	struct grcan_filter *filter;
+	IRQ_GLOBAL_PREPARE(oldLevel);
+
+	FUNCDBG();
+
+	if ( drvmgr_get_dev(&grcan_drv_info.general, minor, &dev) ) {
+		return RTEMS_INVALID_NAME;
+	}
+	pDev = (struct grcan_priv *)dev->priv;
 
-  FUNCDBG();
+	if (!ioarg)
+		return RTEMS_INVALID_NAME;
 
-  if (!ioarg)
-    return RTEMS_INVALID_NAME;
+	ioarg->ioctl_return = 0;
+	switch(ioarg->command) {
+		case GRCAN_IOC_START:
+		if ( pDev->started )
+			return RTEMS_RESOURCE_IN_USE; /* EBUSY */
 
-  ioarg->ioctl_return = 0;
-  switch(ioarg->command) {
-    case GRCAN_IOC_START:
-      if ( pDev->started )
-        return RTEMS_RESOURCE_IN_USE; /* EBUSY */
+		if ( (status=grcan_start(pDev)) != RTEMS_SUCCESSFUL ){
+			return status;
+		}
+		/* Read and write are now open... */
+		pDev->started = 1;
 
-      if ( (status=grcan_start(pDev)) != RTEMS_SUCCESSFUL ){
-        return status;
-      }
-      /* Read and write are now open... */
-      pDev->started = 1;
-      break;
+		/* Register interrupt routine and enable IRQ at IRQ ctrl */
+		drvmgr_interrupt_register(dev, 0, "grcan", grcan_interrupt, pDev);
 
-    case GRCAN_IOC_STOP:
-      if ( !pDev->started )
-        return RTEMS_RESOURCE_IN_USE;
+		break;
 
-      grcan_stop(pDev);
-      pDev->started = 0;
-      break;
+		case GRCAN_IOC_STOP:
+		if ( !pDev->started )
+			return RTEMS_RESOURCE_IN_USE;
 
-    case GRCAN_IOC_ISSTARTED:
-      if ( !pDev->started )
-        return RTEMS_RESOURCE_IN_USE;
-      break;
+		/* Disable interrupts */
+		drvmgr_interrupt_unregister(dev, 0, grcan_interrupt, pDev);
 
-    case GRCAN_IOC_FLUSH:
-      if ( !pDev->started || pDev->flushing || pDev->config.silent )
-        return RTEMS_RESOURCE_IN_USE;
-
-      pDev->flushing = 1;
-      tmp = grcan_tx_flush(pDev);
-      pDev->flushing = 0;
-      if ( tmp ) {
-        /* The wait has been aborted, probably due to
-         * the device driver has been closed by another
-         * thread.
-         */
-         return RTEMS_UNSATISFIED;
-      }
-      break;
+		grcan_stop(pDev);
+		pDev->started = 0;
+		break;
+
+		case GRCAN_IOC_ISSTARTED:
+		if ( !pDev->started )
+			return RTEMS_RESOURCE_IN_USE;
+		break;
+
+		case GRCAN_IOC_FLUSH:
+		if ( !pDev->started || pDev->flushing || pDev->config.silent )
+			return RTEMS_RESOURCE_IN_USE;
+
+		pDev->flushing = 1;
+		tmp = grcan_tx_flush(pDev);
+		pDev->flushing = 0;
+		if ( tmp ) {
+			/* The wait has been aborted, probably due to 
+			 * the device driver has been closed by another
+			 * thread.
+			 */
+			return RTEMS_UNSATISFIED;
+		}
+		break;
 
 #if 0
-    /* Set physical link */
+		/* Set physical link */
 		case GRCAN_IOC_SET_LINK:
 #ifdef REDUNDANT_CHANNELS
-			if ( pDev->started )
-				return RTEMS_RESOURCE_IN_USE; /* EBUSY */
+		if ( pDev->started )
+			return RTEMS_RESOURCE_IN_USE; /* EBUSY */
 
-			/* switch HW channel */
-			pDev->channel = (unsigned int)ioargs->buffer;
+		/* switch HW channel */
+		pDev->channel = (unsigned int)ioargs->buffer;
 #else
-			return RTEMS_NOT_IMPLEMENTED;
+		return RTEMS_NOT_IMPLEMENTED;
 #endif
-			break;
+		break;
 #endif
 
-    case GRCAN_IOC_SET_SILENT:
-      if ( pDev->started )
-        return RTEMS_RESOURCE_IN_USE;
-      pDev->config.silent = (int)ioarg->buffer;
-      pDev->config_changed = 1;
-      break;
-
-    case GRCAN_IOC_SET_ABORT:
-      if ( pDev->started )
-        return RTEMS_RESOURCE_IN_USE;
-      pDev->config.abort = (int)ioarg->buffer;
-      /* This Configuration parameter doesn't need HurriCANe reset
-       * ==> no pDev->config_changed = 1;
-       */
-      break;
-
-    case GRCAN_IOC_SET_SELECTION:
-      if ( pDev->started )
-        return RTEMS_RESOURCE_IN_USE;
-
-      selection = (struct grcan_selection *)ioarg->buffer;
-      if ( !selection )
-        return RTEMS_INVALID_NAME;
-
-      pDev->config.selection = *selection;
-      pDev->config_changed = 1;
-      break;
-
-    case GRCAN_IOC_SET_RXBLOCK:
-      pDev->rxblock = (int)ioarg->buffer;
-      break;
-
-    case GRCAN_IOC_SET_TXBLOCK:
-      pDev->txblock = (int)ioarg->buffer;
-      break;
-
-    case GRCAN_IOC_SET_TXCOMPLETE:
-      pDev->txcomplete = (int)ioarg->buffer;
-      break;
-
-    case GRCAN_IOC_SET_RXCOMPLETE:
-      pDev->rxcomplete = (int)ioarg->buffer;
-      break;
-
-    case GRCAN_IOC_GET_STATS:
-      stats = (struct grcan_stats *)ioarg->buffer;
-      if ( !stats )
-        return RTEMS_INVALID_NAME;
-      *stats = pDev->stats;
-      break;
-
-    case GRCAN_IOC_CLR_STATS:
-      IRQ_GLOBAL_DISABLE(oldLevel);
-      memset(&pDev->stats,0,sizeof(struct grcan_stats));
-      IRQ_GLOBAL_ENABLE(oldLevel);
-      break;
+		case GRCAN_IOC_SET_SILENT:
+		if ( pDev->started )
+			return RTEMS_RESOURCE_IN_USE;
+		pDev->config.silent = (int)ioarg->buffer;
+		pDev->config_changed = 1;
+		break;
+
+		case GRCAN_IOC_SET_ABORT:
+		if ( pDev->started )
+			return RTEMS_RESOURCE_IN_USE;
+		pDev->config.abort = (int)ioarg->buffer;
+		/* This Configuration parameter doesn't need HurriCANe reset
+		 * ==> no pDev->config_changed = 1;
+		 */
+		break;
+
+		case GRCAN_IOC_SET_SELECTION:
+		if ( pDev->started )
+			return RTEMS_RESOURCE_IN_USE;
+
+		selection = (struct grcan_selection *)ioarg->buffer;
+		if ( !selection )
+			return RTEMS_INVALID_NAME;
+
+		pDev->config.selection = *selection;
+		pDev->config_changed = 1;
+		break;
+
+		case GRCAN_IOC_SET_RXBLOCK:
+		pDev->rxblock = (int)ioarg->buffer;
+		break;
+
+		case GRCAN_IOC_SET_TXBLOCK:
+		pDev->txblock = (int)ioarg->buffer;
+		break;
+
+		case GRCAN_IOC_SET_TXCOMPLETE:
+		pDev->txcomplete = (int)ioarg->buffer;
+		break;
+
+		case GRCAN_IOC_SET_RXCOMPLETE:
+		pDev->rxcomplete = (int)ioarg->buffer;
+		break;
+
+		case GRCAN_IOC_GET_STATS:
+		stats = (struct grcan_stats *)ioarg->buffer;
+		if ( !stats )
+			return RTEMS_INVALID_NAME;
+		*stats = pDev->stats;
+		break;
+
+		case GRCAN_IOC_CLR_STATS:
+		IRQ_GLOBAL_DISABLE(oldLevel);
+		memset(&pDev->stats,0,sizeof(struct grcan_stats));
+		IRQ_GLOBAL_ENABLE(oldLevel);
+		break;
 
 		case GRCAN_IOC_SET_SPEED:
-
-			/* cannot change speed during run mode */
-			if ( pDev->started )
-				return RTEMS_RESOURCE_IN_USE; /* EBUSY */
-
-			/* get speed rate from argument */
-			speed = (unsigned int)ioarg->buffer;
-			ret = grcan_calc_timing(speed,pDev->corefreq_hz,GRCAN_SAMPLING_POINT,&timing);
-			if ( ret )
-				return  RTEMS_INVALID_NAME; /* EINVAL */
-
-			/* save timing/speed */
-			pDev->config.timing = timing;
-      pDev->config_changed = 1;
-			break;
-
-    case GRCAN_IOC_SET_BTRS:
-			/* Set BTR registers manually
-			 * Read GRCAN/HurriCANe Manual.
-			 */
-			if ( pDev->started )
-				return RTEMS_RESOURCE_IN_USE; /* EBUSY */
-
-			if ( !ioarg->buffer )
-        return RTEMS_INVALID_NAME;
-
-			pDev->config.timing = *(struct grcan_timing *)ioarg->buffer;
-      pDev->config_changed = 1;
-			break;
-
-    case GRCAN_IOC_SET_AFILTER:
-      filter = (struct grcan_filter *)ioarg->buffer;
-      if ( !filter ){
-        /* Disable filtering - let all messages pass */
-        pDev->afilter.mask = 0x0;
-        pDev->afilter.code = 0x0;
-      }else{
-        /* Save filter */
-        pDev->afilter = *filter;
-      }
-      /* Set hardware acceptance filter */
-      grcan_hw_accept(pDev->regs,&pDev->afilter);
-      break;
-
-    case GRCAN_IOC_SET_SFILTER:
-      filter = (struct grcan_filter *)ioarg->buffer;
-      if ( !filter ){
-        /* disable TX/RX SYNC filtering */
-        pDev->sfilter.mask = 0xffffffff;
-        pDev->sfilter.mask = 0;
-
-        /* disable Sync interrupt */
-        pDev->regs->imr = READ_REG(&pDev->regs->imr) & ~(GRCAN_RXSYNC_IRQ|GRCAN_TXSYNC_IRQ);
-      }else{
-        /* Save filter */
-        pDev->sfilter = *filter;
-
-        /* Enable Sync interrupt */
-        pDev->regs->imr = READ_REG(&pDev->regs->imr) | (GRCAN_RXSYNC_IRQ|GRCAN_TXSYNC_IRQ);
-      }
-      /* Set Sync RX/TX filter */
-      grcan_hw_sync(pDev->regs,&pDev->sfilter);
-      break;
-
-    case GRCAN_IOC_GET_STATUS:
-      if ( !data )
-        return RTEMS_INVALID_NAME;
-      /* Read out the statsu register from the GRCAN core */
-      data[0] = READ_REG(&pDev->regs->stat);
-      break;
-
-    default:
-      return RTEMS_NOT_DEFINED;
-  }
-  return RTEMS_SUCCESSFUL;
-}
-
-#ifndef GRCAN_DONT_DECLARE_IRQ_HANDLER
-/* Find what device caused the IRQ */
-static rtems_isr grcan_interrupt_handler(rtems_vector_number v)
-{
-  int minor=0;
-  while ( minor < grcan_core_cnt ){
-    if ( (grcans[minor].irq+0x10) == v ){
-      grcan_interrupt(&grcans[minor]);
-      break;
-    }
-  }
+		/* cannot change speed during run mode */
+		if ( pDev->started )
+			return RTEMS_RESOURCE_IN_USE; /* EBUSY */
+
+		/* get speed rate from argument */
+		speed = (unsigned int)ioarg->buffer;
+		ret = grcan_calc_timing(speed, pDev->corefreq_hz, GRCAN_SAMPLING_POINT, &timing);
+		if ( ret )
+			return  RTEMS_INVALID_NAME; /* EINVAL */
+
+		/* save timing/speed */
+		pDev->config.timing = timing;
+		pDev->config_changed = 1;
+		break;
+
+		case GRCAN_IOC_SET_BTRS:
+		/* Set BTR registers manually 
+		 * Read GRCAN/HurriCANe Manual.
+		 */
+		if ( pDev->started )
+			return RTEMS_RESOURCE_IN_USE; /* EBUSY */
+		if ( !ioarg->buffer )
+			return RTEMS_INVALID_NAME;
+
+		pDev->config.timing = *(struct grcan_timing *)ioarg->buffer;
+		pDev->config_changed = 1;
+		break;
+
+		case GRCAN_IOC_SET_AFILTER:
+		filter = (struct grcan_filter *)ioarg->buffer;
+		if ( !filter ){
+			/* Disable filtering - let all messages pass */
+			pDev->afilter.mask = 0x0;
+			pDev->afilter.code = 0x0;
+		}else{
+			/* Save filter */
+			pDev->afilter = *filter;
+		}
+		/* Set hardware acceptance filter */      
+		grcan_hw_accept(pDev->regs,&pDev->afilter);
+		break;
+
+		case GRCAN_IOC_SET_SFILTER:
+		filter = (struct grcan_filter *)ioarg->buffer;
+		if ( !filter ){
+			/* disable TX/RX SYNC filtering */
+			pDev->sfilter.mask = 0xffffffff; 
+			pDev->sfilter.mask = 0;
+
+			 /* disable Sync interrupt */
+			pDev->regs->imr = READ_REG(&pDev->regs->imr) & ~(GRCAN_RXSYNC_IRQ|GRCAN_TXSYNC_IRQ);
+		}else{
+			/* Save filter */
+			pDev->sfilter = *filter;
+
+			/* Enable Sync interrupt */
+			pDev->regs->imr = READ_REG(&pDev->regs->imr) | (GRCAN_RXSYNC_IRQ|GRCAN_TXSYNC_IRQ);
+		}
+		/* Set Sync RX/TX filter */
+		grcan_hw_sync(pDev->regs,&pDev->sfilter);
+		break;
+
+		case GRCAN_IOC_GET_STATUS:
+		if ( !data )
+			return RTEMS_INVALID_NAME;
+		/* Read out the statsu register from the GRCAN core */
+		data[0] = READ_REG(&pDev->regs->stat);
+		break;
+
+	default:
+		return RTEMS_NOT_DEFINED;
+	}
+	return RTEMS_SUCCESSFUL;
 }
-#endif
 
 /* Handle the IRQ */
-static void grcan_interrupt(struct grcan_priv *pDev)
+static void grcan_interrupt(void *arg)
 {
-  unsigned int status = READ_REG(&pDev->regs->pimsr);
-  unsigned int canstat = READ_REG(&pDev->regs->stat);
+	struct grcan_priv *pDev = arg;
+	unsigned int status = READ_REG(&pDev->regs->pimsr);
+	unsigned int canstat = READ_REG(&pDev->regs->stat);
 
-  /* Spurious IRQ call? */
-  if ( !status && !canstat )
-    return;
+	/* Spurious IRQ call? */
+	if ( !status && !canstat )
+		return;
 
-  FUNCDBG();
+	FUNCDBG();
 
-  /* Increment number of interrupts counter */
-  pDev->stats.ints++;
+	/* Increment number of interrupts counter */
+	pDev->stats.ints++;
 
-  if ( (status & GRCAN_ERR_IRQ) || (canstat & GRCAN_STAT_PASS) ){
-    /* Error-Passive interrupt */
-    pDev->stats.passive_cnt++;
-  }
+	if ( (status & GRCAN_ERR_IRQ) || (canstat & GRCAN_STAT_PASS) ){
+		/* Error-Passive interrupt */
+		pDev->stats.passive_cnt++;
+	}
 
-  if ( (status & GRCAN_OFF_IRQ) || (canstat & GRCAN_STAT_OFF) ){
-    /* Bus-off condition interrupt
-     * The link is brought down by hardware, we wake all threads
-     * that is blocked in read/write calls and stop futher calls
-     * to read/write until user has called ioctl(fd,START,0).
-     */
-     pDev->started = 0;
-     grcan_stop(pDev); /* this mask all IRQ sources */
-     status=0x1ffff; /* clear all interrupts */
-     goto out;
-  }
+	if ( (status & GRCAN_OFF_IRQ) || (canstat & GRCAN_STAT_OFF) ){
+		/* Bus-off condition interrupt 
+		 * The link is brought down by hardware, we wake all threads
+		 * that is blocked in read/write calls and stop futher calls
+		 * to read/write until user has called ioctl(fd,START,0).
+		 */
+		pDev->started = 0;
+		grcan_stop(pDev); /* this mask all IRQ sources */
+		status=0x1ffff; /* clear all interrupts */
+		goto out;
+	}
 
-  if ( (status & GRCAN_OR_IRQ) || (canstat & GRCAN_STAT_OR) ){
-    /* Over-run during reception interrupt */
-    pDev->stats.overrun_cnt++;
-  }
+	if ( (status & GRCAN_OR_IRQ) || (canstat & GRCAN_STAT_OR) ){
+		/* Over-run during reception interrupt */
+		pDev->stats.overrun_cnt++;
+	}
 
-  if ( (status & GRCAN_RXAHBERR_IRQ) ||
-       (status & GRCAN_TXAHBERR_IRQ) ||
-       (canstat & GRCAN_STAT_AHBERR) ){
-    /* RX or Tx AHB Error interrupt */
-    printk("AHBERROR: status: 0x%x, canstat: 0x%x\n",status,canstat);
-    pDev->stats.ahberr_cnt++;
-  }
+	if ( (status & GRCAN_RXAHBERR_IRQ) ||
+	     (status & GRCAN_TXAHBERR_IRQ) ||
+	     (canstat & GRCAN_STAT_AHBERR) ){
+		/* RX or Tx AHB Error interrupt */
+		printk("AHBERROR: status: 0x%x, canstat: 0x%x\n",status,canstat);
+		pDev->stats.ahberr_cnt++;
+	}
 
-  if ( status & GRCAN_TXLOSS_IRQ ) {
-    pDev->stats.txloss_cnt++;
-  }
+	if ( status & GRCAN_TXLOSS_IRQ ) {
+		pDev->stats.txloss_cnt++;
+	}
 
-  if ( status & GRCAN_RXIRQ_IRQ ){
-    /* RX IRQ pointer interrupt */
-    /*printk("RxIrq 0x%x\n",status);*/
-    pDev->regs->imr = READ_REG(&pDev->regs->imr) & ~GRCAN_RXIRQ_IRQ;
-    rtems_semaphore_release(pDev->rx_sem);
-  }
+	if ( status & GRCAN_RXIRQ_IRQ ){
+		/* RX IRQ pointer interrupt */
+		/*printk("RxIrq 0x%x\n",status);*/
+		pDev->regs->imr = READ_REG(&pDev->regs->imr) & ~GRCAN_RXIRQ_IRQ;
+		rtems_semaphore_release(pDev->rx_sem);
+	}
 
-  if ( status & GRCAN_TXIRQ_IRQ ){
-    /* TX IRQ pointer interrupt */
-    pDev->regs->imr = READ_REG(&pDev->regs->imr) & ~GRCAN_TXIRQ_IRQ;
-    rtems_semaphore_release(pDev->tx_sem);
-  }
+	if ( status & GRCAN_TXIRQ_IRQ ){
+		/* TX IRQ pointer interrupt */
+		pDev->regs->imr = READ_REG(&pDev->regs->imr) & ~GRCAN_TXIRQ_IRQ;
+		rtems_semaphore_release(pDev->tx_sem);
+	}
 
-  if ( status & GRCAN_TXSYNC_IRQ ){
-    /* TxSync message transmitted interrupt */
-    pDev->stats.txsync_cnt++;
-  }
+	if ( status & GRCAN_TXSYNC_IRQ ){
+		/* TxSync message transmitted interrupt */
+		pDev->stats.txsync_cnt++;
+	}
 
-  if ( status & GRCAN_RXSYNC_IRQ ){
-    /* RxSync message received interrupt */
-    pDev->stats.rxsync_cnt++;
-  }
+	if ( status & GRCAN_RXSYNC_IRQ ){
+		/* RxSync message received interrupt */
+		pDev->stats.rxsync_cnt++;
+	}
 
-  if ( status & GRCAN_TXEMPTY_IRQ ){
-    pDev->regs->imr = READ_REG(&pDev->regs->imr) & ~GRCAN_TXEMPTY_IRQ;
-    rtems_semaphore_release(pDev->txempty_sem);
-  }
+	if ( status & GRCAN_TXEMPTY_IRQ ){
+		pDev->regs->imr = READ_REG(&pDev->regs->imr) & ~GRCAN_TXEMPTY_IRQ;
+		rtems_semaphore_release(pDev->txempty_sem);
+	}
 
 out:
-  /* Clear IRQs */
-  pDev->regs->picr = status;
-}
-
-static int grcan_register_internal(void)
-{
-  rtems_status_code r;
-  rtems_device_major_number m;
-
-  if ((r = rtems_io_register_driver(0, &grcan_driver, &m)) !=
-       RTEMS_SUCCESSFUL) {
-    switch(r) {
-      case RTEMS_TOO_MANY:
-        DBG2("failed RTEMS_TOO_MANY\n");
-        break;
-      case RTEMS_INVALID_NUMBER:
-        DBG2("failed RTEMS_INVALID_NUMBER\n");
-        break;
-      case RTEMS_RESOURCE_IN_USE:
-        DBG2("failed RTEMS_RESOURCE_IN_USE\n");
-        break;
-      default:
-        DBG("failed %i\n",r);
-        break;
-    }
-    return 1;
-  }
-  DBG("Registered GRCAN on major %d\n",m);
-  return 0;
-}
-
-
-int grcan_register_abs(struct grcan_device_info *devices, int dev_cnt);
-
-/* Use custom addresses and IRQs to find hardware */
-int GRCAN_PREFIX(_register_abs)(struct grcan_device_info *devices, int dev_cnt)
-{
-  FUNCDBG();
-
-  if ( !devices || (dev_cnt<0) )
-    return 1;
-  grcan_cores = devices;
-  grcan_core_cnt = dev_cnt;
-
-  amba_bus = NULL;
-	return grcan_register_internal();
-}
-
-/* Use prescanned AMBA Plug&Play information to find all GRCAN cores */
-int GRCAN_PREFIX(_register)(struct ambapp_bus *abus)
-{
-  FUNCDBG();
-
-  if ( !abus )
-    return 1;
-  amba_bus = abus;
-  grcan_cores = NULL;
-  grcan_core_cnt = 0;
-  return grcan_register_internal();
+	/* Clear IRQs */
+	pDev->regs->picr = status;
 }
diff --git a/c/src/lib/libbsp/sparc/shared/can/occan.c b/c/src/lib/libbsp/sparc/shared/can/occan.c
index 5920af3f52..8ce6a666bb 100644
--- a/c/src/lib/libbsp/sparc/shared/can/occan.c
+++ b/c/src/lib/libbsp/sparc/shared/can/occan.c
@@ -1,13 +1,11 @@
 /*  OC_CAN driver
  *
  *  COPYRIGHT (c) 2007.
- *  Gaisler Research.
+ *  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.
- *
- *  Author: Daniel Hellström, Gaisler Research AB, www.gaisler.com
  */
 
 #include <rtems/libio.h>
@@ -17,9 +15,8 @@
 #include <bsp.h>
 #include <rtems/bspIo.h> /* printk */
 
-#include <leon.h>
-#include <ambapp.h>
-#include <grlib.h>
+#include <drvmgr/drvmgr.h>
+#include <drvmgr/ambapp_bus.h>
 #include <occan.h>
 
 /* RTEMS -> ERRNO decoding table
@@ -53,27 +50,8 @@ rtems_assoc_t errno_assoc[] = {
  #undef OCCAN_BYTE_REGS
 #endif
 
-#ifndef OCCAN_PREFIX
- #define OCCAN_PREFIX(name) occan##name
-#endif
-
-#if !defined(OCCAN_DEVNAME) || !defined(OCCAN_DEVNAME_NO)
- #undef OCCAN_DEVNAME
- #undef OCCAN_DEVNAME_NO
- #define OCCAN_DEVNAME "/dev/occan0"
- #define OCCAN_DEVNAME_NO(devstr,no) ((devstr)[10]='0'+(no))
-#endif
-
-#ifndef OCCAN_REG_INT
-	#define OCCAN_REG_INT(handler,irq,arg) set_vector(handler,irq+0x10,1)
-  #undef  OCCAN_DEFINE_INTHANDLER
-  #define OCCAN_DEFINE_INTHANDLER
-#endif
-
-/* Default to 40MHz system clock */
-/*#ifndef SYS_FREQ_HZ
- #define SYS_FREQ_HZ 40000000
-#endif*/
+/* Enable Fixup code older OCCAN with a TX IRQ-FLAG bug */
+#define OCCAN_TX_IRQ_FLAG_FIXUP 1
 
 #define OCCAN_WORD_REG_OFS 0x80
 #define OCCAN_NCORE_OFS 0x100
@@ -103,7 +81,7 @@ typedef struct {
 } occan_fifo;
 
 /* PELICAN */
-#ifdef OCCAN_BYTE_REGS
+
 typedef struct {
 	unsigned char
 		mode,
@@ -149,8 +127,8 @@ typedef struct {
 		unsigned char rx_msg_cnt;
 		unsigned char unused1;
 		unsigned char clkdiv;
-} pelican_regs;
-#else
+} pelican8_regs;
+
 typedef struct {
 	unsigned char
 		mode, unused0[3],
@@ -196,9 +174,15 @@ typedef struct {
 		unsigned char rx_msg_cnt,unused16[3];
 		unsigned char unused17[4];
 		unsigned char clkdiv,unused18[3];
-} pelican_regs;
+} pelican32_regs;
+
+#ifdef OCCAN_BYTE_REGS
+#define pelican_regs pelican8_regs
+#else
+#define pelican_regs pelican32_regs
 #endif
 
+
 #define MAX_TSEG2 7
 #define MAX_TSEG1 15
 
@@ -217,12 +201,17 @@ typedef struct {
 } occan_speed_regs;
 
 typedef struct {
+	struct drvmgr_dev *dev;
+	char devName[32];
+
 	/* hardware shortcuts */
 	pelican_regs *regs;
+	int byte_regs;
 	int irq;
 	occan_speed_regs timing;
 	int channel; /* 0=default, 1=second bus */
 	int single_mode;
+	unsigned int sys_freq_hz;
 
 	/* driver state */
 	rtems_id devsem;
@@ -232,6 +221,7 @@ typedef struct {
 	int started;
 	int rxblk;
 	int txblk;
+	int sending;
 	unsigned int status;
 	occan_stats stats;
 
@@ -266,7 +256,7 @@ static int pelican_start(occan_priv *priv);
 static void pelican_stop(occan_priv *priv);
 static int pelican_send(occan_priv *can, CANMsg *msg);
 static void pelican_set_accept(occan_priv *priv, unsigned char *acode, unsigned char *amask);
-static void occan_interrupt(occan_priv *can);
+void occan_interrupt(void *arg);
 #ifdef DEBUG_PRINT_REGMAP
 static void pelican_regadr_print(pelican_regs *regs);
 #endif
@@ -278,33 +268,41 @@ static rtems_device_driver occan_read(rtems_device_major_number major, rtems_dev
 static rtems_device_driver occan_close(rtems_device_major_number major, rtems_device_minor_number minor, void *arg);
 static rtems_device_driver occan_open(rtems_device_major_number major, rtems_device_minor_number minor, void *arg);
 static rtems_device_driver occan_initialize(rtems_device_major_number major, rtems_device_minor_number unused, void *arg);
-#ifdef OCCAN_DEFINE_INTHANDLER
-static void occan_interrupt_handler(rtems_vector_number v);
-#endif
-static int can_cores;
-static occan_priv *cans;
-static struct ambapp_bus *amba_bus;
-static unsigned int sys_freq_hz;
+
+#define OCCAN_DRIVER_TABLE_ENTRY { occan_initialize, occan_open, occan_close, occan_read, occan_write, occan_ioctl }
+static rtems_driver_address_table occan_driver = OCCAN_DRIVER_TABLE_ENTRY;
 
 
 /* Read byte bypassing */
 
-#ifdef OCCAN_DONT_BYPASS_CACHE
- #define READ_REG(address) (*(volatile unsigned char *)(address))
-#else
- /* Bypass cache */
- #define READ_REG(address) _OCCAN_REG_READ((unsigned int)(address))
- static __inline__ unsigned char _OCCAN_REG_READ(unsigned int addr) {
-        unsigned char tmp;
-        __asm__ (" lduba [%1]1, %0 "
-            : "=r"(tmp)
-            : "r"(addr)
-           );
-        return tmp;
+
+/* Bypass cache */
+#define READ_REG(priv, address) occan_reg_read(priv, (unsigned int)address)
+#define WRITE_REG(priv, address, data) occan_reg_write(priv, (unsigned int)address, data)
+
+unsigned int occan_reg_read(occan_priv *priv, unsigned int address)
+{
+	unsigned int adr;
+	if ( priv->byte_regs ) {
+		adr = address;
+	} else {
+		/* Word accessed registers */
+		adr = (address & (~0x7f)) | ((address & 0x7f)<<2);
 	}
-#endif
+	return *(volatile unsigned char *)adr;
+}
 
-#define WRITE_REG(address,data) (*(volatile unsigned char *)(address) = (data))
+void occan_reg_write(occan_priv *priv, unsigned int address, unsigned char value)
+{
+	unsigned int adr;
+	if ( priv->byte_regs ) {
+		adr = address;
+	} else {
+		/* Word accessed registers */
+		adr = (address & (~0x7f)) | ((address & 0x7f)<<2);
+	}
+	*(volatile unsigned char *)adr = value;;
+}
 
 /* Mode register bit definitions */
 #define PELICAN_MOD_RESET          0x1
@@ -391,16 +389,376 @@ static unsigned int sys_freq_hz;
 #define PELICAN_S_       0x80
 */
 
+static int occan_driver_io_registered = 0;
+static rtems_device_major_number occan_driver_io_major = 0;
+
+/******************* Driver manager interface ***********************/
+
+/* Driver prototypes */
+int occan_register_io(rtems_device_major_number *m);
+int occan_device_init(occan_priv *pDev);
+
+int occan_init2(struct drvmgr_dev *dev);
+int occan_init3(struct drvmgr_dev *dev);
+
+struct drvmgr_drv_ops occan_ops = 
+{
+	.init = {NULL, occan_init2, occan_init3, NULL},
+	.remove = NULL,
+	.info = NULL
+};
+
+struct amba_dev_id occan_ids[] = 
+{
+	{VENDOR_GAISLER, GAISLER_CANAHB},
+	{0, 0}		/* Mark end of table */
+};
+
+struct amba_drv_info occan_drv_info =
+{
+	{
+		DRVMGR_OBJ_DRV,			/* Driver */
+		NULL,				/* Next driver */
+		NULL,				/* Device list */
+		DRIVER_AMBAPP_GAISLER_OCCAN_ID,	/* Driver ID */
+		"OCCAN_DRV",			/* Driver Name */
+		DRVMGR_BUS_TYPE_AMBAPP,		/* Bus Type */
+		&occan_ops,
+		NULL,				/* Funcs */
+		0,				/* No devices yet */
+		0,
+	},
+	&occan_ids[0]
+};
+
+void occan_register_drv (void)
+{
+	DBG("Registering OCCAN driver\n");
+	drvmgr_drv_register(&occan_drv_info.general);
+}
+
+int occan_init2(struct drvmgr_dev *dev)
+{
+	occan_priv *priv;
+
+	DBG("OCCAN[%d] on bus %s\n", dev->minor_drv, dev->parent->dev->name);
+	priv = dev->priv = malloc(sizeof(occan_priv));
+	if ( !priv )
+		return DRVMGR_NOMEM;
+	memset(priv, 0, sizeof(*priv));
+	priv->dev = dev;
+
+	return DRVMGR_OK;
+}
+
+int occan_init3(struct drvmgr_dev *dev)
+{
+	occan_priv *priv;
+	char prefix[32];
+	rtems_status_code status;
+
+	priv = dev->priv;
+
+	/* Do initialization */
+
+	if ( occan_driver_io_registered == 0) {
+		/* Register the I/O driver only once for all cores */
+		if ( occan_register_io(&occan_driver_io_major) ) {
+			/* Failed to register I/O driver */
+			dev->priv = NULL;
+			return DRVMGR_FAIL;
+		}
+
+		occan_driver_io_registered = 1;
+	}
+
+	/* I/O system registered and initialized 
+	 * Now we take care of device initialization.
+	 */
+
+	if ( occan_device_init(priv) ) {
+		return DRVMGR_FAIL;
+	}
+
+	/* Get Filesystem name prefix */
+	prefix[0] = '\0';
+	if ( drvmgr_get_dev_prefix(dev, prefix) ) {
+		/* Failed to get prefix, make sure of a unique FS name
+		 * by using the driver minor.
+		 */
+		sprintf(priv->devName, "/dev/occan%d", dev->minor_drv);
+	} else {
+		/* Got special prefix, this means we have a bus prefix
+		 * And we should use our "bus minor"
+		 */
+		sprintf(priv->devName, "/dev/%soccan%d", prefix, dev->minor_bus);
+	}
+
+	/* Register Device */
+	DBG("OCCAN[%d]: Registering %s\n", dev->minor_drv, priv->devName);
+	status = rtems_io_register_name(priv->devName, occan_driver_io_major, dev->minor_drv);
+	if (status != RTEMS_SUCCESSFUL) {
+		return DRVMGR_FAIL;
+	}
+
+	return DRVMGR_OK;
+}
+
+/******************* Driver Implementation ***********************/
+
+int occan_register_io(rtems_device_major_number *m)
+{
+	rtems_status_code r;
+
+	if ((r = rtems_io_register_driver(0, &occan_driver, m)) == RTEMS_SUCCESSFUL) {
+		DBG("OCCAN driver successfully registered, major: %d\n", *m);
+	} else {
+		switch(r) {
+		case RTEMS_TOO_MANY:
+			printk("OCCAN rtems_io_register_driver failed: RTEMS_TOO_MANY\n");
+			return -1;
+		case RTEMS_INVALID_NUMBER:  
+			printk("OCCAN rtems_io_register_driver failed: RTEMS_INVALID_NUMBER\n");
+			return -1;
+		case RTEMS_RESOURCE_IN_USE:
+			printk("OCCAN rtems_io_register_driver failed: RTEMS_RESOURCE_IN_USE\n");
+			return -1;
+		default:
+			printk("OCCAN rtems_io_register_driver failed\n");
+			return -1;
+		}
+	}
+	return 0;
+}
+
+int occan_device_init(occan_priv *pDev)
+{
+	struct amba_dev_info *ambadev;
+	struct ambapp_core *pnpinfo;
+	rtems_status_code status;
+	int minor;
+
+	/* Get device information from AMBA PnP information */
+	ambadev = (struct amba_dev_info *)pDev->dev->businfo;
+	if ( ambadev == NULL ) {
+		return -1;
+	}
+	pnpinfo = &ambadev->info;
+	pDev->irq = pnpinfo->irq;
+	pDev->regs = (pelican_regs *)(pnpinfo->ahb_slv->start[0] + OCCAN_NCORE_OFS*pnpinfo->index);
+	pDev->byte_regs = 1;
+	minor = pDev->dev->minor_drv;
+
+	/* Get frequency in Hz */
+	if ( drvmgr_freq_get(pDev->dev, DEV_AHB_SLV, &pDev->sys_freq_hz) ) {
+		return -1;
+	}
+
+	DBG("OCCAN frequency: %d Hz\n", pDev->sys_freq_hz);
+
+	/* initialize software */
+	pDev->open = 0;
+	pDev->started = 0; /* Needed for spurious interrupts */
+	pDev->rxfifo = NULL;
+	pDev->txfifo = NULL;
+	status = rtems_semaphore_create(
+			rtems_build_name('C', 'd', 'v', '0'+minor),
+			1,
+			RTEMS_FIFO | RTEMS_SIMPLE_BINARY_SEMAPHORE | RTEMS_NO_INHERIT_PRIORITY | \
+			RTEMS_NO_PRIORITY_CEILING,
+			0, 
+			&pDev->devsem);
+	if ( status != RTEMS_SUCCESSFUL ){
+		printk("OCCAN[%d]: Failed to create dev semaphore, (%d)\n\r",minor, status);
+		return RTEMS_UNSATISFIED;
+	}
+	status = rtems_semaphore_create(
+			rtems_build_name('C', 't', 'x', '0'+minor),
+			0, 
+			RTEMS_FIFO | RTEMS_SIMPLE_BINARY_SEMAPHORE | RTEMS_NO_INHERIT_PRIORITY | \
+			RTEMS_NO_PRIORITY_CEILING,
+                        0, 
+			&pDev->txsem);
+	if ( status != RTEMS_SUCCESSFUL ){
+		printk("OCCAN[%d]: Failed to create tx semaphore, (%d)\n\r",minor, status);
+		return RTEMS_UNSATISFIED;
+	}
+	status = rtems_semaphore_create(
+			rtems_build_name('C', 'r', 'x', '0'+minor),
+			0, 
+			RTEMS_FIFO | RTEMS_SIMPLE_BINARY_SEMAPHORE | RTEMS_NO_INHERIT_PRIORITY | \
+			RTEMS_NO_PRIORITY_CEILING,
+			0,
+			&pDev->rxsem);
+	if ( status != RTEMS_SUCCESSFUL ){
+		printk("OCCAN[%d]: Failed to create rx semaphore, (%d)\n\r",minor, status);
+		return RTEMS_UNSATISFIED;
+	}
+
+	/* hardware init/reset */
+	pelican_init(pDev);
+
+#ifdef DEBUG_PRINT_REGMAP
+	pelican_regadr_print(pDev->regs);
+#endif
+
+	return 0;
+}
+
+
+#ifdef DEBUG
+static void pelican_regs_print(occan_priv *pDev){
+	pelican_regs *regs = pDev->regs;
+	printk("--- PELICAN 0x%lx ---\n\r",(unsigned int)regs);
+	printk(" MODE: 0x%02x\n\r",READ_REG(pDev, &regs->mode));
+	printk(" CMD: 0x%02x\n\r",READ_REG(pDev, &regs->cmd));
+	printk(" STATUS: 0x%02x\n\r",READ_REG(pDev, &regs->status));
+	/*printk(" INTFLG: 0x%02x\n\r",READ_REG(pDev, &regs->intflags));*/
+	printk(" INTEN: 0x%02x\n\r",READ_REG(pDev, &regs->inten));
+	printk(" BTR0: 0x%02x\n\r",READ_REG(pDev, &regs->bustim0));
+	printk(" BTR1: 0x%02x\n\r",READ_REG(pDev, &regs->bustim1));
+	printk(" ARBCODE: 0x%02x\n\r",READ_REG(pDev, &regs->arbcode));
+	printk(" ERRCODE: 0x%02x\n\r",READ_REG(pDev, &regs->errcode));
+	printk(" ERRWARN: 0x%02x\n\r",READ_REG(pDev, &regs->errwarn));
+	printk(" RX_ERR_CNT: 0x%02x\n\r",READ_REG(pDev, &regs->rx_err_cnt));
+	printk(" TX_ERR_CNT: 0x%02x\n\r",READ_REG(pDev, &regs->tx_err_cnt));
+	if ( READ_REG(pDev, &regs->mode) & PELICAN_MOD_RESET ){
+		/* in reset mode it is possible to read acceptance filters */
+		printk(" ACR0: 0x%02x (0x%lx)\n\r",READ_REG(pDev, &regs->rx_fi_xff),&regs->rx_fi_xff);
+		printk(" ACR1: 0x%02x (0x%lx)\n\r",READ_REG(pDev, &regs->msg.rst_accept.code[0]),(unsigned int)&regs->msg.rst_accept.code[0]);
+		printk(" ACR1: 0x%02x (0x%lx)\n\r",READ_REG(pDev, &regs->msg.rst_accept.code[1]),(unsigned int)&regs->msg.rst_accept.code[1]);
+		printk(" ACR1: 0x%02x (0x%lx)\n\r",READ_REG(pDev, &regs->msg.rst_accept.code[2]),(unsigned int)&regs->msg.rst_accept.code[2]);
+		printk(" AMR0: 0x%02x (0x%lx)\n\r",READ_REG(pDev, &regs->msg.rst_accept.mask[0]),(unsigned int)&regs->msg.rst_accept.mask[0]);
+		printk(" AMR1: 0x%02x (0x%lx)\n\r",READ_REG(pDev, &regs->msg.rst_accept.mask[1]),(unsigned int)&regs->msg.rst_accept.mask[1]);
+		printk(" AMR2: 0x%02x (0x%lx)\n\r",READ_REG(pDev, &regs->msg.rst_accept.mask[2]),(unsigned int)&regs->msg.rst_accept.mask[2]);
+		printk(" AMR3: 0x%02x (0x%lx)\n\r",READ_REG(pDev, &regs->msg.rst_accept.mask[3]),(unsigned int)&regs->msg.rst_accept.mask[3]);
+		
+	}else{
+		printk(" RXFI_XFF: 0x%02x\n\r",READ_REG(pDev, &regs->rx_fi_xff));
+	}
+	printk(" RX_MSG_CNT: 0x%02x\n\r",READ_REG(pDev, &regs->rx_msg_cnt));
+	printk(" CLKDIV: 0x%02x\n\r",READ_REG(pDev, &regs->clkdiv));
+	printk("-------------------\n\r");
+}
+#endif
+
+#ifdef DEBUG_PRINT_REGMAP
+static void pelican_regadr_print(pelican_regs *regs){
+	printk("--- PELICAN 0x%lx ---\n\r",(unsigned int)regs);
+	printk(" MODE: 0x%lx\n\r",(unsigned int)&regs->mode);
+	printk(" CMD: 0x%lx\n\r",(unsigned int)&regs->cmd);
+	printk(" STATUS: 0x%lx\n\r",(unsigned int)&regs->status);
+	/*printk(" INTFLG: 0x%lx\n\r",&regs->intflags);*/
+	printk(" INTEN: 0x%lx\n\r",(unsigned int)&regs->inten);
+	printk(" BTR0: 0x%lx\n\r",(unsigned int)&regs->bustim0);
+	printk(" BTR1: 0x%lx\n\r",(unsigned int)&regs->bustim1);
+	printk(" ARBCODE: 0x%lx\n\r",(unsigned int)&regs->arbcode);
+	printk(" ERRCODE: 0x%lx\n\r",(unsigned int)&regs->errcode);
+	printk(" ERRWARN: 0x%lx\n\r",(unsigned int)&regs->errwarn);
+	printk(" RX_ERR_CNT: 0x%lx\n\r",(unsigned int)&regs->rx_err_cnt);
+	printk(" TX_ERR_CNT: 0x%lx\n\r",(unsigned int)&regs->tx_err_cnt);
+
+	/* in reset mode it is possible to read acceptance filters */
+	printk(" RXFI_XFF: 0x%lx\n\r",(unsigned int)&regs->rx_fi_xff);
+	
+	/* reset registers */
+	printk(" ACR0: 0x%lx\n\r",(unsigned int)&regs->rx_fi_xff);
+	printk(" ACR1: 0x%lx\n\r",(unsigned int)&regs->msg.rst_accept.code[0]);
+	printk(" ACR2: 0x%lx\n\r",(unsigned int)&regs->msg.rst_accept.code[1]);
+	printk(" ACR3: 0x%lx\n\r",(unsigned int)&regs->msg.rst_accept.code[2]);
+	printk(" AMR0: 0x%lx\n\r",(unsigned int)&regs->msg.rst_accept.mask[0]);
+	printk(" AMR1: 0x%lx\n\r",(unsigned int)&regs->msg.rst_accept.mask[1]);
+	printk(" AMR2: 0x%lx\n\r",(unsigned int)&regs->msg.rst_accept.mask[2]);
+	printk(" AMR3: 0x%lx\n\r",(unsigned int)&regs->msg.rst_accept.mask[3]);
+	
+	/* TX Extended */
+	printk(" EFFTX_ID[0]: 0x%lx\n\r",(unsigned int)&regs->msg.tx_eff.id[0]);
+	printk(" EFFTX_ID[1]: 0x%lx\n\r",(unsigned int)&regs->msg.tx_eff.id[1]);
+	printk(" EFFTX_ID[2]: 0x%lx\n\r",(unsigned int)&regs->msg.tx_eff.id[2]);
+	printk(" EFFTX_ID[3]: 0x%lx\n\r",(unsigned int)&regs->msg.tx_eff.id[3]);
+
+	printk(" EFFTX_DATA[0]: 0x%lx\n\r",(unsigned int)&regs->msg.tx_eff.data[0]);
+	printk(" EFFTX_DATA[1]: 0x%lx\n\r",(unsigned int)&regs->msg.tx_eff.data[1]);
+	printk(" EFFTX_DATA[2]: 0x%lx\n\r",(unsigned int)&regs->msg.tx_eff.data[2]);	
+	printk(" EFFTX_DATA[3]: 0x%lx\n\r",(unsigned int)&regs->msg.tx_eff.data[3]);	
+	printk(" EFFTX_DATA[4]: 0x%lx\n\r",(unsigned int)&regs->msg.tx_eff.data[4]);	
+	printk(" EFFTX_DATA[5]: 0x%lx\n\r",(unsigned int)&regs->msg.tx_eff.data[5]);	
+	printk(" EFFTX_DATA[6]: 0x%lx\n\r",(unsigned int)&regs->msg.tx_eff.data[6]);	
+	printk(" EFFTX_DATA[7]: 0x%lx\n\r",(unsigned int)&regs->msg.tx_eff.data[7]);	
+
+	/* RX Extended */
+	printk(" EFFRX_ID[0]: 0x%lx\n\r",(unsigned int)&regs->msg.rx_eff.id[0]);
+	printk(" EFFRX_ID[1]: 0x%lx\n\r",(unsigned int)&regs->msg.rx_eff.id[1]);
+	printk(" EFFRX_ID[2]: 0x%lx\n\r",(unsigned int)&regs->msg.rx_eff.id[2]);
+	printk(" EFFRX_ID[3]: 0x%lx\n\r",(unsigned int)&regs->msg.rx_eff.id[3]);
+
+	printk(" EFFRX_DATA[0]: 0x%lx\n\r",(unsigned int)&regs->msg.rx_eff.data[0]);
+	printk(" EFFRX_DATA[1]: 0x%lx\n\r",(unsigned int)&regs->msg.rx_eff.data[1]);
+	printk(" EFFRX_DATA[2]: 0x%lx\n\r",(unsigned int)&regs->msg.rx_eff.data[2]);	
+	printk(" EFFRX_DATA[3]: 0x%lx\n\r",(unsigned int)&regs->msg.rx_eff.data[3]);	
+	printk(" EFFRX_DATA[4]: 0x%lx\n\r",(unsigned int)&regs->msg.rx_eff.data[4]);	
+	printk(" EFFRX_DATA[5]: 0x%lx\n\r",(unsigned int)&regs->msg.rx_eff.data[5]);	
+	printk(" EFFRX_DATA[6]: 0x%lx\n\r",(unsigned int)&regs->msg.rx_eff.data[6]);	
+	printk(" EFFRX_DATA[7]: 0x%lx\n\r",(unsigned int)&regs->msg.rx_eff.data[7]);	
+
+
+	/* RX Extended */
+	printk(" SFFRX_ID[0]: 0x%lx\n\r",(unsigned int)&regs->msg.rx_sff.id[0]);
+	printk(" SFFRX_ID[1]: 0x%lx\n\r",(unsigned int)&regs->msg.rx_sff.id[1]);
+
+	printk(" SFFRX_DATA[0]: 0x%lx\n\r",(unsigned int)&regs->msg.rx_sff.data[0]);
+	printk(" SFFRX_DATA[1]: 0x%lx\n\r",(unsigned int)&regs->msg.rx_sff.data[1]);
+	printk(" SFFRX_DATA[2]: 0x%lx\n\r",(unsigned int)&regs->msg.rx_sff.data[2]);	
+	printk(" SFFRX_DATA[3]: 0x%lx\n\r",(unsigned int)&regs->msg.rx_sff.data[3]);	
+	printk(" SFFRX_DATA[4]: 0x%lx\n\r",(unsigned int)&regs->msg.rx_sff.data[4]);	
+	printk(" SFFRX_DATA[5]: 0x%lx\n\r",(unsigned int)&regs->msg.rx_sff.data[5]);	
+	printk(" SFFRX_DATA[6]: 0x%lx\n\r",(unsigned int)&regs->msg.rx_sff.data[6]);	
+	printk(" SFFRX_DATA[7]: 0x%lx\n\r",(unsigned int)&regs->msg.rx_sff.data[7]);	
+
+	/* TX Extended */
+	printk(" SFFTX_ID[0]: 0x%lx\n\r",(unsigned int)&regs->msg.tx_sff.id[0]);
+	printk(" SFFTX_ID[1]: 0x%lx\n\r",(unsigned int)&regs->msg.tx_sff.id[1]);
+
+	printk(" SFFTX_DATA[0]: 0x%lx\n\r",(unsigned int)&regs->msg.tx_sff.data[0]);
+	printk(" SFFTX_DATA[1]: 0x%lx\n\r",(unsigned int)&regs->msg.tx_sff.data[1]);
+	printk(" SFFTX_DATA[2]: 0x%lx\n\r",(unsigned int)&regs->msg.tx_sff.data[2]);
+	printk(" SFFTX_DATA[3]: 0x%lx\n\r",(unsigned int)&regs->msg.tx_sff.data[3]);
+	printk(" SFFTX_DATA[4]: 0x%lx\n\r",(unsigned int)&regs->msg.tx_sff.data[4]);
+	printk(" SFFTX_DATA[5]: 0x%lx\n\r",(unsigned int)&regs->msg.tx_sff.data[5]);
+	printk(" SFFTX_DATA[6]: 0x%lx\n\r",(unsigned int)&regs->msg.tx_sff.data[6]);
+	printk(" SFFTX_DATA[7]: 0x%lx\n\r",(unsigned int)&regs->msg.tx_sff.data[7]);
+
+	printk(" RX_MSG_CNT: 0x%lx\n\r",(unsigned int)&regs->rx_msg_cnt);
+	printk(" CLKDIV: 0x%lx\n\r",(unsigned int)&regs->clkdiv);
+	printk("-------------------\n\r");
+}
+#endif
+
+#ifdef DEBUG
+static void occan_stat_print(occan_stats *stats){
+	printk("----Stats----\n\r");
+	printk("rx_msgs: %d\n\r",stats->rx_msgs);
+	printk("tx_msgs: %d\n\r",stats->tx_msgs);
+	printk("err_warn: %d\n\r",stats->err_warn);
+	printk("err_dovr: %d\n\r",stats->err_dovr);
+	printk("err_errp: %d\n\r",stats->err_errp);
+	printk("err_arb: %d\n\r",stats->err_arb);
+	printk("err_bus: %d\n\r",stats->err_bus);
+	printk("Int cnt: %d\n\r",stats->ints);
+	printk("tx_buf_err: %d\n\r",stats->tx_buf_error);
+	printk("-------------\n\r");
+}
+#endif
+
 static void pelican_init(occan_priv *priv){
 	/* Reset core */
-	priv->regs->mode = PELICAN_MOD_RESET;
+	WRITE_REG(priv, &priv->regs->mode, PELICAN_MOD_RESET);
 
 	/* wait for core to reset complete */
 	/*usleep(1);*/
 }
 
 static void pelican_open(occan_priv *priv){
-	/* unsigned char tmp; */
 	int ret;
 
 	/* Set defaults */
@@ -418,44 +776,45 @@ static void pelican_open(occan_priv *priv){
 
 	/* Set clock divider to extended mode, clkdiv not connected
 	 */
-	priv->regs->clkdiv = (1<<PELICAN_CDR_MODE_BITS) | (DEFAULT_CLKDIV & PELICAN_CDR_DIV);
+	WRITE_REG(priv, &priv->regs->clkdiv, (1<<PELICAN_CDR_MODE_BITS) | (DEFAULT_CLKDIV & PELICAN_CDR_DIV));
 
-	ret = occan_calc_speedregs(sys_freq_hz,priv->speed,&priv->timing);
+	ret = occan_calc_speedregs(priv->sys_freq_hz,priv->speed,&priv->timing);
 	if ( ret ){
 		/* failed to set speed for this system freq, try with 50K instead */
 		priv->speed = OCCAN_SPEED_50K;
-		occan_calc_speedregs(sys_freq_hz,priv->speed,&priv->timing);
+		occan_calc_speedregs(priv->sys_freq_hz, priv->speed,
+			&priv->timing);
 	}
 
 	/* disable all interrupts */
-	priv->regs->inten = 0;
+	WRITE_REG(priv, &priv->regs->inten, 0);
 
 	/* clear pending interrupts by reading */
-	/* tmp = */ READ_REG(&priv->regs->intflags);
+	READ_REG(priv, &priv->regs->intflags);
 }
 
 static int pelican_start(occan_priv *priv){
-	/* unsigned char tmp; */
 	/* Start HW communication */
 
 	if ( !priv->rxfifo || !priv->txfifo )
 		return -1;
 
-  /* In case we were started before and stopped we
-   * should empty the TX fifo or try to resend those
-   * messages. We make it simple...
-   */
-  occan_fifo_clr(priv->txfifo);
+	/* In case we were started before and stopped we
+	 * should empty the TX fifo or try to resend those
+	 * messages. We make it simple...
+	 */
+	occan_fifo_clr(priv->txfifo);
 
 	/* Clear status bits */
 	priv->status = 0;
+	priv->sending = 0;
 
 	/* clear pending interrupts */
-	/* tmp = */ READ_REG(&priv->regs->intflags);
+	READ_REG(priv, &priv->regs->intflags);
 
 	/* clear error counters */
-	priv->regs->rx_err_cnt = 0;
-	priv->regs->tx_err_cnt = 0;
+	WRITE_REG(priv, &priv->regs->rx_err_cnt, 0);
+	WRITE_REG(priv, &priv->regs->tx_err_cnt, 0);
 
 #ifdef REDUNDANT_CHANNELS
 	if ( (priv->channel == 0) || (priv->channel >= REDUNDANT_CHANNELS) ){
@@ -469,15 +828,21 @@ static int pelican_start(occan_priv *priv){
 	/* set the speed regs of the CAN core */
 	occan_set_speedregs(priv,&priv->timing);
 
-	DBG("OCCAN: start: set timing regs btr0: 0x%x, btr1: 0x%x\n\r",READ_REG(&priv->regs->bustim0),READ_REG(&priv->regs->bustim1));
+	DBG("OCCAN: start: set timing regs btr0: 0x%x, btr1: 0x%x\n\r",
+		READ_REG(priv, &priv->regs->bustim0),
+		READ_REG(priv, &priv->regs->bustim1));
 
 	/* Set default acceptance filter */
 	pelican_set_accept(priv,priv->acode,priv->amask);
 
-	/* turn on interrupts */
-	priv->regs->inten = PELICAN_IE_RX | PELICAN_IE_TX | PELICAN_IE_ERRW |
-	                    PELICAN_IE_ERRP | PELICAN_IE_BUS;
+	/* Nothing can fail from here, this must be set before interrupts are
+	 * enabled */
+	priv->started = 1;
 
+	/* turn on interrupts */
+	WRITE_REG(priv, &priv->regs->inten,
+		PELICAN_IE_RX | PELICAN_IE_TX | PELICAN_IE_ERRW |
+		PELICAN_IE_ERRP | PELICAN_IE_BUS);
 #ifdef DEBUG
 	/* print setup before starting */
 	pelican_regs_print(priv->regs);
@@ -485,17 +850,23 @@ static int pelican_start(occan_priv *priv){
 #endif
 
 	/* core already in reset mode,
-	 *  ¤ Exit reset mode
+	 *  ¤ Exit reset mode 
 	 *  ¤ Enter Single/Dual mode filtering.
 	 */
-	priv->regs->mode =  (priv->single_mode << 3);
+	WRITE_REG(priv, &priv->regs->mode, (priv->single_mode << 3));
+
+	/* Register interrupt routine and unmask IRQ at IRQ controller */
+	drvmgr_interrupt_register(priv->dev, 0, "occan", occan_interrupt, priv);
 
 	return 0;
 }
 
-static void pelican_stop(occan_priv *priv){
+static void pelican_stop(occan_priv *priv)
+{
 	/* stop HW */
 
+	drvmgr_interrupt_unregister(priv->dev, 0, occan_interrupt, priv);
+
 #ifdef DEBUG
 	/* print setup before stopping */
 	pelican_regs_print(priv->regs);
@@ -503,14 +874,28 @@ static void pelican_stop(occan_priv *priv){
 #endif
 
 	/* put core in reset mode */
-	priv->regs->mode = PELICAN_MOD_RESET;
+	WRITE_REG(priv, &priv->regs->mode, PELICAN_MOD_RESET);
 
 	/* turn off interrupts */
-	priv->regs->inten = 0;
+	WRITE_REG(priv, &priv->regs->inten, 0);
 
 	priv->status |= OCCAN_STATUS_RESET;
 }
 
+static inline int pelican_tx_ready(occan_priv *can)
+{
+	unsigned char status;
+	pelican_regs *regs = can->regs;
+
+	/* is there room in send buffer? */
+	status = READ_REG(can, &regs->status);
+	if ( !(status & PELICAN_STAT_TXBUF) ) {
+		/* tx fifo taken, we have to wait */
+		return 0;
+	}
+
+	return 1;
+}
 
 /* Try to send message "msg", if hardware txfifo is
  * full, then -1 is returned.
@@ -519,12 +904,11 @@ static void pelican_stop(occan_priv *priv){
  * entering this function.
  */
 static int pelican_send(occan_priv *can, CANMsg *msg){
-	unsigned char tmp,status;
+	unsigned char tmp;
 	pelican_regs *regs = can->regs;
 
 	/* is there room in send buffer? */
-	status = READ_REG(&regs->status);
-	if ( !(status & PELICAN_STAT_TXBUF) ){
+	if ( !pelican_tx_ready(can) ) {
 		/* tx fifo taken, we have to wait */
 		return -1;
 	}
@@ -535,39 +919,40 @@ static int pelican_send(occan_priv *can, CANMsg *msg){
 
 	if ( msg->extended ){
 		/* Extended Frame */
-		regs->rx_fi_xff = 0x80 | tmp;
-		WRITE_REG(&regs->msg.tx_eff.id[0],(msg->id >> (5+8+8)) & 0xff);
-		WRITE_REG(&regs->msg.tx_eff.id[1],(msg->id >> (5+8)) & 0xff);
-		WRITE_REG(&regs->msg.tx_eff.id[2],(msg->id >> (5)) & 0xff);
-		WRITE_REG(&regs->msg.tx_eff.id[3],(msg->id << 3) & 0xf8);
+		WRITE_REG(can, &regs->rx_fi_xff, 0x80 | tmp);
+		WRITE_REG(can, &regs->msg.tx_eff.id[0],(msg->id >> (5+8+8)) & 0xff);
+		WRITE_REG(can, &regs->msg.tx_eff.id[1],(msg->id >> (5+8)) & 0xff);
+		WRITE_REG(can, &regs->msg.tx_eff.id[2],(msg->id >> (5)) & 0xff);
+		WRITE_REG(can, &regs->msg.tx_eff.id[3],(msg->id << 3) & 0xf8);
 		tmp = msg->len;
 		while(tmp--){
-			WRITE_REG(&regs->msg.tx_eff.data[tmp],msg->data[tmp]);
+			WRITE_REG(can, &regs->msg.tx_eff.data[tmp], msg->data[tmp]);
 		}
 	}else{
 		/* Standard Frame */
-		regs->rx_fi_xff = tmp;
-		WRITE_REG(&regs->msg.tx_sff.id[0],(msg->id >> 3) & 0xff);
-		WRITE_REG(&regs->msg.tx_sff.id[1],(msg->id << 5) & 0xe0);
+		WRITE_REG(can, &regs->rx_fi_xff, tmp);
+		WRITE_REG(can, &regs->msg.tx_sff.id[0],(msg->id >> 3) & 0xff);
+		WRITE_REG(can, &regs->msg.tx_sff.id[1],(msg->id << 5) & 0xe0);
 		tmp = msg->len;
 		while(tmp--){
-			WRITE_REG(&regs->msg.tx_sff.data[tmp],msg->data[tmp]);
+			WRITE_REG(can, &regs->msg.tx_sff.data[tmp],msg->data[tmp]);
 		}
 	}
 
 	/* let HW know of new message */
 	if ( msg->sshot ){
-		regs->cmd = PELICAN_CMD_TXREQ | PELICAN_CMD_ABORT;
+		WRITE_REG(can, &regs->cmd, PELICAN_CMD_TXREQ | PELICAN_CMD_ABORT);
 	}else{
 		/* normal case -- try resend until sent */
-		regs->cmd = PELICAN_CMD_TXREQ;
+		WRITE_REG(can, &regs->cmd, PELICAN_CMD_TXREQ);
 	}
 
 	return 0;
 }
 
 
-static void pelican_set_accept(occan_priv *priv, unsigned char *acode, unsigned char *amask){
+static void pelican_set_accept(occan_priv *priv, unsigned char *acode, unsigned char *amask)
+{
 	unsigned char *acode0, *acode1, *acode2, *acode3;
 	unsigned char *amask0, *amask1, *amask2, *amask3;
 
@@ -582,159 +967,17 @@ static void pelican_set_accept(occan_priv *priv, unsigned char *acode, unsigned
 	amask3 = (unsigned char *)&priv->regs->msg.rst_accept.mask[3];
 
 	/* Set new mask & code */
-	*acode0 = acode[0];
-	*acode1 = acode[1];
-	*acode2 = acode[2];
-	*acode3 = acode[3];
-
-	*amask0 = amask[0];
-	*amask1 = amask[1];
-	*amask2 = amask[2];
-	*amask3 = amask[3];
-}
-
-#ifdef DEBUG
-static void pelican_regs_print(pelican_regs *regs){
-	printk("--- PELICAN 0x%lx ---\n\r",(unsigned int)regs);
-	printk(" MODE: 0x%02x\n\r",READ_REG(&regs->mode));
-	printk(" CMD: 0x%02x\n\r",READ_REG(&regs->cmd));
-	printk(" STATUS: 0x%02x\n\r",READ_REG(&regs->status));
-	/*printk(" INTFLG: 0x%02x\n\r",READ_REG(&regs->intflags));*/
-	printk(" INTEN: 0x%02x\n\r",READ_REG(&regs->inten));
-	printk(" BTR0: 0x%02x\n\r",READ_REG(&regs->bustim0));
-	printk(" BTR1: 0x%02x\n\r",READ_REG(&regs->bustim1));
-	printk(" ARBCODE: 0x%02x\n\r",READ_REG(&regs->arbcode));
-	printk(" ERRCODE: 0x%02x\n\r",READ_REG(&regs->errcode));
-	printk(" ERRWARN: 0x%02x\n\r",READ_REG(&regs->errwarn));
-	printk(" RX_ERR_CNT: 0x%02x\n\r",READ_REG(&regs->rx_err_cnt));
-	printk(" TX_ERR_CNT: 0x%02x\n\r",READ_REG(&regs->tx_err_cnt));
-	if ( READ_REG(&regs->mode) & PELICAN_MOD_RESET ){
-		/* in reset mode it is possible to read acceptance filters */
-		printk(" ACR0: 0x%02x (0x%lx)\n\r",READ_REG(&regs->rx_fi_xff),&regs->rx_fi_xff);
-		printk(" ACR1: 0x%02x (0x%lx)\n\r",READ_REG(&regs->msg.rst_accept.code[0]),(unsigned int)&regs->msg.rst_accept.code[0]);
-		printk(" ACR1: 0x%02x (0x%lx)\n\r",READ_REG(&regs->msg.rst_accept.code[1]),(unsigned int)&regs->msg.rst_accept.code[1]);
-		printk(" ACR1: 0x%02x (0x%lx)\n\r",READ_REG(&regs->msg.rst_accept.code[2]),(unsigned int)&regs->msg.rst_accept.code[2]);
-		printk(" AMR0: 0x%02x (0x%lx)\n\r",READ_REG(&regs->msg.rst_accept.mask[0]),(unsigned int)&regs->msg.rst_accept.mask[0]);
-		printk(" AMR1: 0x%02x (0x%lx)\n\r",READ_REG(&regs->msg.rst_accept.mask[1]),(unsigned int)&regs->msg.rst_accept.mask[1]);
-		printk(" AMR2: 0x%02x (0x%lx)\n\r",READ_REG(&regs->msg.rst_accept.mask[2]),(unsigned int)&regs->msg.rst_accept.mask[2]);
-		printk(" AMR3: 0x%02x (0x%lx)\n\r",READ_REG(&regs->msg.rst_accept.mask[3]),(unsigned int)&regs->msg.rst_accept.mask[3]);
-
-	}else{
-		printk(" RXFI_XFF: 0x%02x\n\r",READ_REG(&regs->rx_fi_xff));
-	}
-	printk(" RX_MSG_CNT: 0x%02x\n\r",READ_REG(&regs->rx_msg_cnt));
-	printk(" CLKDIV: 0x%02x\n\r",READ_REG(&regs->clkdiv));
-	printk("-------------------\n\r");
-}
-#endif
-
-#ifdef DEBUG_PRINT_REGMAP
-static void pelican_regadr_print(pelican_regs *regs){
-	printk("--- PELICAN 0x%lx ---\n\r",(unsigned int)regs);
-	printk(" MODE: 0x%lx\n\r",(unsigned int)&regs->mode);
-	printk(" CMD: 0x%lx\n\r",(unsigned int)&regs->cmd);
-	printk(" STATUS: 0x%lx\n\r",(unsigned int)&regs->status);
-	/*printk(" INTFLG: 0x%lx\n\r",&regs->intflags);*/
-	printk(" INTEN: 0x%lx\n\r",(unsigned int)&regs->inten);
-	printk(" BTR0: 0x%lx\n\r",(unsigned int)&regs->bustim0);
-	printk(" BTR1: 0x%lx\n\r",(unsigned int)&regs->bustim1);
-	printk(" ARBCODE: 0x%lx\n\r",(unsigned int)&regs->arbcode);
-	printk(" ERRCODE: 0x%lx\n\r",(unsigned int)&regs->errcode);
-	printk(" ERRWARN: 0x%lx\n\r",(unsigned int)&regs->errwarn);
-	printk(" RX_ERR_CNT: 0x%lx\n\r",(unsigned int)&regs->rx_err_cnt);
-	printk(" TX_ERR_CNT: 0x%lx\n\r",(unsigned int)&regs->tx_err_cnt);
-
-	/* in reset mode it is possible to read acceptance filters */
-	printk(" RXFI_XFF: 0x%lx\n\r",(unsigned int)&regs->rx_fi_xff);
-
-	/* reset registers */
-	printk(" ACR0: 0x%lx\n\r",(unsigned int)&regs->rx_fi_xff);
-	printk(" ACR1: 0x%lx\n\r",(unsigned int)&regs->msg.rst_accept.code[0]);
-	printk(" ACR2: 0x%lx\n\r",(unsigned int)&regs->msg.rst_accept.code[1]);
-	printk(" ACR3: 0x%lx\n\r",(unsigned int)&regs->msg.rst_accept.code[2]);
-	printk(" AMR0: 0x%lx\n\r",(unsigned int)&regs->msg.rst_accept.mask[0]);
-	printk(" AMR1: 0x%lx\n\r",(unsigned int)&regs->msg.rst_accept.mask[1]);
-	printk(" AMR2: 0x%lx\n\r",(unsigned int)&regs->msg.rst_accept.mask[2]);
-	printk(" AMR3: 0x%lx\n\r",(unsigned int)&regs->msg.rst_accept.mask[3]);
-
-	/* TX Extended */
-	printk(" EFFTX_ID[0]: 0x%lx\n\r",(unsigned int)&regs->msg.tx_eff.id[0]);
-	printk(" EFFTX_ID[1]: 0x%lx\n\r",(unsigned int)&regs->msg.tx_eff.id[1]);
-	printk(" EFFTX_ID[2]: 0x%lx\n\r",(unsigned int)&regs->msg.tx_eff.id[2]);
-	printk(" EFFTX_ID[3]: 0x%lx\n\r",(unsigned int)&regs->msg.tx_eff.id[3]);
-
-	printk(" EFFTX_DATA[0]: 0x%lx\n\r",(unsigned int)&regs->msg.tx_eff.data[0]);
-	printk(" EFFTX_DATA[1]: 0x%lx\n\r",(unsigned int)&regs->msg.tx_eff.data[1]);
-	printk(" EFFTX_DATA[2]: 0x%lx\n\r",(unsigned int)&regs->msg.tx_eff.data[2]);
-	printk(" EFFTX_DATA[3]: 0x%lx\n\r",(unsigned int)&regs->msg.tx_eff.data[3]);
-	printk(" EFFTX_DATA[4]: 0x%lx\n\r",(unsigned int)&regs->msg.tx_eff.data[4]);
-	printk(" EFFTX_DATA[5]: 0x%lx\n\r",(unsigned int)&regs->msg.tx_eff.data[5]);
-	printk(" EFFTX_DATA[6]: 0x%lx\n\r",(unsigned int)&regs->msg.tx_eff.data[6]);
-	printk(" EFFTX_DATA[7]: 0x%lx\n\r",(unsigned int)&regs->msg.tx_eff.data[7]);
-
-	/* RX Extended */
-	printk(" EFFRX_ID[0]: 0x%lx\n\r",(unsigned int)&regs->msg.rx_eff.id[0]);
-	printk(" EFFRX_ID[1]: 0x%lx\n\r",(unsigned int)&regs->msg.rx_eff.id[1]);
-	printk(" EFFRX_ID[2]: 0x%lx\n\r",(unsigned int)&regs->msg.rx_eff.id[2]);
-	printk(" EFFRX_ID[3]: 0x%lx\n\r",(unsigned int)&regs->msg.rx_eff.id[3]);
-
-	printk(" EFFRX_DATA[0]: 0x%lx\n\r",(unsigned int)&regs->msg.rx_eff.data[0]);
-	printk(" EFFRX_DATA[1]: 0x%lx\n\r",(unsigned int)&regs->msg.rx_eff.data[1]);
-	printk(" EFFRX_DATA[2]: 0x%lx\n\r",(unsigned int)&regs->msg.rx_eff.data[2]);
-	printk(" EFFRX_DATA[3]: 0x%lx\n\r",(unsigned int)&regs->msg.rx_eff.data[3]);
-	printk(" EFFRX_DATA[4]: 0x%lx\n\r",(unsigned int)&regs->msg.rx_eff.data[4]);
-	printk(" EFFRX_DATA[5]: 0x%lx\n\r",(unsigned int)&regs->msg.rx_eff.data[5]);
-	printk(" EFFRX_DATA[6]: 0x%lx\n\r",(unsigned int)&regs->msg.rx_eff.data[6]);
-	printk(" EFFRX_DATA[7]: 0x%lx\n\r",(unsigned int)&regs->msg.rx_eff.data[7]);
-
-
-	/* RX Extended */
-	printk(" SFFRX_ID[0]: 0x%lx\n\r",(unsigned int)&regs->msg.rx_sff.id[0]);
-	printk(" SFFRX_ID[1]: 0x%lx\n\r",(unsigned int)&regs->msg.rx_sff.id[1]);
-
-	printk(" SFFRX_DATA[0]: 0x%lx\n\r",(unsigned int)&regs->msg.rx_sff.data[0]);
-	printk(" SFFRX_DATA[1]: 0x%lx\n\r",(unsigned int)&regs->msg.rx_sff.data[1]);
-	printk(" SFFRX_DATA[2]: 0x%lx\n\r",(unsigned int)&regs->msg.rx_sff.data[2]);
-	printk(" SFFRX_DATA[3]: 0x%lx\n\r",(unsigned int)&regs->msg.rx_sff.data[3]);
-	printk(" SFFRX_DATA[4]: 0x%lx\n\r",(unsigned int)&regs->msg.rx_sff.data[4]);
-	printk(" SFFRX_DATA[5]: 0x%lx\n\r",(unsigned int)&regs->msg.rx_sff.data[5]);
-	printk(" SFFRX_DATA[6]: 0x%lx\n\r",(unsigned int)&regs->msg.rx_sff.data[6]);
-	printk(" SFFRX_DATA[7]: 0x%lx\n\r",(unsigned int)&regs->msg.rx_sff.data[7]);
-
-	/* TX Extended */
-	printk(" SFFTX_ID[0]: 0x%lx\n\r",(unsigned int)&regs->msg.tx_sff.id[0]);
-	printk(" SFFTX_ID[1]: 0x%lx\n\r",(unsigned int)&regs->msg.tx_sff.id[1]);
-
-	printk(" SFFTX_DATA[0]: 0x%lx\n\r",(unsigned int)&regs->msg.tx_sff.data[0]);
-	printk(" SFFTX_DATA[1]: 0x%lx\n\r",(unsigned int)&regs->msg.tx_sff.data[1]);
-	printk(" SFFTX_DATA[2]: 0x%lx\n\r",(unsigned int)&regs->msg.tx_sff.data[2]);
-	printk(" SFFTX_DATA[3]: 0x%lx\n\r",(unsigned int)&regs->msg.tx_sff.data[3]);
-	printk(" SFFTX_DATA[4]: 0x%lx\n\r",(unsigned int)&regs->msg.tx_sff.data[4]);
-	printk(" SFFTX_DATA[5]: 0x%lx\n\r",(unsigned int)&regs->msg.tx_sff.data[5]);
-	printk(" SFFTX_DATA[6]: 0x%lx\n\r",(unsigned int)&regs->msg.tx_sff.data[6]);
-	printk(" SFFTX_DATA[7]: 0x%lx\n\r",(unsigned int)&regs->msg.tx_sff.data[7]);
-
-	printk(" RX_MSG_CNT: 0x%lx\n\r",(unsigned int)&regs->rx_msg_cnt);
-	printk(" CLKDIV: 0x%lx\n\r",(unsigned int)&regs->clkdiv);
-	printk("-------------------\n\r");
+	WRITE_REG(priv, acode0, acode[0]);
+	WRITE_REG(priv, acode1, acode[1]);
+	WRITE_REG(priv, acode2, acode[2]);
+	WRITE_REG(priv, acode3, acode[3]);
+
+	WRITE_REG(priv, amask0, amask[0]);
+	WRITE_REG(priv, amask1, amask[1]);
+	WRITE_REG(priv, amask2, amask[2]);
+	WRITE_REG(priv, amask3, amask[3]);
 }
-#endif
 
-#ifdef DEBUG
-static void occan_stat_print(occan_stats *stats){
-	printk("----Stats----\n\r");
-	printk("rx_msgs: %d\n\r",stats->rx_msgs);
-	printk("tx_msgs: %d\n\r",stats->tx_msgs);
-	printk("err_warn: %d\n\r",stats->err_warn);
-	printk("err_dovr: %d\n\r",stats->err_dovr);
-	printk("err_errp: %d\n\r",stats->err_errp);
-	printk("err_arb: %d\n\r",stats->err_arb);
-	printk("err_bus: %d\n\r",stats->err_bus);
-	printk("Int cnt: %d\n\r",stats->ints);
-	printk("tx_buf_err: %d\n\r",stats->tx_buf_error);
-	printk("-------------\n\r");
-}
-#endif
 
 /* This function calculates BTR0 and BTR1 values for a given bitrate.
  *
@@ -744,7 +987,8 @@ static void occan_stat_print(occan_stats *stats){
  * \param result Pointer to where resulting BTRs will be stored.
  * \return zero if successful to calculate a baud rate.
  */
-static int occan_calc_speedregs(unsigned int clock_hz, unsigned int rate, occan_speed_regs *result){
+static int occan_calc_speedregs(unsigned int clock_hz, unsigned int rate, occan_speed_regs *result)
+{
 	int best_error = 1000000000;
 	int error;
 	int best_tseg=0, best_brp=0, brp=0;
@@ -827,12 +1071,14 @@ static int occan_calc_speedregs(unsigned int clock_hz, unsigned int rate, occan_
 	return 0;
 }
 
-static int occan_set_speedregs(occan_priv *priv, occan_speed_regs *timing){
+static int occan_set_speedregs(occan_priv *priv, occan_speed_regs *timing)
+{
 	if ( !timing || !priv || !priv->regs)
 		return -1;
 
-	priv->regs->bustim0 = timing->btr0;
-	priv->regs->bustim1 = timing->btr1;
+	WRITE_REG(priv, &priv->regs->bustim0, timing->btr0);
+	WRITE_REG(priv, &priv->regs->bustim1, timing->btr1);
+
 	return 0;
 }
 
@@ -861,7 +1107,7 @@ static int pelican_speed_auto(occan_priv *priv){
 	while ( (speed=pelican_speed_auto_steplist[i]) > 0){
 
 		/* Reset core */
-		priv->regs->mode = PELICAN_MOD_RESET;
+		WRITE_REG(priv, &priv->regs->mode, PELICAN_MOD_RESET);
 
 		/* tell int handler about the auto speed detection test */
 
@@ -873,7 +1119,7 @@ static int pelican_speed_auto(occan_priv *priv){
 		pelican_set_accept(priv);
 
 		/* calc timing params for this */
-		if ( occan_calc_speedregs(sys_freq_hz,speed,&timing) ){
+		if ( occan_calc_speedregs(priv->sys_freq_hz,speed,&timing) ){
 			/* failed to get good timings for this frequency
 			 * test with next
 			 */
@@ -887,13 +1133,13 @@ static int pelican_speed_auto(occan_priv *priv){
 
 		/* Empty previous messages in hardware RX fifo */
 		/*
-		while( READ_REG(&priv->regs->) ){
+		while( READ_REG(priv, &priv->regs->) ){
 
 		}
 		*/
 
 		/* Clear pending interrupts */
-		tmp = READ_REG(&priv->regs->intflags);
+		tmp = READ_REG(priv, &priv->regs->intflags);
 
 		/* enable RX & ERR interrupt */
 		priv->regs->inten =
@@ -911,176 +1157,23 @@ static int pelican_speed_auto(occan_priv *priv){
 #endif
 }
 
-
-static rtems_device_driver occan_initialize(rtems_device_major_number major, rtems_device_minor_number unused, void *arg){
-	int dev_cnt,minor,subcore_cnt,devi,subi,subcores;
-	struct ambapp_ahb_info ambadev;
-	occan_priv *can;
-	char fs_name[20];
-	rtems_status_code status;
-
-	strcpy(fs_name,OCCAN_DEVNAME);
-
-	/* find device on amba bus */
-	dev_cnt = ambapp_get_number_ahbslv_devices(amba_bus, VENDOR_GAISLER,
-                                                   GAISLER_CANAHB);
-	if ( dev_cnt < 1 ){
-		/* Failed to find any CAN cores! */
-		printk("OCCAN: Failed to find any CAN cores\n\r");
-		return -1;
-	}
-
-	/* Detect System Frequency from initialized timer */
-#ifndef SYS_FREQ_HZ
-#if defined(LEON3)
-	/* LEON3: find timer address via AMBA Plug&Play info */
-	{
-		struct ambapp_apb_info gptimer;
-		struct gptimer_regs *tregs;
-
-		if ( ambapp_find_apbslv(&ambapp_plb, VENDOR_GAISLER,
-                                        GAISLER_GPTIMER, &gptimer) == 1 ){
-			tregs = (struct gptimer_regs *)gptimer.start;
-			sys_freq_hz = (tregs->scaler_reload+1)*1000*1000;
-			DBG("OCCAN: detected %dHZ system frequency\n\r",sys_freq_hz);
-		}else{
-			sys_freq_hz = 40000000; /* Default to 40MHz */
-			printk("OCCAN: Failed to detect system frequency\n\r");
-		}
-
-	}
-#elif defined(LEON2)
-	/* LEON2: use hardcoded address to get to timer */
-	{
-		LEON_Register_Map *regs = (LEON_Register_Map *)0x80000000;
-		sys_freq_hz = (regs->Scaler_Reload+1)*1000*1000;
-	}
-#else
-  #error CPU not supported for OC_CAN driver
-#endif
-#else
-	/* Use hardcoded frequency */
-	sys_freq_hz = SYS_FREQ_HZ;
-#endif
-
-	DBG("OCCAN: Detected %dHz system frequency\n\r",sys_freq_hz);
-
-	/* OCCAN speciality:
-	 *  Mulitple cores are supported through the same amba AHB interface.
-	 *  The number of "sub cores" can be detected by decoding the AMBA
-	 *  Plug&Play version information. verion = ncores. A maximum of 8
-	 *  sub cores are supported, each separeated with 0x100 inbetween.
-	 *
-	 *  Now, lets detect sub cores.
-	 */
-
-	for(subcore_cnt=devi=0; devi<dev_cnt; devi++){
-		ambapp_find_ahbslv_next(amba_bus, VENDOR_GAISLER,
-                                        GAISLER_CANAHB, &ambadev, devi);
-		subcore_cnt += (ambadev.ver & 0x7)+1;
-	}
-
-	printk("OCCAN: Found %d devs, totally %d sub cores\n\r",dev_cnt,subcore_cnt);
-
-	/* allocate memory for cores */
-	can_cores = subcore_cnt;
-	cans = calloc(subcore_cnt*sizeof(occan_priv),1);
-
-	minor=0;
-	for(devi=0; devi<dev_cnt; devi++){
-
-		/* Get AHB device info */
-		ambapp_find_ahbslv_next(amba_bus, VENDOR_GAISLER,
-                                        GAISLER_CANAHB, &ambadev, devi);
-		subcores = (ambadev.ver & 0x7)+1;
-		DBG("OCCAN: on dev %d found %d sub cores\n\r",devi,subcores);
-
-		/* loop all subcores, at least 1 */
-		for(subi=0; subi<subcores; subi++){
-			can = &cans[minor];
-
-#ifdef OCCAN_BYTE_REGS
-			/* regs is byte regs */
-			can->regs = (void *)(ambadev.start[0] + OCCAN_NCORE_OFS*subi);
-#else
-			/* regs is word regs, accessed 0x100 from base address */
-			can->regs = (void *)(ambadev.start[0] + OCCAN_NCORE_OFS*subi+ OCCAN_WORD_REG_OFS);
-#endif
-
-			/* remember IRQ number */
-			can->irq = ambadev.irq+subi;
-
-			/* bind filesystem name to device */
-			OCCAN_DEVNAME_NO(fs_name,minor);
-			printk("OCCAN: Registering %s to [%d %d] @ 0x%lx irq %d\n\r",fs_name,major,minor,(unsigned int)can->regs,can->irq);
-			status = rtems_io_register_name(fs_name, major, minor);
-			if (RTEMS_SUCCESSFUL != status )
-				rtems_fatal_error_occurred(status);
-
-			/* initialize software */
-			can->open = 0;
-			can->rxfifo = NULL;
-			can->txfifo = NULL;
-			status = rtems_semaphore_create(
-                        rtems_build_name('C', 'd', 'v', '0'+minor),
-                        1,
-                        RTEMS_FIFO | RTEMS_SIMPLE_BINARY_SEMAPHORE | RTEMS_NO_INHERIT_PRIORITY | \
-                        RTEMS_NO_PRIORITY_CEILING,
-                        0,
-                        &can->devsem);
-			if ( status != RTEMS_SUCCESSFUL ){
-				printk("OCCAN: Failed to create dev semaphore for minor %d, (%d)\n\r",minor,status);
-				return RTEMS_UNSATISFIED;
-			}
-			status = rtems_semaphore_create(
-                        rtems_build_name('C', 't', 'x', '0'+minor),
-                        0,
-                        RTEMS_FIFO | RTEMS_SIMPLE_BINARY_SEMAPHORE | RTEMS_NO_INHERIT_PRIORITY | \
-                        RTEMS_NO_PRIORITY_CEILING,
-                        0,
-                        &can->txsem);
-			if ( status != RTEMS_SUCCESSFUL ){
-				printk("OCCAN: Failed to create tx semaphore for minor %d, (%d)\n\r",minor,status);
-				return RTEMS_UNSATISFIED;
-			}
-			status = rtems_semaphore_create(
-                        rtems_build_name('C', 'r', 'x', '0'+minor),
-                        0,
-                        RTEMS_FIFO | RTEMS_SIMPLE_BINARY_SEMAPHORE | RTEMS_NO_INHERIT_PRIORITY | \
-                        RTEMS_NO_PRIORITY_CEILING,
-                        0,
-                        &can->rxsem);
-			if ( status != RTEMS_SUCCESSFUL ){
-				printk("OCCAN: Failed to create rx semaphore for minor %d, (%d)\n\r",minor,status);
-				return RTEMS_UNSATISFIED;
-			}
-
-			/* hardware init/reset */
-			pelican_init(can);
-
-			/* Setup interrupt handler for each channel */
-    	OCCAN_REG_INT(OCCAN_PREFIX(_interrupt_handler), can->irq, can);
-
-			minor++;
-#ifdef DEBUG_PRINT_REGMAP
-			pelican_regadr_print(can->regs);
-#endif
-		}
-	}
-
+static rtems_device_driver occan_initialize(rtems_device_major_number major, rtems_device_minor_number unused, void *arg)
+{
 	return RTEMS_SUCCESSFUL;
 }
 
 static rtems_device_driver occan_open(rtems_device_major_number major, rtems_device_minor_number minor, void *arg){
 	occan_priv *can;
+	struct drvmgr_dev *dev;
 
 	DBG("OCCAN: Opening %d\n\r",minor);
 
-	if ( minor >= can_cores )
+	/* get can device */	
+	if ( drvmgr_get_dev(&occan_drv_info.general, minor, &dev) ) {
+		DBG("Wrong minor %d\n", minor);
 		return RTEMS_UNSATISFIED; /* NODEV */
-
-	/* get can device */
-	can = &cans[minor];
+	}
+	can = (occan_priv *)dev->priv;
 
 	/* already opened? */
 	rtems_semaphore_obtain(can->devsem,RTEMS_WAIT, RTEMS_NO_TIMEOUT);
@@ -1126,17 +1219,24 @@ static rtems_device_driver occan_open(rtems_device_major_number major, rtems_dev
 	return RTEMS_SUCCESSFUL;
 }
 
-static rtems_device_driver occan_close(rtems_device_major_number major, rtems_device_minor_number minor, void *arg){
-	occan_priv *can = &cans[minor];
+static rtems_device_driver occan_close(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
+{
+	occan_priv *can;
+	struct drvmgr_dev *dev;
 
 	DBG("OCCAN: Closing %d\n\r",minor);
 
+	if ( drvmgr_get_dev(&occan_drv_info.general, minor, &dev) ) {
+		return RTEMS_INVALID_NAME;
+	}
+	can = (occan_priv *)dev->priv;
+
 	/* stop if running */
 	if ( can->started )
 		pelican_stop(can);
 
 	/* Enter Reset Mode */
-	can->regs->mode = PELICAN_MOD_RESET;
+	WRITE_REG(can, &can->regs->mode, PELICAN_MOD_RESET);
 
 	/* free fifo memory */
 	occan_fifo_free(can->rxfifo);
@@ -1145,16 +1245,24 @@ static rtems_device_driver occan_close(rtems_device_major_number major, rtems_de
 	can->rxfifo = NULL;
 	can->txfifo = NULL;
 
+	can->open = 0;
+
 	return RTEMS_SUCCESSFUL;
 }
 
 static rtems_device_driver occan_read(rtems_device_major_number major, rtems_device_minor_number minor, void *arg){
-	occan_priv *can = &cans[minor];
+	occan_priv *can;
+	struct drvmgr_dev *dev;
 	rtems_libio_rw_args_t *rw_args=(rtems_libio_rw_args_t *) arg;
 	CANMsg *dstmsg, *srcmsg;
 	rtems_interrupt_level oldLevel;
 	int left;
 
+	if ( drvmgr_get_dev(&occan_drv_info.general, minor, &dev) ) {
+		return RTEMS_INVALID_NAME;
+	}
+	can = (occan_priv *)dev->priv;
+
 	if ( !can->started ){
 		DBG("OCCAN: cannot read from minor %d when not started\n\r",minor);
 		return RTEMS_RESOURCE_IN_USE; /* -EBUSY*/
@@ -1205,8 +1313,9 @@ static rtems_device_driver occan_read(rtems_device_major_number major, rtems_dev
 
 			DBG("OCCAN: Waiting for RX int\n\r");
 
-			/* wait for incomming messages */
-			rtems_semaphore_obtain(can->rxsem,RTEMS_WAIT,RTEMS_NO_TIMEOUT);
+			/* wait for incoming messages */
+			rtems_semaphore_obtain(can->rxsem, RTEMS_WAIT,
+				RTEMS_NO_TIMEOUT);
 
 			/* did we get woken up by a BUS OFF error? */
 			if ( can->status & (OCCAN_STATUS_ERR_BUSOFF|OCCAN_STATUS_RESET) ){
@@ -1244,7 +1353,8 @@ static rtems_device_driver occan_read(rtems_device_major_number major, rtems_dev
 }
 
 static rtems_device_driver occan_write(rtems_device_major_number major, rtems_device_minor_number minor, void *arg){
-	occan_priv *can = &cans[minor];
+	occan_priv *can;
+	struct drvmgr_dev *dev;
 	rtems_libio_rw_args_t *rw_args=(rtems_libio_rw_args_t *) arg;
 	CANMsg *msg,*fifo_msg;
 	rtems_interrupt_level oldLevel;
@@ -1252,6 +1362,11 @@ static rtems_device_driver occan_write(rtems_device_major_number major, rtems_de
 
 	DBG("OCCAN: Writing %d bytes from 0x%lx (%d)\n\r",rw_args->count,rw_args->buffer,sizeof(CANMsg));
 
+	if ( drvmgr_get_dev(&occan_drv_info.general, minor, &dev) ) {
+		return RTEMS_INVALID_NAME;
+	}
+	can = (occan_priv *)dev->priv;
+
 	if ( !can->started )
 		return RTEMS_RESOURCE_IN_USE; /* EBUSY */
 
@@ -1293,6 +1408,11 @@ static rtems_device_driver occan_write(rtems_device_major_number major, rtems_de
 			left -= sizeof(CANMsg);
 			msg++;
 
+#ifdef OCCAN_TX_IRQ_FLAG_FIXUP
+			/* Mark that we have put at least one msg in TX FIFO */
+			can->sending = 1;
+#endif
+
 			/* bump stat counters */
 			can->stats.tx_msgs++;
 
@@ -1341,11 +1461,16 @@ static rtems_device_driver occan_write(rtems_device_major_number major, rtems_de
 			if ( occan_fifo_empty(can->txfifo) ){
 				if ( !pelican_send(can,msg) ) {
 					/* First message put directly into HW TX fifo
-				   * This will turn TX interrupt on.
-				   */
+					 * This will turn TX interrupt on.
+					 */
 					left -= sizeof(CANMsg);
 					msg++;
 
+#ifdef OCCAN_TX_IRQ_FLAG_FIXUP
+					/* Mark that we have put at least one msg in TX FIFO */
+					can->sending = 1;
+#endif
+
 					/* bump stat counters */
 					can->stats.tx_msgs++;
 
@@ -1381,15 +1506,21 @@ static rtems_device_driver occan_write(rtems_device_major_number major, rtems_de
 static rtems_device_driver occan_ioctl(rtems_device_major_number major, rtems_device_minor_number minor, void *arg){
 	int ret;
 	occan_speed_regs timing;
-	occan_priv *can = &cans[minor];
+	occan_priv *can;
+	struct drvmgr_dev *dev;
 	unsigned int speed;
-	rtems_libio_ioctl_args_t	*ioarg = (rtems_libio_ioctl_args_t *) arg;
+	rtems_libio_ioctl_args_t *ioarg = (rtems_libio_ioctl_args_t *) arg;
 	struct occan_afilter *afilter;
 	occan_stats *dststats;
 	unsigned int rxcnt,txcnt;
 
 	DBG("OCCAN: IOCTL %d\n\r",ioarg->command);
 
+	if ( drvmgr_get_dev(&occan_drv_info.general, minor, &dev) ) {
+		return RTEMS_INVALID_NAME;
+	}
+	can = (occan_priv *)dev->priv;
+
 	ioarg->ioctl_return = 0;
 	switch(ioarg->command){
 		case OCCAN_IOC_SET_SPEED:
@@ -1400,7 +1531,7 @@ static rtems_device_driver occan_ioctl(rtems_device_major_number major, rtems_de
 
 			/* get speed rate from argument */
 			speed = (unsigned int)ioarg->buffer;
-			ret = occan_calc_speedregs(sys_freq_hz,speed,&timing);
+			ret = occan_calc_speedregs(can->sys_freq_hz,speed,&timing);
 			if ( ret )
 				return  RTEMS_INVALID_NAME; /* EINVAL */
 
@@ -1479,8 +1610,8 @@ static rtems_device_driver occan_ioctl(rtems_device_major_number major, rtems_de
 				return  RTEMS_INVALID_NAME; /* EINVAL */
 
 			/* copy data stats into userspace buffer */
-      if ( can->rxfifo )
-        can->stats.rx_sw_dovr = can->rxfifo->ovcnt;
+			if ( can->rxfifo )
+				can->stats.rx_sw_dovr = can->rxfifo->ovcnt;
 			*dststats = can->stats;
 			break;
 
@@ -1543,7 +1674,9 @@ static rtems_device_driver occan_ioctl(rtems_device_major_number major, rtems_de
 				return RTEMS_RESOURCE_IN_USE; /* EBUSY */
 			if ( pelican_start(can) )
 				return RTEMS_NO_MEMORY; /* failed because of no memory, can happen if SET_BUFLEN failed */
-			can->started = 1;
+			/* can->started = 1; -- Is set in pelican_start due to interrupt may occur before we 
+			 * get here.
+			 */
 			break;
 
 		case OCCAN_IOC_STOP:
@@ -1559,7 +1692,9 @@ static rtems_device_driver occan_ioctl(rtems_device_major_number major, rtems_de
 	return RTEMS_SUCCESSFUL;
 }
 
-static void occan_interrupt(occan_priv *can){
+void occan_interrupt(void *arg)
+{
+	occan_priv *can = arg;
 	unsigned char iflags;
 	pelican_regs *regs = can->regs;
 	CANMsg *msg;
@@ -1567,11 +1702,33 @@ static void occan_interrupt(occan_priv *can){
 	unsigned char tmp, errcode, arbcode;
 	int tx_error_cnt,rx_error_cnt;
 
-	can->stats.ints++;
+	if ( !can->started )
+		return; /* Spurious Interrupt, do nothing */
+
+	while (1) {
+
+		iflags = READ_REG(can, &can->regs->intflags);
+
+#ifdef OCCAN_TX_IRQ_FLAG_FIXUP
+		/* TX IRQ may be cleared when reading regs->intflags due
+		 * to a bug in some chips. Instead of looking at the TX_IRQ_FLAG
+		 * the TX-fifo emoty register is looked at when something has
+		 * been scheduled for transmission.
+		 */
+		if ((iflags & PELICAN_IF_TX) == 0) {
+			if (can->sending && pelican_tx_ready(can)) {
+				can->sending = 0;
+				iflags |= PELICAN_IF_TX;
+			}
+		}
+#endif
 
-	while ( (iflags = READ_REG(&can->regs->intflags)) != 0 ){
+		if (iflags == 0)
+			break;
 		/* still interrupts to handle */
 
+		can->stats.ints++;
+
 		if ( iflags & PELICAN_IF_RX ){
 			/* the rx fifo is not empty
 			 * put 1 message into rxfifo for later use
@@ -1579,52 +1736,53 @@ static void occan_interrupt(occan_priv *can){
 
 			/* get empty (or make room) message */
 			msg = occan_fifo_put_claim(can->rxfifo,1);
-			tmp = READ_REG(&regs->rx_fi_xff);
+			tmp = READ_REG(can, &regs->rx_fi_xff);
 			msg->extended = tmp >> 7;
 			msg->rtr = (tmp >> 6) & 1;
 			msg->len = tmp = tmp & 0x0f;
 
 			if ( msg->extended ){
 				/* extended message */
-				msg->id =  READ_REG(&regs->msg.rx_eff.id[0])<<(5+8+8) |
-				           READ_REG(&regs->msg.rx_eff.id[1])<<(5+8) |
-				           READ_REG(&regs->msg.rx_eff.id[2])<<5 |
-				           READ_REG(&regs->msg.rx_eff.id[3])>>3;
+				msg->id =  READ_REG(can, &regs->msg.rx_eff.id[0])<<(5+8+8) |
+				           READ_REG(can, &regs->msg.rx_eff.id[1])<<(5+8) |
+				           READ_REG(can, &regs->msg.rx_eff.id[2])<<5 |
+				           READ_REG(can, &regs->msg.rx_eff.id[3])>>3;
+
 				while(tmp--){
-					msg->data[tmp] = READ_REG(&regs->msg.rx_eff.data[tmp]);
+					msg->data[tmp] = READ_REG(can, &regs->msg.rx_eff.data[tmp]);
 				}
 				/*
-				msg->data[0] = READ_REG(&regs->msg.rx_eff.data[0]);
-				msg->data[1] = READ_REG(&regs->msg.rx_eff.data[1]);
-				msg->data[2] = READ_REG(&regs->msg.rx_eff.data[2]);
-				msg->data[3] = READ_REG(&regs->msg.rx_eff.data[3]);
-				msg->data[4] = READ_REG(&regs->msg.rx_eff.data[4]);
-				msg->data[5] = READ_REG(&regs->msg.rx_eff.data[5]);
-				msg->data[6] = READ_REG(&regs->msg.rx_eff.data[6]);
-				msg->data[7] = READ_REG(&regs->msg.rx_eff.data[7]);
+				msg->data[0] = READ_REG(can, &regs->msg.rx_eff.data[0]);
+				msg->data[1] = READ_REG(can, &regs->msg.rx_eff.data[1]);
+				msg->data[2] = READ_REG(can, &regs->msg.rx_eff.data[2]);
+				msg->data[3] = READ_REG(can, &regs->msg.rx_eff.data[3]);
+				msg->data[4] = READ_REG(can, &regs->msg.rx_eff.data[4]);
+				msg->data[5] = READ_REG(can, &regs->msg.rx_eff.data[5]);
+				msg->data[6] = READ_REG(can, &regs->msg.rx_eff.data[6]);
+				msg->data[7] = READ_REG(can, &regs->msg.rx_eff.data[7]);
 				*/
 			}else{
 				/* standard message */
-				msg->id =  READ_REG(&regs->msg.rx_sff.id[0])<<3 |
-				           READ_REG(&regs->msg.rx_sff.id[1])>>5;
+				msg->id = READ_REG(can, &regs->msg.rx_sff.id[0])<<3 |
+				          READ_REG(can, &regs->msg.rx_sff.id[1])>>5;
 
 				while(tmp--){
-					msg->data[tmp] = READ_REG(&regs->msg.rx_sff.data[tmp]);
+					msg->data[tmp] = READ_REG(can, &regs->msg.rx_sff.data[tmp]);
 				}
 				/*
-				msg->data[0] = READ_REG(&regs->msg.rx_sff.data[0]);
-				msg->data[1] = READ_REG(&regs->msg.rx_sff.data[1]);
-				msg->data[2] = READ_REG(&regs->msg.rx_sff.data[2]);
-				msg->data[3] = READ_REG(&regs->msg.rx_sff.data[3]);
-				msg->data[4] = READ_REG(&regs->msg.rx_sff.data[4]);
-				msg->data[5] = READ_REG(&regs->msg.rx_sff.data[5]);
-				msg->data[6] = READ_REG(&regs->msg.rx_sff.data[6]);
-				msg->data[7] = READ_REG(&regs->msg.rx_sff.data[7]);
+				msg->data[0] = READ_REG(can, &regs->msg.rx_sff.data[0]);
+				msg->data[1] = READ_REG(can, &regs->msg.rx_sff.data[1]);
+				msg->data[2] = READ_REG(can, &regs->msg.rx_sff.data[2]);
+				msg->data[3] = READ_REG(can, &regs->msg.rx_sff.data[3]);
+				msg->data[4] = READ_REG(can, &regs->msg.rx_sff.data[4]);
+				msg->data[5] = READ_REG(can, &regs->msg.rx_sff.data[5]);
+				msg->data[6] = READ_REG(can, &regs->msg.rx_sff.data[6]);
+				msg->data[7] = READ_REG(can, &regs->msg.rx_sff.data[7]);
 				*/
 			}
 
 			/* Re-Enable RX buffer for a new message */
-			regs->cmd = PELICAN_CMD_RELRXBUF;
+			WRITE_REG(can, &regs->cmd, PELICAN_CMD_RELRXBUF);
 
 			/* make message available to the user */
 			occan_fifo_put(can->rxfifo);
@@ -1636,7 +1794,8 @@ static void occan_interrupt(occan_priv *can){
 			signal_rx = 1;
 		}
 
-		if ( iflags & PELICAN_IF_TX ){
+		if ( iflags & PELICAN_IF_TX ) {
+
 			/* there is room in tx fifo of HW */
 
 			if ( !occan_fifo_empty(can->txfifo) ){
@@ -1655,6 +1814,9 @@ static void occan_interrupt(occan_priv *can){
 					can->status |= OCCAN_STATUS_QUEUE_ERROR;
 					can->stats.tx_buf_error++;
 				}
+#ifdef OCCAN_TX_IRQ_FLAG_FIXUP
+				can->sending = 1;
+#endif
 
 				/* free software-fifo space taken by sent message */
 				occan_fifo_get(can->txfifo);
@@ -1668,8 +1830,8 @@ static void occan_interrupt(occan_priv *can){
 		}
 
 		if ( iflags & PELICAN_IF_ERRW ){
-			tx_error_cnt = READ_REG(&regs->tx_err_cnt);
-			rx_error_cnt = READ_REG(&regs->rx_err_cnt);
+			tx_error_cnt = READ_REG(can, &regs->tx_err_cnt);
+			rx_error_cnt = READ_REG(can, &regs->rx_err_cnt);
 
 			/* 1. if bus off tx error counter = 127 */
 			if ( (tx_error_cnt > 96) || (rx_error_cnt > 96) ){
@@ -1677,7 +1839,7 @@ static void occan_interrupt(occan_priv *can){
 				can->status |= OCCAN_STATUS_WARN;
 
 				/* check reset bit for reset mode */
-				if ( READ_REG(&regs->mode) & PELICAN_MOD_RESET ){
+				if ( READ_REG(can, &regs->mode) & PELICAN_MOD_RESET ){
 					/* in reset mode ==> bus off */
 					can->status |= OCCAN_STATUS_ERR_BUSOFF | OCCAN_STATUS_RESET;
 
@@ -1685,7 +1847,7 @@ static void occan_interrupt(occan_priv *can){
 					 * turn off interrupts
 					 * enter reset mode (HW already done that for us)
 					 */
-					regs->inten = 0;
+					WRITE_REG(can, &regs->inten,0);
 
 					/* Indicate that we are not started any more.
 					 * This will make write/read return with EBUSY
@@ -1722,8 +1884,8 @@ static void occan_interrupt(occan_priv *can){
 			/* Let the error counters decide what kind of
 			 * interrupt it was. In/Out of EPassive area.
 			 */
-			tx_error_cnt = READ_REG(&regs->tx_err_cnt);
-			rx_error_cnt = READ_REG(&regs->rx_err_cnt);
+			tx_error_cnt = READ_REG(can, &regs->tx_err_cnt);
+			rx_error_cnt = READ_REG(can, &regs->rx_err_cnt);
 
 			if ( (tx_error_cnt > 127) || (rx_error_cnt > 127) ){
 				can->status |= OCCAN_STATUS_ERR_PASSIVE;
@@ -1736,7 +1898,7 @@ static void occan_interrupt(occan_priv *can){
 		}
 
 		if ( iflags & PELICAN_IF_ARB){
-			arbcode = READ_REG(&regs->arbcode);
+			arbcode = READ_REG(can, &regs->arbcode);
 			can->stats.err_arb_bitnum[arbcode & PELICAN_ARB_BITS]++;
 			can->stats.err_arb++;
 			DBG("OCCAN_INT: ARB (0x%x)\n\r",arbcode & PELICAN_ARB_BITS);
@@ -1747,7 +1909,7 @@ static void occan_interrupt(occan_priv *can){
 			 * statistics. Error Register is decoded
 			 * and put into can->stats.
 			 */
-			errcode = READ_REG(&regs->errcode);
+			errcode = READ_REG(can, &regs->errcode);
 			switch( errcode & PELICAN_ECC_CODE ){
 				case PELICAN_ECC_CODE_BIT:
 					can->stats.err_bus_bit++;
@@ -1788,58 +1950,12 @@ static void occan_interrupt(occan_priv *can){
 	}
 }
 
-#ifdef OCCAN_DEFINE_INTHANDLER
-static void occan_interrupt_handler(rtems_vector_number v){
-	int minor;
-
-	/* convert to */
-  for(minor = 0; minor < can_cores; minor++) {
-  	if ( v == (cans[minor].irq+0x10) ) {
-			occan_interrupt(&cans[minor]);
-			return;
-		}
-	}
-}
-#endif
-
-#define OCCAN_DRIVER_TABLE_ENTRY { occan_initialize, occan_open, occan_close, occan_read, occan_write, occan_ioctl }
-
-static rtems_driver_address_table occan_driver = OCCAN_DRIVER_TABLE_ENTRY;
-
-int OCCAN_PREFIX(_register)(struct ambapp_bus *bus);
-
-int OCCAN_PREFIX(_register)(struct ambapp_bus *bus){
-	rtems_status_code r;
-	rtems_device_major_number m;
-
-	amba_bus = bus;
-	if ( !bus )
-		return 1;
-
-  if ((r = rtems_io_register_driver(0, &occan_driver, &m)) == RTEMS_SUCCESSFUL) {
-		DBG("OCCAN driver successfully registered, major: %d\n\r", m);
-	}else{
-		switch(r) {
-			case RTEMS_TOO_MANY:
-				printk("OCCAN rtems_io_register_driver failed: RTEMS_TOO_MANY\n\r"); break;
-			case RTEMS_INVALID_NUMBER:
-				printk("OCCAN rtems_io_register_driver failed: RTEMS_INVALID_NUMBER\n\r"); break;
-			case RTEMS_RESOURCE_IN_USE:
-				printk("OCCAN rtems_io_register_driver failed: RTEMS_RESOURCE_IN_USE\n\r"); break;
-			default:
-				printk("OCCAN rtems_io_register_driver failed\n\r");
-		}
-		return 1;
-	}
-	return 0;
-}
-
-
 /*******************************************************************************
  * FIFO IMPLEMENTATION
  */
 
-static occan_fifo *occan_fifo_create(int cnt){
+static occan_fifo *occan_fifo_create(int cnt)
+{
 	occan_fifo *fifo;
 	fifo = malloc(sizeof(occan_fifo)+cnt*sizeof(CANMsg));
 	if ( fifo ){
@@ -1854,21 +1970,25 @@ static occan_fifo *occan_fifo_create(int cnt){
 	return fifo;
 }
 
-static void occan_fifo_free(occan_fifo *fifo){
+static void occan_fifo_free(occan_fifo *fifo)
+{
 	if ( fifo )
 		free(fifo);
 }
 
-static int occan_fifo_full(occan_fifo *fifo){
+static int occan_fifo_full(occan_fifo *fifo)
+{
 	return fifo->full;
 }
 
-static int occan_fifo_empty(occan_fifo *fifo){
+static int occan_fifo_empty(occan_fifo *fifo)
+{
 	return (!fifo->full) && (fifo->head == fifo->tail);
 }
 
 /* Stage 1 - get buffer to fill (never fails if force!=0) */
-static CANMsg *occan_fifo_put_claim(occan_fifo *fifo, int force){
+static CANMsg *occan_fifo_put_claim(occan_fifo *fifo, int force)
+{
 	if ( !fifo )
 		return NULL;
 
@@ -1886,7 +2006,8 @@ static CANMsg *occan_fifo_put_claim(occan_fifo *fifo, int force){
 }
 
 /* Stage 2 - increment indexes */
-static void occan_fifo_put(occan_fifo *fifo){
+static void occan_fifo_put(occan_fifo *fifo)
+{
 	if ( occan_fifo_full(fifo) )
 		return;
 
@@ -1897,7 +2018,8 @@ static void occan_fifo_put(occan_fifo *fifo){
 		fifo->full = 1;
 }
 
-static CANMsg *occan_fifo_claim_get(occan_fifo *fifo){
+static CANMsg *occan_fifo_claim_get(occan_fifo *fifo)
+{
 	if ( occan_fifo_empty(fifo) )
 		return NULL;
 
@@ -1906,7 +2028,8 @@ static CANMsg *occan_fifo_claim_get(occan_fifo *fifo){
 }
 
 
-static void occan_fifo_get(occan_fifo *fifo){
+static void occan_fifo_get(occan_fifo *fifo)
+{
 	if ( !fifo )
 		return;
 
@@ -1914,14 +2037,16 @@ static void occan_fifo_get(occan_fifo *fifo){
 		return;
 
 	/* increment indexes */
-	fifo->tail = (fifo->tail >= &fifo->base[fifo->cnt-1])? fifo->base : fifo->tail+1;
+	fifo->tail = (fifo->tail >= &fifo->base[fifo->cnt-1]) ?
+	             fifo->base : fifo->tail+1;
 	fifo->full = 0;
 }
 
-static void occan_fifo_clr(occan_fifo *fifo){
-  fifo->full  = 0;
-  fifo->ovcnt = 0;
-  fifo->head  = fifo->tail = fifo->base;
+static void occan_fifo_clr(occan_fifo *fifo)
+{
+	fifo->full  = 0;
+	fifo->ovcnt = 0;
+	fifo->head  = fifo->tail = fifo->base;
 }
 
-/*******************************************************************************/
+/******************************************************************************/
diff --git a/c/src/lib/libbsp/sparc/shared/i2c/i2cmst.c b/c/src/lib/libbsp/sparc/shared/i2c/i2cmst.c
index 02710e1b84..f71af0be61 100644
--- a/c/src/lib/libbsp/sparc/shared/i2c/i2cmst.c
+++ b/c/src/lib/libbsp/sparc/shared/i2c/i2cmst.c
@@ -1,7 +1,7 @@
 /*
  * Driver for GRLIB port of OpenCores I2C-master
  *
- * COPYRIGHT (c) 2007 Gaisler Research
+ * COPYRIGHT (c) 2007 Cobham Gaisler AB
  * based on the RTEMS MPC83xx I2C driver (c) 2007 Embedded Brains GmbH.
  *
  * The license and distribution terms for this file may be
@@ -9,27 +9,41 @@
  * http://www.rtems.org/license/LICENSE.
  *
  * This file contains the driver and initialization code
- *
- * 2007-09-27: First version of driver (jan@gaisler.com)
  */
 
-
 #include <bsp.h>
-#include <i2cmst.h>
+#include <stdlib.h>
+#include <stdio.h>
 #include <ambapp.h>
-#include <grlib.h>
 #include <rtems/libi2c.h>
+#include <drvmgr/drvmgr.h>
+#include <drvmgr/ambapp_bus.h>
+
+#include <i2cmst.h>
 
 /* Enable debug printks? */
-/* #define DEBUG */
+/*#define DEBUG*/
 
-/* Default to 40 MHz system clock? */
-/*
-  #ifndef SYS_FREQ_kHZ
-  #define SYS_FREQ_kHZ 40000
-  #endif
-*/
+#ifdef DEBUG
+ #define DBG(args...) printk(args)
+#else
+ #define DBG(args...)
+#endif
 
+/* The OC I2C core will perform a write after a start unless the RD bit
+   in the command register has been set. Since the rtems framework has
+   a send_start function we buffer that command and use it when the first
+   data is written. The START is buffered in the sendstart member below */
+typedef struct gr_i2cmst_prv {
+  rtems_libi2c_bus_t              i2clib_desc;
+  struct drvmgr_dev    *dev;
+  gr_i2cmst_regs_t                *reg_ptr;
+  unsigned int                    sysfreq;     /* System clock frequency in kHz */
+  int                             minor;
+  unsigned char                   sendstart;   /* START events are buffered here */ 
+  /* rtems_irq_number             irq_number; */
+  /* rtems_id                     irq_sema_id; */
+} gr_i2cmst_prv_t;
 
 /* Calculates the scaler value for 100 kHz operation */
 static int gr_i2cmst_calc_scaler(int sysfreq)
@@ -42,12 +56,12 @@ static int gr_i2cmst_wait(gr_i2cmst_prv_t *prv_ptr, uint8_t expected_sts)
 {
   uint32_t tout = 0;
   int current_sts;
-#if defined(DEBUG)
-  printk("(gr_i2cmst_wait called...");
-#endif
+
+  DBG("(gr_i2cmst_wait called...");
 
   do {
     if (tout++ > 1000000) {
+      DBG("gr_i2cmst_wait: TIMEOUT\n");
       return RTEMS_TIMEOUT;
     }
   } while (prv_ptr->reg_ptr->cmdsts & GRI2C_STS_TIP);
@@ -57,32 +71,31 @@ static int gr_i2cmst_wait(gr_i2cmst_prv_t *prv_ptr, uint8_t expected_sts)
   if (current_sts != expected_sts) {
 #if defined(DEBUG)
     if (prv_ptr->reg_ptr->cmdsts & GRI2C_STS_RXACK) {
-      printk("Transfer NAKed..");
+      DBG("Transfer NAKed..");
     }
     if (prv_ptr->reg_ptr->cmdsts & GRI2C_STS_AL) {
-      printk("arbitration lost..");
+      DBG("arbitration lost..");
     }
     if (prv_ptr->reg_ptr->cmdsts & GRI2C_STS_TIP) {
-      printk("transfer still in progress, huh?..");
+      DBG("transfer still in progress, huh?..");
     }
-    printk("exited with IO error..)");
+    DBG("exited with IO error..)");
 #endif
+    DBG("gr_i2cmst_wait: IO-ERROR\n");
     return RTEMS_IO_ERROR;
   }
 
-#if defined(DEBUG)
-  printk("exited...)");
-#endif
+  DBG("exited...)");
+
   return RTEMS_SUCCESSFUL;
 }
 
 /* Initialize hardware core */
 static rtems_status_code gr_i2cmst_init(rtems_libi2c_bus_t *bushdl)
 {
-  gr_i2cmst_prv_t *prv_ptr = &(((gr_i2cmst_desc_t *)(bushdl))->prv);
-#if defined(DEBUG)
-  printk("gr_i2cmst_init called...");
-#endif
+  gr_i2cmst_prv_t *prv_ptr = (gr_i2cmst_prv_t *)bushdl;
+
+  DBG("gr_i2cmst_init called...");
 
   /* Disable core before changing prescale register */
   prv_ptr->reg_ptr->ctrl = 0;
@@ -96,54 +109,48 @@ static rtems_status_code gr_i2cmst_init(rtems_libi2c_bus_t *bushdl)
   /* Clear possible START condition */
   prv_ptr->sendstart = 0;
 
-#if defined(DEBUG)
-  printk("exited\n");
-#endif
+  DBG("exited\n");
+
   return RTEMS_SUCCESSFUL;
 }
 
 static rtems_status_code gr_i2cmst_send_start(rtems_libi2c_bus_t *bushdl)
 {
-  gr_i2cmst_prv_t *prv_ptr = &(((gr_i2cmst_desc_t *)(bushdl))->prv);
-#if defined(DEBUG)
-  printk("gr_i2cmst_send_start called...");
-#endif
+  gr_i2cmst_prv_t *prv_ptr = (gr_i2cmst_prv_t *)bushdl;
+
+  DBG("gr_i2cmst_send_start called...");
 
   /* The OC I2C core does not work with stand alone START events,
      instead the event is buffered */
   prv_ptr->sendstart = GRI2C_CMD_STA;
 
-#if defined(DEBUG)
-  printk("exited\n");
-#endif
+  DBG("exited\n");
+
   return RTEMS_SUCCESSFUL;
 }
 
 static rtems_status_code gr_i2cmst_send_stop(rtems_libi2c_bus_t *bushdl)
 {
-  gr_i2cmst_prv_t *prv_ptr = &(((gr_i2cmst_desc_t *)(bushdl))->prv);
-#if defined(DEBUG)
-  printk("gr_i2cmst_send_stop called...");
-#endif
+  gr_i2cmst_prv_t *prv_ptr = (gr_i2cmst_prv_t *)bushdl;
+
+  DBG("gr_i2cmst_send_stop called...");
 
   prv_ptr->reg_ptr->cmdsts = GRI2C_CMD_STO;
 
-#if defined(DEBUG)
-  printk("exited\n");
-#endif
+  DBG("exited\n");
+
   return RTEMS_SUCCESSFUL;
 }
 
 static rtems_status_code gr_i2cmst_send_addr(rtems_libi2c_bus_t *bushdl,
 					     uint32_t addr, int rw)
 {
-  gr_i2cmst_prv_t *prv_ptr = &(((gr_i2cmst_desc_t *)(bushdl))->prv);
+  gr_i2cmst_prv_t *prv_ptr = (gr_i2cmst_prv_t *)bushdl;
   uint8_t addr_byte;
   rtems_status_code rc;
-#if defined(DEBUG)
-  printk("gr_i2cmst_send_addr called, addr = 0x%x, rw = %d...",
+
+  DBG("gr_i2cmst_send_addr called, addr = 0x%x, rw = %d...", 
 	 addr, rw);
-#endif
 
   /*  Check if long address is needed */
   if (addr > 0x7f) {
@@ -156,9 +163,9 @@ static rtems_status_code gr_i2cmst_send_addr(rtems_libi2c_bus_t *bushdl,
     /* Wait for transfer to complete */
     rc = gr_i2cmst_wait(prv_ptr, GRI2C_STATUS_IDLE);
     if (rc != RTEMS_SUCCESSFUL) {
-#if defined(DEBUG)
-  printk("exited with error\n");
-#endif
+
+      DBG("exited with error\n");
+
       return -rc;
     }
   }
@@ -176,16 +183,12 @@ static rtems_status_code gr_i2cmst_send_addr(rtems_libi2c_bus_t *bushdl,
     /* Wait for transfer to complete */
     rc = gr_i2cmst_wait(prv_ptr, GRI2C_STATUS_IDLE);
     if (rc != RTEMS_SUCCESSFUL) {
-#if defined(DEBUG)
-  printk("exited with error\n");
-#endif
+      DBG("exited with error\n");
       return -rc;
     }
- }
+  }
 
-#if defined(DEBUG)
-  printk("exited\n");
-#endif
+  DBG("exited\n");
   return rc;
 }
 
@@ -193,13 +196,12 @@ static rtems_status_code gr_i2cmst_send_addr(rtems_libi2c_bus_t *bushdl,
 static int gr_i2cmst_read_bytes(rtems_libi2c_bus_t *bushdl,
 				unsigned char *bytes, int nbytes)
 {
-  gr_i2cmst_prv_t *prv_ptr = &(((gr_i2cmst_desc_t *)(bushdl))->prv);
+  gr_i2cmst_prv_t *prv_ptr = (gr_i2cmst_prv_t *)bushdl;
   unsigned char *buf = bytes;
   rtems_status_code rc;
   unsigned char expected_sts = GRI2C_STATUS_IDLE;
-#if defined(DEBUG)
-  printk("gr_i2cmst_read_bytes called, nbytes = %d...", nbytes);
-#endif
+
+  DBG("gr_i2cmst_read_bytes called, nbytes = %d...", nbytes);
 
   while (nbytes-- > 0) {
     if (nbytes == 0) {
@@ -214,34 +216,30 @@ static int gr_i2cmst_read_bytes(rtems_libi2c_bus_t *bushdl,
     /* Wait until end of transfer */
     rc = gr_i2cmst_wait(prv_ptr, expected_sts);
     if (rc != RTEMS_SUCCESSFUL) {
+      DBG("exited with error\n");
       return -rc;
-#if defined(DEBUG)
-  printk("exited with error\n");
-#endif
     }
     *buf++ = prv_ptr->reg_ptr->tdrd;
   }
 
-#if defined(DEBUG)
-  printk("exited\n");
-#endif
+  DBG("exited\n");
+
   return buf - bytes;
 }
 
 static int gr_i2cmst_write_bytes(rtems_libi2c_bus_t *bushdl,
 				unsigned char *bytes, int nbytes)
 {
-  gr_i2cmst_prv_t *prv_ptr = &(((gr_i2cmst_desc_t *)(bushdl))->prv);
+  gr_i2cmst_prv_t *prv_ptr = (gr_i2cmst_prv_t *)bushdl;
   unsigned char *buf = bytes;
   rtems_status_code rc;
-#if defined(DEBUG)
-  printk("gr_i2cmst_write_bytes called, nbytes = %d...", nbytes);
-#endif
+
+  DBG("gr_i2cmst_write_bytes called, nbytes = %d...", nbytes);
 
   while (nbytes-- > 0) {
-#if defined(DEBUG)
-  printk("writing byte 0x%02X...", *buf);
-#endif
+
+    DBG("writing byte 0x%02X...", *buf);
+
     prv_ptr->reg_ptr->tdrd = *buf++;
     prv_ptr->reg_ptr->cmdsts = GRI2C_CMD_WR | prv_ptr->sendstart;
     prv_ptr->sendstart = 0;
@@ -250,16 +248,13 @@ static int gr_i2cmst_write_bytes(rtems_libi2c_bus_t *bushdl,
     rc = gr_i2cmst_wait(prv_ptr, GRI2C_STATUS_IDLE);
 
     if (rc != RTEMS_SUCCESSFUL) {
-#if defined(DEBUG)
-  printk("exited with error\n");
-#endif
+      DBG("exited with error\n");
       return -rc;
     }
   }
 
-#if defined(DEBUG)
-  printk("exited\n");
-#endif
+  DBG("exited\n");
+
   return buf - bytes;
 }
 
@@ -272,88 +267,148 @@ static rtems_libi2c_bus_ops_t gr_i2cmst_ops = {
   write_bytes: gr_i2cmst_write_bytes,
 };
 
+/* Get Hardware and disable it */
+int i2cmst_device_init(gr_i2cmst_prv_t *priv)
+{
+	struct amba_dev_info *ambadev;
+	struct ambapp_core *pnpinfo;
 
-static gr_i2cmst_desc_t gr_i2cmst_desc = {
-  { /* rtems_libi2c_bus_t */
-    ops     : &gr_i2cmst_ops,
-    size    : sizeof(gr_i2cmst_ops),
-  },
-  { /* gr_i2cmst_prv_t, private data */
-    reg_ptr : NULL,
-    sysfreq : 40000,
-  }
+	/* Get device information from AMBA PnP information */
+	ambadev = (struct amba_dev_info *)priv->dev->businfo;
+	if ( ambadev == NULL ) {
+		return -1;
+	}
+	pnpinfo = &ambadev->info;
+	priv->reg_ptr = (gr_i2cmst_regs_t *)pnpinfo->apb_slv->start;
+
+	/* Disable core */
+	priv->reg_ptr->ctrl = 0;
+
+	priv->i2clib_desc.ops = &gr_i2cmst_ops;
+	priv->i2clib_desc.size = sizeof(gr_i2cmst_ops);
+	return 0;
+}
+
+
+/******************* Driver Manager Part ***********************/
+
+int i2cmst_init2(struct drvmgr_dev *dev);
+int i2cmst_init3(struct drvmgr_dev *dev);
 
+struct drvmgr_drv_ops i2cmst_ops = 
+{
+	.init = {NULL, i2cmst_init2, i2cmst_init3, NULL},
+	.remove = NULL,
+	.info = NULL
 };
 
-/* Scans for I2CMST core and initalizes i2c library */
-rtems_status_code leon_register_i2c(struct ambapp_bus *abus)
+struct amba_dev_id i2cmst_ids[] = 
 {
-#if defined(DEBUG)
-  printk("leon_register_i2c called...");
-#endif
+	{VENDOR_GAISLER, GAISLER_I2CMST},
+	{0, 0}		/* Mark end of table */
+};
 
-  int rc;
-  int device_found = 0;
-  struct ambapp_apb_info apbi2cmst;
+struct amba_drv_info i2cmst_drv_info =
+{
+	{
+		DRVMGR_OBJ_DRV,				/* Driver */
+		NULL,					/* Next driver */
+		NULL,					/* Device list */
+		DRIVER_AMBAPP_GAISLER_I2CMST_ID,	/* Driver ID */
+		"I2CMST_DRV",				/* Driver Name */
+		DRVMGR_BUS_TYPE_AMBAPP,			/* Bus Type */
+		&i2cmst_ops,
+		NULL,					/* Funcs */
+		0,					/* No devices yet */
+		0,
+	},
+	&i2cmst_ids[0]
+};
 
-  /* Scan AMBA bus for I2CMST core */
-  device_found = ambapp_find_apbslv(abus, VENDOR_GAISLER, GAISLER_I2CMST,
-				    &apbi2cmst);
+void i2cmst_register_drv (void)
+{
+	DBG("Registering I2CMST driver\n");
+	drvmgr_drv_register(&i2cmst_drv_info.general);
+}
 
-  if (device_found == 1) {
+/* The I2CMST Driver is informed about a new hardware device */
+int i2cmst_init2(struct drvmgr_dev *dev)
+{
+	gr_i2cmst_prv_t *priv;
 
-    /* Initialize i2c library */
-    rc = rtems_libi2c_initialize();
-    if (rc < 0) {
-#if defined(DEBUG)
-      printk("rtems_libi2x_initialize failed, exiting...\n");
-#endif
-      return rc;
-    }
+	DBG("I2CMST[%d] on bus %s\n", dev->minor_drv, dev->parent->dev->name);
 
-    gr_i2cmst_desc.prv.reg_ptr = (gr_i2cmst_regs_t *)apbi2cmst.start;
+	priv = dev->priv = malloc(sizeof(gr_i2cmst_prv_t));
+	if ( !priv )
+		return DRVMGR_NOMEM;
+	memset(priv, 0, sizeof(*priv));
+	priv->dev = dev;
 
-    /* Detect system frequency, same as in apbuart_initialize */
-#ifndef SYS_FREQ_kHZ
-#if defined(LEON3)
-	/* LEON3: find timer address via AMBA Plug&Play info */
-	{
-	  struct ambapp_apb_info gptimer;
-	  struct gptimer_regs *tregs;
-
-	  if (ambapp_find_apbslv(abus, VENDOR_GAISLER,
-				 GAISLER_GPTIMER, &gptimer) == 1 ) {
-	    tregs = (struct gptimer_regs *)gptimer.start;
-	    gr_i2cmst_desc.prv.sysfreq = (tregs->scaler_reload+1)*1000;
-	  } else {
-	    gr_i2cmst_desc.prv.sysfreq = 40000; /* Default to 40MHz */
-	  }
+	/* This core will not find other cores, so we wait for init2() */
+
+	return DRVMGR_OK;
+}
+
+/* Init stage 2 */
+int i2cmst_init3(struct drvmgr_dev *dev)
+{
+	gr_i2cmst_prv_t *priv;
+	char prefix[32];
+	char devName[32];
+	int rc;
+
+	priv = (gr_i2cmst_prv_t *)dev->priv;
+
+	/* Do initialization */
+
+	/* Initialize i2c library */
+	rc = rtems_libi2c_initialize();
+	if (rc != 0) {
+		DBG("I2CMST: rtems_libi2c_initialize failed, exiting...\n");
+		free(dev->priv);
+		dev->priv = NULL;
+		return DRVMGR_FAIL;
 	}
-#elif defined(LEON2)
-	/* LEON2: use hardcoded address to get to timer */
-	{
-	  LEON_Register_Map *regs = (LEON_Register_Map *)0x80000000;
-	  gr_i2cmst_desc.prv.sysfreq = (regs->Scaler_Reload+1)*1000;
+
+	/* I/O system registered and initialized 
+	 * Now we take care of device initialization.
+	 */
+
+	/* Get frequency */
+	if ( drvmgr_freq_get(dev, DEV_APB_SLV, &priv->sysfreq) ) {
+		return DRVMGR_FAIL;
 	}
-#else
-#error CPU not supported for I2CMST driver */
-#endif
-#else
-	/* Use hardcoded frequency */
-	gr_i2cmst_desc.prv.sysfreq = SYS_FREQ_kHZ;
-#endif
+	priv->sysfreq = priv->sysfreq / 1000; /* Convert to kHz */
 
-    rc = rtems_libi2c_register_bus("/dev/i2c1", &gr_i2cmst_desc.bus_desc);
-    if (rc < 0) {
-#if defined(DEBUG)
-      printk("rtems_libi2c_register_bus failed, exiting..\n");
-#endif
-      return -rc;
-    }
-  }
+	if ( i2cmst_device_init(priv) ) {
+		free(dev->priv);
+		dev->priv = NULL;
+		return DRVMGR_FAIL;
+	}
 
-#if defined(DEBUG)
-  printk("exited\n");
-#endif
-  return 0;
+	/* Get Filesystem name prefix */
+	prefix[0] = '\0';
+	if ( drvmgr_get_dev_prefix(dev, prefix) ) {
+		/* Failed to get prefix, make sure of a unique FS name
+		 * by using the driver minor.
+		 */
+		sprintf(devName, "/dev/i2c%d", dev->minor_drv+1);
+	} else {
+		/* Got special prefix, this means we have a bus prefix
+		 * And we should use our "bus minor"
+		 */
+		sprintf(devName, "/dev/%si2c%d", prefix, dev->minor_bus+1);
+	}
+
+	/* Register Bus for this Device */
+	rc = rtems_libi2c_register_bus(devName, &priv->i2clib_desc);
+	if (rc < 0) {
+		DBG("I2CMST: rtems_libi2c_register_bus(%s) failed, exiting..\n", devName);
+		free(dev->priv);
+		dev->priv = NULL;
+		return DRVMGR_FAIL;
+	}
+	priv->minor = rc;
+
+	return DRVMGR_OK;
 }
diff --git a/c/src/lib/libbsp/sparc/shared/include/b1553brm.h b/c/src/lib/libbsp/sparc/shared/include/b1553brm.h
index 2cc5b8b267..1c3b38b748 100644
--- a/c/src/lib/libbsp/sparc/shared/include/b1553brm.h
+++ b/c/src/lib/libbsp/sparc/shared/include/b1553brm.h
@@ -3,24 +3,21 @@
  * @ingroup sparc_bsp
  * @defgroup 1553 B1553BRM
  * @ingroup 1553
- * @brief Macros used for brm controller
+ * @brief B1553BRM device driver
  */
 
 /*
  *  COPYRIGHT (c) 2006.
- *  Gaisler Research
+ *  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.
- *
  */
 
 #ifndef __B1553BRM_H__
 #define __B1553BRM_H__
 
-#include <ambapp.h>
-
 #ifdef __cplusplus
 extern "C" {
 #endif
@@ -102,6 +99,7 @@ struct bc_msg {
 #define BC_RTRT   0x0002
 #define BC_BUSA   0x0004
 #define BC_EOL    0x0020
+#define BC_SKIP   0x0040
 #define BC_BAME   0x8000
 
 #define BRM_MBC_IRQ        1                    /* Monitor Block Counter irq */
@@ -114,7 +112,7 @@ struct bc_msg {
 #define BRM_IXEQ0_IRQ      256                  /* Index Equal Zero irq */
 #define BRM_BDRCV_IRQ      512                  /* Broadcast Command Received irq */
 #define BRM_SUBAD_IRQ      1024                 /* Subaddress Accessed irq */
-#define BRM_MERR_IRQ       4096                 /* Message Error irq */
+#define BRM_MERR_IRQ       2048                 /* Message Error irq */
 #define BRM_TAPF_IRQ       8192                 /* Terminal Address Parity Fail irq */
 #define BRM_WRAPF_IRQ      16384                /* Wrap Fail irq */
 #define BRM_DMAF_IRQ       32768                /* DMA Fail irq */
@@ -144,13 +142,6 @@ struct bc_msg {
 #define BRM_MODE_BM 0x2
 #define BRM_MODE_BM_RT 0x3 /* both RT and BM */
 
-
-/* Register RAMON FPGA BRM driver, calls brm_register */
-int brm_register_leon3_ramon_fpga(void);
-
-/* Register RAMON ASIC BRM driver, calls brm_register */
-int brm_register_leon3_ramon_asic(void);
-
 #define BRM_FREQ_12MHZ 0
 #define BRM_FREQ_16MHZ 1
 #define BRM_FREQ_20MHZ 2
@@ -161,15 +152,11 @@ int brm_register_leon3_ramon_asic(void);
 
 #define CLKSEL_MASK 0x7
 
-/* Register BRM driver
- * See (struct brm_reg).w_ctrl for clksel and clkdiv.
- * See Enhanced register (the least signinficant 2 bits) in BRM Core for brm_freq
- * bus = &ambapp_plb for LEON3. (LEON2 not yet supported for this driver)
- */
-int b1553brm_register(struct ambapp_bus *bus, unsigned int clksel, unsigned int clkdiv, unsigned int brm_freq);
+void b1553brm_register_drv(void);
 
 #ifdef __cplusplus
 }
 #endif
 
 #endif /* __BRM_H__ */
+
diff --git a/c/src/lib/libbsp/sparc/shared/include/grcan.h b/c/src/lib/libbsp/sparc/shared/include/grcan.h
index 9f014550a7..5f4da5d7db 100644
--- a/c/src/lib/libbsp/sparc/shared/include/grcan.h
+++ b/c/src/lib/libbsp/sparc/shared/include/grcan.h
@@ -8,19 +8,16 @@
 
 /*
  *  COPYRIGHT (c) 2007.
- *  Gaisler Research
+ *  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.
- *
  */
 
 #ifndef __GRCAN_H__
 #define __GRCAN_H__
 
-#include <ambapp.h>
-
 #ifdef __cplusplus
 extern "C" {
 #endif
@@ -187,21 +184,8 @@ typedef struct {
 #define GRCAN_IOC_SET_SFILTER    40  /* Set Sync Messages RX/TX filters, NULL disables the IRQ completely */
 #define GRCAN_IOC_GET_STATUS     41  /* Get status register of GRCAN core */
 
-struct grcan_device_info {
-  unsigned int base_address;
-  int irq;
-};
 
-/* Use hard coded addresses and IRQs to find hardware */
-int grcan_rasta_register_abs(struct grcan_device_info *devices, int dev_cnt);
-
-/* Use prescanned AMBA Plug&Play information to find all GRFIFO cores */
-int grcan_rasta_register(struct ambapp_bus *abus);
-int grcan_register(struct ambapp_bus *abus);
-#if 0
-void grcan_register(unsigned int baseaddr, unsigned int ram_base);
-void grcan_interrupt_handler(rtems_vector_number v);
-#endif
+void grcan_register_drv(void);
 
 #ifdef __cplusplus
 }
diff --git a/c/src/lib/libbsp/sparc/shared/include/grspw.h b/c/src/lib/libbsp/sparc/shared/include/grspw.h
index 77495be009..3270e8a5a2 100644
--- a/c/src/lib/libbsp/sparc/shared/include/grspw.h
+++ b/c/src/lib/libbsp/sparc/shared/include/grspw.h
@@ -3,12 +3,12 @@
  * @ingroup sparc_bsp
  * @defgroup spw SpaceWire
  * @ingroup spw
- * @brief Macros used for Spacewire bus
+ * @brief GRSPW Device Driver
  */
 
 /*
  *  COPYRIGHT (c) 2007.
- *  Gaisler Research
+ *  Cobham Gaisler AB.
  *
  *  The license and distribution terms for this file may be
  *  found in the file LICENSE in this distribution or at
@@ -32,12 +32,18 @@ typedef struct {
    unsigned int txhsize;
 } spw_ioctl_packetsize;
 
+#define GRSPW_PKTSEND_OPTION_HDR_CRC  0x1
+#define GRSPW_PKTSEND_OPTION_DATA_CRC 0x2
+#define GRSPW_PKTSEND_OPTION_NOCRCLEN(len) ((len & 0xf) << 8)
+#define GRSPW_PKTSEND_OPTION_NOCRCLEN_MASK 0xf00
+
 typedef struct {
    unsigned int hlen;
    char *hdr;
    unsigned int dlen;
    char *data;
    unsigned int sent;
+	 unsigned int options;
 } spw_ioctl_pkt_send;
 
 typedef struct {
@@ -84,6 +90,11 @@ typedef struct {
    unsigned int is_rmapcrc;
 
    unsigned int nodemask;
+	 unsigned int keep_source; /* copy source address to user-buffer in read() operations
+                              * Note that rm_prot_id has no effect when keep_source is
+                              * set.
+                              */
+	 unsigned int rtimeout; /* Read timeout if != 0 */
 } spw_config;
 
 #define SPACEWIRE_IOCTRL_SET_NODEADDR        1
@@ -113,30 +124,42 @@ typedef struct {
 #define SPACEWIRE_IOCTRL_SET_COREFREQ        32
 #define SPACEWIRE_IOCTRL_SET_CLKDIVSTART     33
 #define SPACEWIRE_IOCTRL_SET_NODEMASK        34
+#define SPACEWIRE_IOCTRL_SET_KEEP_SOURCE     35
+#define SPACEWIRE_IOCTRL_SET_TCODE_CTRL      36
+#define SPACEWIRE_IOCTRL_SET_TCODE           37
+#define SPACEWIRE_IOCTRL_GET_TCODE           38
+#define SPACEWIRE_IOCTRL_SET_READ_TIMEOUT    39
 
 #define SPACEWIRE_IOCTRL_START               64
 #define SPACEWIRE_IOCTRL_STOP                65
 
-int grspw_register(struct ambapp_bus *bus);
+/* Defines what register bits that will be touched 
+ * for SPACEWIRE_IOCTRL_SET_TCODE_CTRL
+ */
+#define SPACEWIRE_TCODE_CTRL_IE_MSK          0x001
+#define SPACEWIRE_TCODE_CTRL_TT_MSK          0x004
+#define SPACEWIRE_TCODE_CTRL_TR_MSK          0x008
 
+/* Defines what register bits that should be set
+ * for SPACEWIRE_IOCTRL_SET_TCODE_CTRL
+ */
+#define SPACEWIRE_TCODE_CTRL_IE              0x100
+#define SPACEWIRE_TCODE_CTRL_TT              0x400
+#define SPACEWIRE_TCODE_CTRL_TR              0x800
 
-#if 0
-struct grspw_buf;
+/* SPACEWIRE_IOCTRL_SET_TCODE argument mask */
+#define SPACEWIRE_TCODE_TCODE                0x0ff
+#define SPACEWIRE_TCODE_SET                  0x100 /* Set Timecode register */
+#define SPACEWIRE_TCODE_TX                   0x400
 
-struct grspw_buf {
-  grspw_buf *next;            /* next packet in chain */
+void grspw_register_drv (void);
 
-	/* Always used */
-	unsigned int dlen;          /* data length of '*data' */
-	unsigned int max_dlen;      /* allocated length of '*data' */
-	void *data;                 /* pointer to beginning of cargo data */
+void grspw_print(int options);
+
+/* Global GRSPW Function pointer called upon timecode receive interrupt */
+extern void (*grspw_timecode_callback)
+    (void *pDev, void *regs, int minor, unsigned int tc);
 
-	/* Only used when transmitting */
-	unsigned int hlen;          /* length of header '*header' */
-	unsigned int max_hlen;      /* allocated length of '*header' */
-	void *header;               /* pointer to beginning of header data */
-};
-#endif
 
 #ifdef __cplusplus
 }
diff --git a/c/src/lib/libbsp/sparc/shared/include/i2cmst.h b/c/src/lib/libbsp/sparc/shared/include/i2cmst.h
index 2db5cd0f4f..e63d2f8274 100644
--- a/c/src/lib/libbsp/sparc/shared/include/i2cmst.h
+++ b/c/src/lib/libbsp/sparc/shared/include/i2cmst.h
@@ -7,13 +7,12 @@
  */
 
 /*
- * COPYRIGHT (c) 2007 Gaisler Research
+ * COPYRIGHT (c) 2007 Cobham Gaisler AB
  * with parts from the RTEMS MPC83xx I2C driver (c) 2007 Embedded Brains GmbH.
  *
  * 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.
- *
  */
 
 #ifndef _I2CMST_H
@@ -56,26 +55,6 @@ typedef struct gr_i2cmst_regs {
 
 #define GRI2C_STATUS_IDLE     0x00000000
 
-/* The OC I2C core will perform a write after a start unless the RD bit
-   in the command register has been set. Since the rtems framework has
-   a send_start function we buffer that command and use it when the first
-   data is written. The START is buffered in the sendstart member below */
-typedef struct gr_i2cmst_prv {
-  gr_i2cmst_regs_t    *reg_ptr;
-  unsigned int        sysfreq;     /* System clock frequency in kHz */
-  unsigned char       sendstart;   /* START events are buffered here */
-  /* rtems_irq_number irq_number; */
-  /* rtems_id         irq_sema_id; */
-} gr_i2cmst_prv_t;
-
-typedef struct gr_i2cmst_desc {
-  rtems_libi2c_bus_t  bus_desc;
-  gr_i2cmst_prv_t     prv;
-} gr_i2cmst_desc_t;
-
-/* Scans for I2CMST core and initalizes i2c library */
-rtems_status_code leon_register_i2c(struct ambapp_bus *abus);
-
 #ifdef __cplusplus
 }
 #endif
diff --git a/c/src/lib/libbsp/sparc/shared/include/occan.h b/c/src/lib/libbsp/sparc/shared/include/occan.h
index 0217446c42..0bf34dee48 100644
--- a/c/src/lib/libbsp/sparc/shared/include/occan.h
+++ b/c/src/lib/libbsp/sparc/shared/include/occan.h
@@ -6,20 +6,15 @@
 
 /*
  *  COPYRIGHT (c) 2007.
- *  Gaisler Research.
+ *  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.
- *
- *  Author: Daniel Hellström, Gaisler Research AB, www.gaisler.com
  */
 
-
-#ifndef __OCCAN_H__
-#define __OCCAN_H__
-
-#include <ambapp.h>
+#ifndef __OCCAN_DRIVER_H__
+#define __OCCAN_DRIVER_H__
 
 #ifdef __cplusplus
 extern "C" {
@@ -152,7 +147,7 @@ struct occan_afilter {
 #define OCCAN_BLK_MODE_RX 0x1
 #define OCCAN_BLK_MODE_TX 0x2
 
-int occan_register(struct ambapp_bus *bus);
+void occan_register_drv (void);
 
 
 #define OCCAN_SPEED_500K 500000
diff --git a/c/src/lib/libbsp/sparc/shared/spw/grspw.c b/c/src/lib/libbsp/sparc/shared/spw/grspw.c
index d869d17b42..83f3b971cd 100644
--- a/c/src/lib/libbsp/sparc/shared/spw/grspw.c
+++ b/c/src/lib/libbsp/sparc/shared/spw/grspw.c
@@ -1,81 +1,14 @@
 /*
  *  This file contains the GRSPW SpaceWire Driver for LEON2 and LEON3.
  *
- *  COPYRIGHT (c) 2007
- *  Gaisler Research.
+ *  COPYRIGHT (c) 2006
+ *  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.
- *
- * Changes:
- *
- * 2007-09-27, Daniel Hellstrom <daniel@gaisler.com>
- *  Added basic support for GRSPW2 core.
- *
- * 2007-07-12, Daniel Hellstrom <daniel@gaisler.com>
- *  Fixed bug in TXBLOCK mode (normally called flush).
- *
- * 2007-05-28, Daniel Hellstrom <daniel@gaisler.com>
- *  Changed register call from spacewire_register to
- *  grspw_register. Added one parameter (the AMBA bus
- *  pointer) for LEON2 and LEON3 PCI compability.
- *  Typical LEON3 register: grspw_register(&amba_conf);
- *
- * 2007-05-28, Daniel Hellstrom <daniel@gaisler.com>
- *  Changed errno return values, compatible with RASTA
- *  Spacewire driver
- *
- * 2007-05-25, Daniel Hellstrom <daniel@gaisler.com>
- *  Changed name from /dev/spacewire,/dev/spacewire_b...
- *  to /dev/grspw0,/dev/grspw1...
- *
- * 2007-05-24, Daniel Hellstrom <daniel@gaisler.com>
- *  Merged LEON3, LEON2 and RASTA driver to one - this.
- *  The driver is included and configured from grspw_pci.c
- *  and grspw_rasta.c.
- *
- * 2007-05-23, Daniel Hellstrom <daniel@gaisler.com>
- *  Changed open call, now one need to first call open
- *  and then ioctl(fd,START,timeout) in order to setup
- *  hardware for communication.
- *
- * 2007-05-23, Daniel Hellstrom <daniel@gaisler.com>
- *  Added ioctl(fd,SET_COREFREQ,freq_arg), the command
- *  can autodetect the register values disconnect and
- *  timer64. It is still possible to change them manually
- *  by ioctl(fd,SET_{DISCONNECT,TIMER},arg).
- *
  */
 
-/* default name to /dev/grspw0 */
-#if !defined(GRSPW_DEVNAME) || !defined(GRSPW_DEVNAME_NO)
- #undef GRSPW_DEVNAME
- #undef GRSPW_DEVNAME_NO
- #define GRSPW_DEVNAME "/dev/grspw0"
- #define GRSPW_DEVNAME_NO(devstr,no) ((devstr)[10]='0'+(no))
-#endif
-
-#ifndef GRSPW_PREFIX
- #define GRSPW_PREFIX(name) grspw##name
-#else
- #define GRSPW_REGISTER_STATIC
-#endif
-
-/* default to no translation */
-#ifndef GRSPW_ADR_TO
- #define memarea_to_hw(x) ((unsigned int)(x))
-#endif
-#ifndef GRSPW_ADR_FROM
- #define hw_to_memarea(x) ((unsigned int)(x))
-#endif
-
-#ifndef GRSPW_REG_INT
- #define GRSPW_REG_INT(handler,irqno,arg) set_vector(handler,irqno+0x10,1)
- #undef  GRSPW_DEFINE_INTHANDLER
- #define GRSPW_DEFINE_INTHANDLER
-#endif
-
 #include <bsp.h>
 #include <rtems/libio.h>
 #include <stdlib.h>
@@ -86,7 +19,9 @@
 #include <rtems/bspIo.h>
 #include <rtems/malloc.h>
 #include <ambapp.h>
-#include <grlib.h>
+
+#include <drvmgr/drvmgr.h>
+#include <drvmgr/ambapp_bus.h>
 #include <grspw.h>
 
 #define DBGSPW_IOCALLS 1
@@ -97,7 +32,7 @@
 #define DEBUG_SPACEWIRE_FLAGS (DBGSPW_IOCALLS | DBGSPW_TX | DBGSPW_RX )
 
 /* #define DEBUG_SPACEWIRE_ONOFF */
-
+ 
 #ifdef DEBUG_SPACEWIRE_ONOFF
 #define SPACEWIRE_DBG(fmt, args...)    do { { printk(" : %03d @ %18s()]:" fmt , __LINE__,__FUNCTION__,## args); }} while(0)
 #define SPACEWIRE_DBG2(fmt)            do { { printk(" : %03d @ %18s()]:" fmt , __LINE__,__FUNCTION__); }} while(0)
@@ -117,15 +52,15 @@ typedef struct {
    volatile unsigned int time;
    volatile unsigned int timer;
    volatile unsigned int pad;
-
-   volatile unsigned int dma0ctrl;
+   
+   volatile unsigned int dma0ctrl; 
    volatile unsigned int dma0rxmax;
    volatile unsigned int dma0txdesc;
    volatile unsigned int dma0rxdesc;
-
+   
    /* For GRSPW core 2 and onwards */
    volatile unsigned int dma0addr;
-
+   
 } LEON3_SPACEWIRE_Regs_Map;
 
 typedef struct {
@@ -151,7 +86,10 @@ typedef struct {
 #define BUFMEM_PER_LINK (SPACEWIRE_TXBUFS_NR*(SPACEWIRE_TXD_SIZE+SPACEWIRE_TXH_SIZE) + SPACEWIRE_RXBUFS_NR*SPACEWIRE_RXPCK_SIZE)
 
 typedef struct {
-   /* configuration parameters */
+   /* configuration parameters */ 
+   struct drvmgr_dev *dev; /* Driver manager device */
+   char devName[32]; /* Device Name */
+   LEON3_SPACEWIRE_Regs_Map *regs;
    spw_config config;
 
    unsigned int tx_all_in_use;
@@ -165,12 +103,23 @@ typedef struct {
    unsigned int txbufcnt;
    unsigned int rxbufcnt;
 
+   /* DMA Area set by user */
+   unsigned int rx_dma_area;
+   unsigned int tx_data_dma_area;
+   unsigned int tx_hdr_dma_area;
+   unsigned int bd_dma_area;
+
    /* statistics */
    spw_stats stat;
 
    char *ptr_rxbuf0;
    char *ptr_txdbuf0;
    char *ptr_txhbuf0;
+   char *ptr_bd0;
+
+   char *ptr_rxbuf0_remote;
+   char *ptr_txdbuf0_remote;
+   char *ptr_txhbuf0_remote;
 
    unsigned int irq;
    int minor;
@@ -178,7 +127,7 @@ typedef struct {
    int open;
    int running;
    unsigned int core_freq_khz;
-
+   unsigned int rtimeout;
 
    /* semaphores*/
    rtems_id txsp;
@@ -187,23 +136,20 @@ typedef struct {
    SPACEWIRE_RXBD *rx;
    SPACEWIRE_TXBD *tx;
 
-#ifdef GRSPW_STATIC_MEM
-   unsigned int membase, memend, mem_bdtable;
-#endif
-
-   LEON3_SPACEWIRE_Regs_Map *regs;
+   unsigned int rx_remote;
+   unsigned int tx_remote;
 } GRSPW_DEV;
 
-static int spw_cores;
-static int spw_cores2;
-static unsigned int sys_freq_khz;
-static GRSPW_DEV *grspw_devs;
+/* Function pointer called upon timecode receive */
+void (*grspw_timecode_callback)
+    (void *pDev, void *regs, int minor, unsigned int tc) = NULL;
 
 #ifdef GRSPW_DONT_BYPASS_CACHE
 #define _SPW_READ(address) (*(volatile unsigned int *)(address))
-#define _MEM_READ(address) (*(volatile unsigned char *)(address))
+#define _MEM_READ8(address) (*(volatile unsigned char *)(address))
+#define _MEM_READ32(address) (*(volatile unsigned int *)(address))
 #else
-static unsigned int _SPW_READ(void *addr) {
+static inline unsigned int _SPW_READ(volatile void *addr) {
         unsigned int tmp;
         __asm__ (" lda [%1]1, %0 "
             : "=r"(tmp)
@@ -212,20 +158,29 @@ static unsigned int _SPW_READ(void *addr) {
         return tmp;
 }
 
-static unsigned int _MEM_READ(void *addr) {
+static inline unsigned int _MEM_READ8(volatile void *addr) {
         unsigned int tmp;
         __asm__ (" lduba [%1]1, %0 "
             : "=r"(tmp)
             : "r"(addr)
            );
         return tmp;
+}
 
+static inline unsigned int _MEM_READ32(volatile void *addr) {
+        unsigned int tmp;
+        __asm__ (" lda [%1]1, %0 "
+            : "=r"(tmp)
+            : "r"(addr)
+           );
+        return tmp;
 }
 #endif
 
-#define MEM_READ(addr) _MEM_READ((void *)(addr))
-#define SPW_READ(addr) _SPW_READ((void *)(addr))
-#define SPW_WRITE(addr,v) *addr=v
+#define MEM_READ8(addr) _MEM_READ8((volatile void *)(addr))
+#define MEM_READ32(addr) _MEM_READ32((volatile void *)(addr))
+#define SPW_READ(addr) _SPW_READ((volatile void *)(addr))
+#define SPW_WRITE(addr,v) (*(volatile unsigned int *)addr)=v
 
 #define SPW_REG(c,r) (c->regs->r)
 #define SPW_REG_CTRL(c) SPW_REG(c,ctrl)
@@ -263,6 +218,8 @@ static unsigned int _MEM_READ(void *addr) {
 #define SPW_TXBD_WR (1 << 13)
 #define SPW_TXBD_IE (1 << 14)
 #define SPW_TXBD_LE (1 << 15)
+#define SPW_TXBD_HC (1 << 16)
+#define SPW_TXBD_DC (1 << 17)
 
 #define SPW_TXBD_ERROR (SPW_TXBD_LE)
 
@@ -311,7 +268,7 @@ static unsigned int _MEM_READ(void *addr) {
 #define SPW_PREPAREMASK_RX (SPW_DMACTRL_TXEN | SPW_DMACTRL_TXIE | SPW_DMACTRL_AI | SPW_DMACTRL_PR | SPW_DMACTRL_RA)
 
 static int grspw_hw_init(GRSPW_DEV *pDev);
-static int grspw_hw_send(GRSPW_DEV *pDev, unsigned int hlen, char *hdr, unsigned int dlen, char *data);
+static int grspw_hw_send(GRSPW_DEV *pDev, unsigned int hlen, char *hdr, unsigned int dlen, char *data, unsigned int options);
 static int grspw_hw_receive(GRSPW_DEV *pDev,char *b,int c);
 static int grspw_hw_startup (GRSPW_DEV *pDev, int timeout);
 static int grspw_hw_stop (GRSPW_DEV *pDev, int rx, int tx);
@@ -322,7 +279,8 @@ static void grspw_hw_read_config(GRSPW_DEV *pDev);
 
 static void check_rx_errors(GRSPW_DEV *pDev, int ctrl);
 static void grspw_rxnext(GRSPW_DEV *pDev);
-static void grspw_interrupt(GRSPW_DEV *pDev);
+static void grspw_interrupt(void *arg);
+static int grspw_buffer_alloc(GRSPW_DEV *pDev);
 
 static rtems_device_driver grspw_initialize(
         rtems_device_major_number  major,
@@ -369,78 +327,271 @@ static rtems_device_driver grspw_control(
     grspw_control }
 
 static rtems_driver_address_table grspw_driver = GRSPW_DRIVER_TABLE_ENTRY;
-static struct ambapp_bus *amba_bus;
+static int grspw_driver_io_registered = 0;
+static rtems_device_major_number grspw_driver_io_major = 0;
 
-#ifdef GRSPW_REGISTER_STATIC
-static
-#endif
-int GRSPW_PREFIX(_register)(struct ambapp_bus *bus)
-{
-        rtems_status_code r;
-        rtems_device_major_number m;
+/******************* Driver manager interface ***********************/
 
-        /* Get System clock frequency */
-        sys_freq_khz = 0;
+/* Driver prototypes */
+int grspw_register_io(rtems_device_major_number *m);
+int grspw_device_init(GRSPW_DEV *pDev);
 
-        amba_bus = bus;
+int grspw_init2(struct drvmgr_dev *dev);
+int grspw_init3(struct drvmgr_dev *dev);
 
-        /* Auto Detect the GRSPW core frequency by assuming that the system frequency is
-         * is the same as the GRSPW core frequency.
-         */
-#ifndef SYS_FREQ_KHZ
-#ifdef LEON3
-	/* LEON3: find timer address via AMBA Plug&Play info */
+struct drvmgr_drv_ops grspw_ops = 
+{
+	.init = {NULL,  grspw_init2, grspw_init3, NULL},
+	.remove = NULL,
+	.info = NULL
+};
+
+struct amba_dev_id grspw_ids[] = 
+{
+	{VENDOR_GAISLER, GAISLER_SPW},
+	{VENDOR_GAISLER, GAISLER_SPW2},
+	{VENDOR_GAISLER, GAISLER_SPW2_DMA},
+	{0, 0}		/* Mark end of table */
+};
+
+struct amba_drv_info grspw_drv_info =
+{
 	{
-		struct ambapp_apb_info gptimer;
-		struct gptimer_regs *tregs;
-
-		if ( ambapp_find_apbslv(&ambapp_plb, VENDOR_GAISLER,
-                                        GAISLER_GPTIMER, &gptimer) == 1 ) {
-			tregs = (struct gptimer_regs *)gptimer.start;
-			sys_freq_khz = (tregs->scaler_reload+1)*1000;
-			SPACEWIRE_DBG("GRSPW: detected %dkHZ system frequency\n\r",sys_freq_khz);
-		}else{
-			sys_freq_khz = 40000; /* Default to 40MHz */
-			printk("GRSPW: Failed to detect system frequency\n\r");
+		DRVMGR_OBJ_DRV,			/* Driver */
+		NULL,				/* Next driver */
+		NULL,				/* Device list */
+		DRIVER_AMBAPP_GAISLER_GRSPW_ID,	/* Driver ID */
+		"GRSPW_DRV",			/* Driver Name */
+		DRVMGR_BUS_TYPE_AMBAPP,		/* Bus Type */
+		&grspw_ops,
+		NULL,				/* Funcs */
+		0,				/* No devices yet */
+		0,
+	},
+	&grspw_ids[0]
+};
+
+void grspw_register_drv (void)
+{
+	SPACEWIRE_DBG("Registering GRSPW driver\n");
+	drvmgr_drv_register(&grspw_drv_info.general);
+}
+
+int grspw_init2(struct drvmgr_dev *dev)
+{
+	GRSPW_DEV *priv;
+
+	SPACEWIRE_DBG("GRSPW[%d] on bus %s\n", dev->minor_drv,
+		dev->parent->dev->name);
+	priv = dev->priv = malloc(sizeof(GRSPW_DEV));
+	if ( !priv )
+		return DRVMGR_NOMEM;
+	memset(priv, 0, sizeof(*priv));
+	priv->dev = dev;
+
+	/* This core will not find other cores, so we wait for init2() */
+
+	return DRVMGR_OK;
+}
+
+int grspw_init3(struct drvmgr_dev *dev)
+{
+	GRSPW_DEV *priv;
+	char prefix[32];
+	rtems_status_code status;
+
+	priv = dev->priv;
+
+	/* Do initialization */
+
+	if ( grspw_driver_io_registered == 0) {
+		/* Register the I/O driver only once for all cores */
+		if ( grspw_register_io(&grspw_driver_io_major) ) {
+			/* Failed to register I/O driver */
+			free(dev->priv);
+			dev->priv = NULL;
+			return DRVMGR_FAIL;
 		}
 
+		grspw_driver_io_registered = 1;
 	}
-#elif defined(LEON2)
-        /* LEON2: use hardcoded address to get to timer */
-        {
-                LEON_Register_Map *regs = (LEON_Register_Map *)0x80000000;
-                sys_freq_khz = (regs->Scaler_Reload+1)*1000;
-        }
-#else
- #error CPU not supported by GRSPW driver
-#endif
-#else
-	/* Use hardcoded frequency */
-	sys_freq_khz = SYS_FREQ_KHZ;
-#endif
 
-        SPACEWIRE_DBG2("register driver\n");
-        if ((r = rtems_io_register_driver(0, &grspw_driver, &m)) == RTEMS_SUCCESSFUL) {
-                SPACEWIRE_DBG2("success\n");
-                return 0;
-        } else {
-                switch(r) {
-                        case RTEMS_TOO_MANY:
-                                SPACEWIRE_DBG2("failed RTEMS_TOO_MANY\n");
-                                break;
-                        case RTEMS_INVALID_NUMBER:
-                                SPACEWIRE_DBG2("failed RTEMS_INVALID_NUMBER\n");
-                                break;
-                        case RTEMS_RESOURCE_IN_USE:
-                                SPACEWIRE_DBG2("failed RTEMS_RESOURCE_IN_USE\n");
-                                break;
-                        default:
-                                SPACEWIRE_DBG("failed %i\n",r);
-                                break;
-                }
-                return 1;
-        }
+	/* I/O system registered and initialized 
+	 * Now we take care of device initialization.
+	 */
+
+	/* Get frequency in Hz */
+	if ( drvmgr_freq_get(dev, DEV_APB_SLV, &priv->core_freq_khz) ) {
+		return DRVMGR_FAIL;
+	}
+	/* Convert from Hz -> kHz */
+	priv->core_freq_khz = priv->core_freq_khz / 1000;
 
+	if ( grspw_device_init(priv) ) {
+		return DRVMGR_FAIL;
+	}
+
+	/* Get Filesystem name prefix */
+	prefix[0] = '\0';
+	if ( drvmgr_get_dev_prefix(dev, prefix) ) {
+		/* Failed to get prefix, make sure of a unique FS name
+		 * by using the driver minor.
+		 */
+		sprintf(priv->devName, "/dev/grspw%d", dev->minor_drv);
+	} else {
+		/* Got special prefix, this means we have a bus prefix
+		 * And we should use our "bus minor"
+		 */
+		sprintf(priv->devName, "/dev/%sgrspw%d", prefix, dev->minor_bus);
+	}
+
+	/* Register Device */
+	status = rtems_io_register_name(priv->devName, grspw_driver_io_major, dev->minor_drv);
+	if (status != RTEMS_SUCCESSFUL) {
+		return DRVMGR_FAIL;
+	}
+
+	return DRVMGR_OK;
+}
+
+/******************* Driver Implementation ***********************/
+
+int grspw_register_io(rtems_device_major_number *m)
+{
+	rtems_status_code r;
+
+	if ((r = rtems_io_register_driver(0, &grspw_driver, m)) == RTEMS_SUCCESSFUL) {
+		SPACEWIRE_DBG("GRSPW driver successfully registered, major: %d\n", *m);
+	} else {
+		switch(r) {
+		case RTEMS_TOO_MANY:
+			printk("GRSPW rtems_io_register_driver failed: RTEMS_TOO_MANY\n");
+			return -1;
+		case RTEMS_INVALID_NUMBER:  
+			printk("GRSPW rtems_io_register_driver failed: RTEMS_INVALID_NUMBER\n");
+			return -1;
+		case RTEMS_RESOURCE_IN_USE:
+			printk("GRSPW rtems_io_register_driver failed: RTEMS_RESOURCE_IN_USE\n");
+			return -1;
+		default:
+			printk("GRSPW rtems_io_register_driver failed\n");
+			return -1;
+		}
+	}
+	return 0;
+}
+
+int grspw_device_init(GRSPW_DEV *pDev)
+{
+	struct amba_dev_info *ambadev;
+	struct ambapp_core *pnpinfo;
+	union drvmgr_key_value *value;
+
+	/* Get device information from AMBA PnP information */
+	ambadev = (struct amba_dev_info *)pDev->dev->businfo;
+	if ( ambadev == NULL ) {
+		return -1;
+	}
+	pnpinfo = &ambadev->info;
+	pDev->irq = pnpinfo->irq;
+	pDev->regs = (LEON3_SPACEWIRE_Regs_Map *)pnpinfo->apb_slv->start;
+	pDev->minor = pDev->dev->minor_drv;
+
+	/* Get SpaceWire core version */
+	switch( pnpinfo->device ) {
+		case GAISLER_SPW:
+			pDev->core_ver = 1;
+			break;
+		case GAISLER_SPW2:
+			pDev->core_ver = 2;
+			break;
+		case GAISLER_SPW2_DMA:
+			pDev->core_ver = 3;
+			break;
+		default:
+			return -1;
+	}
+
+	/* initialize the code with some resonable values,
+	 * actual initialization is done later using ioctl(fd)
+	 * on the opened device */
+	pDev->config.rxmaxlen = SPACEWIRE_RXPCK_SIZE;
+	pDev->txdbufsize = SPACEWIRE_TXD_SIZE;
+	pDev->txhbufsize = SPACEWIRE_TXH_SIZE;
+	pDev->rxbufsize = SPACEWIRE_RXPCK_SIZE;
+	pDev->txbufcnt = SPACEWIRE_TXBUFS_NR;
+	pDev->rxbufcnt = SPACEWIRE_RXBUFS_NR;
+
+	pDev->ptr_rxbuf0 = 0;
+	pDev->ptr_txdbuf0 = 0;
+	pDev->ptr_txhbuf0 = 0;
+	pDev->ptr_bd0 = 0;
+	pDev->rx_dma_area = 0;
+	pDev->tx_data_dma_area = 0;
+	pDev->tx_hdr_dma_area = 0;
+	pDev->bd_dma_area = 0;
+
+	/* Get Configuration from Bus resources (Let user override defaults) */
+
+	value = drvmgr_dev_key_get(pDev->dev, "txBdCnt", KEY_TYPE_INT);
+	if ( value )
+		pDev->txbufcnt = value->i;
+
+	value = drvmgr_dev_key_get(pDev->dev, "rxBdCnt", KEY_TYPE_INT);
+	if ( value )
+		pDev->rxbufcnt = value->i;
+
+	value = drvmgr_dev_key_get(pDev->dev, "txDataSize", KEY_TYPE_INT);
+	if ( value )
+		pDev->txdbufsize = value->i;
+
+	value = drvmgr_dev_key_get(pDev->dev, "txHdrSize", KEY_TYPE_INT);
+	if ( value )
+		pDev->txhbufsize = value->i;
+
+	value = drvmgr_dev_key_get(pDev->dev, "rxPktSize", KEY_TYPE_INT);
+	if ( value )
+		pDev->rxbufsize = value->i;
+	
+	value = drvmgr_dev_key_get(pDev->dev, "rxDmaArea", KEY_TYPE_INT);
+	if ( value )
+		pDev->rx_dma_area = value->i;
+
+	value = drvmgr_dev_key_get(pDev->dev, "txDataDmaArea", KEY_TYPE_INT);
+	if ( value )
+		pDev->tx_data_dma_area = value->i;
+
+	value = drvmgr_dev_key_get(pDev->dev, "txHdrDmaArea", KEY_TYPE_INT);
+	if ( value )
+		pDev->tx_hdr_dma_area = value->i;
+
+	value = drvmgr_dev_key_get(pDev->dev, "bdDmaArea", KEY_TYPE_INT);
+	if ( value )
+		pDev->bd_dma_area = value->i;
+
+	if (grspw_buffer_alloc(pDev)) 
+		return RTEMS_NO_MEMORY;
+
+	/* Create semaphores */
+	rtems_semaphore_create(
+		rtems_build_name('T', 'x', 'S', '0' + pDev->minor), 
+		0, 
+		RTEMS_FIFO | RTEMS_SIMPLE_BINARY_SEMAPHORE | RTEMS_NO_INHERIT_PRIORITY | \
+		RTEMS_NO_PRIORITY_CEILING, 
+		0, 
+		&(pDev->txsp));
+
+	rtems_semaphore_create(
+		rtems_build_name('R', 'x', 'S', '0' + pDev->minor), 
+		0, 
+		RTEMS_FIFO | RTEMS_SIMPLE_BINARY_SEMAPHORE | RTEMS_NO_INHERIT_PRIORITY | \
+		RTEMS_NO_PRIORITY_CEILING, 
+		0, 
+		&(pDev->rxsp));
+
+	grspw_hw_init(pDev);
+
+	return 0;
 }
 
 /* Get a value at least 6.4us in number of clock cycles */
@@ -455,1182 +606,1212 @@ static unsigned int grspw_calc_disconnect(int freq_khz){
 	return disconnect & 0x3ff;
 }
 
-#if 0
-static int grspw_buffer_alloc(GRSPW_DEV *pDev) {
-        if (pDev->ptr_rxbuf0) {
-                free(pDev->ptr_rxbuf0);
-        }
-        if (pDev->ptr_txdbuf0) {
-                free(pDev->ptr_txdbuf0);
-        }
-        if (pDev->ptr_txhbuf0) {
-                free(pDev->ptr_txhbuf0);
-        }
-        pDev->ptr_rxbuf0 = (char *) malloc(pDev->rxbufsize * pDev->rxbufcnt);
-        pDev->ptr_txdbuf0 = (char *) malloc(pDev->txdbufsize * pDev->txbufcnt);
-        pDev->ptr_txhbuf0 = (char *) malloc(pDev->txhbufsize * pDev->txbufcnt);
-        if ((pDev->ptr_rxbuf0 == NULL) ||
-            (pDev->ptr_txdbuf0 == NULL) || (pDev->ptr_txhbuf0 == NULL)) {
-                return 1;
-        } else {
-                return 0;
-        }
-}
-#endif
-
 static int grspw_buffer_alloc(GRSPW_DEV *pDev)
 {
-#ifndef GRSPW_STATIC_MEM
-        if (pDev->ptr_rxbuf0) {
-                free(pDev->ptr_rxbuf0);
-        }
-        if (pDev->ptr_txdbuf0) {
-                free(pDev->ptr_txdbuf0);
-        }
-        if (pDev->ptr_txhbuf0) {
-                free(pDev->ptr_txhbuf0);
-        }
-
-        pDev->ptr_rxbuf0 = (char *) malloc(pDev->rxbufsize * pDev->rxbufcnt);
-        pDev->ptr_txdbuf0 = (char *) malloc(pDev->txdbufsize * pDev->txbufcnt);
-        pDev->ptr_txhbuf0 = (char *) malloc(pDev->txhbufsize * pDev->txbufcnt);
-        if ((pDev->ptr_rxbuf0 == NULL) ||
-            (pDev->ptr_txdbuf0 == NULL) || (pDev->ptr_txhbuf0 == NULL)) {
-                return 1;
-        } else {
-                return 0;
-        }
-#else
-        if ( (pDev->membase + pDev->rxbufsize*pDev->rxbufcnt + pDev->txdbufsize*pDev->txbufcnt) >= pDev->memend ) {
-                return -1;
-        }
-        pDev->ptr_rxbuf0  = (char *) pDev->membase;
-        pDev->ptr_txdbuf0 = pDev->ptr_rxbuf0 + pDev->rxbufsize * pDev->rxbufcnt;
-        pDev->ptr_txhbuf0 = pDev->ptr_txdbuf0 + pDev->txdbufsize * pDev->txbufcnt;
-        return 0;
-#endif
+	if ( pDev->rx_dma_area ) {
+#warning Check size?
+		if ( pDev->rx_dma_area & 1 ) {
+			/* Address given in remote address */
+			drvmgr_translate(pDev->dev, 1, 1, (void *)(pDev->rx_dma_area & ~1), (void **)&pDev->ptr_rxbuf0);
+		} else {
+			pDev->ptr_rxbuf0 = pDev->rx_dma_area;
+		}
+	} else {
+		if (pDev->ptr_rxbuf0) {
+			free(pDev->ptr_rxbuf0);
+		}
+		pDev->ptr_rxbuf0 = (char *) malloc(pDev->rxbufsize * pDev->rxbufcnt);
+		if ( !pDev->ptr_rxbuf0 )
+			return 1;
+	}
+	if ( pDev->tx_data_dma_area ) {
+		if ( pDev->tx_data_dma_area & 1 ) {
+			/* Address given in remote address */
+			drvmgr_translate(pDev->dev, 1, 1, (void *)(pDev->tx_data_dma_area & ~1), (void **)&pDev->ptr_txdbuf0);
+		} else {
+			pDev->ptr_txdbuf0 = pDev->tx_data_dma_area;
+		}
+	} else {
+		if (pDev->ptr_txdbuf0) {
+			free(pDev->ptr_txdbuf0);
+		}
+		pDev->ptr_txdbuf0 = (char *) malloc(pDev->txdbufsize * pDev->txbufcnt);
+		if ( !pDev->ptr_txdbuf0 )
+			return 1;
+	}
+	if ( pDev->tx_hdr_dma_area ) {
+		if ( pDev->tx_hdr_dma_area & 1 ) {
+			/* Address given in remote address */
+			drvmgr_translate(pDev->dev, 1, 1, (void *)(pDev->tx_hdr_dma_area & ~1), (void **)&pDev->ptr_txhbuf0);
+		} else {
+			pDev->ptr_txhbuf0 = pDev->tx_hdr_dma_area;
+		}
+	} else {
+		if (pDev->ptr_txhbuf0) {
+			free(pDev->ptr_txhbuf0);
+		}
+		pDev->ptr_txhbuf0 = (char *) malloc(pDev->txhbufsize * pDev->txbufcnt);
+		if ( !pDev->ptr_txhbuf0 )
+			return 1;
+	}
+	if ( pDev->bd_dma_area ) {
+		if ( pDev->bd_dma_area & 1 ) {
+			/* Address given in remote address */
+			drvmgr_translate(pDev->dev, 1, 1, (void *)(pDev->bd_dma_area & ~1), (void **)&pDev->ptr_bd0);
+		} else {
+			pDev->ptr_bd0 = pDev->bd_dma_area;
+		}
+	} else {
+		if (pDev->ptr_bd0) {
+			free(pDev->ptr_bd0);
+		}
+		pDev->ptr_bd0 = (char *)
+			rtems_heap_allocate_aligned_with_boundary(SPACEWIRE_BDTABLE_SIZE*2, 1024, 0);
+		if ( !pDev->ptr_bd0 )
+			return 1;
+	}
 
+	/* Translate into remote address */
+	drvmgr_translate(pDev->dev, 0, 0, (void *)pDev->ptr_rxbuf0, (void **)&pDev->ptr_rxbuf0_remote);
+	drvmgr_translate(pDev->dev, 0, 0, (void *)pDev->ptr_txdbuf0,(void **)&pDev->ptr_txdbuf0_remote);
+	drvmgr_translate(pDev->dev, 0, 0, (void *)pDev->ptr_txhbuf0, (void **)&pDev->ptr_txhbuf0_remote);
+	return 0;
 }
 
-#ifdef GRSPW_DEFINE_INTHANDLER
-/*
- *  Standard Interrupt handler
- */
-static rtems_isr grspw_interrupt_handler(rtems_vector_number v)
+static void grspw_interrupt(void *arg)
 {
-        int minor;
-
-        for(minor = 0; minor < spw_cores+spw_cores2; minor++) {
-                if (v == (grspw_devs[minor].irq+0x10) ) {
-                        grspw_interrupt(&grspw_devs[minor]);
-                        break;
-                }
-        }
-}
-#endif
+	GRSPW_DEV *pDev = (GRSPW_DEV *)arg;
+	int dmactrl;
+	int status;
+	int ctrl;
+	unsigned int timecode;
+
+	status = SPW_STATUS_READ(pDev);
+	/*SPW_STATUS_WRITE(pDev, SPW_STATUS_CE | SPW_STATUS_ER | SPW_STATUS_DE | SPW_STATUS_PE | SPW_STATUS_WE | SPW_STATUS_IA | SPW_STATUS_EE | SPW_STATUS_TO);*/
+	SPW_STATUS_WRITE(pDev, status & (SPW_STATUS_CE | SPW_STATUS_ER | SPW_STATUS_DE | SPW_STATUS_PE | SPW_STATUS_WE | SPW_STATUS_IA | SPW_STATUS_EE));
+
+	/* Make sure to put the timecode handling first in order to get the smallest
+	 * possible interrupt latency
+	 */
+	if ( (status & SPW_STATUS_TO) && (grspw_timecode_callback != NULL) ) {
+	    /* Timecode received. Let custom function handle this */
+	    SPW_STATUS_WRITE(pDev, SPW_STATUS_TO);
+	    timecode = SPW_READ(&pDev->regs->time);
+	    (grspw_timecode_callback)(pDev,pDev->regs,pDev->minor,timecode);
+	}
+
+	/* Clear SPW_DMACTRL_PR if set */
+	dmactrl = SPW_READ(&pDev->regs->dma0ctrl);
+	/*SPW_WRITE(&pDev->regs->dma0ctrl, dmactrl | SPW_DMACTRL_PR);*/
+	SPW_WRITE(&pDev->regs->dma0ctrl, dmactrl);
+
+	/* If linkinterrupts are enabled check if it was a linkerror irq and then send an event to the 
+	   process set in the config */
+	if (pDev->config.link_err_irq) {
+		if (status & (SPW_STATUS_CE | SPW_STATUS_ER | SPW_STATUS_DE | SPW_STATUS_PE | SPW_STATUS_WE)) {
+			rtems_event_send(pDev->config.event_id, SPW_LINKERR_EVENT);
+			if (pDev->config.disable_err) {
+				/* disable link*/
+				SPW_CTRL_WRITE(pDev, (SPW_CTRL_READ(pDev) & 0xFFFFFFFC) | SPW_CTRL_LINKDISABLED);
+				pDev->config.linkdisabled = 1;
+				pDev->config.linkstart = 0;
+				pDev->running = 0;
+			}
+		}
+	}
+	if (status & SPW_STATUS_CE) {
+		pDev->stat.credit_err++;
+	}
+	if (status & SPW_STATUS_ER) {
+		pDev->stat.escape_err++;
+	}
+	if (status & SPW_STATUS_DE) {
+		pDev->stat.disconnect_err++;
+	}
+	if (status & SPW_STATUS_PE) {
+		pDev->stat.parity_err++;
+	}
+	if (status & SPW_STATUS_WE) {
+		pDev->stat.write_sync_err++;
+	}
+	if (status & SPW_STATUS_IA) {
+		pDev->stat.invalid_address++;
+	}
+	if (status & SPW_STATUS_EE) {
+		pDev->stat.early_ep++;
+	}
+
+	/* Check for tx interrupts */
+	while( (pDev->tx_sent != pDev->tx_cur) || pDev->tx_all_in_use) {
+		/* Has this descriptor been sent? */
+		ctrl = SPW_READ((volatile void *)&pDev->tx[pDev->tx_sent].ctrl);
+		if ( ctrl & SPW_TXBD_EN ) {
+			break;
+		}
+		/* Yes, increment status counters & tx_sent so we can use this descriptor to send more packets with */
+		pDev->stat.packets_sent++;
 
-static void grspw_interrupt(GRSPW_DEV *pDev){
-        int dmactrl;
-        int status;
-        int ctrl;
-
-        status = SPW_STATUS_READ(pDev);
-        SPW_STATUS_WRITE(pDev, SPW_STATUS_CE | SPW_STATUS_ER | SPW_STATUS_DE | SPW_STATUS_PE | SPW_STATUS_WE | SPW_STATUS_IA | SPW_STATUS_EE);
-        dmactrl = SPW_READ(&pDev->regs->dma0ctrl);
-        SPW_WRITE(&pDev->regs->dma0ctrl, dmactrl | SPW_DMACTRL_PR);
-        /* If linkinterrupts are enabled check if it was a linkerror irq and then send an event to the
-           process set in the config */
-        if (pDev->config.link_err_irq) {
-                if (status & (SPW_STATUS_CE | SPW_STATUS_ER | SPW_STATUS_DE | SPW_STATUS_PE | SPW_STATUS_WE)) {
-                        rtems_event_send(pDev->config.event_id, SPW_LINKERR_EVENT);
-                        if (pDev->config.disable_err) {
-                                /* disable link*/
-                                SPW_CTRL_WRITE(pDev, (SPW_CTRL_READ(pDev) & 0xFFFFFFFC) | SPW_CTRL_LINKDISABLED);
-                                pDev->config.linkdisabled = 1;
-                                pDev->config.linkstart = 0;
-                                pDev->running = 0;
-                        }
-                }
-        }
-        if (status & SPW_STATUS_CE) {
-                pDev->stat.credit_err++;
-        }
-        if (status & SPW_STATUS_ER) {
-                pDev->stat.escape_err++;
-        }
-        if (status & SPW_STATUS_DE) {
-                pDev->stat.disconnect_err++;
-        }
-        if (status & SPW_STATUS_PE) {
-                pDev->stat.parity_err++;
-        }
-        if (status & SPW_STATUS_WE) {
-                pDev->stat.write_sync_err++;
-        }
-        if (status & SPW_STATUS_IA) {
-                pDev->stat.invalid_address++;
-        }
-        if (status & SPW_STATUS_EE) {
-                pDev->stat.early_ep++;
-        }
-
-        /* Check for tx interrupts */
-        while( (pDev->tx_sent != pDev->tx_cur) || pDev->tx_all_in_use) {
-                /* Has this descriptor been sent? */
-                ctrl = SPW_READ((volatile void *)&pDev->tx[pDev->tx_sent].ctrl);
-                if ( ctrl & SPW_TXBD_EN ) {
-                        break;
-                }
-                /* Yes, increment status counters & tx_sent so we can use this descriptor to send more packets with */
-                pDev->stat.packets_sent++;
-
-                rtems_semaphore_release(pDev->txsp);
-
-                if ( ctrl & SPW_TXBD_LE ) {
-                        pDev->stat.tx_link_err++;
-                }
-
-                /* step to next descriptor */
-                pDev->tx_sent = (pDev->tx_sent + 1) % pDev->txbufcnt;
-                pDev->tx_all_in_use = 0; /* not all of the descriptors can be in use since we just freed one. */
-        }
-
-        /* Check for rx interrupts */
-        if (dmactrl & SPW_DMACTRL_PR) {
-                rtems_semaphore_release(pDev->rxsp);
-        }
+		rtems_semaphore_release(pDev->txsp);
+
+		if ( ctrl & SPW_TXBD_LE ) {
+			pDev->stat.tx_link_err++;
+		}
+
+		/* step to next descriptor */
+		pDev->tx_sent = (pDev->tx_sent + 1) % pDev->txbufcnt;
+		pDev->tx_all_in_use = 0; /* not all of the descriptors can be in use since we just freed one. */
+	}
+
+	/* Check for rx interrupts */
+	if (dmactrl & SPW_DMACTRL_PR) {
+		rtems_semaphore_release(pDev->rxsp);
+	}
 }
 
 static rtems_device_driver grspw_initialize(
-        rtems_device_major_number  major,
-        rtems_device_minor_number  minor,
-        void                      *arg
+	rtems_device_major_number major,
+	rtems_device_minor_number minor,
+	void *arg
 )
 {
-        rtems_status_code status;
-        int i=0;
-        char c;
-        GRSPW_DEV *pDev;
-        char console_name[20];
-        struct ambapp_apb_info dev;
-
-        SPACEWIRE_DBG2("spacewire driver initialization\n");
-
-        /* Copy device name */
-        strcpy(console_name,GRSPW_DEVNAME);
-
-        /* Get the number of GRSPW cores */
-        i=0; spw_cores = 0; spw_cores2 = 0;
-
-        /* get number of GRSPW cores */
-        spw_cores = ambapp_get_number_apbslv_devices(amba_bus, VENDOR_GAISLER,
-                                                     GAISLER_SPW);
-        spw_cores2 = ambapp_get_number_apbslv_devices(amba_bus, VENDOR_GAISLER,
-                                                      GAISLER_SPW2);
-
-        if ( (spw_cores+spw_cores2) < 1 ){
-                /* No GRSPW cores around... */
-                return RTEMS_SUCCESSFUL;
-        }
-
-        /* Allocate memory for all spacewire cores */
-        grspw_devs = (GRSPW_DEV *)malloc((spw_cores+spw_cores2) * sizeof(GRSPW_DEV));
-
-        /* Zero out all memory */
-        memset(grspw_devs,0,(spw_cores+spw_cores2) * sizeof(GRSPW_DEV));
-
-        /* loop all found spacewire cores */
-        i = 0;
-        for(minor=0; minor<(spw_cores+spw_cores2); minor++){
-
-                pDev = &grspw_devs[minor];
-
-                /* Get device */
-                if ( spw_cores > minor ) {
-                        ambapp_find_apbslv_next(amba_bus, VENDOR_GAISLER,
-                                                GAISLER_SPW, &dev, minor);
-                        pDev->core_ver = 1;
-                } else {
-                        ambapp_find_apbslv_next(amba_bus, VENDOR_GAISLER,
-                                                GAISLER_SPW2, &dev,
-                                                minor - spw_cores);
-                        pDev->core_ver = 2;
-                }
-
-                pDev->regs = (LEON3_SPACEWIRE_Regs_Map *)dev.start;
-                pDev->irq = dev.irq;
-                pDev->minor = minor;
-                pDev->open = 0;
-
-                /* register interrupt routine */
-                GRSPW_REG_INT(GRSPW_PREFIX(_interrupt_handler), pDev->irq, pDev);
-
-                SPACEWIRE_DBG("spacewire core at [0x%x]\n", (unsigned int) pDev->regs);
-
-                /* initialize the code with some resonable values,
-                   actual initialization is done later using ioctl(fd)
-                   on the opened device */
-                pDev->config.rxmaxlen = SPACEWIRE_RXPCK_SIZE;
-                pDev->txdbufsize = SPACEWIRE_TXD_SIZE;
-                pDev->txhbufsize = SPACEWIRE_TXH_SIZE;
-                pDev->rxbufsize = SPACEWIRE_RXPCK_SIZE;
-                pDev->txbufcnt = SPACEWIRE_TXBUFS_NR;
-                pDev->rxbufcnt = SPACEWIRE_RXBUFS_NR;
-                pDev->config.check_rmap_err = 0;
-                pDev->config.tx_blocking = 0;
-                pDev->config.tx_block_on_full = 0;
-                pDev->config.rx_blocking = 0;
-                pDev->config.disable_err = 0;
-                pDev->config.link_err_irq = 0;
-                pDev->config.event_id = 0;
-
-                pDev->ptr_rxbuf0 = 0;
-                pDev->ptr_txdbuf0 = 0;
-                pDev->ptr_txhbuf0 = 0;
-
-#ifdef GRSPW_STATIC_MEM
-                GRSPW_CALC_MEMOFS(spw_cores,minor,&pDev->membase,&pDev->memend,&pDev->mem_bdtable);
-#endif
-
-                if (grspw_buffer_alloc(pDev))
-                        return RTEMS_NO_MEMORY;
-
-        }
-
-        /*  Register Device Names, /dev/grspw0, /dev/grspw1  ... */
-        for (i = 0; i < spw_cores+spw_cores2; i++) {
-           GRSPW_DEVNAME_NO(console_name,i);
-           SPACEWIRE_DBG("registering minor %i as %s\n", i, console_name);
-           status = rtems_io_register_name( console_name, major, i);
-           if (status != RTEMS_SUCCESSFUL){
-              rtems_fatal_error_occurred(status);
-           }
-        }
-
-        /* Initialize Hardware and semaphores*/
-        c = 'a';
-        for (i = 0; i < spw_cores+spw_cores2; i++) {
-                pDev = &grspw_devs[i];
-                rtems_semaphore_create(
-                        rtems_build_name('T', 'x', 'S', c),
-                        0,
-                        RTEMS_FIFO | RTEMS_SIMPLE_BINARY_SEMAPHORE | RTEMS_NO_INHERIT_PRIORITY | \
-                        RTEMS_NO_PRIORITY_CEILING,
-                        0,
-                        &(pDev->txsp));
-                rtems_semaphore_create(
-                        rtems_build_name('R', 'x', 'S', c),
-                        0,
-                        RTEMS_FIFO | RTEMS_SIMPLE_BINARY_SEMAPHORE | RTEMS_NO_INHERIT_PRIORITY | \
-                        RTEMS_NO_PRIORITY_CEILING,
-                        0,
-                        &(pDev->rxsp));
-                c++;
-                grspw_hw_init(pDev);
-        }
-
-        return RTEMS_SUCCESSFUL;
+	/* Initialize device-common data structures here */
+	return RTEMS_SUCCESSFUL;
 }
 
 static rtems_device_driver grspw_open(
         rtems_device_major_number major,
         rtems_device_minor_number minor,
-        void                    * arg
-        )
+        void * arg
+        ) 
 {
-        GRSPW_DEV *pDev;
-        SPACEWIRE_DBGC(DBGSPW_IOCALLS, "open [%i,%i]\n", major, minor);
-        if ( minor >= (spw_cores+spw_cores2) ) {
-                SPACEWIRE_DBG("minor %i too big\n", minor);
-                return RTEMS_INVALID_NAME;
-        }
-        pDev = &grspw_devs[minor];
-
-        if ( pDev->open )
-                return RTEMS_RESOURCE_IN_USE;
-
-        /* Mark device open */
-        pDev->open = 1;
-
-        pDev->stat.tx_link_err = 0;
-        pDev->stat.rx_rmap_header_crc_err = 0;
-        pDev->stat.rx_rmap_data_crc_err = 0;
-        pDev->stat.rx_eep_err = 0;
-        pDev->stat.rx_truncated = 0;
-        pDev->stat.parity_err = 0;
-        pDev->stat.escape_err = 0;
-        pDev->stat.credit_err = 0;
-        pDev->stat.write_sync_err = 0;
-        pDev->stat.disconnect_err = 0;
-        pDev->stat.early_ep = 0;
-        pDev->stat.invalid_address = 0;
-        pDev->stat.packets_sent = 0;
-        pDev->stat.packets_received = 0;
-
-        pDev->running = 0;
-        pDev->core_freq_khz = 0;
-
-        /* Reset Core */
-        grspw_hw_reset(pDev);
-
-        /* Read default configuration */
-        grspw_hw_read_config(pDev);
-
-        return RTEMS_SUCCESSFUL;
+	GRSPW_DEV *pDev;
+	struct drvmgr_dev *dev;
+	SPACEWIRE_DBGC(DBGSPW_IOCALLS, "open [%i,%i]\n", major, minor);
+
+	if ( drvmgr_get_dev(&grspw_drv_info.general, minor, &dev) ) {
+		SPACEWIRE_DBG("Wrong minor %d\n", minor);
+		return RTEMS_INVALID_NAME;
+	}
+	pDev = (GRSPW_DEV *)dev->priv;
+
+	if ( pDev->open )
+		return RTEMS_RESOURCE_IN_USE;
+
+	/* Mark device open */
+	pDev->open = 1;
+
+	pDev->stat.tx_link_err = 0;
+	pDev->stat.rx_rmap_header_crc_err = 0;
+	pDev->stat.rx_rmap_data_crc_err = 0;
+	pDev->stat.rx_eep_err = 0;
+	pDev->stat.rx_truncated = 0;
+	pDev->stat.parity_err = 0;
+	pDev->stat.escape_err = 0;
+	pDev->stat.credit_err = 0;
+	pDev->stat.write_sync_err = 0;
+	pDev->stat.disconnect_err = 0;
+	pDev->stat.early_ep = 0;
+	pDev->stat.invalid_address = 0;
+	pDev->stat.packets_sent = 0;
+	pDev->stat.packets_received = 0;
+
+	pDev->config.rm_prot_id = 0;
+	pDev->config.keep_source = 0;
+	pDev->config.check_rmap_err = 0;
+	pDev->config.tx_blocking = 0;
+	pDev->config.tx_block_on_full = 0;
+	pDev->config.rx_blocking = 0;
+	pDev->config.disable_err = 0;
+	pDev->config.link_err_irq = 0; 
+	pDev->config.event_id = 0;
+	pDev->config.rtimeout = 0;
+
+	pDev->running = 0;
+	pDev->core_freq_khz = 0;
+
+	/* Reset Core */
+	grspw_hw_reset(pDev);
+
+	/* Read default configuration */
+	grspw_hw_read_config(pDev);
+
+	return RTEMS_SUCCESSFUL;
 }
 
 static rtems_device_driver grspw_close(
-        rtems_device_major_number major,
-        rtems_device_minor_number minor,
-        void                    * arg
-        )
-{
-        GRSPW_DEV *pDev = &grspw_devs[minor];
+	rtems_device_major_number major,
+	rtems_device_minor_number minor,
+	void * arg
+	)
+{	
+	GRSPW_DEV *pDev;
+	struct drvmgr_dev *dev;
+
+	if ( drvmgr_get_dev(&grspw_drv_info.general, minor, &dev) ) {
+		return RTEMS_INVALID_NAME;
+	}
+	pDev = (GRSPW_DEV *)dev->priv;
 
-        SPACEWIRE_DBGC(DBGSPW_IOCALLS, "close [%i,%i]\n", major, minor);
-        rtems_semaphore_delete(pDev->txsp);
-        rtems_semaphore_delete(pDev->rxsp);
+	SPACEWIRE_DBGC(DBGSPW_IOCALLS, "close [%i,%i]\n", major, minor);	
+	rtems_semaphore_delete(pDev->txsp);
+	rtems_semaphore_delete(pDev->rxsp);
 
-        grspw_hw_stop(pDev,1,1);
+	grspw_hw_stop(pDev,1,1);
 
-        grspw_hw_reset(pDev);
+	grspw_hw_reset(pDev);
 
-        /* Mark device closed - not open */
-        pDev->open = 0;
+	/* Mark device closed - not open */
+	pDev->open = 0;
 
-        return RTEMS_SUCCESSFUL;
+	return RTEMS_SUCCESSFUL;
 }
 
 static rtems_device_driver grspw_read(
-        rtems_device_major_number major,
-        rtems_device_minor_number minor,
-        void                    * arg
-        )
+	rtems_device_major_number major,
+	rtems_device_minor_number minor,
+	void		    * arg
+	)
 {
-        GRSPW_DEV *pDev = &grspw_devs[minor];
-        rtems_libio_rw_args_t *rw_args;
-        unsigned int count = 0;
-        rw_args = (rtems_libio_rw_args_t *) arg;
+	rtems_libio_rw_args_t *rw_args;
+	unsigned int count = 0;
+	GRSPW_DEV *pDev;
+	struct drvmgr_dev *dev;
+	int status;
+
+	if ( drvmgr_get_dev(&grspw_drv_info.general, minor, &dev) ) {
+		return RTEMS_INVALID_NAME;
+	}
+	pDev = (GRSPW_DEV *)dev->priv;
+
+	rw_args = (rtems_libio_rw_args_t *) arg;
 
-        /* is link up? */
-        if ( !pDev->running ) {
-                return RTEMS_INVALID_NAME;
-        }
+	/* is link up? */
+	if ( !pDev->running ) {
+		return RTEMS_INVALID_NAME;
+	}
 
-        if ((rw_args->count < 1) || (rw_args->buffer == NULL)) {
-                return RTEMS_INVALID_NAME;
-        }
+	if ((rw_args->count < 1) || (rw_args->buffer == NULL)) {
+		return RTEMS_INVALID_NAME;
+	}
 
-        SPACEWIRE_DBGC(DBGSPW_IOCALLS, "read  [%i,%i]: buf:0x%x len:%i \n", major, minor, (unsigned int)rw_args->buffer, rw_args->count);
+	SPACEWIRE_DBGC(DBGSPW_IOCALLS, "read  [%i,%i]: buf:0x%x len:%i \n", major, minor, (unsigned int)rw_args->buffer, rw_args->count);
 
-        while ( (count = grspw_hw_receive(pDev, rw_args->buffer, rw_args->count)) == 0) {
-		/* wait a moment for any descriptors to get available
-		 *
+	while ( (count = grspw_hw_receive(pDev, rw_args->buffer, rw_args->count)) == 0) {
+		/* wait a moment for any descriptors to get available 
+		 * 
 		 * Semaphore is signaled by interrupt handler
 		 */
 		if (pDev->config.rx_blocking) {
 			SPACEWIRE_DBG2("Rx blocking\n");
-			rtems_semaphore_obtain(pDev->rxsp, RTEMS_WAIT, RTEMS_NO_TIMEOUT);
+			if ( pDev->config.rtimeout ) {
+				status = rtems_semaphore_obtain(pDev->rxsp, RTEMS_WAIT, pDev->config.rtimeout);
+				if ( status == RTEMS_TIMEOUT )
+					return RTEMS_TIMEOUT;
+			} else {
+				rtems_semaphore_obtain(pDev->rxsp, RTEMS_WAIT, RTEMS_NO_TIMEOUT);
+			}
 		} else {
-                        SPACEWIRE_DBG2("Rx non blocking\n");
+			SPACEWIRE_DBG2("Rx non blocking\n");
 			return RTEMS_RESOURCE_IN_USE;
 		}
 	}
 
-#ifdef DEBUG_SPACEWIRE_ONOFF
-        if (DEBUG_SPACEWIRE_FLAGS & DBGSPW_DUMP) {
-                int k;
-                for (k = 0; k < count; k++){
-                        if (k % 16 == 0) {
-                                printf ("\n");
-                        }
-                        printf ("%.2x(%c) ", rw_args->buffer[k] & 0xff, isprint(rw_args->buffer[k] & 0xff) ? rw_args->buffer[k] & 0xff : ' ');
-                }
-                printf ("\n");
-        }
+#ifdef DEBUG_SPACEWIRE_ONOFF  
+	if (DEBUG_SPACEWIRE_FLAGS & DBGSPW_DUMP) {
+		int k;
+		for (k = 0; k < count; k++){
+			if (k % 16 == 0) {
+				printf ("\n");
+			}
+			printf ("%.2x(%c) ", rw_args->buffer[k] & 0xff, isprint(rw_args->buffer[k] & 0xff) ? rw_args->buffer[k] & 0xff : ' ');
+		}
+		printf ("\n");
+	}
 #endif
 
-        rw_args->bytes_moved = count;
-        return RTEMS_SUCCESSFUL;
-
+	rw_args->bytes_moved = count;
+	return RTEMS_SUCCESSFUL;
 }
 
 static rtems_device_driver grspw_write(
-        rtems_device_major_number major,
-        rtems_device_minor_number minor,
-        void                    * arg
+	rtems_device_major_number major,
+	rtems_device_minor_number minor,
+	void		    * arg
 )
 {
-        GRSPW_DEV *pDev = &grspw_devs[minor];
-        rtems_libio_rw_args_t *rw_args;
-        rw_args = (rtems_libio_rw_args_t *) arg;
-        SPACEWIRE_DBGC(DBGSPW_IOCALLS, "write [%i,%i]: buf:0x%x len:%i\n", major, minor, (unsigned int)rw_args->buffer, rw_args->count);
-
-        /* is link up? */
-        if ( !pDev->running ) {
-                return RTEMS_INVALID_NAME;
-        }
-
-        if ((rw_args->count > pDev->txdbufsize) || (rw_args->count < 1) || (rw_args->buffer == NULL)) {
-                return RTEMS_INVALID_NAME;
-        }
-
-        while ((rw_args->bytes_moved = grspw_hw_send(pDev, 0, NULL, rw_args->count, rw_args->buffer)) == 0) {
-                if (pDev->config.tx_block_on_full == 1) {
-                        SPACEWIRE_DBG2("Tx Block on full \n");
-                        rtems_semaphore_obtain(pDev->txsp, RTEMS_WAIT, RTEMS_NO_TIMEOUT);
-                } else {
-                        SPACEWIRE_DBG2("Tx non blocking return when full \n");
-                        return RTEMS_RESOURCE_IN_USE;
-                }
-        }
-        return RTEMS_SUCCESSFUL;
+	rtems_libio_rw_args_t *rw_args;
+	GRSPW_DEV *pDev;
+	struct drvmgr_dev *dev;
+
+	if ( drvmgr_get_dev(&grspw_drv_info.general, minor, &dev) ) {
+		return RTEMS_INVALID_NAME;
+	}
+	pDev = (GRSPW_DEV *)dev->priv;
+
+	rw_args = (rtems_libio_rw_args_t *) arg;
+	SPACEWIRE_DBGC(DBGSPW_IOCALLS, "write [%i,%i]: buf:0x%x len:%i\n", major, minor, (unsigned int)rw_args->buffer, rw_args->count);
+
+	/* is link up? */
+	if ( !pDev->running ) {
+		return RTEMS_INVALID_NAME;
+	}
+
+	if ((rw_args->count > pDev->txdbufsize) || (rw_args->count < 1) || (rw_args->buffer == NULL)) {
+		return RTEMS_INVALID_NAME;
+	}
+
+	while ((rw_args->bytes_moved = grspw_hw_send(pDev, 0, NULL, rw_args->count, rw_args->buffer, 0)) == 0) {
+		if (pDev->config.tx_block_on_full == 1) { 
+			SPACEWIRE_DBG2("Tx Block on full \n");
+			rtems_semaphore_obtain(pDev->txsp, RTEMS_WAIT, RTEMS_NO_TIMEOUT);
+		} else {
+			SPACEWIRE_DBG2("Tx non blocking return when full \n");
+			return RTEMS_RESOURCE_IN_USE;
+		}
+	}
+	return RTEMS_SUCCESSFUL;
 }
 
 static rtems_device_driver grspw_control(
-        rtems_device_major_number major,
-        rtems_device_minor_number minor,
-        void                    * arg
-        )
+	rtems_device_major_number major,
+	rtems_device_minor_number minor,
+	void		    * arg
+	)
 {
-        GRSPW_DEV *pDev = &grspw_devs[minor];
-        spw_ioctl_pkt_send *args;
-        spw_ioctl_packetsize *ps;
-        int status;
-        unsigned int tmp,nodeaddr,nodemask;
-        int timeout;
-        rtems_device_driver ret;
-        rtems_libio_ioctl_args_t	*ioarg = (rtems_libio_ioctl_args_t *) arg;
-        SPACEWIRE_DBGC(DBGSPW_IOCALLS, "ctrl [%i,%i]\n", major, minor);
-
-        if (!ioarg)
-                return RTEMS_INVALID_NAME;
-
-
-        ioarg->ioctl_return = 0;
-        switch(ioarg->command) {
-                case SPACEWIRE_IOCTRL_SET_NODEADDR:
-                        /*set node address*/
-                        SPACEWIRE_DBGC(DBGSPW_IOCTRL, "SPACEWIRE_IOCTRL_SET_NODEADDR %i\n",(unsigned int)ioarg->buffer);
-                        if ((unsigned int)ioarg->buffer > 255) {
-                                return RTEMS_INVALID_NAME;
-                        }
-                        nodeaddr = ((unsigned int)ioarg->buffer) & 0xff;
-                        tmp = SPW_READ(&pDev->regs->nodeaddr);
-                        tmp &= 0xffffff00; /* Remove old address */
-                        tmp |= nodeaddr;
-                        SPW_WRITE(&pDev->regs->nodeaddr, tmp);
-                        if ((SPW_READ(&pDev->regs->nodeaddr)&0xff) != nodeaddr) {
-                                return RTEMS_IO_ERROR;
-                        }
-                        pDev->config.nodeaddr = nodeaddr;
-                        break;
-                case SPACEWIRE_IOCTRL_SET_NODEMASK:
-                        /*set node address*/
-                        SPACEWIRE_DBGC(DBGSPW_IOCTRL, "SPACEWIRE_IOCTRL_SET_NODEMASK %i\n",(unsigned int)ioarg->buffer);
-                        if ( pDev->core_ver > 1 ){
-                          if ((unsigned int)ioarg->buffer > 255) {
-                                  return RTEMS_INVALID_NAME;
-                          }
-                          nodemask = ((unsigned int)ioarg->buffer) & 0xff;
-                          tmp = SPW_READ(&pDev->regs->nodeaddr);
-                          tmp &= 0xffff00ff; /* Remove old mask */
-                          tmp |= nodemask<<8;
-                          SPW_WRITE(&pDev->regs->nodeaddr, tmp);
-                          if (((SPW_READ(&pDev->regs->nodeaddr)>>8)&0xff) != nodemask) {
-                                  return RTEMS_IO_ERROR;
-                          }
-                          pDev->config.nodemask = nodemask;
-                        }else{
-                          SPACEWIRE_DBG("SPACEWIRE_IOCTRL_SET_NODEMASK: not implemented in GRSPW1 HW\n");
-                        }
-                        break;
-                case SPACEWIRE_IOCTRL_SET_RXBLOCK:
-                        SPACEWIRE_DBGC(DBGSPW_IOCTRL, "SPACEWIRE_IOCTRL_SET_RXBLOCK %i \n", (unsigned int)ioarg->buffer);
-                        if ((unsigned int)ioarg->buffer > 1) {
-                                return RTEMS_INVALID_NAME;
-                        }
-                        pDev->config.rx_blocking = (unsigned int)ioarg->buffer;
-                        break;
-                case SPACEWIRE_IOCTRL_SET_DESTKEY:
-                        SPACEWIRE_DBGC(DBGSPW_IOCTRL,"SPACEWIRE_IOCTRL_SET_DESTKEY %i\n", (unsigned int)ioarg->buffer);
-                        if (!pDev->config.is_rmap) {
-                                return RTEMS_NOT_IMPLEMENTED;
-                        }
-                        if ((unsigned int)ioarg->buffer > 255) {
-                                return RTEMS_INVALID_NAME;
-                        }
-                        SPW_WRITE(&pDev->regs->destkey, (unsigned int)ioarg->buffer);
-                        if (SPW_READ(&pDev->regs->destkey) != (unsigned int)ioarg->buffer) {
-                                return RTEMS_IO_ERROR;
-                        }
-                        pDev->config.destkey = (unsigned int)ioarg->buffer;
-                        break;
-                case SPACEWIRE_IOCTRL_SET_CLKDIV:
-                        SPACEWIRE_DBGC(DBGSPW_IOCTRL,"SPACEWIRE_IOCTRL_SET_CLKDIV %i\n", (unsigned int)ioarg->buffer);
-                        if ((unsigned int)ioarg->buffer > 255) {
-                                return RTEMS_INVALID_NAME;
-                        }
-                        tmp = SPW_READ(&pDev->regs->clkdiv);
-                        tmp &= ~0xff; /* Remove old Clockdiv Setting */
-                        tmp |= ((unsigned int)ioarg->buffer) & 0xff; /* add new clockdiv setting */
-                        SPW_WRITE(&pDev->regs->clkdiv, tmp);
-                        if (SPW_READ(&pDev->regs->clkdiv) != tmp) {
-                                return RTEMS_IO_ERROR;
-                        }
-                        pDev->config.clkdiv = tmp;
-                        break;
-                case SPACEWIRE_IOCTRL_SET_CLKDIVSTART:
-                        SPACEWIRE_DBGC(DBGSPW_IOCTRL,"SPACEWIRE_IOCTRL_SET_CLKDIVSTART %i\n", (unsigned int)ioarg->buffer);
-                        if ((unsigned int)ioarg->buffer > 255) {
-                                return RTEMS_INVALID_NAME;
-                        }
-                        tmp = SPW_READ(&pDev->regs->clkdiv);
-                        tmp &= ~0xff00; /* Remove old Clockdiv Start Setting */
-                        tmp |= (((unsigned int)ioarg->buffer) & 0xff)<<8; /* add new clockdiv start setting */
-                        SPW_WRITE(&pDev->regs->clkdiv, tmp);
-                        if (SPW_READ(&pDev->regs->clkdiv) != tmp) {
-                                return RTEMS_IO_ERROR;
-                        }
-                        pDev->config.clkdiv = tmp;
-                        break;
-                case SPACEWIRE_IOCTRL_SET_TIMER:
-                        SPACEWIRE_DBGC(DBGSPW_IOCTRL,"SPACEWIRE_IOCTRL_SET_TIMER %i\n", (unsigned int)ioarg->buffer);
-                        if ( pDev->core_ver <= 1 ) {
-                          if ((unsigned int)ioarg->buffer > 4095) {
-                                  return RTEMS_INVALID_NAME;
-                          }
-                          SPW_WRITE(&pDev->regs->timer, (SPW_READ(&pDev->regs->timer) & 0xFFFFF000) | ((unsigned int)ioarg->buffer & 0xFFF));
-                          if ((SPW_READ(&pDev->regs->timer) & 0xFFF) != (unsigned int)ioarg->buffer) {
-                                  return RTEMS_IO_ERROR;
-                          }
-                          pDev->config.timer = (unsigned int)ioarg->buffer;
-                        }else{
-                          SPACEWIRE_DBG("SPACEWIRE_IOCTRL_SET_TIMER: removed in GRSPW2 HW\n");
-                        }
-                        break;
-                case SPACEWIRE_IOCTRL_SET_DISCONNECT:
-                        SPACEWIRE_DBGC(DBGSPW_IOCTRL,"SPACEWIRE_IOCTRL_SET_DISCONNECT %i\n", (unsigned int)ioarg->buffer);
-                        if ( pDev->core_ver <= 1 ) {
-                          if ((unsigned int)ioarg->buffer > 1023) {
-                                  return RTEMS_INVALID_NAME;
-                          }
-                          SPW_WRITE(&pDev->regs->timer, (SPW_READ(&pDev->regs->timer) & 0xFFC00FFF) | (((unsigned int)ioarg->buffer & 0x3FF) << 12));
-                          if (((SPW_READ(&pDev->regs->timer) >> 12) & 0x3FF) != (unsigned int)ioarg->buffer) {
-                                  return RTEMS_IO_ERROR;
-                          }
-                          pDev->config.disconnect = (unsigned int)ioarg->buffer;
-                        }else{
-                          SPACEWIRE_DBG("SPACEWIRE_IOCTRL_SET_DISCONNECT: not implemented for GRSPW2\n");
-                        }
-                        break;
-                case SPACEWIRE_IOCTRL_SET_PROMISCUOUS:
-                        SPACEWIRE_DBGC(DBGSPW_IOCTRL,"SPACEWIRE_IOCTRL_SET_PROMISCUOUS %i \n", (unsigned int)ioarg->buffer);
-                        if ((unsigned int)ioarg->buffer > 1) {
-                                return RTEMS_INVALID_NAME;
-                        }
-                        SPW_CTRL_WRITE(pDev, SPW_CTRL_READ(pDev) | ((unsigned int)ioarg->buffer << 5));
-                        if (((SPW_CTRL_READ(pDev) >> 5) & 1) != (unsigned int)ioarg->buffer) {
-                                return RTEMS_IO_ERROR;
-                        }
-                        pDev->config.promiscuous = (unsigned int)ioarg->buffer;
-                        break;
-                case SPACEWIRE_IOCTRL_SET_RMAPEN:
-                        SPACEWIRE_DBGC(DBGSPW_IOCTRL,"SPACEWIRE_IOCTRL_SET_RMAPEN %i \n", (unsigned int)ioarg->buffer);
-                        if ((unsigned int)ioarg->buffer > 1) {
-                                return RTEMS_INVALID_NAME;
-                        }
-                        SPW_CTRL_WRITE(pDev, (SPW_STATUS_READ(pDev) & 0xFFFEFFFF) | ((unsigned int)ioarg->buffer << 16));
-                        if (((SPW_CTRL_READ(pDev) >> 16) & 1) != (unsigned int)ioarg->buffer) {
-                                return RTEMS_IO_ERROR;
-                        }
-                        pDev->config.rmapen = (unsigned int)ioarg->buffer;
-                        break;
-                case SPACEWIRE_IOCTRL_SET_RMAPBUFDIS:
-                        SPACEWIRE_DBGC(DBGSPW_IOCTRL,"SPACEWIRE_IOCTRL_SET_RMAPBUFDIS %i \n", (unsigned int)ioarg->buffer);
-                        if ((unsigned int)ioarg->buffer > 1) {
-                                return RTEMS_INVALID_NAME;
-                        }
-                        SPW_CTRL_WRITE(pDev, (SPW_STATUS_READ(pDev) & 0xFFFDFFFF) | ((unsigned int)ioarg->buffer << 17));
-                        if (((SPW_CTRL_READ(pDev) >> 17) & 1) != (unsigned int)ioarg->buffer) {
-                                return RTEMS_IO_ERROR;
-                        }
-                        pDev->config.rmapbufdis = (unsigned int)ioarg->buffer;
-                        break;
-                case SPACEWIRE_IOCTRL_SET_CHECK_RMAP:
-                        SPACEWIRE_DBGC(DBGSPW_IOCTRL,"SPACEWIRE_IOCTRL_SET_CHECK_RMAP %i \n", (unsigned int)ioarg->buffer);
-                        if ((unsigned int)ioarg->buffer > 1) {
-                                return RTEMS_INVALID_NAME;
-                        }
-                        pDev->config.check_rmap_err = (unsigned int)ioarg->buffer;
-                        break;
-                case SPACEWIRE_IOCTRL_SET_RM_PROT_ID:
-                        SPACEWIRE_DBGC(DBGSPW_IOCTRL, "SPACEWIRE_IOCTRL_SET_RM_PROT_ID %i \n", (unsigned int)ioarg->buffer);
-                        if ((unsigned int)ioarg->buffer > 1) {
-                                return RTEMS_INVALID_NAME;
-                        }
-                        pDev->config.rm_prot_id = (unsigned int)ioarg->buffer;
-                        break;
-                case SPACEWIRE_IOCTRL_SET_TXBLOCK:
-                        SPACEWIRE_DBGC(DBGSPW_IOCTRL, "SPACEWIRE_IOCTRL_SET_TXBLOCK %i \n", (unsigned int)ioarg->buffer);
-                        if ((unsigned int)ioarg->buffer > 1) {
-                                return RTEMS_INVALID_NAME;
-                        }
-                        pDev->config.tx_blocking = (unsigned int)ioarg->buffer;
-                        break;
-                case SPACEWIRE_IOCTRL_SET_TXBLOCK_ON_FULL:
-                        SPACEWIRE_DBGC(DBGSPW_IOCTRL, "SPACEWIRE_IOCTRL_SET_TXBLOCK_ON_FULL %i \n", (unsigned int)ioarg->buffer);
-                        if ((unsigned int)ioarg->buffer > 1) {
-                                return RTEMS_INVALID_NAME;
-                        }
-                        pDev->config.tx_block_on_full = (unsigned int)ioarg->buffer;
-                        break;
-                case SPACEWIRE_IOCTRL_SET_DISABLE_ERR:
-                        SPACEWIRE_DBGC(DBGSPW_IOCTRL, "SPACEWIRE_IOCTRL_SET_DISABLE_ERR %i \n", (unsigned int)ioarg->buffer);
-                        if ((unsigned int)ioarg->buffer > 1) {
-                                return RTEMS_INVALID_NAME;
-                        }
-                        pDev->config.disable_err = (unsigned int)ioarg->buffer;
-                        break;
-                case SPACEWIRE_IOCTRL_SET_LINK_ERR_IRQ:
-                        SPACEWIRE_DBGC(DBGSPW_IOCTRL, "SPACEWIRE_IOCTRL_SET_LINK_ERR_IRQ %i \n", (unsigned int)ioarg->buffer);
-                        SPACEWIRE_DBGC(DBGSPW_IOCTRL, "CTRL REG: %x\n", SPW_CTRL_READ(pDev));
-                        if ((unsigned int)ioarg->buffer > 1) {
-                                return RTEMS_INVALID_NAME;
-                        }
-                        SPW_CTRL_WRITE(pDev, (SPW_CTRL_READ(pDev) & 0xFFFFFDF7) | ((unsigned int)ioarg->buffer << 9) | (pDev->config.link_err_irq << 3));
-                        SPACEWIRE_DBGC(DBGSPW_IOCTRL, "CTRL REG: %x\n", SPW_CTRL_READ(pDev));
-                        if (((SPW_CTRL_READ(pDev) >> 9) & 1) != (unsigned int)ioarg->buffer) {
-                                return RTEMS_IO_ERROR;
-                        }
-                        pDev->config.link_err_irq = (unsigned int)ioarg->buffer;
-                        break;
-                case SPACEWIRE_IOCTRL_SET_EVENT_ID:
-                        SPACEWIRE_DBGC(DBGSPW_IOCTRL, "SPACEWIRE_IOCTRL_SET_EVENT_ID %i \n", (unsigned int)ioarg->buffer);
-                        pDev->config.event_id = (rtems_id)ioarg->buffer;
-                        SPACEWIRE_DBGC(DBGSPW_IOCTRL, "Event id: %i\n", pDev->config.event_id);
-                        break;
-
-                /* Change MAX Packet size by:
-                 *  - stop RX/TX (if on)
-                 *  - wait for hw to complete RX DMA (if on)
-                 *  - reallocate buffers with new size
-                 *  - tell hw about new size & start RX/TX again (if previously on)
-                 */
-                case SPACEWIRE_IOCTRL_SET_PACKETSIZE:
-                        if (ioarg->buffer == NULL)
-                                return RTEMS_INVALID_NAME;
-                        ps = (spw_ioctl_packetsize*) ioarg->buffer;
-                        SPACEWIRE_DBGC(DBGSPW_IOCTRL,"SPACEWIRE_IOCTRL_SET_RXPACKETSIZE %i \n", (unsigned int)ioarg->buffer);
-
-                        tmp = pDev->running;
-
-                        if ( pDev->running ){
-                                /* Stop RX */
-                                grspw_hw_stop(pDev,1,1);
-
-                                /* If packetsize fails it is good to know if in running mode */
-                                pDev->running = 0;
-
-                                /* Wait for Receiver to finnish pending DMA transfers if any */
-                                grspw_hw_wait_rx_inactive(pDev);
-                        }
-
-                        /* Save new buffer sizes */
-                        pDev->rxbufsize = ps->rxsize;
-                        pDev->txdbufsize = ps->txdsize;
-                        pDev->txhbufsize = ps->txhsize;
-                        pDev->config.rxmaxlen = pDev->rxbufsize;
-
-                        /* Free previous buffers & allocate buffers with new size */
-                        if (grspw_buffer_alloc(pDev))
-                                return RTEMS_NO_MEMORY;
-
-                        /* if RX was actived before, we reactive it again */
-                        if ( tmp ) {
-                                if ( (status = grspw_hw_startup(pDev,-1)) != RTEMS_SUCCESSFUL ) {
-                                        return status;
-                                }
-                                pDev->running = 1;
-                        }
+	spw_ioctl_pkt_send *args;
+	spw_ioctl_packetsize *ps;
+	int status;
+	unsigned int tmp,mask,nodeaddr,nodemask;
+	int timeout;
+	rtems_device_driver ret;
+	rtems_libio_ioctl_args_t *ioarg = (rtems_libio_ioctl_args_t *) arg;
+	GRSPW_DEV *pDev;
+	struct drvmgr_dev *dev;
+
+	SPACEWIRE_DBGC(DBGSPW_IOCALLS, "ctrl [%i,%i]\n", major, minor);
+
+	if ( drvmgr_get_dev(&grspw_drv_info.general, minor, &dev) ) {
+		return RTEMS_INVALID_NAME;
+	}
+	pDev = (GRSPW_DEV *)dev->priv;
+
+	if (!ioarg)
+		return RTEMS_INVALID_NAME;
+
+	ioarg->ioctl_return = 0;
+	switch(ioarg->command) {
+		case SPACEWIRE_IOCTRL_SET_NODEADDR:
+			/*set node address*/
+			SPACEWIRE_DBGC(DBGSPW_IOCTRL, "SPACEWIRE_IOCTRL_SET_NODEADDR %i\n",(unsigned int)ioarg->buffer);
+			if ((unsigned int)ioarg->buffer > 255) {
+				return RTEMS_INVALID_NAME;
+			}
+			nodeaddr = ((unsigned int)ioarg->buffer) & 0xff;
+			tmp = SPW_READ(&pDev->regs->nodeaddr);
+			tmp &= 0xffffff00; /* Remove old address */
+			tmp |= nodeaddr;
+			SPW_WRITE(&pDev->regs->nodeaddr, tmp);
+			if ((SPW_READ(&pDev->regs->nodeaddr)&0xff) != nodeaddr) {
+				return RTEMS_IO_ERROR;
+			}
+			pDev->config.nodeaddr = nodeaddr;
+			break;
+		case SPACEWIRE_IOCTRL_SET_NODEMASK:
+			/*set node address*/
+			SPACEWIRE_DBGC(DBGSPW_IOCTRL, "SPACEWIRE_IOCTRL_SET_NODEMASK %i\n",(unsigned int)ioarg->buffer);
+			if ( pDev->core_ver > 1 ){
+				if ((unsigned int)ioarg->buffer > 255) {
+					return RTEMS_INVALID_NAME;
+				}
+				nodemask = ((unsigned int)ioarg->buffer) & 0xff;
+				tmp = SPW_READ(&pDev->regs->nodeaddr);
+				tmp &= 0xffff00ff; /* Remove old mask */
+				tmp |= nodemask<<8;
+				SPW_WRITE(&pDev->regs->nodeaddr, tmp);
+				if (((SPW_READ(&pDev->regs->nodeaddr)>>8)&0xff) != nodemask) {
+					return RTEMS_IO_ERROR;
+				}
+				pDev->config.nodemask = nodemask;
+			}else{
+				SPACEWIRE_DBG("SPACEWIRE_IOCTRL_SET_NODEMASK: not implemented in GRSPW1 HW\n");
+			}
+			break;
+		case SPACEWIRE_IOCTRL_SET_RXBLOCK:
+			SPACEWIRE_DBGC(DBGSPW_IOCTRL, "SPACEWIRE_IOCTRL_SET_RXBLOCK %i \n", (unsigned int)ioarg->buffer);
+			if ((unsigned int)ioarg->buffer > 1) {
+				return RTEMS_INVALID_NAME;
+			}
+			pDev->config.rx_blocking = (unsigned int)ioarg->buffer;
+			break;
+		case SPACEWIRE_IOCTRL_SET_DESTKEY:
+			SPACEWIRE_DBGC(DBGSPW_IOCTRL,"SPACEWIRE_IOCTRL_SET_DESTKEY %i\n", (unsigned int)ioarg->buffer);
+			if (!pDev->config.is_rmap) {
+				return RTEMS_NOT_IMPLEMENTED;
+			}
+			if ((unsigned int)ioarg->buffer > 255) {
+				return RTEMS_INVALID_NAME;
+			}
+			SPW_WRITE(&pDev->regs->destkey, (unsigned int)ioarg->buffer);
+			if (SPW_READ(&pDev->regs->destkey) != (unsigned int)ioarg->buffer) {
+				return RTEMS_IO_ERROR;
+			}
+			pDev->config.destkey = (unsigned int)ioarg->buffer;
+			break;
+		case SPACEWIRE_IOCTRL_SET_CLKDIV:
+			SPACEWIRE_DBGC(DBGSPW_IOCTRL,"SPACEWIRE_IOCTRL_SET_CLKDIV %i\n", (unsigned int)ioarg->buffer);
+			if ((unsigned int)ioarg->buffer > 255) {
+				return RTEMS_INVALID_NAME;
+			}
+			if ( pDev->core_ver == 3 )
+				break;
+			tmp = SPW_READ(&pDev->regs->clkdiv);
+			tmp &= ~0xff; /* Remove old Clockdiv Setting */
+			tmp |= ((unsigned int)ioarg->buffer) & 0xff; /* add new clockdiv setting */
+			SPW_WRITE(&pDev->regs->clkdiv, tmp);
+			if (SPW_READ(&pDev->regs->clkdiv) != tmp) {
+				return RTEMS_IO_ERROR;
+			}
+			pDev->config.clkdiv = tmp;
+			break;
+		case SPACEWIRE_IOCTRL_SET_CLKDIVSTART:
+			SPACEWIRE_DBGC(DBGSPW_IOCTRL,"SPACEWIRE_IOCTRL_SET_CLKDIVSTART %i\n", (unsigned int)ioarg->buffer);
+			if ((unsigned int)ioarg->buffer > 255) {
+				return RTEMS_INVALID_NAME;
+			}
+			if ( pDev->core_ver == 3 )
+				break;
+			tmp = SPW_READ(&pDev->regs->clkdiv);
+			tmp &= ~0xff00; /* Remove old Clockdiv Start Setting */
+			tmp |= (((unsigned int)ioarg->buffer) & 0xff)<<8; /* add new clockdiv start setting */
+			SPW_WRITE(&pDev->regs->clkdiv, tmp);
+			if (SPW_READ(&pDev->regs->clkdiv) != tmp) {
+				return RTEMS_IO_ERROR;
+			}
+			pDev->config.clkdiv = tmp;
+			break;			
+		case SPACEWIRE_IOCTRL_SET_TIMER:
+			SPACEWIRE_DBGC(DBGSPW_IOCTRL,"SPACEWIRE_IOCTRL_SET_TIMER %i\n", (unsigned int)ioarg->buffer);
+			if ( pDev->core_ver <= 1 ) {
+				if ((unsigned int)ioarg->buffer > 4095) {
+					return RTEMS_INVALID_NAME;
+				}
+				SPW_WRITE(&pDev->regs->timer, (SPW_READ(&pDev->regs->timer) & 0xFFFFF000) | ((unsigned int)ioarg->buffer & 0xFFF));
+				if ((SPW_READ(&pDev->regs->timer) & 0xFFF) != (unsigned int)ioarg->buffer) {
+					return RTEMS_IO_ERROR;
+				}
+				pDev->config.timer = (unsigned int)ioarg->buffer;
+			}else{
+				SPACEWIRE_DBG("SPACEWIRE_IOCTRL_SET_TIMER: removed in GRSPW2 HW\n");
+			}
+			break;
+		case SPACEWIRE_IOCTRL_SET_DISCONNECT:
+			SPACEWIRE_DBGC(DBGSPW_IOCTRL,"SPACEWIRE_IOCTRL_SET_DISCONNECT %i\n", (unsigned int)ioarg->buffer);
+			if ( pDev->core_ver <= 1 ) {
+				if ((unsigned int)ioarg->buffer > 1023) {
+					return RTEMS_INVALID_NAME;
+				}
+				SPW_WRITE(&pDev->regs->timer, (SPW_READ(&pDev->regs->timer) & 0xFFC00FFF) | (((unsigned int)ioarg->buffer & 0x3FF) << 12));
+				if (((SPW_READ(&pDev->regs->timer) >> 12) & 0x3FF) != (unsigned int)ioarg->buffer) {
+					return RTEMS_IO_ERROR;
+				}
+				pDev->config.disconnect = (unsigned int)ioarg->buffer;
+			}else{
+				SPACEWIRE_DBG("SPACEWIRE_IOCTRL_SET_DISCONNECT: not implemented for GRSPW2\n");
+			}
+			break;
+		case SPACEWIRE_IOCTRL_SET_PROMISCUOUS:	
+			SPACEWIRE_DBGC(DBGSPW_IOCTRL,"SPACEWIRE_IOCTRL_SET_PROMISCUOUS %i \n", (unsigned int)ioarg->buffer);
+			if ((unsigned int)ioarg->buffer > 1) {
+				return RTEMS_INVALID_NAME;
+			}
+			SPW_CTRL_WRITE(pDev, SPW_CTRL_READ(pDev) | ((unsigned int)ioarg->buffer << 5));
+			if (((SPW_CTRL_READ(pDev) >> 5) & 1) != (unsigned int)ioarg->buffer) {
+				return RTEMS_IO_ERROR;
+			}
+			pDev->config.promiscuous = (unsigned int)ioarg->buffer;
+			break;
+		case SPACEWIRE_IOCTRL_SET_RMAPEN:
+			SPACEWIRE_DBGC(DBGSPW_IOCTRL,"SPACEWIRE_IOCTRL_SET_RMAPEN %i \n", (unsigned int)ioarg->buffer);
+			if ((unsigned int)ioarg->buffer > 1) {
+				return RTEMS_INVALID_NAME;
+			}
+			SPW_CTRL_WRITE(pDev, (SPW_STATUS_READ(pDev) & 0xFFFEFFFF) | ((unsigned int)ioarg->buffer << 16));
+			if (((SPW_CTRL_READ(pDev) >> 16) & 1) != (unsigned int)ioarg->buffer) {
+				return RTEMS_IO_ERROR;
+			}
+			pDev->config.rmapen = (unsigned int)ioarg->buffer;
+			break;
+		case SPACEWIRE_IOCTRL_SET_RMAPBUFDIS: 
+			SPACEWIRE_DBGC(DBGSPW_IOCTRL,"SPACEWIRE_IOCTRL_SET_RMAPBUFDIS %i \n", (unsigned int)ioarg->buffer);
+			if ((unsigned int)ioarg->buffer > 1) {
+				return RTEMS_INVALID_NAME;
+			}
+			SPW_CTRL_WRITE(pDev, (SPW_STATUS_READ(pDev) & 0xFFFDFFFF) | ((unsigned int)ioarg->buffer << 17));
+			if (((SPW_CTRL_READ(pDev) >> 17) & 1) != (unsigned int)ioarg->buffer) {
+				return RTEMS_IO_ERROR;
+			}
+			pDev->config.rmapbufdis = (unsigned int)ioarg->buffer;
+			break;
+		case SPACEWIRE_IOCTRL_SET_CHECK_RMAP: 
+			SPACEWIRE_DBGC(DBGSPW_IOCTRL,"SPACEWIRE_IOCTRL_SET_CHECK_RMAP %i \n", (unsigned int)ioarg->buffer);
+			if ((unsigned int)ioarg->buffer > 1) {
+				return RTEMS_INVALID_NAME;
+			}
+			pDev->config.check_rmap_err = (unsigned int)ioarg->buffer;
+			break;
+		case SPACEWIRE_IOCTRL_SET_RM_PROT_ID: 
+			SPACEWIRE_DBGC(DBGSPW_IOCTRL, "SPACEWIRE_IOCTRL_SET_RM_PROT_ID %i \n", (unsigned int)ioarg->buffer);
+			if ((unsigned int)ioarg->buffer > 1) {
+				return RTEMS_INVALID_NAME;
+			}
+			pDev->config.rm_prot_id = (unsigned int)ioarg->buffer;
+			break;
+		case SPACEWIRE_IOCTRL_SET_KEEP_SOURCE: 
+			SPACEWIRE_DBGC(DBGSPW_IOCTRL, "SPACEWIRE_IOCTRL_SET_KEEP_SOURCE %i \n", (unsigned int)ioarg->buffer);
+			if ((unsigned int)ioarg->buffer > 1) {
+				return RTEMS_INVALID_NAME;
+			}
+			pDev->config.keep_source = (unsigned int)ioarg->buffer;
+			break;
+		case SPACEWIRE_IOCTRL_SET_TXBLOCK: 
+			SPACEWIRE_DBGC(DBGSPW_IOCTRL, "SPACEWIRE_IOCTRL_SET_TXBLOCK %i \n", (unsigned int)ioarg->buffer);
+			if ((unsigned int)ioarg->buffer > 1) {
+				return RTEMS_INVALID_NAME;
+			}
+			pDev->config.tx_blocking = (unsigned int)ioarg->buffer;
+			break;
+		case SPACEWIRE_IOCTRL_SET_TXBLOCK_ON_FULL: 
+			SPACEWIRE_DBGC(DBGSPW_IOCTRL, "SPACEWIRE_IOCTRL_SET_TXBLOCK_ON_FULL %i \n", (unsigned int)ioarg->buffer);
+			if ((unsigned int)ioarg->buffer > 1) {
+				return RTEMS_INVALID_NAME;
+			}
+			pDev->config.tx_block_on_full = (unsigned int)ioarg->buffer;
+			break;	
+		case SPACEWIRE_IOCTRL_SET_DISABLE_ERR: 
+			SPACEWIRE_DBGC(DBGSPW_IOCTRL, "SPACEWIRE_IOCTRL_SET_DISABLE_ERR %i \n", (unsigned int)ioarg->buffer);
+			if ((unsigned int)ioarg->buffer > 1) {
+				return RTEMS_INVALID_NAME;
+			}
+			pDev->config.disable_err = (unsigned int)ioarg->buffer;
+			break;
+		case SPACEWIRE_IOCTRL_SET_LINK_ERR_IRQ: 
+			SPACEWIRE_DBGC(DBGSPW_IOCTRL, "SPACEWIRE_IOCTRL_SET_LINK_ERR_IRQ %i \n", (unsigned int)ioarg->buffer);
+			SPACEWIRE_DBGC(DBGSPW_IOCTRL, "CTRL REG: %x\n", SPW_CTRL_READ(pDev));
+			if ((unsigned int)ioarg->buffer > 1) {
+				return RTEMS_INVALID_NAME;
+			}
+			tmp = (SPW_CTRL_READ(pDev) & 0xFFFFFDF7) | ((unsigned int)ioarg->buffer << 9);
+			if (tmp & (SPW_CTRL_LI|SPW_CTRL_TQ))
+				tmp |= SPW_CTRL_IE;
+			SPW_CTRL_WRITE(pDev, tmp);
+			SPACEWIRE_DBGC(DBGSPW_IOCTRL, "CTRL REG: %x\n", SPW_CTRL_READ(pDev));
+			if (((SPW_CTRL_READ(pDev) >> 9) & 1) != (unsigned int)ioarg->buffer) {
+				return RTEMS_IO_ERROR;
+			}
+			pDev->config.link_err_irq = (unsigned int)ioarg->buffer;
+			break;
+		case SPACEWIRE_IOCTRL_SET_EVENT_ID:
+			SPACEWIRE_DBGC(DBGSPW_IOCTRL, "SPACEWIRE_IOCTRL_SET_EVENT_ID %i \n", (unsigned int)ioarg->buffer);
+			pDev->config.event_id = (rtems_id)ioarg->buffer;
+			SPACEWIRE_DBGC(DBGSPW_IOCTRL, "Event id: %i\n", pDev->config.event_id);
+			break;
+
+		/* Change MAX Packet size by:
+		 *  - stop RX/TX (if on)
+		 *  - wait for hw to complete RX DMA (if on)
+		 *  - reallocate buffers with new size
+		 *  - tell hw about new size & start RX/TX again (if previously on)
+		 */
+		case SPACEWIRE_IOCTRL_SET_PACKETSIZE:
+			if (ioarg->buffer == NULL)
+				return RTEMS_INVALID_NAME;
+			ps = (spw_ioctl_packetsize*) ioarg->buffer;
+			SPACEWIRE_DBGC(DBGSPW_IOCTRL,"SPACEWIRE_IOCTRL_SET_RXPACKETSIZE %i \n", (unsigned int)ioarg->buffer);
+
+			tmp = pDev->running;
+
+			if ( pDev->running ){
+				/* Stop RX */
+				grspw_hw_stop(pDev,1,1); 
+
+				/* If packetsize fails it is good to know if in running mode */
+				pDev->running = 0;
+
+				/* Wait for Receiver to finnish pending DMA transfers if any */
+				grspw_hw_wait_rx_inactive(pDev);
+			}
+
+			/* Save new buffer sizes */
+			pDev->rxbufsize = ps->rxsize;
+			pDev->txdbufsize = ps->txdsize;
+			pDev->txhbufsize = ps->txhsize;
+			pDev->config.rxmaxlen = pDev->rxbufsize;
+
+			/* Free previous buffers & allocate buffers with new size */
+			if (grspw_buffer_alloc(pDev)) 
+				return RTEMS_NO_MEMORY;
+
+			/* if RX was actived before, we reactive it again */
+			if ( tmp ) {
+				if ( (status = grspw_hw_startup(pDev,-1)) != RTEMS_SUCCESSFUL ) {
+					return status;
+				}
+				pDev->running = 1;
+			}
 #if 0
-                        /* Rewrite previous config which was wasted due to reset in hw_startup */
-                        SPW_WRITE(&pDev->regs->nodeaddr, pDev->config.nodeaddr);
-                        SPW_WRITE(&pDev->regs->destkey, pDev->config.destkey);
-                        SPW_WRITE(&pDev->regs->clkdiv, pDev->config.clkdiv);
-                        SPW_WRITE(&pDev->regs->timer, pDev->config.timer | ( (pDev->config.disconnect & 0x3FF) << 12) );
-                        SPW_CTRL_WRITE(pDev, (SPW_CTRL_READ(pDev) & !(SPW_CTRL_LINKSTART | SPW_CTRL_PM | SPW_CTRL_RE | SPW_CTRL_RD | SPW_CTRL_TT | SPW_CTRL_TR)) | \
-                                              (pDev->config.promiscuous << 5) | (pDev->config.rmapen << 16) | (pDev->config.rmapbufdis << 17) | \
-                                              (pDev->config.linkdisabled) | (pDev->config.linkstart << 1));
+			/* Rewrite previous config which was wasted due to reset in hw_startup */
+			SPW_WRITE(&pDev->regs->nodeaddr, pDev->config.nodeaddr);
+			SPW_WRITE(&pDev->regs->destkey, pDev->config.destkey);
+			SPW_WRITE(&pDev->regs->clkdiv, pDev->config.clkdiv);
+			SPW_WRITE(&pDev->regs->timer, pDev->config.timer | ( (pDev->config.disconnect & 0x3FF) << 12) );
+			SPW_CTRL_WRITE(pDev, (SPW_CTRL_READ(pDev) & !(SPW_CTRL_LINKSTART | SPW_CTRL_PM | SPW_CTRL_RE | SPW_CTRL_RD | SPW_CTRL_TT | SPW_CTRL_TR)) | \
+					      (pDev->config.promiscuous << 5) | (pDev->config.rmapen << 16) | (pDev->config.rmapbufdis << 17) | \
+					      (pDev->config.linkdisabled) | (pDev->config.linkstart << 1));
 #endif
-                        break;
-                case SPACEWIRE_IOCTRL_GET_CONFIG:
-                        if (ioarg->buffer == NULL)
-                                return RTEMS_INVALID_NAME;
-                        SPACEWIRE_DBG2("SPACEWIRE_IOCTRL_GET_CONFIG \n");
-                        (*(spw_config *)ioarg->buffer).nodeaddr = pDev->config.nodeaddr;
-                        (*(spw_config *)ioarg->buffer).nodemask = pDev->config.nodemask;
-                        (*(spw_config *)ioarg->buffer).destkey = pDev->config.destkey;
-                        (*(spw_config *)ioarg->buffer).clkdiv = pDev->config.clkdiv;
-                        (*(spw_config *)ioarg->buffer).rxmaxlen = pDev->config.rxmaxlen;
-                        (*(spw_config *)ioarg->buffer).timer = pDev->config.timer;
-                        (*(spw_config *)ioarg->buffer).disconnect = pDev->config.disconnect;
-                        (*(spw_config *)ioarg->buffer).promiscuous = pDev->config.promiscuous;
-                        (*(spw_config *)ioarg->buffer).rmapen = pDev->config.rmapen;
-                        (*(spw_config *)ioarg->buffer).rmapbufdis = pDev->config.rmapbufdis;
-                        (*(spw_config *)ioarg->buffer).check_rmap_err = pDev->config.check_rmap_err;
-                        (*(spw_config *)ioarg->buffer).rm_prot_id = pDev->config.rm_prot_id;
-                        (*(spw_config *)ioarg->buffer).tx_blocking = pDev->config.tx_blocking;
-                        (*(spw_config *)ioarg->buffer).disable_err = pDev->config.disable_err;
-                        (*(spw_config *)ioarg->buffer).link_err_irq = pDev->config.link_err_irq;
-                        (*(spw_config *)ioarg->buffer).event_id = pDev->config.event_id;
-                        (*(spw_config *)ioarg->buffer).is_rmap = pDev->config.is_rmap;
-                        (*(spw_config *)ioarg->buffer).is_rmapcrc = pDev->config.is_rmapcrc;
-                        (*(spw_config *)ioarg->buffer).is_rxunaligned = pDev->config.is_rxunaligned;
-                        (*(spw_config *)ioarg->buffer).linkdisabled = pDev->config.linkdisabled;
-                        (*(spw_config *)ioarg->buffer).linkstart = pDev->config.linkstart;
-                        (*(spw_config *)ioarg->buffer).rx_blocking = pDev->config.rx_blocking;
-                        (*(spw_config *)ioarg->buffer).tx_block_on_full = pDev->config.tx_block_on_full;
-                        break;
-                case SPACEWIRE_IOCTRL_GET_LINK_STATUS:
-                        SPACEWIRE_DBGC(DBGSPW_IOCTRL,"SPACEWIRE_IOCTRL_GET_STATUS=%i \n", (unsigned int)((SPW_STATUS_READ(pDev) >> 21) & 0x7));
-                        *(unsigned int *)ioarg->buffer = (unsigned int )((SPW_STATUS_READ(pDev) >> 21) & 0x7);
-                        break;
-                case SPACEWIRE_IOCTRL_GET_STATISTICS:
-                        if (ioarg->buffer == NULL)
-                                return RTEMS_INVALID_NAME;
-                        SPACEWIRE_DBG2("SPACEWIRE_IOCTRL_GET_STATISTICS \n");
-                        (*(spw_stats *)ioarg->buffer).tx_link_err = pDev->stat.tx_link_err;
-                        (*(spw_stats *)ioarg->buffer).rx_rmap_header_crc_err = pDev->stat.rx_rmap_header_crc_err;
-                        (*(spw_stats *)ioarg->buffer).rx_rmap_data_crc_err = pDev->stat.rx_rmap_data_crc_err;
-                        (*(spw_stats *)ioarg->buffer).rx_eep_err =  pDev->stat.rx_eep_err;
-                        (*(spw_stats *)ioarg->buffer).rx_truncated = pDev->stat.rx_truncated;
-                        (*(spw_stats *)ioarg->buffer).parity_err = pDev->stat.parity_err;
-                        (*(spw_stats *)ioarg->buffer).escape_err = pDev->stat.escape_err;
-                        (*(spw_stats *)ioarg->buffer).credit_err = pDev->stat.credit_err;
-                        (*(spw_stats *)ioarg->buffer).write_sync_err = pDev->stat.write_sync_err;
-                        (*(spw_stats *)ioarg->buffer).disconnect_err = pDev->stat.disconnect_err;
-                        (*(spw_stats *)ioarg->buffer).early_ep = pDev->stat.early_ep;
-                        (*(spw_stats *)ioarg->buffer).invalid_address = pDev->stat.invalid_address;
-                        (*(spw_stats *)ioarg->buffer).packets_sent = pDev->stat.packets_sent;
-                        (*(spw_stats *)ioarg->buffer).packets_received = pDev->stat.packets_received;
-                        break;
-                case SPACEWIRE_IOCTRL_CLR_STATISTICS:
-                        SPACEWIRE_DBG2("SPACEWIRE_IOCTRL_CLR_STATISTICS \n");
-                        pDev->stat.tx_link_err = 0;
-                        pDev->stat.rx_rmap_header_crc_err = 0;
-                        pDev->stat.rx_rmap_data_crc_err = 0;
-                        pDev->stat.rx_eep_err = 0;
-                        pDev->stat.rx_truncated = 0;
-                        pDev->stat.parity_err = 0;
-                        pDev->stat.escape_err = 0;
-                        pDev->stat.credit_err = 0;
-                        pDev->stat.write_sync_err = 0;
-                        pDev->stat.disconnect_err = 0;
-                        pDev->stat.early_ep = 0;
-                        pDev->stat.invalid_address = 0;
-                        pDev->stat.packets_sent = 0;
-                        pDev->stat.packets_received = 0;
-                        break;
-                case SPACEWIRE_IOCTRL_SEND:
-                        if (ioarg->buffer == NULL)
-                                return RTEMS_INVALID_NAME;
-                        args = (spw_ioctl_pkt_send *)ioarg->buffer;
-                        args->sent = 0;
-
-                        /* is link up? */
-                        if ( !pDev->running ) {
-                                return RTEMS_INVALID_NAME;
-                        }
-
-                        SPACEWIRE_DBGC(DBGSPW_IOCALLS, "write [%i,%i]: hlen: %i hbuf:0x%x dlen:%i dbuf:0x%x\n", major, minor,
-                                       (unsigned int)args->hlen, (int)args->hdr,(unsigned int)args->dlen, (int)args->data);
-
-                        if ((args->hlen > pDev->txhbufsize) || (args->dlen > pDev->txdbufsize) ||
-                            ((args->hlen+args->dlen) < 1) ||
-                            ((args->hdr == NULL) && (args->hlen != 0)) || ((args->data == NULL) && (args->dlen != 0))) {
-                                return RTEMS_INVALID_NAME;
-                        }
-                        while ((args->sent = grspw_hw_send(pDev, args->hlen, args->hdr, args->dlen, args->data)) == 0) {
-                                if (pDev->config.tx_block_on_full == 1) {
-                                        SPACEWIRE_DBG2("Tx Block on full \n");
-                                        rtems_semaphore_obtain(pDev->txsp, RTEMS_WAIT, RTEMS_NO_TIMEOUT);
-                                } else {
-                                        SPACEWIRE_DBG2("Tx non blocking return when full \n");
-                                        return RTEMS_RESOURCE_IN_USE;
-                                }
-                        }
-                        SPACEWIRE_DBGC(DBGSPW_IOCALLS, "Tx ioctl return: %i  \n", args->sent);
-                        break;
-
-                case SPACEWIRE_IOCTRL_LINKDISABLE:
-                        pDev->config.linkdisabled = 1;
-                        pDev->config.linkstart = 0;
-                        SPW_CTRL_WRITE(pDev, (SPW_CTRL_READ(pDev) & 0xFFFFFFFC) | 1);
-                        if ((SPW_CTRL_READ(pDev) & 3) != 1) {
-                                return RTEMS_IO_ERROR;
-                        }
-                        break;
-
-                case SPACEWIRE_IOCTRL_LINKSTART:
-                        pDev->config.linkdisabled = 0;
-                        pDev->config.linkstart = 1;
-                        SPW_CTRL_WRITE(pDev, (SPW_CTRL_READ(pDev) & 0xFFFFFFFC) | 2);
-                        if ((SPW_CTRL_READ(pDev) & 3) != 2) {
-                                return RTEMS_IO_ERROR;
-                        }
-                        break;
-
-                /* Calculate timer register from GRSPW Core frequency
-                 * Also possible to set disconnect and timer64 from
-                 *  - SPACEWIRE_IOCTRL_SET_DISCONNECT
-                 *  - SPACEWIRE_IOCTRL_SET_TIMER
-                 */
-                case SPACEWIRE_IOCTRL_SET_COREFREQ:
-                        pDev->core_freq_khz = (unsigned int)ioarg->buffer;
-                        if ( pDev->core_freq_khz == 0 ){
-                                /* Get GRSPW clock frequency from system clock.
-                                 * System clock has been read from timer inited
-                                 * by RTEMS loader (mkprom)
-                                 */
-                                 pDev->core_freq_khz = sys_freq_khz;
-                        }
-
-                        /* Only GRSPW1 needs the Timer64 and Disconnect values
-                         * GRSPW2 and onwards doesn't have this register.
-                         */
-                        if ( pDev->core_ver <= 1 ){
-                          /* Calculate Timer64 & Disconnect */
-                          pDev->config.timer = grspw_calc_timer64(pDev->core_freq_khz);
-                          pDev->config.disconnect = grspw_calc_disconnect(pDev->core_freq_khz);
-
-                          /* Set Timer64 & Disconnect Register */
-                          SPW_WRITE(&pDev->regs->timer,
-                                 (SPW_READ(&pDev->regs->timer) & 0xFFC00000) |
-                                 ((pDev->config.disconnect & 0x3FF)<<12) |
-                                 (pDev->config.timer & 0xFFF));
-
-                          /* Check that the registers were written successfully */
-                          tmp = SPW_READ(&pDev->regs->timer) & 0x003fffff;
-                          if ( ((tmp & 0xFFF) != pDev->config.timer) ||
-                               (((tmp >> 12) & 0x3FF) != pDev->config.disconnect) ) {
-                                  return RTEMS_IO_ERROR;
-                          }
-                        }
-                        break;
-
-                case SPACEWIRE_IOCTRL_START:
-                        if ( pDev->running ){
-                                return RTEMS_INVALID_NAME;
-                        }
-
-                        /* Get timeout from userspace
-                         *  timeout:
-                         *   ¤  -1           = Default timeout
-                         *   ¤  less than -1 = forever
-                         *   ¤  0            = no wait, proceed if link is up
-                         *   ¤  positive     = specifies number of system clock ticks that
-                         *                     startup will wait for link to enter ready mode.
-                         */
-                        timeout = (int)ioarg->buffer;
-
-                        if ( (ret=grspw_hw_startup(pDev,timeout)) != RTEMS_SUCCESSFUL ) {
-                                return ret;
-                        }
-                        pDev->running = 1;
-                        break;
-
-                case SPACEWIRE_IOCTRL_STOP:
-                        if ( !pDev->running ){
-                                return RTEMS_INVALID_NAME;
-                        }
-                        pDev->running = 0;
-
-                        /* Stop Receiver and transmitter */
-                        grspw_hw_stop(pDev,1,1);
-                        break;
-
-                default:
-                        return RTEMS_NOT_IMPLEMENTED;
-        }
-
-        SPACEWIRE_DBGC(DBGSPW_IOCALLS, "SPW_IOCTRL Return\n");
-        return RTEMS_SUCCESSFUL;
+			break;
+		case SPACEWIRE_IOCTRL_GET_CONFIG:
+			if (ioarg->buffer == NULL)
+				return RTEMS_INVALID_NAME;
+			SPACEWIRE_DBG2("SPACEWIRE_IOCTRL_GET_CONFIG \n");
+			(*(spw_config *)ioarg->buffer).nodeaddr = pDev->config.nodeaddr;
+			(*(spw_config *)ioarg->buffer).nodemask = pDev->config.nodemask;
+			(*(spw_config *)ioarg->buffer).destkey = pDev->config.destkey;
+			(*(spw_config *)ioarg->buffer).clkdiv = pDev->config.clkdiv;
+			(*(spw_config *)ioarg->buffer).rxmaxlen = pDev->config.rxmaxlen;
+			(*(spw_config *)ioarg->buffer).timer = pDev->config.timer;
+			(*(spw_config *)ioarg->buffer).disconnect = pDev->config.disconnect;
+			(*(spw_config *)ioarg->buffer).promiscuous = pDev->config.promiscuous;
+			(*(spw_config *)ioarg->buffer).rmapen = pDev->config.rmapen;
+			(*(spw_config *)ioarg->buffer).rmapbufdis = pDev->config.rmapbufdis;
+			(*(spw_config *)ioarg->buffer).check_rmap_err = pDev->config.check_rmap_err;
+			(*(spw_config *)ioarg->buffer).rm_prot_id = pDev->config.rm_prot_id;
+			(*(spw_config *)ioarg->buffer).tx_blocking = pDev->config.tx_blocking;
+			(*(spw_config *)ioarg->buffer).disable_err = pDev->config.disable_err;
+			(*(spw_config *)ioarg->buffer).link_err_irq = pDev->config.link_err_irq;
+			(*(spw_config *)ioarg->buffer).event_id = pDev->config.event_id;
+			(*(spw_config *)ioarg->buffer).is_rmap = pDev->config.is_rmap;
+			(*(spw_config *)ioarg->buffer).is_rmapcrc = pDev->config.is_rmapcrc;
+			(*(spw_config *)ioarg->buffer).is_rxunaligned = pDev->config.is_rxunaligned;
+			(*(spw_config *)ioarg->buffer).linkdisabled = pDev->config.linkdisabled;
+			(*(spw_config *)ioarg->buffer).linkstart = pDev->config.linkstart;
+			(*(spw_config *)ioarg->buffer).rx_blocking = pDev->config.rx_blocking;
+			(*(spw_config *)ioarg->buffer).tx_block_on_full = pDev->config.tx_block_on_full;
+			(*(spw_config *)ioarg->buffer).keep_source = pDev->config.keep_source;
+			(*(spw_config *)ioarg->buffer).rtimeout = pDev->config.rtimeout;
+			break;
+		case SPACEWIRE_IOCTRL_GET_LINK_STATUS:
+			SPACEWIRE_DBGC(DBGSPW_IOCTRL,"SPACEWIRE_IOCTRL_GET_STATUS=%i \n", (unsigned int)((SPW_STATUS_READ(pDev) >> 21) & 0x7));
+			*(unsigned int *)ioarg->buffer = (unsigned int )((SPW_STATUS_READ(pDev) >> 21) & 0x7);
+			break;
+		case SPACEWIRE_IOCTRL_GET_STATISTICS: 
+			if (ioarg->buffer == NULL)
+				return RTEMS_INVALID_NAME;
+			SPACEWIRE_DBG2("SPACEWIRE_IOCTRL_GET_STATISTICS \n");
+			(*(spw_stats *)ioarg->buffer).tx_link_err = pDev->stat.tx_link_err;
+			(*(spw_stats *)ioarg->buffer).rx_rmap_header_crc_err = pDev->stat.rx_rmap_header_crc_err;
+			(*(spw_stats *)ioarg->buffer).rx_rmap_data_crc_err = pDev->stat.rx_rmap_data_crc_err;
+			(*(spw_stats *)ioarg->buffer).rx_eep_err =  pDev->stat.rx_eep_err;
+			(*(spw_stats *)ioarg->buffer).rx_truncated = pDev->stat.rx_truncated;
+			(*(spw_stats *)ioarg->buffer).parity_err = pDev->stat.parity_err;
+			(*(spw_stats *)ioarg->buffer).escape_err = pDev->stat.escape_err;
+			(*(spw_stats *)ioarg->buffer).credit_err = pDev->stat.credit_err;
+			(*(spw_stats *)ioarg->buffer).write_sync_err = pDev->stat.write_sync_err;
+			(*(spw_stats *)ioarg->buffer).disconnect_err = pDev->stat.disconnect_err;
+			(*(spw_stats *)ioarg->buffer).early_ep = pDev->stat.early_ep;
+			(*(spw_stats *)ioarg->buffer).invalid_address = pDev->stat.invalid_address;
+			(*(spw_stats *)ioarg->buffer).packets_sent = pDev->stat.packets_sent;
+			(*(spw_stats *)ioarg->buffer).packets_received = pDev->stat.packets_received;
+			break;
+		case SPACEWIRE_IOCTRL_CLR_STATISTICS:
+			SPACEWIRE_DBG2("SPACEWIRE_IOCTRL_CLR_STATISTICS \n");
+			pDev->stat.tx_link_err = 0;
+			pDev->stat.rx_rmap_header_crc_err = 0;
+			pDev->stat.rx_rmap_data_crc_err = 0;
+			pDev->stat.rx_eep_err = 0;
+			pDev->stat.rx_truncated = 0;
+			pDev->stat.parity_err = 0;
+			pDev->stat.escape_err = 0;
+			pDev->stat.credit_err = 0;
+			pDev->stat.write_sync_err = 0;
+			pDev->stat.disconnect_err = 0;
+			pDev->stat.early_ep = 0;
+			pDev->stat.invalid_address = 0;
+			pDev->stat.packets_sent = 0;
+			pDev->stat.packets_received = 0;
+			break;
+		case SPACEWIRE_IOCTRL_SEND:
+			if (ioarg->buffer == NULL)
+				return RTEMS_INVALID_NAME;
+			args = (spw_ioctl_pkt_send *)ioarg->buffer;
+			args->sent = 0;
+
+			/* is link up? */
+			if ( !pDev->running ) {
+				return RTEMS_INVALID_NAME;
+			}
+	
+			SPACEWIRE_DBGC(DBGSPW_IOCALLS, "write [%i,%i]: hlen: %i hbuf:0x%x dlen:%i dbuf:0x%x\n", major, minor, 
+				       (unsigned int)args->hlen, (int)args->hdr,(unsigned int)args->dlen, (int)args->data);
+			
+			if ((args->hlen > pDev->txhbufsize) || (args->dlen > pDev->txdbufsize) || 
+			    ((args->hlen+args->dlen) < 1) || 
+			    ((args->hdr == NULL) && (args->hlen != 0)) || ((args->data == NULL) && (args->dlen != 0))) {
+				return RTEMS_INVALID_NAME;
+			}
+			while ((args->sent = grspw_hw_send(pDev, args->hlen, args->hdr, args->dlen, args->data, args->options)) == 0) {
+				if (pDev->config.tx_block_on_full == 1) { 
+					SPACEWIRE_DBG2("Tx Block on full \n");
+					rtems_semaphore_obtain(pDev->txsp, RTEMS_WAIT, RTEMS_NO_TIMEOUT);
+				} else {
+					SPACEWIRE_DBG2("Tx non blocking return when full \n");
+					return RTEMS_RESOURCE_IN_USE;
+				}
+			}
+			SPACEWIRE_DBGC(DBGSPW_IOCALLS, "Tx ioctl return: %i  \n", args->sent);
+			break;
+
+		case SPACEWIRE_IOCTRL_LINKDISABLE:
+			pDev->config.linkdisabled = 1;
+			pDev->config.linkstart = 0;
+			if ( pDev->core_ver != 3 ) {
+				SPW_CTRL_WRITE(pDev, (SPW_CTRL_READ(pDev) & 0xFFFFFFFC) | 1);
+				if ((SPW_CTRL_READ(pDev) & 3) != 1) {
+					return RTEMS_IO_ERROR;
+				}
+			}
+			break;
+
+		case SPACEWIRE_IOCTRL_LINKSTART:
+			pDev->config.linkdisabled = 0;
+			pDev->config.linkstart = 1;
+			if ( pDev->core_ver != 3 ) {
+				SPW_CTRL_WRITE(pDev, (SPW_CTRL_READ(pDev) & 0xFFFFFFFC) | 2);
+				if ((SPW_CTRL_READ(pDev) & 3) != 2) {
+					return RTEMS_IO_ERROR;
+				}
+			}
+			break;
+
+		/* Calculate timer register from GRSPW Core frequency 
+		 * Also possible to set disconnect and timer64 from
+		 *  - SPACEWIRE_IOCTRL_SET_DISCONNECT
+		 *  - SPACEWIRE_IOCTRL_SET_TIMER
+		 */
+		case SPACEWIRE_IOCTRL_SET_COREFREQ:
+			pDev->core_freq_khz = (unsigned int)ioarg->buffer;
+			if ( pDev->core_freq_khz == 0 ){
+				/* Get GRSPW clock frequency from system clock.
+				 * System clock has been read from timer inited
+				 * by RTEMS loader (mkprom)
+				 */
+				drvmgr_freq_get(pDev->dev, DEV_APB_SLV,
+					&pDev->core_freq_khz);
+				/* Convert from Hz -> kHz */
+				pDev->core_freq_khz = pDev->core_freq_khz / 1000;
+			}
+
+			/* Only GRSPW1 needs the Timer64 and Disconnect values 
+			 * GRSPW2 and onwards doesn't have this register.
+			 */
+			if ( pDev->core_ver <= 1 ){
+				/* Calculate Timer64 & Disconnect */
+				pDev->config.timer = grspw_calc_timer64(pDev->core_freq_khz);
+				pDev->config.disconnect = grspw_calc_disconnect(pDev->core_freq_khz);
+
+				/* Set Timer64 & Disconnect Register */
+				SPW_WRITE(&pDev->regs->timer, 
+				(SPW_READ(&pDev->regs->timer) & 0xFFC00000) |
+				((pDev->config.disconnect & 0x3FF)<<12) |
+				(pDev->config.timer & 0xFFF));
+
+				/* Check that the registers were written successfully */
+				tmp = SPW_READ(&pDev->regs->timer) & 0x003fffff;
+				if ( ((tmp & 0xFFF) != pDev->config.timer) ||
+				     (((tmp >> 12) & 0x3FF) != pDev->config.disconnect) ) {
+					return RTEMS_IO_ERROR;
+				}
+			}
+			break;
+
+		case SPACEWIRE_IOCTRL_START:
+			if ( pDev->running ){
+				return RTEMS_INVALID_NAME;
+			}
+
+			/* Get timeout from userspace
+			 *  timeout:
+			 *   ¤  -1	   = Default timeout
+			 *   ¤  less than -1 = forever
+			 *   ¤  0	    = no wait, proceed if link is up
+			 *   ¤  positive     = specifies number of system clock ticks that 
+			 *		     startup will wait for link to enter ready mode.
+			 */
+			timeout = (int)ioarg->buffer;
+			
+			if ( (ret=grspw_hw_startup(pDev,timeout)) != RTEMS_SUCCESSFUL ) {
+				return ret;
+			}
+			pDev->running = 1;
+			/* Register interrupt routine and unmask IRQ */
+			drvmgr_interrupt_register(pDev->dev, 0, "grspw", grspw_interrupt, pDev);
+
+			break;
+
+		case SPACEWIRE_IOCTRL_STOP:
+			if ( !pDev->running ){
+				return RTEMS_INVALID_NAME;
+			}
+			/* Disable interrupts */
+			drvmgr_interrupt_unregister(dev, 0, grspw_interrupt, pDev);
+
+			pDev->running = 0;
+
+			/* Stop Receiver and transmitter */
+			grspw_hw_stop(pDev,1,1);
+			break;
+
+		/* Set time-code control register bits, and Enables/Disables 
+		 * Time code interrupt, make sure to connect the callback 
+		 * grspw_timecode_callback if using interrupts.
+		 */
+		case SPACEWIRE_IOCTRL_SET_TCODE_CTRL:
+			tmp = (unsigned int)ioarg->buffer;
+			mask = tmp & (SPACEWIRE_TCODE_CTRL_IE_MSK|SPACEWIRE_TCODE_CTRL_TT_MSK|SPACEWIRE_TCODE_CTRL_TR_MSK);
+			mask <<= 8;
+			tmp &= mask;
+			tmp = (SPW_CTRL_READ(pDev) & ~(mask | SPW_CTRL_IE)) | tmp;
+			if (tmp & (SPW_CTRL_LI|SPW_CTRL_TQ))
+				tmp |= SPW_CTRL_IE;
+			SPW_CTRL_WRITE(pDev, tmp);
+			break;
+
+		/* Set time register and optionaly send a time code */
+		case SPACEWIRE_IOCTRL_SET_TCODE:
+			tmp = (unsigned int)ioarg->buffer;
+			/* Set timecode register */
+			if (tmp & SPACEWIRE_TCODE_SET) {
+				SPW_WRITE(&pDev->regs->time,
+				    ((SPW_READ(&pDev->regs->time) & ~(0xff)) |
+				    (tmp & SPACEWIRE_TCODE_TCODE)));
+			}
+			/* Send timecode directly (tick-in) ? */
+			if (tmp & SPACEWIRE_TCODE_TX) {
+			    SPW_CTRL_WRITE(pDev,
+				((SPW_CTRL_READ(pDev) & ~(SPW_CTRL_TI)) | SPW_CTRL_TI));
+			}
+			break;
+
+		/* Read time code register and tick-out status bit */
+		case SPACEWIRE_IOCTRL_GET_TCODE:
+			tmp = (unsigned int)ioarg->buffer;
+			if ( !tmp ){
+			    return RTEMS_INVALID_NAME;
+			}
+
+			/* Copy timecode register */
+			if (SPW_READ(&pDev->regs->status) & SPW_STATUS_TO) {
+				*(unsigned int *)tmp = (1 << 8) | SPW_READ(&pDev->regs->time);
+			} else {
+				*(unsigned int *)tmp = SPW_READ(&pDev->regs->time);
+			}
+			break;
+
+		case SPACEWIRE_IOCTRL_SET_READ_TIMEOUT:
+			pDev->config.rtimeout = (unsigned int)ioarg->buffer;
+			break;
+
+		default:
+			return RTEMS_NOT_IMPLEMENTED;
+	}
+
+	SPACEWIRE_DBGC(DBGSPW_IOCALLS, "SPW_IOCTRL Return\n");
+	return RTEMS_SUCCESSFUL;
 }
 
 /* ============================================================================== */
 
 static int grspw_set_rxmaxlen(GRSPW_DEV *pDev) {
-        unsigned int rxmax;
-        SPW_WRITE(&pDev->regs->dma0rxmax,pDev->config.rxmaxlen); /*set rx maxlength*/
-        rxmax = SPW_READ(&pDev->regs->dma0rxmax);
-        if (rxmax != pDev->config.rxmaxlen) {
-                return 0;
-        }
-        return 1;
+	unsigned int rxmax;
+	SPW_WRITE(&pDev->regs->dma0rxmax,pDev->config.rxmaxlen); /*set rx maxlength*/
+	rxmax = SPW_READ(&pDev->regs->dma0rxmax);
+	if (rxmax != pDev->config.rxmaxlen) {
+		return 0;
+	}
+	return 1;
 }
 
 static int grspw_hw_init(GRSPW_DEV *pDev) {
-        unsigned int ctrl;
+	unsigned int ctrl;
 
-        ctrl = SPW_CTRL_READ(pDev);
+	ctrl = SPW_CTRL_READ(pDev);
 
-#ifdef GRSPW_STATIC_MEM
-        pDev->rx = (SPACEWIRE_RXBD *) pDev->mem_bdtable;
-        pDev->tx = (SPACEWIRE_RXBD *) pDev->mem_bdtable + SPACEWIRE_BDTABLE_SIZE;
-#else
-        pDev->rx = (SPACEWIRE_RXBD *) rtems_heap_allocate_aligned_with_boundary( SPACEWIRE_BDTABLE_SIZE, 1024, 0 );
-        pDev->tx = (SPACEWIRE_TXBD *) rtems_heap_allocate_aligned_with_boundary( SPACEWIRE_BDTABLE_SIZE, 1024, 0 );
-#endif
-        SPACEWIRE_DBG("hw_init [minor %i]\n", pDev->minor);
+	pDev->rx = (SPACEWIRE_RXBD *) pDev->ptr_bd0;
+	pDev->tx = (SPACEWIRE_TXBD *) &pDev->rx[SPACEWIRE_RXBUFS_NR];
+
+	/* Translate into remote address */
+	drvmgr_translate(pDev->dev, 0, 0, (void *)pDev->rx, (void **)&pDev->rx_remote);
+	drvmgr_translate(pDev->dev, 0, 0, (void *)pDev->tx, (void **)&pDev->tx_remote);
+
+	SPACEWIRE_DBG("hw_init [minor %i]\n", pDev->minor);
 
-        pDev->config.is_rmap = ctrl & SPW_CTRL_RA;
-        pDev->config.is_rxunaligned = ctrl & SPW_CTRL_RX;
-        pDev->config.is_rmapcrc = ctrl & SPW_CTRL_RC;
-        return 0;
+	pDev->config.is_rmap = ctrl & SPW_CTRL_RA;
+	pDev->config.is_rxunaligned = ctrl & SPW_CTRL_RX;
+	pDev->config.is_rmapcrc = ctrl & SPW_CTRL_RC;
+	return 0;
 }
 
-static int grspw_hw_waitlink (GRSPW_DEV *pDev, int timeout)
+static int grspw_hw_waitlink (GRSPW_DEV *pDev, int timeout) 
 {
-        int j;
-
-        if ( timeout == -1 ){
-                /* Wait default timeout */
-                timeout = SPACEWIRE_INIT_TIMEOUT;
-        }
-
-        j=0;
-        while (SPW_LINKSTATE(SPW_STATUS_READ(pDev)) != 5) {
-                if ( timeout < -1 ) {
-                        /* wait forever */
-                }else if ( j >= timeout ){
-                        /* timeout reached, return fail */
-                        return 1;
-                }
-
-                /* Sleep for 10 ticks */
-                rtems_task_wake_after(10);
-                j+=10;
-        }
-        return 0;
+	int j;
+
+	/* No actual link interface on a DMA-only GRSPW2 connected to the
+	 * SPW router
+	 */
+	if (pDev->core_ver == 3)
+		return 0;
+
+	if ( timeout == -1 ){
+		/* Wait default timeout */
+		timeout = SPACEWIRE_INIT_TIMEOUT;
+	}
+
+	j=0;
+	while (SPW_LINKSTATE(SPW_STATUS_READ(pDev)) != 5) {
+		if ( timeout < -1 ) {
+			/* wait forever */
+		}else if ( j >= timeout ){
+			/* timeout reached, return fail */
+			return 1;
+		}
+
+		/* Sleep for 10 ticks */
+		rtems_task_wake_after(10);
+		j+=10;
+	}
+	return 0;
 }
 
 static void grspw_hw_reset(GRSPW_DEV *pDev)
 {
-        SPW_CTRL_WRITE(pDev, SPW_CTRL_RESET); /*reset core*/
-        SPW_STATUS_WRITE(pDev, SPW_STATUS_TO | SPW_STATUS_CE | SPW_STATUS_ER | SPW_STATUS_DE | SPW_STATUS_PE |
-                         SPW_STATUS_WE | SPW_STATUS_IA | SPW_STATUS_EE); /*clear status*/
-        SPW_CTRL_WRITE(pDev, SPW_CTRL_LINKSTART); /*start link core*/
-        #ifndef GRSPW_STATIC_MEM
-                free(pDev->rx);
-                free(pDev->tx);
-        #endif
+	SPW_CTRL_WRITE(pDev, SPW_CTRL_RESET); /*reset core*/
+	SPW_STATUS_WRITE(pDev, SPW_STATUS_TO | SPW_STATUS_CE | SPW_STATUS_ER | SPW_STATUS_DE | SPW_STATUS_PE | 
+			 SPW_STATUS_WE | SPW_STATUS_IA | SPW_STATUS_EE); /*clear status*/
+	SPW_CTRL_WRITE(pDev, SPW_CTRL_LINKSTART); /*start link core*/
 }
 
 static void grspw_hw_read_config(GRSPW_DEV *pDev)
 {
-        unsigned int tmp;
+	unsigned int tmp;
+
+	tmp = SPW_READ(&pDev->regs->nodeaddr);
+	pDev->config.nodeaddr = 0xFF & tmp;
+	pDev->config.nodemask = 0xFF & (tmp>>8);
+	pDev->config.destkey = 0xFF & SPW_READ(&pDev->regs->destkey);
+	pDev->config.clkdiv = 0xFFFF & SPW_READ(&pDev->regs->clkdiv);
+
+	tmp = SPW_CTRL_READ(pDev);
+	pDev->config.promiscuous = 1 & (tmp >> 5);
+	pDev->config.rmapen = 1 & (tmp >> 16);
+	pDev->config.rmapbufdis = 1 & (tmp >> 17);
+	pDev->config.is_rmap = 1 & (tmp >> 31);
+	pDev->config.is_rxunaligned = 1 & (tmp >> 30);
+	pDev->config.is_rmapcrc = 1 & (tmp >> 29);
+	pDev->config.linkdisabled = 1 & tmp;
+	pDev->config.linkstart = 1 & (tmp >> 1);
+
+	if ( pDev->core_ver <= 1 ){
+		tmp = SPW_READ(&pDev->regs->timer);
+		pDev->config.timer = 0xFFF & tmp;
+		pDev->config.disconnect = 0x3FF & (tmp >> 12);
+	}else{
+		pDev->config.timer = 0;
+		pDev->config.disconnect = 0;
+	}
 
-        tmp = SPW_READ(&pDev->regs->nodeaddr);
-        pDev->config.nodeaddr = 0xFF & tmp;
-        pDev->config.nodemask = 0xFF & (tmp>>8);
-        pDev->config.destkey = 0xFF & SPW_READ(&pDev->regs->destkey);
-        pDev->config.clkdiv = 0xFFFF & SPW_READ(&pDev->regs->clkdiv);
-
-        tmp = SPW_CTRL_READ(pDev);
-        pDev->config.promiscuous = 1 & (tmp >> 5);
-        pDev->config.rmapen = 1 & (tmp >> 16);
-        pDev->config.rmapbufdis = 1 & (tmp >> 17);
-        pDev->config.is_rmap = 1 & (tmp >> 31);
-        pDev->config.is_rxunaligned = 1 & (tmp >> 30);
-        pDev->config.is_rmapcrc = 1 & (tmp >> 29);
-        pDev->config.linkdisabled = 1 & tmp;
-        pDev->config.linkstart = 1 & (tmp >> 1);
-
-        if ( pDev->core_ver <= 1 ){
-          tmp = SPW_READ(&pDev->regs->timer);
-          pDev->config.timer = 0xFFF & tmp;
-          pDev->config.disconnect = 0x3FF & (tmp >> 12);
-        }else{
-          pDev->config.timer = 0;
-          pDev->config.disconnect = 0;
-        }
-
-        return;
+	return;
 }
 
 /* timeout is given in ticks */
 static int grspw_hw_startup (GRSPW_DEV *pDev, int timeout)
 {
-        int i;
-        unsigned int dmactrl;
-
-        SPW_WRITE(&pDev->regs->status, (SPW_STATUS_TO|SPW_STATUS_CE|SPW_STATUS_ER|SPW_STATUS_DE|SPW_STATUS_PE|SPW_STATUS_WE|SPW_STATUS_IA|SPW_STATUS_EE)); /*clear status*/
-
-        if (grspw_hw_waitlink(pDev,timeout)) {
-                SPACEWIRE_DBG2("Device open. Link is not up\n");
-                return RTEMS_TIMEOUT;
-        }
-
-        SPW_WRITE(&pDev->regs->dma0ctrl, SPW_DMACTRL_PS | SPW_DMACTRL_PR | SPW_DMACTRL_TA | SPW_DMACTRL_RA); /*clear status, set ctrl*/
-
-
-        if ((dmactrl = SPW_READ(&pDev->regs->dma0ctrl)) != 0) {
-                SPACEWIRE_DBG2("DMACtrl is not cleared\n");
-                return RTEMS_IO_ERROR;
-        }
-
-        /* prepare transmit buffers */
-        for (i = 0; i < pDev->txbufcnt; i++) {
-                pDev->tx[i].ctrl = 0;
-                pDev->tx[i].addr_header = memarea_to_hw(((unsigned int)&pDev->ptr_txhbuf0[0]) + (i * pDev->txhbufsize));
-                pDev->tx[i].addr_data = memarea_to_hw(((unsigned int)&pDev->ptr_txdbuf0[0]) + (i * pDev->txdbufsize));
-        }
-        pDev->tx_cur = 0;
-        pDev->tx_sent = 0;
-        pDev->tx_all_in_use = 0;
-
-        /* prepare receive buffers */
-        for (i = 0; i < pDev->rxbufcnt; i++) {
-                if (i+1 == pDev->rxbufcnt) {
-                        pDev->rx[i].ctrl = SPW_RXBD_IE | SPW_RXBD_EN | SPW_RXBD_WR;
-                } else {
-                        pDev->rx[i].ctrl = SPW_RXBD_IE | SPW_RXBD_EN;
-                }
-                pDev->rx[i].addr = memarea_to_hw(((unsigned int)&pDev->ptr_rxbuf0[0]) + (i * pDev->rxbufsize));
-        }
-        pDev->rxcur = 0;
-        pDev->rxbufcur = -1;
-        grspw_set_rxmaxlen(pDev);
-
-        SPW_WRITE(&pDev->regs->dma0txdesc, memarea_to_hw((unsigned int) pDev->tx));
-        SPW_WRITE(&pDev->regs->dma0rxdesc, memarea_to_hw((unsigned int) pDev->rx));
-
-        /* start RX */
-        dmactrl = SPW_READ(&pDev->regs->dma0ctrl);
-        SPW_WRITE(&pDev->regs->dma0ctrl, (dmactrl & SPW_PREPAREMASK_RX) | SPW_DMACTRL_RD | SPW_DMACTRL_RXEN | SPW_DMACTRL_NS | SPW_DMACTRL_TXIE | SPW_DMACTRL_RXIE);
-
-        SPACEWIRE_DBGC(DBGSPW_TX,"0x%x: setup complete\n", (unsigned int)pDev->regs);
-        return RTEMS_SUCCESSFUL;
+	int i;
+	unsigned int dmactrl;
+
+	SPW_WRITE(&pDev->regs->status, (SPW_STATUS_TO|SPW_STATUS_CE|SPW_STATUS_ER|SPW_STATUS_DE|SPW_STATUS_PE|SPW_STATUS_WE|SPW_STATUS_IA|SPW_STATUS_EE)); /*clear status*/
+
+	if (grspw_hw_waitlink(pDev,timeout)) {
+		SPACEWIRE_DBG2("Device open. Link is not up\n");
+		return RTEMS_TIMEOUT;
+	}
+
+	SPW_WRITE(&pDev->regs->dma0ctrl, SPW_DMACTRL_PS | SPW_DMACTRL_PR | SPW_DMACTRL_TA | SPW_DMACTRL_RA); /*clear status, set ctrl*/
+
+	if ((dmactrl = SPW_READ(&pDev->regs->dma0ctrl)) != 0) {
+		SPACEWIRE_DBG2("DMACtrl is not cleared\n");
+		return RTEMS_IO_ERROR;
+	}
+
+	/* prepare transmit buffers */
+	for (i = 0; i < pDev->txbufcnt; i++) {
+		pDev->tx[i].ctrl = 0;
+		pDev->tx[i].addr_header = ((unsigned int)&pDev->ptr_txhbuf0_remote[0]) + (i * pDev->txhbufsize);
+		pDev->tx[i].addr_data = ((unsigned int)&pDev->ptr_txdbuf0_remote[0]) + (i * pDev->txdbufsize);
+	}
+	pDev->tx_cur = 0;
+	pDev->tx_sent = 0;
+	pDev->tx_all_in_use = 0;
+
+	/* prepare receive buffers */
+	for (i = 0; i < pDev->rxbufcnt; i++) {
+		if (i+1 == pDev->rxbufcnt) {
+			pDev->rx[i].ctrl = SPW_RXBD_IE | SPW_RXBD_EN | SPW_RXBD_WR;
+		} else {
+			pDev->rx[i].ctrl = SPW_RXBD_IE | SPW_RXBD_EN;
+		}
+		pDev->rx[i].addr = ((unsigned int)&pDev->ptr_rxbuf0_remote[0]) + (i * pDev->rxbufsize);
+	}
+	pDev->rxcur = 0;
+	pDev->rxbufcur = -1;
+	grspw_set_rxmaxlen(pDev);
+
+	SPW_WRITE(&pDev->regs->dma0txdesc, pDev->tx_remote);
+	SPW_WRITE(&pDev->regs->dma0rxdesc, pDev->rx_remote);
+
+	/* start RX */
+	dmactrl = SPW_READ(&pDev->regs->dma0ctrl);
+	SPW_WRITE(&pDev->regs->dma0ctrl, (dmactrl & SPW_PREPAREMASK_RX) | SPW_DMACTRL_RD | SPW_DMACTRL_RXEN | SPW_DMACTRL_NS | SPW_DMACTRL_TXIE | SPW_DMACTRL_RXIE);
+
+	SPACEWIRE_DBGC(DBGSPW_TX,"0x%x: setup complete\n", (unsigned int)pDev->regs);
+	return RTEMS_SUCCESSFUL;
 }
 
 /* Wait until the receiver is inactive */
 static void grspw_hw_wait_rx_inactive(GRSPW_DEV *pDev)
 {
-        while( SPW_READ(&pDev->regs->dma0ctrl) & SPW_DMACTRL_RX ){
-                /* switching may be needed:
-                 *  - low frequency GRSPW
-                 *  - mega packet incoming
-                 */
-                rtems_task_wake_after(1);
-        }
+	while( SPW_READ(&pDev->regs->dma0ctrl) & SPW_DMACTRL_RX ){
+		/* switching may be needed: 
+		 *  - low frequency GRSPW 
+		 *  - mega packet incoming
+		 */
+		rtems_task_wake_after(1);
+	}
 }
 
 /* Stop the rx or/and tx by disabling the receiver/transmitter */
-static int grspw_hw_stop (GRSPW_DEV *pDev, int rx, int tx)
-{
-        unsigned int dmactrl;
-
-        /* stop rx and/or tx */
-        dmactrl = SPW_READ(&pDev->regs->dma0ctrl);
-        if ( rx ) {
-                dmactrl &= ~(SPW_DMACTRL_RXEN|SPW_DMACTRL_RXIE|SPW_DMACTRL_RD);
-        }
-        if ( tx ) {
-                dmactrl &= ~(SPW_DMACTRL_TXEN|SPW_DMACTRL_TXIE);
-        }
-        /*SPW_WRITE(&pDev->regs->dma0ctrl, (dmactrl & SPW_PREPAREMASK_RX) & ~(SPW_DMACTRL_RD | SPW_DMACTRL_RXEN) & ~(SPW_DMACTRL_TXEN));*/
-
-        /* don't clear status flags */
-        dmactrl &= ~(SPW_DMACTRL_RA|SPW_DMACTRL_PR|SPW_DMACTRL_AI);
-        SPW_WRITE(&pDev->regs->dma0ctrl, dmactrl);
-        return RTEMS_SUCCESSFUL;
-}
-
-int grspw_hw_send(GRSPW_DEV *pDev, unsigned int hlen, char *hdr, unsigned int dlen, char *data)
+static int grspw_hw_stop (GRSPW_DEV *pDev, int rx, int tx) 
 {
+	unsigned int dmactrl;
 
-        unsigned int dmactrl, ctrl;
-#ifdef DEBUG_SPACEWIRE_ONOFF
-				unsigned int k;
-#endif
-        rtems_interrupt_level level;
-        unsigned int cur = pDev->tx_cur;
-        char *txh = pDev->ptr_txhbuf0 + (cur * pDev->txhbufsize);
-        char *txd = pDev->ptr_txdbuf0 + (cur * pDev->txdbufsize);
+	/* stop rx and/or tx */
+	dmactrl = SPW_READ(&pDev->regs->dma0ctrl);
+	if ( rx ) {
+		dmactrl &= ~(SPW_DMACTRL_RXEN|SPW_DMACTRL_RXIE|SPW_DMACTRL_RD);
+	}
+	if ( tx ) {
+		dmactrl &= ~(SPW_DMACTRL_TXEN|SPW_DMACTRL_TXIE);
+	}
+	/*SPW_WRITE(&pDev->regs->dma0ctrl, (dmactrl & SPW_PREPAREMASK_RX) & ~(SPW_DMACTRL_RD | SPW_DMACTRL_RXEN) & ~(SPW_DMACTRL_TXEN));*/
 
-        ctrl = SPW_READ((volatile void *)&pDev->tx[cur].ctrl);
+	/* don't clear status flags */
+	dmactrl &= ~(SPW_DMACTRL_RA|SPW_DMACTRL_PR|SPW_DMACTRL_AI);
+	SPW_WRITE(&pDev->regs->dma0ctrl, dmactrl);
+	return RTEMS_SUCCESSFUL;
+}
 
-        if (ctrl & SPW_TXBD_EN) {
-                return 0;
-        }
 
-        memcpy(&txd[0], data, dlen);
-        memcpy(&txh[0], hdr, hlen);
 
+int grspw_hw_send(GRSPW_DEV *pDev, unsigned int hlen, char *hdr, unsigned int dlen, char *data, unsigned int options) 
+{
+	unsigned int dmactrl, ctrl;
 #ifdef DEBUG_SPACEWIRE_ONOFF
-        if (DEBUG_SPACEWIRE_FLAGS & DBGSPW_DUMP) {
-                for (k = 0; k < hlen; k++){
-                        if (k % 16 == 0) {
-                                printf ("\n");
-                        }
-                        printf ("%.2x(%c) ",txh[k] & 0xff,isprint(txh[k] & 0xff) ? txh[k] & 0xff : ' ');
-                }
-                printf ("\n");
-        }
-        if (DEBUG_SPACEWIRE_FLAGS & DBGSPW_DUMP) {
-                for (k = 0; k < dlen; k++){
-                        if (k % 16 == 0) {
-                                printf ("\n");
-                        }
-                        printf ("%.2x(%c) ",txd[k] & 0xff,isprint(txd[k] & 0xff) ? txd[k] & 0xff : ' ');
-                }
-                printf ("\n");
-        }
+	unsigned int k;
 #endif
+	rtems_interrupt_level level;
+	unsigned int cur = pDev->tx_cur, bdctrl;
+	char *txh = pDev->ptr_txhbuf0 + (cur * pDev->txhbufsize);
+	char *txd = pDev->ptr_txdbuf0 + (cur * pDev->txdbufsize);
+	char *txh_remote = pDev->ptr_txhbuf0_remote + (cur * pDev->txhbufsize);
+	char *txd_remote = pDev->ptr_txdbuf0_remote + (cur * pDev->txdbufsize);
+	unsigned int tmp, tmp2;
+	
+	ctrl = SPW_READ((volatile void *)&pDev->tx[cur].ctrl);
+
+	if (ctrl & SPW_TXBD_EN) {
+		return 0;
+	}
 
-        pDev->tx[cur].addr_header = memarea_to_hw((unsigned int)txh);
-        pDev->tx[cur].len = dlen;
-        pDev->tx[cur].addr_data = memarea_to_hw((unsigned int)txd);
-        if (pDev->tx_cur == (pDev->txbufcnt - 1) ) {
-                pDev->tx[cur].ctrl = SPW_TXBD_IE | SPW_TXBD_WR | SPW_TXBD_EN | hlen;
-        } else {
-                pDev->tx[cur].ctrl = SPW_TXBD_IE | SPW_TXBD_EN | hlen;
-        }
+	memcpy(&txd[0], data, dlen);
+	memcpy(&txh[0], hdr, hlen);
 
-        dmactrl = SPW_READ(&pDev->regs->dma0ctrl);
-        SPW_WRITE(&pDev->regs->dma0ctrl, (dmactrl & SPW_PREPAREMASK_TX) | SPW_DMACTRL_TXEN | SPW_DMACTRL_TXIE);
+#ifdef DEBUG_SPACEWIRE_ONOFF  
+	if (DEBUG_SPACEWIRE_FLAGS & DBGSPW_DUMP) {
+		for (k = 0; k < hlen; k++){
+			if (k % 16 == 0) {
+				printf ("\n");
+			}
+			printf ("%.2x(%c) ",txh[k] & 0xff,isprint(txh[k] & 0xff) ? txh[k] & 0xff : ' ');
+		}
+		printf ("\n");
+	}
+	if (DEBUG_SPACEWIRE_FLAGS & DBGSPW_DUMP) {
+		for (k = 0; k < dlen; k++){
+			if (k % 16 == 0) {
+				printf ("\n");
+			}
+			printf ("%.2x(%c) ",txd[k] & 0xff,isprint(txd[k] & 0xff) ? txd[k] & 0xff : ' ');
+		}
+		printf ("\n");
+	}
+#endif
+	
+	pDev->tx[cur].addr_header = (unsigned int)txh_remote;
+	pDev->tx[cur].len = dlen;
+	pDev->tx[cur].addr_data = (unsigned int)txd_remote;
+
+	bdctrl = SPW_TXBD_IE | SPW_TXBD_EN | hlen;
+	if ( options & GRSPW_PKTSEND_OPTION_HDR_CRC )
+		bdctrl |= SPW_TXBD_HC;
+	if ( options & GRSPW_PKTSEND_OPTION_DATA_CRC )
+		bdctrl |= SPW_TXBD_DC;
+	bdctrl |= options & GRSPW_PKTSEND_OPTION_NOCRCLEN_MASK;
+
+	/* Update counters */
+	rtems_interrupt_disable(level);
+	if (pDev->tx_cur == (pDev->txbufcnt - 1) ) {
+		bdctrl |= SPW_TXBD_WR;
+	}
+	pDev->tx[cur].ctrl = bdctrl;
 
-        /* Update counters */
-        rtems_interrupt_disable(level);
+	dmactrl = SPW_READ(&pDev->regs->dma0ctrl);
+	SPW_WRITE(&pDev->regs->dma0ctrl, (dmactrl & SPW_PREPAREMASK_TX) | SPW_DMACTRL_TXEN | SPW_DMACTRL_TXIE);
 
-        pDev->tx_cur = (pDev->tx_cur + 1) % pDev->txbufcnt;
-        if (pDev->tx_cur == pDev->tx_sent) {
-                pDev->tx_all_in_use = 1;
-        }
-        rtems_interrupt_enable(level);
+	pDev->tx_cur = (pDev->tx_cur + 1) % pDev->txbufcnt;
+	if (pDev->tx_cur == pDev->tx_sent) {
+		pDev->tx_all_in_use = 1;
+	}
+	rtems_interrupt_enable(level);
 
-        /* In blocking mode wait until message is sent */
+	/* In blocking mode wait until message is sent */
 	if (pDev->config.tx_blocking) {
 		while ( SPW_READ(&pDev->regs->dma0ctrl) & SPW_DMACTRL_TXEN) {
 			/* if changed to blocking mode  */
-                        SPACEWIRE_DBGC(DBGSPW_TX, "Tx blocking\n");
-                        rtems_semaphore_obtain(pDev->txsp, RTEMS_WAIT, RTEMS_NO_TIMEOUT);
+			SPACEWIRE_DBGC(DBGSPW_TX, "Tx blocking\n");
+			rtems_semaphore_obtain(pDev->txsp, RTEMS_WAIT, RTEMS_NO_TIMEOUT);
 		}
 	}
-        SPACEWIRE_DBGC(DBGSPW_TX, "0x%x: transmitted <%i> bytes\n", (unsigned int) pDev->regs, dlen+hlen);
-        return hlen + dlen;
+	SPACEWIRE_DBGC(DBGSPW_TX, "0x%x: transmitted <%i> bytes\n", (unsigned int) pDev->regs, dlen+hlen);
+	return hlen + dlen;
 }
 
 static int grspw_hw_receive(GRSPW_DEV *pDev, char *b, int c) {
-        unsigned int len, rxlen, ctrl;
-        unsigned int cur;
-        unsigned int tmp;
-        unsigned int dump_start_len;
-        int i;
-        char *rxb;
-
-        if ( pDev->config.promiscuous ) {
+	unsigned int len, rxlen, ctrl;
+	unsigned int cur; 
+	unsigned int tmp;
+	unsigned int dump_start_len;
+	int i;
+	char *rxb;  
+
+	if ( pDev->config.promiscuous || pDev->config.keep_source ) {
 		dump_start_len = 0; /* make sure address and prot can be read in promiscuous mode */
 	} else if (pDev->config.rm_prot_id) {
 		dump_start_len = 2; /* skip source address and protocol id */
@@ -1638,92 +1819,153 @@ static int grspw_hw_receive(GRSPW_DEV *pDev, char *b, int c) {
 		dump_start_len = 1; /* default: skip only source address */
 	}
 
-        rxlen = 0;
-        cur = pDev->rxcur;
-        rxb = pDev->ptr_rxbuf0 + (cur * pDev->rxbufsize);
-
-        SPACEWIRE_DBGC(DBGSPW_RX, "0x%x: waitin packet at pos %i\n", (unsigned int) pDev->regs, cur);
-
-        ctrl = SPW_READ((volatile void *)&pDev->rx[cur].ctrl);
-        if (ctrl & SPW_RXBD_EN) {
-                return rxlen;
-        }
-        SPACEWIRE_DBGC(DBGSPW_RX, "checking packet\n");
-
-        len = SPW_RXBD_LENGTH & ctrl;
-        if (!((ctrl & SPW_RXBD_ERROR) || (pDev->config.check_rmap_err && (ctrl & SPW_RXBD_RMAPERROR)))) {
-                if (pDev->rxbufcur == -1) {
-                        SPACEWIRE_DBGC(DBGSPW_RX, "incoming packet len %i\n", len);
-                        pDev->stat.packets_received++;
-                        pDev->rxbufcur = dump_start_len;
-                }
-                rxlen = tmp = len - pDev->rxbufcur;
-                SPACEWIRE_DBGC(DBGSPW_RX, "C %i\n", c);
-                SPACEWIRE_DBGC(DBGSPW_RX, "Dump %i\n", dump_start_len);
-                SPACEWIRE_DBGC(DBGSPW_RX, "Bufcur %i\n", pDev->rxbufcur);
-                SPACEWIRE_DBGC(DBGSPW_RX, "Rxlen %i\n", rxlen );
-                if (rxlen > c) {
-                        rxlen = c;
-                }
-                if (CPU_SPARC_HAS_SNOOPING) {
-                        memcpy(b, rxb+pDev->rxbufcur, rxlen);
-                } else {
-                        for(i = 0; i < rxlen; i++) {
-                                b[i] = MEM_READ(rxb+pDev->rxbufcur);
-                        }
-                }
-
-                pDev->rxbufcur += rxlen;
-                if (c >= tmp) {
-                        SPACEWIRE_DBGC(DBGSPW_RX, "Next descriptor\n");
-                        grspw_rxnext(pDev);
-                }
-        } else {
-                check_rx_errors(pDev, ctrl);
-        }
-        return rxlen;
+	rxlen = 0;
+	cur = pDev->rxcur;
+	rxb = pDev->ptr_rxbuf0 + (cur * pDev->rxbufsize);
+
+	SPACEWIRE_DBGC(DBGSPW_RX, "0x%x: waitin packet at pos %i\n", (unsigned int) pDev->regs, cur);
+
+	ctrl = SPW_READ((volatile void *)&pDev->rx[cur].ctrl);
+	if (ctrl & SPW_RXBD_EN) {
+		return rxlen;
+	}
+	SPACEWIRE_DBGC(DBGSPW_RX, "checking packet\n");
+
+	len = SPW_RXBD_LENGTH & ctrl;
+	if (!((ctrl & SPW_RXBD_ERROR) || (pDev->config.check_rmap_err && (ctrl & SPW_RXBD_RMAPERROR)))) {
+		if (pDev->rxbufcur == -1) {
+			SPACEWIRE_DBGC(DBGSPW_RX, "incoming packet len %i\n", len);
+			pDev->stat.packets_received++;
+			pDev->rxbufcur = dump_start_len;
+		}
+		rxlen = tmp = len - pDev->rxbufcur;
+		SPACEWIRE_DBGC(DBGSPW_RX, "C %i\n", c);
+		SPACEWIRE_DBGC(DBGSPW_RX, "Dump %i\n", dump_start_len);
+		SPACEWIRE_DBGC(DBGSPW_RX, "Bufcur %i\n", pDev->rxbufcur);
+		SPACEWIRE_DBGC(DBGSPW_RX, "Rxlen %i\n", rxlen );
+		if (rxlen > c) {
+			rxlen = c;
+		}
+		if (CPU_SPARC_HAS_SNOOPING) {
+/*		if ( 1 ) {*/
+			/*printf("RX_MEMCPY(0x%x, 0x%x, 0x%x)\n", (unsigned int)b, (unsigned int)(rxb+pDev->rxbufcur), (unsigned int)rxlen);*/
+			memcpy(b, rxb+pDev->rxbufcur, rxlen);
+		} else {
+			int left = rxlen;
+			/* Copy word wise if Aligned */
+			if ( (((int)b & 3) == 0) && (((int)(rxb+pDev->rxbufcur) & 3) == 0) ){
+				while(left>=32){
+					*(int *)(b+0) = MEM_READ32(rxb+pDev->rxbufcur+0);
+					*(int *)(b+4) = MEM_READ32(rxb+pDev->rxbufcur+4);
+					*(int *)(b+8) = MEM_READ32(rxb+pDev->rxbufcur+8);
+					*(int *)(b+12) = MEM_READ32(rxb+pDev->rxbufcur+12);
+					*(int *)(b+16) = MEM_READ32(rxb+pDev->rxbufcur+16);
+					*(int *)(b+20) = MEM_READ32(rxb+pDev->rxbufcur+20);
+					*(int *)(b+24) = MEM_READ32(rxb+pDev->rxbufcur+24);
+					*(int *)(b+28) = MEM_READ32(rxb+pDev->rxbufcur+28);
+					rxb+=32;
+					b+=32;
+					left-=32;
+				}
+				while(left>=4){
+					*(int *)b = MEM_READ32(rxb+pDev->rxbufcur);
+					rxb+=4;
+					b+=4;
+					left-=4;
+				}
+			}
+			for(i = 0; i < left; i++) {
+				b[i] = MEM_READ8(rxb+pDev->rxbufcur+i);
+			}
+		}
+
+		pDev->rxbufcur += rxlen;
+		if (c >= tmp) {
+			SPACEWIRE_DBGC(DBGSPW_RX, "Next descriptor\n");
+			grspw_rxnext(pDev);
+		}
+	} else {
+		check_rx_errors(pDev, ctrl);
+		grspw_rxnext(pDev);
+	}
+	return rxlen;
 }
 
-static void grspw_rxnext(GRSPW_DEV *pDev)
+static void grspw_rxnext(GRSPW_DEV *pDev) 
 {
-        unsigned int dmactrl;
-        unsigned int cur = pDev->rxcur;
-        unsigned int ctrl = 0;
-        if (cur == (pDev->rxbufcnt - 1)) {
-                pDev->rx[cur].ctrl = ctrl | SPW_RXBD_EN | SPW_RXBD_IE | SPW_RXBD_WR;
-                cur = 0;
-        } else {
-                pDev->rx[cur].ctrl = ctrl | SPW_RXBD_EN | SPW_RXBD_IE;
-                cur++;
-        }
-
-        pDev->rxcur = cur;
-        pDev->rxbufcur = -1;
-
-        /* start RX */
-        dmactrl = SPW_READ(&pDev->regs->dma0ctrl);
-        SPW_WRITE(&pDev->regs->dma0ctrl, (dmactrl & SPW_PREPAREMASK_RX) | SPW_DMACTRL_RD | SPW_DMACTRL_RXEN | SPW_DMACTRL_RXIE | SPW_DMACTRL_NS);
+	unsigned int dmactrl;
+	unsigned int cur = pDev->rxcur;
+	unsigned int ctrl = 0;
+	rtems_interrupt_level level;
+
+	rtems_interrupt_disable(level);
+
+	if (cur == (pDev->rxbufcnt - 1)) {
+		pDev->rx[cur].ctrl = ctrl | SPW_RXBD_EN | SPW_RXBD_IE | SPW_RXBD_WR;
+		cur = 0;
+	} else {
+		pDev->rx[cur].ctrl = ctrl | SPW_RXBD_EN | SPW_RXBD_IE;
+		cur++;
+	}
+
+	pDev->rxcur = cur;
+	pDev->rxbufcur = -1;
+
+	/* start RX */
+	dmactrl = SPW_READ(&pDev->regs->dma0ctrl);
+	SPW_WRITE(&pDev->regs->dma0ctrl, (dmactrl & SPW_PREPAREMASK_RX) | SPW_DMACTRL_RD | SPW_DMACTRL_RXEN | SPW_DMACTRL_RXIE | SPW_DMACTRL_NS);
 
+	rtems_interrupt_enable(level);
+}
+
+static void check_rx_errors(GRSPW_DEV *pDev, int ctrl) 
+{
+	if (ctrl & SPW_RXBD_EEP) {
+		pDev->stat.rx_eep_err++;
+	}
+	if (ctrl & SPW_RXBD_EHC) {
+		if (pDev->config.check_rmap_err) {
+			pDev->stat.rx_rmap_header_crc_err++;
+		}
+	}  
+	if (ctrl & SPW_RXBD_EDC) {
+		if (pDev->config.check_rmap_err) {
+			pDev->stat.rx_rmap_data_crc_err++;
+		}
+	}
+	if (ctrl & SPW_RXBD_ETR) {
+		pDev->stat.rx_truncated++;
+	}
 }
 
-static void check_rx_errors(GRSPW_DEV *pDev, int ctrl)
+
+void grspw_print_dev(struct drvmgr_dev *dev, int options)
 {
-        if (ctrl & SPW_RXBD_EEP) {
-                pDev->stat.rx_eep_err++;
-        }
-        if (ctrl & SPW_RXBD_EHC) {
-                if (pDev->config.check_rmap_err) {
-                        pDev->stat.rx_rmap_header_crc_err++;
-                }
-        }
-        if (ctrl & SPW_RXBD_EDC) {
-                if (pDev->config.check_rmap_err) {
-                        pDev->stat.rx_rmap_data_crc_err++;
-                }
-        }
-        if (ctrl & SPW_RXBD_ETR) {
-                pDev->stat.rx_truncated++;
-        }
+	GRSPW_DEV *pDev = dev->priv;
+	struct amba_dev_info *devinfo;
+
+	devinfo = (struct amba_dev_info *)pDev->dev->businfo;
+
+	/* Print */
+	printf("--- GRSPW %s ---\n", pDev->devName);
+	printf(" REGS:            0x%x\n", (unsigned int)pDev->regs);
+	printf(" IRQ:             %d\n", pDev->irq);
+	printf(" CORE VERSION:    %d\n", pDev->core_ver);
+	printf(" CTRL:            0x%x\n", pDev->regs->ctrl);
+	printf(" STATUS:          0x%x\n", pDev->regs->status);
+	printf(" DMA0CTRL:        0x%x\n", pDev->regs->dma0ctrl);
+	printf(" TXBD:            0x%x\n", (unsigned int)pDev->tx);
+	printf(" RXBD:            0x%x\n", (unsigned int)pDev->rx);
 }
 
+void grspw_print(int options)
+{
+	struct amba_drv_info *drv = &grspw_drv_info;
+	struct drvmgr_dev *dev;
 
+	dev = drv->general.dev;
+	while(dev) {
+		grspw_print_dev(dev, options);
+		dev = dev->next_in_drv;
+	}
+}