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.

2 files changed, 1525 insertions, 1403 deletions

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;
 }
 
-/*******************************************************************************/
+/******************************************************************************/