grlib: Move source files

Update #3678.

4 files changed, 4862 insertions, 0 deletions

diff --git a/bsps/shared/grlib/can/canmux.c b/bsps/shared/grlib/can/canmux.c
new file mode 100644
index 0000000000..369cb3dd0a
--- /dev/null
+++ b/bsps/shared/grlib/can/canmux.c
@@ -0,0 +1,199 @@
+/*
+ *  CAN_MUX driver. Present in GR712RC.
+ *
+ *  COPYRIGHT (c) 2008.
+ *  Cobham Gaisler AB.
+ *
+ *  The license and distribution terms for this file may be
+ *  found in the file LICENSE in this distribution or at
+ *  http://www.rtems.org/license/LICENSE.
+ */
+
+#include <rtems/libio.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+#include <bsp.h>
+#include <rtems/bspIo.h> /* printk */
+
+#include <grlib/canmux.h>
+#include <grlib/ambapp.h>
+
+#include <grlib/grlib_impl.h>
+
+#ifndef GAISLER_CANMUX
+#define GAISLER_CANMUX 0x081
+#endif
+
+#if !defined(CANMUX_DEVNAME) 
+ #undef CANMUX_DEVNAME
+ #define CANMUX_DEVNAME "/dev/canmux"
+#endif
+
+/* Enable debug output? */
+/* #define DEBUG */
+
+#ifdef DEBUG
+#define DBG(x...) printk(x)
+#else
+#define DBG(x...) 
+#endif
+
+#define BUSA_SELECT (1 << 0)
+#define BUSB_SELECT (1 << 1)
+
+struct canmux_priv {
+	volatile unsigned int *muxreg;
+	rtems_id devsem;
+	int open;
+};
+
+static struct canmux_priv *priv;
+
+static rtems_device_driver canmux_ioctl(rtems_device_major_number major, rtems_device_minor_number minor, void *arg);
+static rtems_device_driver canmux_write(rtems_device_major_number major, rtems_device_minor_number minor, void *arg);
+static rtems_device_driver canmux_read(rtems_device_major_number major, rtems_device_minor_number minor, void *arg);
+static rtems_device_driver canmux_close(rtems_device_major_number major, rtems_device_minor_number minor, void *arg);
+static rtems_device_driver canmux_open(rtems_device_major_number major, rtems_device_minor_number minor, void *arg);
+static rtems_device_driver canmux_initialize(rtems_device_major_number major, rtems_device_minor_number unused, void *arg);
+
+
+static rtems_device_driver canmux_ioctl(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
+{
+	rtems_libio_ioctl_args_t *ioarg = (rtems_libio_ioctl_args_t*)arg;
+	
+	DBG("CAN_MUX:  IOCTL %d\n\r", ioarg->command);
+
+	ioarg->ioctl_return = 0;
+	switch(ioarg->command) {
+	case CANMUX_IOC_BUSA_SATCAN: *priv->muxreg &= ~BUSA_SELECT; break;
+	case CANMUX_IOC_BUSA_OCCAN1: *priv->muxreg |= BUSA_SELECT;  break;
+	case CANMUX_IOC_BUSB_SATCAN: *priv->muxreg &= ~BUSB_SELECT; break;
+	case CANMUX_IOC_BUSB_OCCAN2: *priv->muxreg |= BUSB_SELECT; break;
+	default: return RTEMS_NOT_DEFINED;
+	}
+
+	return RTEMS_SUCCESSFUL;
+}
+
+static rtems_device_driver canmux_write(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
+{
+	rtems_libio_rw_args_t *rw_args=(rtems_libio_rw_args_t*)arg;
+	
+	rw_args->bytes_moved = 0;
+
+	return RTEMS_SUCCESSFUL;
+}
+
+static rtems_device_driver canmux_read(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
+{
+	rtems_libio_rw_args_t *rw_args = (rtems_libio_rw_args_t*)arg;
+	
+	rw_args->bytes_moved = 0;
+	
+	return RTEMS_SUCCESSFUL;
+}
+
+
+static rtems_device_driver canmux_close(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
+{
+	DBG("CAN_MUX: Closing %d\n\r",minor);
+
+	priv->open = 0;
+	return RTEMS_SUCCESSFUL;
+}
+
+
+static rtems_device_driver canmux_open(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
+{
+	DBG("CAN_MUX: Opening %d\n\r",minor);
+	
+	rtems_semaphore_obtain(priv->devsem,RTEMS_WAIT, RTEMS_NO_TIMEOUT);
+	if (priv->open) {
+		rtems_semaphore_release(priv->devsem);
+		return RTEMS_RESOURCE_IN_USE; /* EBUSY */
+	}
+	priv->open = 1;
+	rtems_semaphore_release(priv->devsem);
+
+	DBG("CAN_MUX: Opening %d success\n\r",minor);
+
+	return RTEMS_SUCCESSFUL;
+}
+
+static rtems_device_driver canmux_initialize(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
+{
+	struct ambapp_apb_info d;
+	char fs_name[20];
+	rtems_status_code status;
+
+	DBG("CAN_MUX: Initialize..\n\r");
+
+	strcpy(fs_name, CANMUX_DEVNAME);
+	
+	/* Find core and initialize register pointer */
+	if (!ambapp_find_apbslv(&ambapp_plb, VENDOR_GAISLER, GAISLER_CANMUX, &d)) {
+		printk("CAN_MUX: Failed to find CAN_MUX core\n\r");
+		return -1;
+	}
+	
+	status = rtems_io_register_name(fs_name, major, minor);
+	if (RTEMS_SUCCESSFUL != status)
+		rtems_fatal_error_occurred(status);
+
+	/* Create private structure */
+	if ((priv = grlib_malloc(sizeof(*priv))) == NULL) {
+		printk("CAN_MUX driver could not allocate memory for priv structure\n\r");
+		return -1;
+	}
+	
+	priv->muxreg = (unsigned int*)d.start;
+
+	status = rtems_semaphore_create(
+		rtems_build_name('M', 'd', 'v', '0'),
+		1,
+		RTEMS_FIFO | RTEMS_SIMPLE_BINARY_SEMAPHORE | RTEMS_NO_INHERIT_PRIORITY | \
+		RTEMS_NO_PRIORITY_CEILING,
+		0, 
+		&priv->devsem);
+	if (status != RTEMS_SUCCESSFUL) {
+		printk("CAN_MUX: Failed to create dev semaphore (%d)\n\r", status);
+		free(priv);
+		return RTEMS_UNSATISFIED;
+	}
+	
+	priv->open = 0;
+
+	return RTEMS_SUCCESSFUL;
+}
+
+
+#define CANMUX_DRIVER_TABLE_ENTRY { canmux_initialize, canmux_open, canmux_close, canmux_read, canmux_write, canmux_ioctl }
+
+static rtems_driver_address_table canmux_driver = CANMUX_DRIVER_TABLE_ENTRY;
+
+int canmux_register(void)
+{
+	rtems_status_code r;
+	rtems_device_major_number m;
+
+	DBG("CAN_MUX: canmux_register called\n\r");
+
+	if ((r = rtems_io_register_driver(0, &canmux_driver, &m)) == RTEMS_SUCCESSFUL) {
+		DBG("CAN_MUX driver successfully registered, major: %d\n\r", m);
+	} else {
+		switch(r) {
+		case RTEMS_TOO_MANY:
+			printk("CAN_MUX rtems_io_register_driver failed: RTEMS_TOO_MANY\n\r"); break;
+		case RTEMS_INVALID_NUMBER:  
+			printk("CAN_MUX rtems_io_register_driver failed: RTEMS_INVALID_NUMBER\n\r"); break;
+		case RTEMS_RESOURCE_IN_USE:
+			printk("CAN_MUX rtems_io_register_driver failed: RTEMS_RESOURCE_IN_USE\n\r"); break;
+		default:
+			printk("CAN_MUX rtems_io_register_driver failed\n\r");
+		}
+		return 1;
+	}
+
+	return 0;
+}
diff --git a/bsps/shared/grlib/can/grcan.c b/bsps/shared/grlib/can/grcan.c
new file mode 100644
index 0000000000..55154d823a
--- /dev/null
+++ b/bsps/shared/grlib/can/grcan.c
@@ -0,0 +1,1976 @@
+/*
+ *  GRCAN driver
+ *
+ *  COPYRIGHT (c) 2007.
+ *  Cobham Gaisler AB.
+ *
+ *  The license and distribution terms for this file may be
+ *  found in the file LICENSE in this distribution or at
+ *  http://www.rtems.org/license/LICENSE.
+ */
+
+#include <bsp.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+#include <assert.h>
+#include <ctype.h>
+#include <rtems/bspIo.h>
+
+#include <grlib/grcan.h>
+#include <drvmgr/drvmgr.h>
+#include <grlib/ambapp_bus.h>
+#include <grlib/ambapp.h>
+
+#include <grlib/grlib_impl.h>
+
+/* Maximum number of GRCAN devices supported by driver */
+#define GRCAN_COUNT_MAX 8
+
+#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)
+#endif
+
+/* Make receiver buffers bigger than transmitt */
+#ifndef RX_BUF_SIZE
+ #define RX_BUF_SIZE ((3*BLOCK_SIZE)*16)
+#endif
+
+#ifndef IRQ_CLEAR_PENDING
+ #define IRQ_CLEAR_PENDING(irqno)
+#endif
+
+#ifndef IRQ_UNMASK
+ #define IRQ_UNMASK(irqno)
+#endif
+
+#ifndef IRQ_MASK
+ #define IRQ_MASK(irqno)
+#endif
+
+#ifndef GRCAN_DEFAULT_BAUD
+ /* default to 500kbits/s */
+ #define GRCAN_DEFAULT_BAUD 500000
+#endif
+
+#ifndef GRCAN_SAMPLING_POINT
+ #define GRCAN_SAMPLING_POINT 80
+#endif
+
+/* Uncomment for debug output */
+/****************** DEBUG Definitions ********************/
+#define DBG_TX 2
+#define DBG_RX 4
+#define DBG_STATE 8
+
+#define DEBUG_FLAGS (DBG_STATE | DBG_RX | DBG_TX )
+/*
+#define DEBUG
+#define DEBUGFUNCS
+*/
+#include <grlib/debug_defs.h>
+
+/*********************************************************/
+
+int state2err[4] = {
+	/* STATE_STOPPED */ GRCAN_RET_NOTSTARTED,
+	/* STATE_STARTED */ GRCAN_RET_OK,
+	/* STATE_BUSOFF  */ GRCAN_RET_BUSOFF,
+	/* STATE_AHBERR  */ GRCAN_RET_AHBERR
+};
+
+struct grcan_msg {
+	unsigned int head[2];
+	unsigned char data[8];
+};
+
+struct grcan_config {
+	struct grcan_timing timing;
+	struct grcan_selection selection;
+	int abort;
+	int silent;
+};
+
+struct grcan_priv {
+	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;
+
+	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;
+	SPIN_DECLARE(devlock);
+};
+
+static void __inline__ grcan_hw_reset(struct grcan_regs *regs);
+
+static int grcan_hw_read_try(
+	struct grcan_priv *pDev,
+	struct grcan_regs *regs,
+	CANMsg *buffer,
+	int max);
+
+static int grcan_hw_write_try(
+	struct grcan_priv *pDev,
+	struct grcan_regs *regs,
+	CANMsg *buffer,
+	int count);
+
+static void grcan_hw_config(
+	struct grcan_regs *regs,
+	struct grcan_config *conf);
+
+static void grcan_hw_accept(
+	struct grcan_regs *regs,
+	struct grcan_filter *afilter);
+
+static void grcan_hw_sync(
+	struct grcan_regs *regs,
+	struct grcan_filter *sfilter);
+
+static void grcan_interrupt(void *arg);
+
+#ifdef GRCAN_REG_BYPASS_CACHE
+#define READ_REG(address) _grcan_read_nocache((unsigned int)(address))
+#else
+#define READ_REG(address) (*(volatile unsigned int *)(address))
+#endif
+
+#ifdef GRCAN_DMA_BYPASS_CACHE
+#define READ_DMA_WORD(address) _grcan_read_nocache((unsigned int)(address))
+#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;
+}
+#else
+#define READ_DMA_WORD(address) (*(volatile unsigned int *)(address))
+#define READ_DMA_BYTE(address) (*(volatile unsigned char *)(address))
+#endif
+
+#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;
+}
+#endif
+
+#define NELEM(a) ((int) (sizeof (a) / sizeof (a[0])))
+
+static int grcan_count = 0;
+static struct grcan_priv *priv_tab[GRCAN_COUNT_MAX];
+
+/******************* Driver manager interface ***********************/
+
+/* Driver prototypes */
+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);
+	if (GRCAN_COUNT_MAX <= grcan_count)
+		return DRVMGR_ENORES;
+	priv = dev->priv = grlib_calloc(1, sizeof(*priv));
+	if ( !priv )
+		return DRVMGR_NOMEM;
+	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];
+
+	priv = dev->priv;
+
+	/*
+	 * Now we take care of device initialization.
+	 */
+
+	if ( grcan_device_init(priv) ) {
+		return DRVMGR_FAIL;
+	}
+
+	priv_tab[grcan_count] = priv;
+	grcan_count++;
+
+	/* 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, "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, "%sgrcan%d", prefix, dev->minor_bus);
+	}
+
+	return DRVMGR_OK;
+}
+
+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)
+{
+	regs->ctrl = GRCAN_CTRL_RESET;
+}
+
+static rtems_device_driver grcan_hw_start(struct grcan_priv *pDev)
+{
+	/*
+	 * tmp is set but never used. GCC gives a warning for this
+	 * and we need to tell GCC not to complain.
+	 */
+	unsigned int tmp RTEMS_UNUSED;
+
+	SPIN_IRQFLAGS(oldLevel);
+
+	FUNCDBG();
+
+	/* Check that memory has been allocated successfully */
+	if (!pDev->tx || !pDev->rx)
+		return RTEMS_NO_MEMORY;
+
+	/* Configure FIFO configuration register
+	 * and Setup timing
+	 */
+	if (pDev->config_changed) {
+		grcan_hw_config(pDev->regs, &pDev->config);
+		pDev->config_changed = 0;
+	}
+
+	/* Setup receiver */
+	pDev->regs->rx0addr = (unsigned int)pDev->_rx_hw;
+	pDev->regs->rx0size = pDev->rxbuf_size;
+
+	/* Setup Transmitter */
+	pDev->regs->tx0addr = (unsigned int)pDev->_tx_hw;
+	pDev->regs->tx0size = pDev->txbuf_size;
+
+	/* Setup acceptance filters */
+	grcan_hw_accept(pDev->regs, &pDev->afilter);
+
+	/* Sync filters */
+	grcan_hw_sync(pDev->regs, &pDev->sfilter);
+
+	/* Clear status bits */
+	tmp = READ_REG(&pDev->regs->stat);
+	pDev->regs->stat = 0;
+
+	/* Setup IRQ handling */
+
+	/* Clear all IRQs */
+	tmp = READ_REG(&pDev->regs->pir);
+	pDev->regs->picr = 0x1ffff;
+
+	/* unmask TxLoss|TxErrCntr|RxErrCntr|TxAHBErr|RxAHBErr|OR|OFF|PASS */
+	pDev->regs->imr = 0x1601f;
+
+	/* Enable routing of the IRQs */
+	SPIN_LOCK_IRQ(&pDev->devlock, oldLevel);
+	IRQ_UNMASK(pDev->irq + GRCAN_IRQ_TXSYNC);
+	IRQ_UNMASK(pDev->irq + GRCAN_IRQ_RXSYNC);
+	IRQ_UNMASK(pDev->irq + GRCAN_IRQ_IRQ);
+	SPIN_UNLOCK_IRQ(&pDev->devlock, oldLevel);
+
+	/* Enable receiver/transmitter */
+	pDev->regs->rx0ctrl = GRCAN_RXCTRL_ENABLE;
+	pDev->regs->tx0ctrl = GRCAN_TXCTRL_ENABLE;
+
+	/* Enable HurriCANe core */
+	pDev->regs->ctrl = GRCAN_CTRL_ENABLE;
+
+	/* Leave transmitter disabled, it is enabled when
+	 * trying to send something.
+	 */
+	return RTEMS_SUCCESSFUL;
+}
+
+static void grcan_hw_stop(struct grcan_priv *pDev)
+{
+	FUNCDBG();
+
+	/* Mask all IRQs */
+	pDev->regs->imr = 0;
+	IRQ_MASK(pDev->irq + GRCAN_IRQ_TXSYNC);
+	IRQ_MASK(pDev->irq + GRCAN_IRQ_RXSYNC);
+	IRQ_MASK(pDev->irq + GRCAN_IRQ_IRQ);
+
+	/* Disable receiver & transmitter */
+	pDev->regs->rx0ctrl = 0;
+	pDev->regs->tx0ctrl = 0;
+}
+
+static void grcan_sw_stop(struct grcan_priv *pDev)
+{
+	/*
+	 * Release semaphores to wake all threads waiting for an IRQ.
+	 * The threads that
+	 * get woken up must check started state in
+	 * order to determine that they should return to
+	 * user space with error status.
+	 *
+	 * Entering into started mode again will reset the
+	 * semaphore count.
+	 */
+	rtems_semaphore_release(pDev->rx_sem);
+	rtems_semaphore_release(pDev->tx_sem);
+	rtems_semaphore_release(pDev->txempty_sem);
+}
+
+static void grcan_hw_config(struct grcan_regs *regs, struct grcan_config *conf)
+{
+	unsigned int config = 0;
+
+	/* Reset HurriCANe Core */
+	regs->ctrl = 0;
+
+	if (conf->silent)
+		config |= GRCAN_CFG_SILENT;
+
+	if (conf->abort)
+		config |= GRCAN_CFG_ABORT;
+
+	if (conf->selection.selection)
+		config |= GRCAN_CFG_SELECTION;
+
+	if (conf->selection.enable0)
+		config |= GRCAN_CFG_ENABLE0;
+
+	if (conf->selection.enable1)
+		config |= GRCAN_CFG_ENABLE1;
+
+	/* Timing */
+	config |= (conf->timing.bpr << GRCAN_CFG_BPR_BIT) & GRCAN_CFG_BPR;
+	config |= (conf->timing.rsj << GRCAN_CFG_RSJ_BIT) & GRCAN_CFG_RSJ;
+	config |= (conf->timing.ps1 << GRCAN_CFG_PS1_BIT) & GRCAN_CFG_PS1;
+	config |= (conf->timing.ps2 << GRCAN_CFG_PS2_BIT) & GRCAN_CFG_PS2;
+	config |=
+	    (conf->timing.scaler << GRCAN_CFG_SCALER_BIT) & GRCAN_CFG_SCALER;
+
+	/* Write configuration */
+	regs->conf = config;
+
+	/* Enable HurriCANe Core */
+	regs->ctrl = GRCAN_CTRL_ENABLE;
+}
+
+static void grcan_hw_accept(
+	struct grcan_regs *regs,
+	struct grcan_filter *afilter
+)
+{
+	/* Disable Sync mask totaly (if we change scode or smask
+	 * in an unfortunate way we may trigger a sync match)
+	 */
+	regs->rx0mask = 0xffffffff;
+
+	/* Set Sync Filter in a controlled way */
+	regs->rx0code = afilter->code;
+	regs->rx0mask = afilter->mask;
+}
+
+static void grcan_hw_sync(struct grcan_regs *regs, struct grcan_filter *sfilter)
+{
+	/* Disable Sync mask totaly (if we change scode or smask
+	 * in an unfortunate way we may trigger a sync match)
+	 */
+	regs->smask = 0xffffffff;
+
+	/* Set Sync Filter in a controlled way */
+	regs->scode = sfilter->code;
+	regs->smask = sfilter->mask;
+}
+
+static unsigned int grcan_hw_rxavail(
+	unsigned int rp,
+	unsigned int wp, unsigned int size
+)
+{
+	if (rp == wp) {
+		/* read pointer and write pointer is equal only
+		 * when RX buffer is empty.
+		 */
+		return 0;
+	}
+
+	if (wp > rp) {
+		return (wp - rp) / GRCAN_MSG_SIZE;
+	} else {
+		return (size - (rp - wp)) / GRCAN_MSG_SIZE;
+	}
+}
+
+static unsigned int grcan_hw_txspace(
+	unsigned int rp,
+	unsigned int wp,
+	unsigned int size
+)
+{
+	unsigned int left;
+
+	if (rp == wp) {
+		/* read pointer and write pointer is equal only
+		 * when TX buffer is empty.
+		 */
+		return size / GRCAN_MSG_SIZE - WRAP_AROUND_TX_MSGS;
+	}
+
+	/* size - 4 - abs(read-write) */
+	if (wp > rp) {
+		left = size - (wp - rp);
+	} else {
+		left = rp - wp;
+	}
+
+	return left / GRCAN_MSG_SIZE - WRAP_AROUND_TX_MSGS;
+}
+
+#define MIN_TSEG1 1
+#define MIN_TSEG2 2
+#define MAX_TSEG1 14
+#define MAX_TSEG2 8
+
+static int grcan_calc_timing(
+	unsigned int baud,	/* The requested BAUD to calculate timing for */
+	unsigned int core_hz,	/* Frequency in Hz of GRCAN Core */
+	unsigned int sampl_pt,
+	struct grcan_timing *timing	/* result is placed here */
+)
+{
+	int best_error = 1000000000;
+	int error;
+	int best_tseg = 0, best_brp = 0, brp = 0;
+	int tseg = 0, tseg1 = 0, tseg2 = 0;
+	int sjw = 1;
+
+	/* Default to 90% */
+	if ((sampl_pt < 50) || (sampl_pt > 99)) {
+		sampl_pt = GRCAN_SAMPLING_POINT;
+	}
+
+	if ((baud < 5000) || (baud > 1000000)) {
+		/* invalid speed mode */
+		return -1;
+	}
+
+	/* find best match, return -2 if no good reg
+	 * combination is available for this frequency
+	 */
+
+	/* some heuristic specials */
+	if (baud > ((1000000 + 500000) / 2))
+		sampl_pt = 75;
+
+	if (baud < ((12500 + 10000) / 2))
+		sampl_pt = 75;
+
+	/* tseg even = round down, odd = round up */
+	for (
+		tseg = (MIN_TSEG1 + MIN_TSEG2 + 2) * 2;
+		tseg <= (MAX_TSEG2 + MAX_TSEG1 + 2) * 2 + 1;
+		tseg++
+	) {
+		brp = core_hz / ((1 + tseg / 2) * baud) + tseg % 2;
+		if (
+			(brp <= 0) ||
+			((brp > 256 * 1) && (brp <= 256 * 2) && (brp & 0x1)) ||
+			((brp > 256 * 2) && (brp <= 256 * 4) && (brp & 0x3)) ||
+			((brp > 256 * 4) && (brp <= 256 * 8) && (brp & 0x7)) ||
+			(brp > 256 * 8)
+		)
+			continue;
+
+		error = baud - core_hz / (brp * (1 + tseg / 2));
+		if (error < 0) {
+			error = -error;
+		}
+
+		if (error <= best_error) {
+			best_error = error;
+			best_tseg = tseg / 2;
+			best_brp = brp - 1;
+		}
+	}
+
+	if (best_error && (baud / best_error < 10)) {
+		return -2;
+	} else if (!timing)
+		return 0;	/* nothing to store result in, but a valid bitrate can be calculated */
+
+	tseg2 = best_tseg - (sampl_pt * (best_tseg + 1)) / 100;
+
+	if (tseg2 < MIN_TSEG2) {
+		tseg2 = MIN_TSEG2;
+	}
+
+	if (tseg2 > MAX_TSEG2) {
+		tseg2 = MAX_TSEG2;
+	}
+
+	tseg1 = best_tseg - tseg2 - 2;
+
+	if (tseg1 > MAX_TSEG1) {
+		tseg1 = MAX_TSEG1;
+		tseg2 = best_tseg - tseg1 - 2;
+	}
+
+	/* Get scaler and BRP from pseudo BRP */
+	if (best_brp <= 256) {
+		timing->scaler = best_brp;
+		timing->bpr = 0;
+	} else if (best_brp <= 256 * 2) {
+		timing->scaler = ((best_brp + 1) >> 1) - 1;
+		timing->bpr = 1;
+	} else if (best_brp <= 256 * 4) {
+		timing->scaler = ((best_brp + 1) >> 2) - 1;
+		timing->bpr = 2;
+	} else {
+		timing->scaler = ((best_brp + 1) >> 3) - 1;
+		timing->bpr = 3;
+	}
+
+	timing->ps1 = tseg1 + 1;
+	timing->ps2 = tseg2;
+	timing->rsj = sjw;
+
+	return 0;
+}
+
+static int grcan_hw_read_try(
+	struct grcan_priv *pDev,
+	struct grcan_regs *regs,
+	CANMsg * buffer,
+	int max
+)
+{
+	int i, j;
+	CANMsg *dest;
+	struct grcan_msg *source, tmp;
+	unsigned int wp, rp, size, rxmax, addr;
+	int trunk_msg_cnt;
+
+	FUNCDBG();
+
+	wp = READ_REG(&regs->rx0wr);
+	rp = READ_REG(&regs->rx0rd);
+
+	/*
+	 * Due to hardware wrap around simplification write pointer will
+	 * never reach the read pointer, at least a gap of 8 bytes.
+	 * The only time they are equal is when the read pointer has
+	 * reached the write pointer (empty buffer)
+	 *
+	 */
+	if (wp != rp) {
+		/* Not empty, we have received chars...
+		 * Read as much as possible from DMA buffer
+		 */
+		size = READ_REG(&regs->rx0size);
+
+		/* Get number of bytes available in RX buffer */
+		trunk_msg_cnt = grcan_hw_rxavail(rp, wp, size);
+
+		/* truncate size if user space buffer hasn't room for
+		 * all received chars.
+		 */
+		if (trunk_msg_cnt > max)
+			trunk_msg_cnt = max;
+
+		/* Read until i is 0 */
+		i = trunk_msg_cnt;
+
+		addr = (unsigned int)pDev->rx;
+		source = (struct grcan_msg *)(addr + rp);
+		dest = buffer;
+		rxmax = addr + (size - GRCAN_MSG_SIZE);
+
+		/* Read as many can messages as possible */
+		while (i > 0) {
+			/* Read CAN message from DMA buffer */
+			tmp.head[0] = READ_DMA_WORD(&source->head[0]);
+			tmp.head[1] = READ_DMA_WORD(&source->head[1]);
+			if (tmp.head[1] & 0x4) {
+				DBGC(DBG_RX, "overrun\n");
+			}
+			if (tmp.head[1] & 0x2) {
+				DBGC(DBG_RX, "bus-off mode\n");
+			}
+			if (tmp.head[1] & 0x1) {
+				DBGC(DBG_RX, "error-passive mode\n");
+			}
+			/* Convert one grcan CAN message to one "software" CAN message */
+			dest->extended = tmp.head[0] >> 31;
+			dest->rtr = (tmp.head[0] >> 30) & 0x1;
+			if (dest->extended) {
+				dest->id = tmp.head[0] & 0x3fffffff;
+			} else {
+				dest->id = (tmp.head[0] >> 18) & 0xfff;
+			}
+			dest->len = tmp.head[1] >> 28;
+			for (j = 0; j < dest->len; j++)
+				dest->data[j] = READ_DMA_BYTE(&source->data[j]);
+
+			/* wrap around if neccessary */
+			source =
+			    ((unsigned int)source >= rxmax) ?
+			    (struct grcan_msg *)addr : source + 1;
+			dest++;	/* straight user buffer */
+			i--;
+		}
+		{
+			/* A bus off interrupt may have occured after checking pDev->started */
+			SPIN_IRQFLAGS(oldLevel);
+
+			SPIN_LOCK_IRQ(&pDev->devlock, oldLevel);
+			if (pDev->started == STATE_STARTED) {
+				regs->rx0rd = (unsigned int) source - addr;
+				regs->rx0ctrl = GRCAN_RXCTRL_ENABLE;
+			} else {
+				DBGC(DBG_STATE, "cancelled due to a BUS OFF error\n");
+				trunk_msg_cnt = state2err[pDev->started];
+			}
+			SPIN_UNLOCK_IRQ(&pDev->devlock, oldLevel);
+		}
+		return trunk_msg_cnt;
+	}
+	return 0;
+}
+
+static int grcan_hw_write_try(
+	struct grcan_priv *pDev,
+	struct grcan_regs *regs,
+	CANMsg * buffer,
+	int count
+)
+{
+	unsigned int rp, wp, size, txmax, addr;
+	int ret;
+	struct grcan_msg *dest;
+	CANMsg *source;
+	int space_left;
+	unsigned int tmp;
+	int i;
+
+	DBGC(DBG_TX, "\n");
+	/*FUNCDBG(); */
+
+	rp = READ_REG(&regs->tx0rd);
+	wp = READ_REG(&regs->tx0wr);
+	size = READ_REG(&regs->tx0size);
+
+	space_left = grcan_hw_txspace(rp, wp, size);
+
+	/* is circular fifo full? */
+	if (space_left < 1)
+		return 0;
+
+	/* Truncate size */
+	if (space_left > count)
+		space_left = count;
+	ret = space_left;
+
+	addr = (unsigned int)pDev->tx;
+
+	dest = (struct grcan_msg *)(addr + wp);
+	source = (CANMsg *) buffer;
+	txmax = addr + (size - GRCAN_MSG_SIZE);
+
+	while (space_left > 0) {
+		/* Convert and write CAN message to DMA buffer */
+		if (source->extended) {
+			tmp = (1 << 31) | (source->id & 0x3fffffff);
+		} else {
+			tmp = (source->id & 0xfff) << 18;
+		}
+		if (source->rtr)
+			tmp |= (1 << 30);
+		dest->head[0] = tmp;
+		dest->head[1] = source->len << 28;
+		for (i = 0; i < source->len; i++)
+			dest->data[i] = source->data[i];
+		source++;	/* straight user buffer */
+		dest =
+		    ((unsigned int)dest >= txmax) ?
+		    (struct grcan_msg *)addr : dest + 1;
+		space_left--;
+	}
+
+	{
+		/* A bus off interrupt may have occured after checking pDev->started */
+		SPIN_IRQFLAGS(oldLevel);
+
+		SPIN_LOCK_IRQ(&pDev->devlock, oldLevel);
+		if (pDev->started == STATE_STARTED) {
+			regs->tx0wr = (unsigned int) dest - addr;
+			regs->tx0ctrl = GRCAN_TXCTRL_ENABLE;
+		} else {
+			DBGC(DBG_STATE, "cancelled due to a BUS OFF error\n");
+			ret = state2err[pDev->started];
+		}
+		SPIN_UNLOCK_IRQ(&pDev->devlock, oldLevel);
+	}
+	return ret;
+}
+
+static int grcan_wait_rxdata(struct grcan_priv *pDev, int min)
+{
+	unsigned int wp, rp, size, irq;
+	unsigned int irq_trunk, dataavail;
+	int wait, state;
+	SPIN_IRQFLAGS(oldLevel);
+
+	FUNCDBG();
+
+	/*** block until receive IRQ received
+	 * Set up a valid IRQ point so that an IRQ is received
+	 * when one or more messages are received
+	 */
+	SPIN_LOCK_IRQ(&pDev->devlock, oldLevel);
+	state = pDev->started;
+
+	/* A bus off interrupt may have occured after checking pDev->started */
+	if (state != STATE_STARTED) {
+		SPIN_UNLOCK_IRQ(&pDev->devlock, oldLevel);
+		if (state == STATE_BUSOFF) {
+			DBGC(DBG_STATE, "cancelled due to a BUS OFF error\n");
+		} else if (state == STATE_AHBERR) {
+			DBGC(DBG_STATE, "cancelled due to a AHB error\n");
+		} else {
+			DBGC(DBG_STATE, "cancelled due to STOP (unexpected) \n");
+		}
+		return state2err[state];
+	}
+
+	size = READ_REG(&pDev->regs->rx0size);
+	rp = READ_REG(&pDev->regs->rx0rd);
+	wp = READ_REG(&pDev->regs->rx0wr);
+
+	/**** Calculate IRQ Pointer ****/
+	irq = wp + min * GRCAN_MSG_SIZE;
+	/* wrap irq around */
+	if (irq >= size) {
+		irq_trunk = irq - size;
+	} else
+		irq_trunk = irq;
+
+	/* init IRQ HW */
+	pDev->regs->rx0irq = irq_trunk;
+
+	/* Clear pending Rx IRQ */
+	pDev->regs->picr = GRCAN_RXIRQ_IRQ;
+
+	wp = READ_REG(&pDev->regs->rx0wr);
+
+	/* Calculate messages available */
+	dataavail = grcan_hw_rxavail(rp, wp, size);
+
+	if (dataavail < min) {
+		/* Still empty, proceed with sleep - Turn on IRQ (unmask irq) */
+		pDev->regs->imr = READ_REG(&pDev->regs->imr) | GRCAN_RXIRQ_IRQ;
+		wait = 1;
+	} else {
+		/* enough message has been received, abort sleep - don't unmask interrupt */
+		wait = 0;
+	}
+	SPIN_UNLOCK_IRQ(&pDev->devlock, oldLevel);
+
+	/* Wait for IRQ to fire only if has been triggered */
+	if (wait) {
+		rtems_semaphore_obtain(pDev->rx_sem, RTEMS_WAIT, RTEMS_NO_TIMEOUT);
+		/*
+		 * The semaphore is released either due to the expected IRQ
+		 * condition or by BUSOFF, AHBERROR or another thread calling
+		 * grcan_stop(). In either case, state2err[] has the correnct
+		 * return value.
+		 */
+		return state2err[pDev->started];
+	}
+
+	return 0;
+}
+
+/* Wait for TX circular buffer to have room for min CAN messagges. TXIRQ is used to pin
+ * point the location of the CAN message corresponding to min.
+ *
+ * min must be at least WRAP_AROUND_TX_MSGS less than max buffer capacity
+ * (pDev->txbuf_size/GRCAN_MSG_SIZE) for this algo to work.
+ */
+static int grcan_wait_txspace(struct grcan_priv *pDev, int min)
+{
+	int wait, state;
+	unsigned int irq, rp, wp, size, space_left;
+	unsigned int irq_trunk;
+	SPIN_IRQFLAGS(oldLevel);
+
+	DBGC(DBG_TX, "\n");
+	/*FUNCDBG(); */
+
+	SPIN_LOCK_IRQ(&pDev->devlock, oldLevel);
+	state = pDev->started;
+	/* A bus off interrupt may have occured after checking pDev->started */
+	if (state != STATE_STARTED) {
+		SPIN_UNLOCK_IRQ(&pDev->devlock, oldLevel);
+		if (state == STATE_BUSOFF) {
+			DBGC(DBG_STATE, "cancelled due to a BUS OFF error\n");
+		} else if (state == STATE_AHBERR) {
+			DBGC(DBG_STATE, "cancelled due to a AHB error\n");
+		} else {
+			DBGC(DBG_STATE, "cancelled due to STOP (unexpected)\n");
+		}
+		return state2err[state];
+	}
+
+	pDev->regs->tx0ctrl = GRCAN_TXCTRL_ENABLE;
+
+	size = READ_REG(&pDev->regs->tx0size);
+	wp = READ_REG(&pDev->regs->tx0wr);
+
+	rp = READ_REG(&pDev->regs->tx0rd);
+
+	/**** Calculate IRQ Pointer ****/
+	irq = rp + min * GRCAN_MSG_SIZE;
+	/* wrap irq around */
+	if (irq >= size) {
+		irq_trunk = irq - size;
+	} else
+		irq_trunk = irq;
+
+	/* trigger HW to do a IRQ when enough room in buffer */
+	pDev->regs->tx0irq = irq_trunk;
+
+	/* Clear pending Tx IRQ */
+	pDev->regs->picr = GRCAN_TXIRQ_IRQ;
+
+	/* One problem, if HW already gone past IRQ place the IRQ will
+	 * never be received resulting in a thread hang. We check if so
+	 * before proceeding.
+	 *
+	 * has the HW already gone past the IRQ generation place?
+	 *  == does min fit info tx buffer?
+	 */
+	rp = READ_REG(&pDev->regs->tx0rd);
+
+	space_left = grcan_hw_txspace(rp, wp, size);
+
+	if (space_left < min) {
+		/* Still too full, proceed with sleep - Turn on IRQ (unmask irq) */
+		pDev->regs->imr = READ_REG(&pDev->regs->imr) | GRCAN_TXIRQ_IRQ;
+		wait = 1;
+	} else {
+		/* There are enough room in buffer, abort wait - don't unmask interrupt */
+		wait = 0;
+	}
+	SPIN_UNLOCK_IRQ(&pDev->devlock, oldLevel);
+
+	/* Wait for IRQ to fire only if it has been triggered */
+	if (wait) {
+		rtems_semaphore_obtain(pDev->tx_sem, RTEMS_WAIT, RTEMS_NO_TIMEOUT);
+		return state2err[pDev->started];
+	}
+
+	/* At this point the TxIRQ has been masked, we ned not to mask it */
+	return 0;
+}
+
+static int grcan_tx_flush(struct grcan_priv *pDev)
+{
+	int wait, state;
+	unsigned int rp, wp;
+	SPIN_IRQFLAGS(oldLevel);
+	FUNCDBG();
+
+	/* loop until all data in circular buffer has been read by hw.
+	 * (write pointer != read pointer )
+	 *
+	 * Hardware doesn't update write pointer - we do
+	 */
+	while (
+		(wp = READ_REG(&pDev->regs->tx0wr)) !=
+		(rp = READ_REG(&pDev->regs->tx0rd))
+	) {
+		/* Wait for TX empty IRQ */
+		SPIN_LOCK_IRQ(&pDev->devlock, oldLevel);
+		state = pDev->started;
+
+		/* A bus off interrupt may have occured after checking pDev->started */
+		if (state != STATE_STARTED) {
+			SPIN_UNLOCK_IRQ(&pDev->devlock, oldLevel);
+			if (state == STATE_BUSOFF) {
+				DBGC(DBG_STATE, "cancelled due to a BUS OFF error\n");
+			} else if (state == STATE_AHBERR) {
+				DBGC(DBG_STATE, "cancelled due to a AHB error\n");
+			} else {
+				DBGC(DBG_STATE, "cancelled due to STOP (unexpected)\n");
+			}
+			return state2err[state];
+		}
+
+		/* Clear pending TXEmpty IRQ */
+		pDev->regs->picr = GRCAN_TXEMPTY_IRQ;
+
+		if (wp != READ_REG(&pDev->regs->tx0rd)) {
+			/* Still not empty, proceed with sleep - Turn on IRQ (unmask irq) */
+			pDev->regs->imr =
+			    READ_REG(&pDev->regs->imr) | GRCAN_TXEMPTY_IRQ;
+			wait = 1;
+		} else {
+			/* TX fifo is empty */
+			wait = 0;
+		}
+		SPIN_UNLOCK_IRQ(&pDev->devlock, oldLevel);
+		if (!wait)
+			break;
+
+		/* Wait for IRQ to wake us */
+		rtems_semaphore_obtain(pDev->txempty_sem, RTEMS_WAIT, RTEMS_NO_TIMEOUT);
+		state = pDev->started;
+		if (state != STATE_STARTED) {
+			return state2err[state];
+		}
+	}
+	return 0;
+}
+
+static int grcan_alloc_buffers(struct grcan_priv *pDev, int rx, int tx)
+{
+	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 = grlib_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);
+		}
+	}
+
+	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 = grlib_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();
+
+	if (tx && pDev->_tx) {
+		free(pDev->_tx);
+		pDev->_tx = NULL;
+		pDev->tx = NULL;
+	}
+
+	if (rx && pDev->_rx) {
+		free(pDev->_rx);
+		pDev->_rx = NULL;
+		pDev->rx = NULL;
+	}
+}
+
+int grcan_dev_count(void)
+{
+	return grcan_count;
+}
+
+void *grcan_open_by_name(char *name, int *dev_no)
+{
+	int i;
+	for (i = 0; i < grcan_count; i++){
+		struct grcan_priv *pDev;
+
+		pDev = priv_tab[i];
+		if (NULL == pDev) {
+			continue;
+		}
+		if (strncmp(pDev->devName, name, NELEM(pDev->devName)) == 0) {
+			if (dev_no)
+				*dev_no = i;
+			return grcan_open(i);
+		}
+	}
+	return NULL;
+}
+
+void *grcan_open(int dev_no)
+{
+	struct grcan_priv *pDev;
+	void *ret;
+	union drvmgr_key_value *value;
+
+	FUNCDBG();
+
+	if (grcan_count == 0 || (grcan_count <= dev_no)) {
+		return NULL;
+	}
+
+	pDev = priv_tab[dev_no];
+
+	/* Wait until we get semaphore */
+	if (rtems_semaphore_obtain(pDev->dev_sem, RTEMS_WAIT, RTEMS_NO_TIMEOUT)
+	    != RTEMS_SUCCESSFUL) {
+		return NULL;
+	}
+
+	/* is device busy/taken? */
+	if ( pDev->open ) {
+		ret = NULL;
+		goto out;
+	}
+
+	SPIN_INIT(&pDev->devlock, pDev->devName);
+
+	/* Mark device taken */
+	pDev->open = 1;
+
+	pDev->txblock = pDev->rxblock = 1;
+	pDev->txcomplete = pDev->rxcomplete = 0;
+	pDev->started = STATE_STOPPED;
+	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", DRVMGR_KT_INT);
+	if ( value )
+		pDev->txbuf_size = value->i;
+
+	value = drvmgr_dev_key_get(pDev->dev, "rxBufSize", DRVMGR_KT_INT);
+	if ( value )
+		pDev->rxbuf_size = value->i;
+
+	value = drvmgr_dev_key_get(pDev->dev, "txBufAdr", DRVMGR_KT_POINTER);
+	if ( value )
+		pDev->txbuf_adr = value->ptr;
+
+	value = drvmgr_dev_key_get(pDev->dev, "rxBufAdr", DRVMGR_KT_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 = NULL;
+		goto out;
+	}
+
+	/* Clear statistics */
+	memset(&pDev->stats,0,sizeof(struct grcan_stats));
+
+	ret = pDev;
+out:
+	rtems_semaphore_release(pDev->dev_sem);
+	return ret;
+}
+
+int grcan_close(void *d)
+{
+	struct grcan_priv *pDev = d;
+
+	FUNCDBG();
+
+	grcan_stop(d);
+
+	grcan_hw_reset(pDev->regs);
+
+	grcan_free_buffers(pDev,1,1);
+
+	/* Mark Device as closed */
+	pDev->open = 0;
+
+	return 0;
+}
+
+int grcan_read(void *d, CANMsg *msg, size_t ucount)
+{
+	struct grcan_priv *pDev = d;
+	CANMsg *dest;
+	unsigned int count, left;
+	int nread;
+	int req_cnt;
+
+	FUNCDBG();
+
+	dest = msg;
+	req_cnt = ucount;
+
+	if ( (!dest) || (req_cnt<1) )
+		return GRCAN_RET_INVARG;
+
+	if (pDev->started != STATE_STARTED) {
+		return GRCAN_RET_NOTSTARTED;
+	}
+
+	DBGC(DBG_RX, "grcan_read [%p]: buf: %p len: %u\n", d, msg, (unsigned int) ucount);
+
+	nread = grcan_hw_read_try(pDev,pDev->regs,dest,req_cnt);
+	if (nread < 0) {
+		return nread;
+	}
+	count = nread;
+	if ( !( pDev->rxblock && pDev->rxcomplete && (count!=req_cnt) ) ){
+		if ( count > 0 ) {
+			/* Successfully received messages (at least one) */
+			return count;
+		}
+
+		/* nothing read, shall we block? */
+		if ( !pDev->rxblock ) {
+			/* non-blocking mode */
+			return GRCAN_RET_TIMEOUT;
+		}
+	}
+
+	while (count == 0 || (pDev->rxcomplete && (count!=req_cnt))) {
+		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;
+			}
+		}
+
+		nread = grcan_wait_rxdata(pDev, left);
+		if (nread) {
+			/* The wait has been aborted, probably due to 
+			 * the device driver has been closed by another
+			 * thread or a bus-off. Return error code.
+			 */
+			return nread;
+		}
+
+		/* Try read bytes from circular buffer */
+		nread = grcan_hw_read_try(
+				pDev,
+				pDev->regs,
+				dest+count,
+				req_cnt-count);
+
+		if (nread < 0) {
+			/* The read was aborted by bus-off. */
+			return nread;
+		}
+		count += nread;
+	}
+	/* no need to unmask IRQ as IRQ Handler do that for us. */
+	return count;
+}
+
+int grcan_write(void *d, CANMsg *msg, size_t ucount)
+{
+	struct grcan_priv *pDev = d;
+	CANMsg *source;
+	unsigned int count, left;
+	int nwritten;
+	int req_cnt;
+
+	DBGC(DBG_TX,"\n");
+
+	if ((pDev->started != STATE_STARTED) || pDev->config.silent || pDev->flushing)
+		return GRCAN_RET_NOTSTARTED;
+
+	req_cnt = ucount;
+	source = (CANMsg *) msg;
+
+	/* check proper length and buffer pointer */
+	if (( req_cnt < 1) || (source == NULL) ){
+		return GRCAN_RET_INVARG;
+	}
+
+	nwritten = grcan_hw_write_try(pDev,pDev->regs,source,req_cnt);
+	if (nwritten < 0) {
+		return nwritten;
+	}
+	count = nwritten;
+	if ( !(pDev->txblock && pDev->txcomplete && (count!=req_cnt)) ) {
+		if ( count > 0 ) {
+			/* Successfully transmitted chars (at least one char) */
+			return count;
+		}
+
+		/* nothing written, shall we block? */
+		if ( !pDev->txblock ) {
+			/* non-blocking mode */
+			return GRCAN_RET_TIMEOUT;
+		}
+	}
+
+	/* 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;
+			}
+		}
+
+		nwritten = grcan_wait_txspace(pDev,left);
+		/* Wait until more room in transmit buffer */
+		if ( nwritten ) {
+			/* 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.
+			 */
+			return nwritten;
+		}
+
+		/* Try read bytes from circular buffer */
+		nwritten = grcan_hw_write_try(
+			pDev,
+			pDev->regs,
+			source+count,
+			req_cnt-count);
+
+		if (nwritten < 0) {
+			/* Write was aborted by bus-off. */
+			return nwritten;
+		}
+		count += nwritten;
+	}
+	/* no need to unmask IRQ as IRQ Handler do that for us. */
+
+	return count;
+}
+
+int grcan_start(void *d)
+{
+	struct grcan_priv *pDev = d;
+
+	FUNCDBG();
+
+	if (grcan_get_state(d) == STATE_STARTED) {
+		return -1;
+	}
+
+	if ( (grcan_hw_start(pDev)) != RTEMS_SUCCESSFUL ){
+		return -2;
+	}
+
+	/* Clear semaphore state. This is to avoid effects from previous
+	 * bus-off/stop where semahpores where flushed() but the count remained.
+	 */
+	rtems_semaphore_obtain(pDev->rx_sem, RTEMS_NO_WAIT, 0);
+	rtems_semaphore_obtain(pDev->tx_sem, RTEMS_NO_WAIT, 0);
+	rtems_semaphore_obtain(pDev->txempty_sem, RTEMS_NO_WAIT, 0);
+
+	/* Read and write are now open... */
+	pDev->started = STATE_STARTED;
+	DBGC(DBG_STATE, "STOPPED|BUSOFF|AHBERR->STARTED\n");
+
+	/* Register interrupt routine and enable IRQ at IRQ ctrl */
+	drvmgr_interrupt_register(pDev->dev, 0, pDev->devName,
+					grcan_interrupt, pDev);
+
+	return 0;
+}
+
+int grcan_stop(void *d)
+{
+	struct grcan_priv *pDev = d;
+	SPIN_IRQFLAGS(oldLevel);
+	int do_sw_stop;
+
+	FUNCDBG();
+
+	if (pDev->started == STATE_STOPPED)
+		return -1;
+
+	SPIN_LOCK_IRQ(&pDev->devlock, oldLevel);
+	if (pDev->started == STATE_STARTED) {
+		grcan_hw_stop(pDev);
+		do_sw_stop = 1;
+		DBGC(DBG_STATE, "STARTED->STOPPED\n");
+	} else {
+		/*
+		 * started == STATE_[STOPPED|BUSOFF|AHBERR] so grcan_hw_stop()
+		 * might already been called from ISR.
+		 */
+		DBGC(DBG_STATE, "[STOPPED|BUSOFF|AHBERR]->STOPPED\n");
+		do_sw_stop = 0;
+	}
+	pDev->started = STATE_STOPPED;
+	SPIN_UNLOCK_IRQ(&pDev->devlock, oldLevel);
+
+	if (do_sw_stop)
+		grcan_sw_stop(pDev);
+
+	/* Disable interrupts */
+	drvmgr_interrupt_unregister(pDev->dev, 0, grcan_interrupt, pDev);
+
+	return 0;
+}
+
+int grcan_get_state(void *d)
+{
+	struct grcan_priv *pDev = d;
+
+	FUNCDBG();
+
+	return pDev->started;
+}
+
+int grcan_flush(void *d)
+{
+	struct grcan_priv *pDev = d;
+	int tmp;
+
+	FUNCDBG();
+
+	if ((pDev->started != STATE_STARTED) || pDev->flushing || pDev->config.silent)
+		return -1;
+
+	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 -1;
+	}
+
+	return 0;
+}
+
+int grcan_set_silent(void* d, int silent)
+{
+	struct grcan_priv *pDev = d;
+
+	FUNCDBG();
+
+	if (pDev->started == STATE_STARTED)
+		return -1;
+
+	pDev->config.silent = silent;
+	pDev->config_changed = 1;
+
+	return 0;
+}
+
+int grcan_set_abort(void* d, int abort)
+{
+	struct grcan_priv *pDev = d;
+
+	FUNCDBG();
+
+	if (pDev->started == STATE_STARTED)
+		return -1;
+
+	pDev->config.abort = abort;
+	/* This Configuration parameter doesn't need HurriCANe reset
+	 * ==> no pDev->config_changed = 1;
+	 */
+
+	return 0;
+}
+
+int grcan_set_selection(void *d, const struct grcan_selection *selection)
+{
+	struct grcan_priv *pDev = d;
+
+	FUNCDBG();
+
+	if (pDev->started == STATE_STARTED)
+		return -1;
+
+	if ( !selection )
+		return -2;
+
+	pDev->config.selection = *selection;
+	pDev->config_changed = 1;
+
+	return 0;
+}
+
+int grcan_set_rxblock(void *d, int block)
+{
+	struct grcan_priv *pDev = d;
+
+	FUNCDBG();
+
+	pDev->rxblock = block;
+
+	return 0;
+}
+
+int grcan_set_txblock(void *d, int block)
+{
+	struct grcan_priv *pDev = d;
+
+	FUNCDBG();
+
+	pDev->txblock = block;
+
+	return 0;
+}
+
+int grcan_set_txcomplete(void *d, int complete)
+{
+	struct grcan_priv *pDev = d;
+
+	FUNCDBG();
+
+	pDev->txcomplete = complete;
+
+	return 0;
+}
+
+int grcan_set_rxcomplete(void *d, int complete)
+{
+	struct grcan_priv *pDev = d;
+
+	FUNCDBG();
+
+	pDev->rxcomplete = complete;
+
+	return 0;
+}
+
+int grcan_get_stats(void *d, struct grcan_stats *stats)
+{
+	struct grcan_priv *pDev = d;
+	SPIN_IRQFLAGS(oldLevel);
+
+	FUNCDBG();
+
+	if ( !stats )
+		return -1;
+
+	SPIN_LOCK_IRQ(&pDev->devlock, oldLevel);
+	*stats = pDev->stats;
+	SPIN_UNLOCK_IRQ(&pDev->devlock, oldLevel);
+
+	return 0;
+}
+
+int grcan_clr_stats(void *d)
+{
+	struct grcan_priv *pDev = d;
+	SPIN_IRQFLAGS(oldLevel);
+
+	FUNCDBG();
+
+	SPIN_LOCK_IRQ(&pDev->devlock, oldLevel);
+	memset(&pDev->stats,0,sizeof(struct grcan_stats));
+	SPIN_UNLOCK_IRQ(&pDev->devlock, oldLevel);
+
+	return 0;
+}
+
+int grcan_set_speed(void *d, unsigned int speed)
+{
+	struct grcan_priv *pDev = d;
+	struct grcan_timing timing;
+	int ret;
+
+	FUNCDBG();
+
+	/* cannot change speed during run mode */
+	if (pDev->started == STATE_STARTED)
+		return -1;
+
+	/* get speed rate from argument */
+	ret = grcan_calc_timing(speed, pDev->corefreq_hz, GRCAN_SAMPLING_POINT, &timing);
+	if ( ret )
+		return -2;
+
+	/* save timing/speed */
+	pDev->config.timing = timing;
+	pDev->config_changed = 1;
+
+	return 0;
+}
+
+int grcan_set_btrs(void *d, const struct grcan_timing *timing)
+{
+	struct grcan_priv *pDev = d;
+
+	FUNCDBG();
+
+	/* Set BTR registers manually
+	 * Read GRCAN/HurriCANe Manual.
+	 */
+	if (pDev->started == STATE_STARTED)
+		return -1;
+
+	if ( !timing )
+		return -2;
+
+	pDev->config.timing = *timing;
+	pDev->config_changed = 1;
+
+	return 0;
+}
+
+int grcan_set_afilter(void *d, const struct grcan_filter *filter)
+{
+	struct grcan_priv *pDev = d;
+
+	FUNCDBG();
+
+	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);
+
+	return 0;
+}
+
+int grcan_set_sfilter(void *d, const struct grcan_filter *filter)
+{
+	struct grcan_priv *pDev = d;
+	SPIN_IRQFLAGS(oldLevel);
+
+	FUNCDBG();
+
+	if ( !filter ){
+		/* disable TX/RX SYNC filtering */
+		pDev->sfilter.mask = 0xffffffff;
+		pDev->sfilter.mask = 0;
+
+		 /* disable Sync interrupt */
+		SPIN_LOCK_IRQ(&pDev->devlock, oldLevel);
+		pDev->regs->imr = READ_REG(&pDev->regs->imr) & ~(GRCAN_RXSYNC_IRQ|GRCAN_TXSYNC_IRQ);
+		SPIN_UNLOCK_IRQ(&pDev->devlock, oldLevel);
+	}else{
+		/* Save filter */
+		pDev->sfilter = *filter;
+
+		/* Enable Sync interrupt */
+		SPIN_LOCK_IRQ(&pDev->devlock, oldLevel);
+		pDev->regs->imr = READ_REG(&pDev->regs->imr) | (GRCAN_RXSYNC_IRQ|GRCAN_TXSYNC_IRQ);
+		SPIN_UNLOCK_IRQ(&pDev->devlock, oldLevel);
+	}
+	/* Set Sync RX/TX filter */
+	grcan_hw_sync(pDev->regs,&pDev->sfilter);
+
+	return 0;
+}
+
+int grcan_get_status(void* d, unsigned int *data)
+{
+	struct grcan_priv *pDev = d;
+
+	FUNCDBG();
+
+	if ( !data )
+		return -1;
+
+	/* Read out the statsu register from the GRCAN core */
+	data[0] = READ_REG(&pDev->regs->stat);
+
+	return 0;
+}
+
+/* Error indicators */
+#define GRCAN_IRQ_ERRORS \
+		(GRCAN_RXAHBERR_IRQ | GRCAN_TXAHBERR_IRQ | GRCAN_OFF_IRQ)
+#define GRCAN_STAT_ERRORS (GRCAN_STAT_AHBERR | GRCAN_STAT_OFF)
+/* Warning & RX/TX sync indicators */
+#define GRCAN_IRQ_WARNS \
+		(GRCAN_ERR_IRQ | GRCAN_OR_IRQ | GRCAN_TXLOSS_IRQ | \
+		 GRCAN_RXSYNC_IRQ | GRCAN_TXSYNC_IRQ)
+#define GRCAN_STAT_WARNS (GRCAN_STAT_OR | GRCAN_STAT_PASS)
+
+/* Handle the IRQ */
+static void grcan_interrupt(void *arg)
+{
+	struct grcan_priv *pDev = arg;
+	unsigned int status = READ_REG(&pDev->regs->pimsr);
+	unsigned int canstat = READ_REG(&pDev->regs->stat);
+	unsigned int imr_clear;
+	SPIN_ISR_IRQFLAGS(irqflags);
+
+	/* Spurious IRQ call? */
+	if ( !status && !canstat )
+		return;
+
+	if (pDev->started != STATE_STARTED) {
+		DBGC(DBG_STATE, "not STARTED (unexpected interrupt)\n");
+		pDev->regs->picr = status;
+		return;
+	}
+
+	FUNCDBG();
+
+	if ( (status & GRCAN_IRQ_ERRORS) || (canstat & GRCAN_STAT_ERRORS) ) {
+		/* 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).
+		 */
+		SPIN_LOCK(&pDev->devlock, irqflags);
+		DBGC(DBG_STATE, "STARTED->BUSOFF|AHBERR\n");
+		pDev->stats.ints++;
+		if ((status & GRCAN_OFF_IRQ) || (canstat & GRCAN_STAT_OFF)) {
+			/* CAN Bus-off interrupt */
+			DBGC(DBG_STATE, "BUSOFF: status: 0x%x, canstat: 0x%x\n",
+				status, canstat);
+			pDev->started = STATE_BUSOFF;
+			pDev->stats.busoff_cnt++;
+		} else {
+			/* RX or Tx AHB Error interrupt */
+			printk("AHBERROR: status: 0x%x, canstat: 0x%x\n",
+				status, canstat);
+			pDev->started = STATE_AHBERR;
+			pDev->stats.ahberr_cnt++;
+		}
+		grcan_hw_stop(pDev); /* this mask all IRQ sources */
+		pDev->regs->picr = 0x1ffff; /* clear all interrupts */
+		/*
+		 * Prevent driver from affecting bus. Driver can be started
+		 * again with grcan_start().
+		 */
+		SPIN_UNLOCK(&pDev->devlock, irqflags);
+
+		/* Release semaphores to wake blocked threads. */
+		grcan_sw_stop(pDev);
+
+		/*
+		 * NOTE: Another interrupt may be pending now so ISR could be
+		 * executed one more time aftert this (first) return.
+		 */
+		return;
+	}
+
+	/* Mask interrupts in one place under spin-lock. */
+	imr_clear = status & (GRCAN_RXIRQ_IRQ | GRCAN_TXIRQ_IRQ | GRCAN_TXEMPTY_IRQ);
+
+	SPIN_LOCK(&pDev->devlock, irqflags);
+
+	/* Increment number of interrupts counter */
+	pDev->stats.ints++;
+	if ((status & GRCAN_IRQ_WARNS) || (canstat & GRCAN_STAT_WARNS)) {
+
+		if ( (status & GRCAN_ERR_IRQ) || (canstat & GRCAN_STAT_PASS) ) {
+			/* Error-Passive interrupt */
+			pDev->stats.passive_cnt++;
+		}
+
+		if ( (status & GRCAN_OR_IRQ) || (canstat & GRCAN_STAT_OR) ) {
+			/* Over-run during reception interrupt */
+			pDev->stats.overrun_cnt++;
+		}
+
+		if ( status & GRCAN_TXLOSS_IRQ ) {
+			pDev->stats.txloss_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 (imr_clear) {
+		pDev->regs->imr = READ_REG(&pDev->regs->imr) & ~imr_clear;
+
+		SPIN_UNLOCK(&pDev->devlock, irqflags);
+
+		if ( status & GRCAN_RXIRQ_IRQ ) {
+			/* RX IRQ pointer interrupt */
+			rtems_semaphore_release(pDev->rx_sem);
+		}
+
+		if ( status & GRCAN_TXIRQ_IRQ ) {
+			/* TX IRQ pointer interrupt */
+			rtems_semaphore_release(pDev->tx_sem);
+		}
+
+		if (status & GRCAN_TXEMPTY_IRQ ) {
+			rtems_semaphore_release(pDev->txempty_sem);
+		}
+	} else {
+		SPIN_UNLOCK(&pDev->devlock, irqflags);
+	}
+
+	/* Clear IRQs */
+	pDev->regs->picr = status;
+}
diff --git a/bsps/shared/grlib/can/occan.c b/bsps/shared/grlib/can/occan.c
new file mode 100644
index 0000000000..59b4f234f6
--- /dev/null
+++ b/bsps/shared/grlib/can/occan.c
@@ -0,0 +1,1971 @@
+/*  OC_CAN driver
+ *
+ *  COPYRIGHT (c) 2007.
+ *  Cobham Gaisler AB.
+ *
+ *  The license and distribution terms for this file may be
+ *  found in the file LICENSE in this distribution or at
+ *  http://www.rtems.org/license/LICENSE.
+ */
+
+#include <rtems.h>
+#include <rtems/libio.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+#include <bsp.h>
+#include <rtems/bspIo.h> /* printk */
+
+#include <drvmgr/drvmgr.h>
+#include <grlib/ambapp_bus.h>
+#include <grlib/occan.h>
+
+#include <grlib/grlib_impl.h>
+
+/* RTEMS -> ERRNO decoding table
+
+rtems_assoc_t errno_assoc[] = {
+    { "OK",                 RTEMS_SUCCESSFUL,                0 },
+    { "BUSY",               RTEMS_RESOURCE_IN_USE,           EBUSY },
+    { "INVALID NAME",       RTEMS_INVALID_NAME,              EINVAL },
+    { "NOT IMPLEMENTED",    RTEMS_NOT_IMPLEMENTED,           ENOSYS },
+    { "TIMEOUT",            RTEMS_TIMEOUT,                   ETIMEDOUT },
+    { "NO MEMORY",          RTEMS_NO_MEMORY,                 ENOMEM },
+    { "NO DEVICE",          RTEMS_UNSATISFIED,               ENODEV },
+    { "INVALID NUMBER",     RTEMS_INVALID_NUMBER,            EBADF},
+    { "NOT RESOURCE OWNER", RTEMS_NOT_OWNER_OF_RESOURCE,     EPERM},
+    { "IO ERROR",           RTEMS_IO_ERROR,                  EIO},
+    { 0, 0, 0 },
+};
+
+*/
+
+/*
+#undef DEBUG
+#undef DEBUG_EXTRA
+#undef DEBUG_PRINT_REGMAP
+*/
+
+/* default to byte regs */
+#ifndef OCCAN_WORD_REGS
+ #define OCCAN_BYTE_REGS
+#else
+ #undef OCCAN_BYTE_REGS
+#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
+#define DEFAULT_CLKDIV 0x7
+#define DEFAULT_EXTENDED_MODE 1
+#define DEFAULT_RX_FIFO_LEN 64
+#define DEFAULT_TX_FIFO_LEN 64
+
+/* not implemented yet */
+#undef REDUNDANT_CHANNELS
+
+/* Define common debug macros */
+#ifdef DEBUG
+	#define DBG(fmt, vargs...) printk(fmt, ## vargs )
+#else
+	#define DBG(fmt, vargs...)
+#endif
+
+/* fifo interface */
+typedef struct {
+	int cnt;
+	int ovcnt; /* overwrite count */
+	int full; /* 1 = base contain cnt CANMsgs, tail==head */
+	CANMsg *tail, *head;
+	CANMsg *base;
+	CANMsg fifoarea[0];
+} occan_fifo;
+
+/* PELICAN */
+
+typedef struct {
+	unsigned char
+		mode,
+		cmd,
+		status,
+		intflags,
+		inten,
+		resv0,
+		bustim0,
+		bustim1,
+		unused0[2],
+		resv1,
+		arbcode,
+		errcode,
+		errwarn,
+		rx_err_cnt,
+		tx_err_cnt,
+		rx_fi_xff; /* this is also acceptance code 0 in reset mode */
+		union{
+			struct {
+				unsigned char id[2];
+				unsigned char data[8];
+				unsigned char next_in_fifo[2];
+			} rx_sff;
+			struct {
+				unsigned char id[4];
+				unsigned char data[8];
+			} rx_eff;
+			struct {
+				unsigned char id[2];
+				unsigned char data[8];
+				unsigned char unused[2];
+			} tx_sff;
+			struct {
+				unsigned char id[4];
+				unsigned char data[8];
+			} tx_eff;
+			struct {
+				unsigned char code[3];
+				unsigned char mask[4];
+			} rst_accept;
+		} msg;
+		unsigned char rx_msg_cnt;
+		unsigned char unused1;
+		unsigned char clkdiv;
+} pelican8_regs;
+
+typedef struct {
+	unsigned char
+		mode, unused0[3],
+		cmd, unused1[3],
+		status, unused2[3],
+		intflags, unused3[3],
+		inten, unused4[3],
+		resv0, unused5[3],
+		bustim0, unused6[3],
+		bustim1, unused7[3],
+		unused8[8],
+		resv1,unused9[3],
+		arbcode,unused10[3],
+		errcode,unused11[3],
+		errwarn,unused12[3],
+		rx_err_cnt,unused13[3],
+		tx_err_cnt,unused14[3],
+		rx_fi_xff, unused15[3]; /* this is also acceptance code 0 in reset mode */
+		/* make sure to use pointers when writing (byte access) to these registers */
+		union{
+			struct {
+				unsigned int id[2];
+				unsigned int data[8];
+				unsigned int next_in_fifo[2];
+			} rx_sff;
+			struct {
+				unsigned int id[4];
+				unsigned int data[8];
+			} rx_eff;
+			struct {
+				unsigned int id[2];
+				unsigned int data[8];
+			} tx_sff;
+			struct {
+				unsigned int id[4];
+				unsigned int data[8];
+			} tx_eff;
+			struct {
+				unsigned int code[3];
+				unsigned int mask[4];
+			} rst_accept;
+		} msg;
+		unsigned char rx_msg_cnt,unused16[3];
+		unsigned char unused17[4];
+		unsigned char clkdiv,unused18[3];
+} 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
+
+#if 0
+typedef struct {
+	unsigned char brp;
+	unsigned char sjw;
+	unsigned char tseg1;
+	unsigned char tseg2;
+	unsigned char sam;
+} occan_speed_regs;
+#endif
+typedef struct {
+	unsigned char btr0;
+	unsigned char btr1;
+} occan_speed_regs;
+
+typedef struct {
+	struct drvmgr_dev *dev;
+	char devName[32];
+	SPIN_DECLARE(devlock);
+
+	/* 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;
+	rtems_id txsem;
+	rtems_id rxsem;
+	int open;
+	int started;
+	int rxblk;
+	int txblk;
+	int sending;
+	unsigned int status;
+	occan_stats stats;
+
+	/* rx&tx fifos */
+	occan_fifo *rxfifo;
+	occan_fifo *txfifo;
+
+	/* Config */
+	unsigned int speed; /* speed in HZ */
+	unsigned char acode[4];
+	unsigned char amask[4];
+} occan_priv;
+
+/********** FIFO INTERFACE **********/
+static void occan_fifo_put(occan_fifo *fifo);
+static CANMsg *occan_fifo_put_claim(occan_fifo *fifo, int force);
+static occan_fifo *occan_fifo_create(int cnt);
+static void occan_fifo_free(occan_fifo *fifo);
+static int occan_fifo_full(occan_fifo *fifo);
+static int occan_fifo_empty(occan_fifo *fifo);
+static void occan_fifo_get(occan_fifo *fifo);
+static CANMsg *occan_fifo_claim_get(occan_fifo *fifo);
+static void occan_fifo_clr(occan_fifo *fifo);
+
+/**** Hardware related Interface ****/
+static int occan_calc_speedregs(unsigned int clock_hz, unsigned int rate, occan_speed_regs *result);
+static int occan_set_speedregs(occan_priv *priv, occan_speed_regs *timing);
+static void pelican_init(occan_priv *priv);
+static void pelican_open(occan_priv *priv);
+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);
+void occan_interrupt(void *arg);
+#ifdef DEBUG_PRINT_REGMAP
+static void pelican_regadr_print(pelican_regs *regs);
+#endif
+
+/***** Driver related interface *****/
+static rtems_device_driver occan_ioctl(rtems_device_major_number major, rtems_device_minor_number minor, void *arg);
+static rtems_device_driver occan_write(rtems_device_major_number major, rtems_device_minor_number minor, void *arg);
+static rtems_device_driver occan_read(rtems_device_major_number major, rtems_device_minor_number minor, void *arg);
+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);
+
+#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 */
+
+
+/* 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)
+
+static 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);
+	}
+	return *(volatile unsigned char *)adr;
+}
+
+static 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
+#define PELICAN_MOD_LISTEN         0x2
+#define PELICAN_MOD_SELFTEST       0x4
+#define PELICAN_MOD_ACCEPT         0x8
+
+/* Command register bit definitions */
+#define PELICAN_CMD_TXREQ          0x1
+#define PELICAN_CMD_ABORT          0x2
+#define PELICAN_CMD_RELRXBUF       0x4
+#define PELICAN_CMD_CLRDOVR        0x8
+#define PELICAN_CMD_SELFRXRQ       0x10
+
+/* Status register bit definitions */
+#define PELICAN_STAT_RXBUF         0x1
+#define PELICAN_STAT_DOVR          0x2
+#define PELICAN_STAT_TXBUF         0x4
+#define PELICAN_STAT_TXOK          0x8
+#define PELICAN_STAT_RX            0x10
+#define PELICAN_STAT_TX            0x20
+#define PELICAN_STAT_ERR           0x40
+#define PELICAN_STAT_BUS           0x80
+
+/* Interrupt register bit definitions */
+#define PELICAN_IF_RX         0x1
+#define PELICAN_IF_TX         0x2
+#define PELICAN_IF_ERRW       0x4
+#define PELICAN_IF_DOVR       0x8
+#define PELICAN_IF_ERRP       0x20
+#define PELICAN_IF_ARB        0x40
+#define PELICAN_IF_BUS        0x80
+
+/* Interrupt Enable register bit definitions */
+#define PELICAN_IE_RX         0x1
+#define PELICAN_IE_TX         0x2
+#define PELICAN_IE_ERRW       0x4
+#define PELICAN_IE_DOVR       0x8
+#define PELICAN_IE_ERRP       0x20
+#define PELICAN_IE_ARB        0x40
+#define PELICAN_IE_BUS        0x80
+
+/* Arbitration lost capture register bit definitions */
+#define PELICAN_ARB_BITS      0x1f
+
+/*  register bit definitions */
+#define PELICAN_ECC_CODE_BIT     0x00
+#define PELICAN_ECC_CODE_FORM    0x40
+#define PELICAN_ECC_CODE_STUFF   0x80
+#define PELICAN_ECC_CODE_OTHER   0xc0
+#define PELICAN_ECC_CODE    0xc0
+
+#define PELICAN_ECC_DIR     0x20
+#define PELICAN_ECC_SEG     0x1f
+
+/* Clock divider register bit definitions */
+#define PELICAN_CDR_DIV  0x7
+#define PELICAN_CDR_OFF  0x8
+#define PELICAN_CDR_MODE 0x80
+#define PELICAN_CDR_MODE_PELICAN 0x80
+#define PELICAN_CDR_MODE_BITS 7
+#define PELICAN_CDR_MODE_BASICAN 0x00
+
+
+/*  register bit definitions */
+#define OCCAN_BUSTIM_SJW       0xc0
+#define OCCAN_BUSTIM_BRP       0x3f
+#define OCCAN_BUSTIM_SJW_BIT   6
+
+#define OCCAN_BUSTIM_SAM       0x80
+#define OCCAN_BUSTIM_TSEG2     0x70
+#define OCCAN_BUSTIM_TSEG2_BIT 4
+#define OCCAN_BUSTIM_TSEG1     0x0f
+
+/*  register bit definitions */
+/*
+#define PELICAN_S_       0x1
+#define PELICAN_S_       0x2
+#define PELICAN_S_       0x4
+#define PELICAN_S_       0x8
+#define PELICAN_S_       0x10
+#define PELICAN_S_       0x20
+#define PELICAN_S_       0x40
+#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 = grlib_calloc(1, sizeof(*priv));
+	if ( !priv )
+		return DRVMGR_NOMEM;
+	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 */
+	WRITE_REG(priv, &priv->regs->mode, PELICAN_MOD_RESET);
+
+	/* wait for core to reset complete */
+	/*usleep(1);*/
+}
+
+static void pelican_open(occan_priv *priv){
+	int ret;
+
+	/* Set defaults */
+	priv->speed = OCCAN_SPEED_250K;
+
+	/* set acceptance filters to accept all messages */
+	priv->acode[0] = 0;
+	priv->acode[1] = 0;
+	priv->acode[2] = 0;
+	priv->acode[3] = 0;
+	priv->amask[0] = 0xff;
+	priv->amask[1] = 0xff;
+	priv->amask[2] = 0xff;
+	priv->amask[3] = 0xff;
+
+	/* Set clock divider to extended mode, clkdiv not connected
+	 */
+	WRITE_REG(priv, &priv->regs->clkdiv, (1<<PELICAN_CDR_MODE_BITS) | (DEFAULT_CLKDIV & PELICAN_CDR_DIV));
+
+	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(priv->sys_freq_hz, priv->speed,
+			&priv->timing);
+	}
+
+	/* disable all interrupts */
+	WRITE_REG(priv, &priv->regs->inten, 0);
+
+	/* clear pending interrupts by reading */
+	READ_REG(priv, &priv->regs->intflags);
+}
+
+static int pelican_start(occan_priv *priv){
+	/* 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);
+
+	/* Clear status bits */
+	priv->status = 0;
+	priv->sending = 0;
+
+	/* clear pending interrupts */
+	READ_REG(priv, &priv->regs->intflags);
+
+	/* clear error counters */
+	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) ){
+		/* Select the first (default) channel */
+		OCCAN_SET_CHANNEL(priv,0);
+	}else{
+		/* set gpio bit, or something */
+		OCCAN_SET_CHANNEL(priv,priv->channel);
+	}
+#endif
+	/* 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, &priv->regs->bustim0),
+		READ_REG(priv, &priv->regs->bustim1));
+
+	/* Set default acceptance filter */
+	pelican_set_accept(priv,priv->acode,priv->amask);
+
+	/* 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);
+	occan_stat_print(&priv->stats);
+#endif
+
+	/* core already in reset mode,
+	 *  ¤ Exit reset mode 
+	 *  ¤ Enter Single/Dual mode filtering.
+	 */
+	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)
+{
+	/* stop HW */
+
+	drvmgr_interrupt_unregister(priv->dev, 0, occan_interrupt, priv);
+
+#ifdef DEBUG
+	/* print setup before stopping */
+	pelican_regs_print(priv->regs);
+	occan_stat_print(&priv->stats);
+#endif
+
+	/* put core in reset mode */
+	WRITE_REG(priv, &priv->regs->mode, PELICAN_MOD_RESET);
+
+	/* turn off interrupts */
+	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.
+ *
+ * Be sure to have disabled CAN interrupts when
+ * entering this function.
+ */
+static int pelican_send(occan_priv *can, CANMsg *msg){
+	unsigned char tmp;
+	pelican_regs *regs = can->regs;
+
+	/* is there room in send buffer? */
+	if ( !pelican_tx_ready(can) ) {
+		/* tx fifo taken, we have to wait */
+		return -1;
+	}
+
+	tmp = msg->len & 0xf;
+	if ( msg->rtr )
+		tmp |= 0x40;
+
+	if ( msg->extended ){
+		/* Extended Frame */
+		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(can, &regs->msg.tx_eff.data[tmp], msg->data[tmp]);
+		}
+	}else{
+		/* Standard Frame */
+		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(can, &regs->msg.tx_sff.data[tmp],msg->data[tmp]);
+		}
+	}
+
+	/* let HW know of new message */
+	if ( msg->sshot ){
+		WRITE_REG(can, &regs->cmd, PELICAN_CMD_TXREQ | PELICAN_CMD_ABORT);
+	}else{
+		/* normal case -- try resend until sent */
+		WRITE_REG(can, &regs->cmd, PELICAN_CMD_TXREQ);
+	}
+
+	return 0;
+}
+
+
+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;
+
+	acode0 = &priv->regs->rx_fi_xff;
+	acode1 = (unsigned char *)&priv->regs->msg.rst_accept.code[0];
+	acode2 = (unsigned char *)&priv->regs->msg.rst_accept.code[1];
+	acode3 = (unsigned char *)&priv->regs->msg.rst_accept.code[2];
+
+	amask0 = (unsigned char *)&priv->regs->msg.rst_accept.mask[0];
+	amask1 = (unsigned char *)&priv->regs->msg.rst_accept.mask[1];
+	amask2 = (unsigned char *)&priv->regs->msg.rst_accept.mask[2];
+	amask3 = (unsigned char *)&priv->regs->msg.rst_accept.mask[3];
+
+	/* Set new mask & code */
+	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]);
+}
+
+
+/* This function calculates BTR0 and BTR1 values for a given bitrate.
+ *
+ * Set communication parameters.
+ * \param clock_hz OC_CAN Core frequency in Hz.
+ * \param rate Requested baud rate in bits/second.
+ * \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)
+{
+	int best_error = 1000000000;
+	int error;
+	int best_tseg=0, best_brp=0, brp=0;
+	int tseg=0, tseg1=0, tseg2=0;
+	int sjw = 0;
+	int clock = clock_hz / 2;
+	int sampl_pt = 90;
+
+	if ( (rate<5000) || (rate>1000000) ){
+		/* invalid speed mode */
+		return -1;
+	}
+
+	/* find best match, return -2 if no good reg
+	 * combination is available for this frequency */
+
+	/* some heuristic specials */
+	if (rate > ((1000000 + 500000) / 2))
+		sampl_pt = 75;
+
+	if (rate < ((12500 + 10000) / 2))
+		sampl_pt = 75;
+
+	if (rate < ((100000 + 125000) / 2))
+		sjw = 1;
+
+	/* tseg even = round down, odd = round up */
+	for (tseg = (0 + 0 + 2) * 2;
+	     tseg <= (MAX_TSEG2 + MAX_TSEG1 + 2) * 2 + 1;
+	     tseg++)
+	{
+		brp = clock / ((1 + tseg / 2) * rate) + tseg % 2;
+		if ((brp == 0) || (brp > 64))
+			continue;
+
+		error = rate - clock / (brp * (1 + tseg / 2));
+		if (error < 0)
+		{
+			error = -error;
+		}
+
+		if (error <= best_error)
+		{
+			best_error = error;
+			best_tseg = tseg/2;
+			best_brp = brp-1;
+		}
+	}
+
+	if (best_error && (rate / best_error < 10))
+	{
+		printk("OCCAN: bitrate %d is not possible with %d Hz clock\n\r",rate, clock);
+		return -2;
+	}else if ( !result )
+		return 0; /* nothing to store result in, but a valid bitrate can be calculated */
+
+	tseg2 = best_tseg - (sampl_pt * (best_tseg + 1)) / 100;
+
+	if (tseg2 < 0)
+	{
+		tseg2 = 0;
+	}
+
+	if (tseg2 > MAX_TSEG2)
+	{
+		tseg2 = MAX_TSEG2;
+	}
+
+	tseg1 = best_tseg - tseg2 - 2;
+
+	if (tseg1 > MAX_TSEG1)
+	{
+		tseg1 = MAX_TSEG1;
+		tseg2 = best_tseg - tseg1 - 2;
+	}
+
+	result->btr0 = (sjw<<OCCAN_BUSTIM_SJW_BIT) | (best_brp&OCCAN_BUSTIM_BRP);
+	result->btr1 = (0<<7) | (tseg2<<OCCAN_BUSTIM_TSEG2_BIT) | tseg1;
+
+	return 0;
+}
+
+static int occan_set_speedregs(occan_priv *priv, occan_speed_regs *timing)
+{
+	if ( !timing || !priv || !priv->regs)
+		return -1;
+
+	WRITE_REG(priv, &priv->regs->bustim0, timing->btr0);
+	WRITE_REG(priv, &priv->regs->bustim1, timing->btr1);
+
+	return 0;
+}
+
+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);
+
+	/* get can device */	
+	if ( drvmgr_get_dev(&occan_drv_info.general, minor, &dev) ) {
+		DBG("Wrong minor %d\n", minor);
+		return RTEMS_UNSATISFIED; /* NODEV */
+	}
+	can = (occan_priv *)dev->priv;
+
+	/* already opened? */
+	rtems_semaphore_obtain(can->devsem, RTEMS_WAIT, RTEMS_NO_TIMEOUT);
+	if ( can->open ){
+		rtems_semaphore_release(can->devsem);
+		return RTEMS_RESOURCE_IN_USE; /* EBUSY */
+	}
+	can->open = 1;
+	rtems_semaphore_release(can->devsem);
+
+	SPIN_INIT(&can->devlock, can->devName);
+
+	/* allocate fifos */
+	can->rxfifo = occan_fifo_create(DEFAULT_RX_FIFO_LEN);
+	if ( !can->rxfifo ){
+		can->open = 0;
+		return RTEMS_NO_MEMORY; /* ENOMEM */
+	}
+
+	can->txfifo = occan_fifo_create(DEFAULT_TX_FIFO_LEN);
+	if ( !can->txfifo ){
+		occan_fifo_free(can->rxfifo);
+		can->rxfifo= NULL;
+		can->open = 0;
+		return RTEMS_NO_MEMORY; /* ENOMEM */
+	}
+
+	DBG("OCCAN: Opening %d success\n\r",minor);
+
+	can->started = 0;
+	can->channel = 0; /* Default to first can link */
+	can->txblk = 1; /* Default to Blocking mode */
+	can->rxblk = 1; /* Default to Blocking mode */
+	can->single_mode = 1; /* single mode acceptance filter */
+
+	/* reset stat counters */
+	memset(&can->stats,0,sizeof(occan_stats));
+
+	/* HW must be in reset mode here (close and initializes resets core...)
+	 *
+	 * 1. set default modes/speeds
+	 */
+	pelican_open(can);
+
+	return RTEMS_SUCCESSFUL;
+}
+
+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 */
+	WRITE_REG(can, &can->regs->mode, PELICAN_MOD_RESET);
+
+	/* free fifo memory */
+	occan_fifo_free(can->rxfifo);
+	occan_fifo_free(can->txfifo);
+
+	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;
+	struct drvmgr_dev *dev;
+	rtems_libio_rw_args_t *rw_args=(rtems_libio_rw_args_t *) arg;
+	CANMsg *dstmsg, *srcmsg;
+	SPIN_IRQFLAGS(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*/
+	}
+
+	/* does at least one message fit */
+	left = rw_args->count;
+	if ( left < sizeof(CANMsg) ){
+		DBG("OCCAN: minor %d length of buffer must be at least %d, our is %d\n\r",minor,sizeof(CANMsg),left);
+		return  RTEMS_INVALID_NAME; /* -EINVAL */
+	}
+
+	/* get pointer to start where to put CAN messages */
+	dstmsg = (CANMsg *)rw_args->buffer;
+	if ( !dstmsg ){
+		DBG("OCCAN: minor %d read: input buffer is NULL\n\r",minor);
+		return  RTEMS_INVALID_NAME; /* -EINVAL */
+	}
+
+	while (left >= sizeof(CANMsg) ){
+
+		/* turn off interrupts */
+		SPIN_LOCK_IRQ(&can->devlock, oldLevel);
+
+		/* A bus off interrupt may have occured after checking can->started */
+		if ( can->status & (OCCAN_STATUS_ERR_BUSOFF|OCCAN_STATUS_RESET) ){
+			SPIN_UNLOCK_IRQ(&can->devlock, oldLevel);
+			DBG("OCCAN: read is cancelled due to a BUS OFF error\n\r");
+			rw_args->bytes_moved = rw_args->count-left;
+			return RTEMS_IO_ERROR; /* EIO */
+		}
+
+		srcmsg = occan_fifo_claim_get(can->rxfifo);
+		if ( !srcmsg ){
+			/* no more messages in reception fifo.
+			 * Wait for incoming packets only if in
+			 * blocking mode AND no messages been
+			 * read before.
+			 */
+			if ( !can->rxblk || (left != rw_args->count) ){
+				/* turn on interrupts again */
+				SPIN_UNLOCK_IRQ(&can->devlock, oldLevel);
+				break;
+			}
+
+			/* turn on interrupts again */
+			SPIN_UNLOCK_IRQ(&can->devlock, oldLevel);
+
+			DBG("OCCAN: Waiting for RX int\n\r");
+
+			/* 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) ){
+				DBG("OCCAN: Blocking read got woken up by BUS OFF error\n\r");
+				/* At this point it should not matter how many messages we handled */
+				rw_args->bytes_moved = rw_args->count-left;
+				return RTEMS_IO_ERROR; /* EIO */
+			}
+
+			/* no errors detected, it must be a message */
+			continue;
+		}
+
+		/* got message, copy it to userspace buffer */
+		*dstmsg = *srcmsg;
+
+		/* Return borrowed message, RX interrupt can use it again */
+		occan_fifo_get(can->rxfifo);
+
+		/* turn on interrupts again */
+		SPIN_UNLOCK_IRQ(&can->devlock, oldLevel);
+
+		/* increase pointers */
+		left -= sizeof(CANMsg);
+		dstmsg++;
+	}
+
+	/* save number of read bytes. */
+	rw_args->bytes_moved = rw_args->count-left;
+	if ( rw_args->bytes_moved == 0 ){
+		DBG("OCCAN: minor %d read would block, returning\n\r",minor);
+		return RTEMS_TIMEOUT; /* ETIMEDOUT should be EAGAIN/EWOULDBLOCK */
+	}
+	return RTEMS_SUCCESSFUL;
+}
+
+static rtems_device_driver occan_write(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
+{
+	occan_priv *can;
+	struct drvmgr_dev *dev;
+	rtems_libio_rw_args_t *rw_args=(rtems_libio_rw_args_t *) arg;
+	CANMsg *msg,*fifo_msg;
+	SPIN_IRQFLAGS(oldLevel);
+	int left;
+
+	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 */
+
+	left = rw_args->count;
+	if ( (left < sizeof(CANMsg)) || (!rw_args->buffer) ){
+		return RTEMS_INVALID_NAME; /* EINVAL */
+	}
+
+	msg = (CANMsg *)rw_args->buffer;
+
+	/* limit CAN message length to 8 */
+	msg->len = (msg->len > 8) ? 8 : msg->len;
+
+#ifdef DEBUG_VERBOSE
+	pelican_regs_print(can->regs);
+	occan_stat_print(&can->stats);
+#endif
+
+	/* turn off interrupts */
+	SPIN_LOCK_IRQ(&can->devlock, oldLevel);
+
+	/* A bus off interrupt may have occured after checking can->started */
+	if ( can->status & (OCCAN_STATUS_ERR_BUSOFF|OCCAN_STATUS_RESET) ){
+		SPIN_UNLOCK_IRQ(&can->devlock, oldLevel);
+		rw_args->bytes_moved = 0;
+		return RTEMS_IO_ERROR; /* EIO */
+	}
+
+	/* If no messages in software tx fifo, we will
+	 * try to send first message by putting it directly
+	 * into the HW TX fifo.
+	 */
+	if ( occan_fifo_empty(can->txfifo) ){
+		/*pelican_regs_print(cans[minor+1].regs);*/
+		if ( !pelican_send(can,msg) ) {
+			/* First message put directly into HW TX fifo
+			 * 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++;
+
+			DBG("OCCAN: Sending direct via HW\n\r");
+		}
+	}
+
+	/* Put messages into software fifo */
+	while ( left >= sizeof(CANMsg) ){
+
+		/* limit CAN message length to 8 */
+		msg->len = (msg->len > 8) ? 8 : msg->len;
+
+		fifo_msg = occan_fifo_put_claim(can->txfifo,0);
+		if ( !fifo_msg ){
+
+			DBG("OCCAN: FIFO is full\n\r");
+			/* Block only if no messages previously sent
+			 * and no in blocking mode
+			 */
+			if ( !can->txblk  || (left != rw_args->count) )
+				break;
+
+			/* turn on interupts again and wait
+				INT_ON
+				WAIT FOR FREE BUF;
+				INT_OFF;
+				CHECK_IF_FIFO_EMPTY ==> SEND DIRECT VIA HW;
+			*/
+			SPIN_UNLOCK_IRQ(&can->devlock, oldLevel);
+
+			DBG("OCCAN: Waiting for tx int\n\r");
+
+			rtems_semaphore_obtain(can->txsem, 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) ){
+				DBG("OCCAN: Blocking write got woken up by BUS OFF error or RESET event\n\r");
+				/* At this point it should not matter how many messages we handled */
+				rw_args->bytes_moved = rw_args->count-left;
+				return RTEMS_IO_ERROR; /* EIO */
+			}
+
+			SPIN_LOCK_IRQ(&can->devlock, oldLevel);
+
+			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.
+					 */
+					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++;
+
+					DBG("OCCAN: Sending direct2 via HW\n\r");
+				}
+			}
+			continue;
+		}
+
+		/* copy message into fifo area */
+		*fifo_msg = *msg;
+
+		/* tell interrupt handler about the message */
+		occan_fifo_put(can->txfifo);
+
+		DBG("OCCAN: Put info fifo SW\n\r");
+
+		/* Prepare insert of next message */
+		msg++;
+		left-=sizeof(CANMsg);
+	}
+
+	SPIN_UNLOCK_IRQ(&can->devlock, oldLevel);
+
+	rw_args->bytes_moved = rw_args->count-left;
+	DBG("OCCAN: Sent %d\n\r",rw_args->bytes_moved);
+
+	if ( left == rw_args->count )
+		return RTEMS_TIMEOUT; /* ETIMEDOUT should be EAGAIN/EWOULDBLOCK */
+	return RTEMS_SUCCESSFUL;
+}
+
+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;
+	struct drvmgr_dev *dev;
+	unsigned int speed;
+	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:
+
+			/* cannot change speed during run mode */
+			if ( can->started )
+				return RTEMS_RESOURCE_IN_USE; /* EBUSY */
+
+			/* get speed rate from argument */
+			speed = (unsigned int)ioarg->buffer;
+			ret = occan_calc_speedregs(can->sys_freq_hz,speed,&timing);
+			if ( ret )
+				return  RTEMS_INVALID_NAME; /* EINVAL */
+
+			/* set the speed regs of the CAN core */
+			/* occan_set_speedregs(can,timing); */
+
+			/* save timing/speed */
+			can->speed = speed;
+			can->timing = timing;
+			break;
+
+		case OCCAN_IOC_SET_BTRS:
+			/* Set BTR registers manually
+			 * Read OCCAN Manual.
+			 */
+			if ( can->started )
+				return RTEMS_RESOURCE_IN_USE; /* EBUSY */
+
+			can->speed = 0; /* custom */
+			can->timing.btr1 = (unsigned int)ioarg->buffer & 0xff;
+			can->timing.btr0 = ((unsigned int)ioarg->buffer>>8) & 0xff;
+/*
+			can->timing.sjw = (btr0 >> OCCAN_BUSTIM_SJW_BIT) & 0x3;
+			can->timing.brp = btr0 & OCCAN_BUSTIM_BRP;
+			can->timing.tseg1 = btr1 & 0xf;
+			can->timing.tseg2 = (btr1 >> OCCAN_BUSTIM_TSEG2_BIT) & 0x7;
+			can->timing.sam = (btr1 >> 7) & 0x1;
+			*/
+			break;
+
+		case OCCAN_IOC_SPEED_AUTO:
+			return RTEMS_NOT_IMPLEMENTED;
+
+		case OCCAN_IOC_SET_BUFLEN:
+			/* set rx & tx fifo buffer length */
+			if ( can->started )
+				return RTEMS_RESOURCE_IN_USE; /* EBUSY */
+
+			rxcnt = (unsigned int)ioarg->buffer & 0x0000ffff;
+			txcnt = (unsigned int)ioarg->buffer >> 16;
+
+			occan_fifo_free(can->rxfifo);
+			occan_fifo_free(can->txfifo);
+
+			/* allocate new buffers */
+			can->rxfifo = occan_fifo_create(rxcnt);
+			can->txfifo = occan_fifo_create(txcnt);
+
+			if ( !can->rxfifo || !can->txfifo )
+				return RTEMS_NO_MEMORY; /* ENOMEM */
+			break;
+
+		case OCCAN_IOC_GET_CONF:
+			return RTEMS_NOT_IMPLEMENTED;
+			break;
+
+		case OCCAN_IOC_GET_STATS:
+			dststats = (occan_stats *)ioarg->buffer;
+			if ( !dststats )
+				return  RTEMS_INVALID_NAME; /* EINVAL */
+
+			/* copy data stats into userspace buffer */
+			if ( can->rxfifo )
+				can->stats.rx_sw_dovr = can->rxfifo->ovcnt;
+			*dststats = can->stats;
+			break;
+
+		case OCCAN_IOC_GET_STATUS:
+			/* return the status of the */
+			if ( !ioarg->buffer )
+				return RTEMS_INVALID_NAME;
+
+			*(unsigned int *)ioarg->buffer = can->status;
+			break;
+
+		/* Set physical link */
+		case OCCAN_IOC_SET_LINK:
+#ifdef REDUNDANT_CHANNELS
+			if ( can->started )
+				return RTEMS_RESOURCE_IN_USE; /* EBUSY */
+
+			/* switch HW channel */
+			can->channel = (unsigned int)ioargs->buffer;
+#else
+			return RTEMS_NOT_IMPLEMENTED;
+#endif
+			break;
+
+		case OCCAN_IOC_SET_FILTER:
+			if ( can->started )
+				return RTEMS_RESOURCE_IN_USE; /* EBUSY */
+
+			afilter = (struct occan_afilter *)ioarg->buffer;
+
+			if ( !afilter )
+				return RTEMS_INVALID_NAME; /* EINVAL */
+
+			/* copy acceptance filter */
+			can->acode[0] = afilter->code[0];
+			can->acode[1] = afilter->code[1];
+			can->acode[2] = afilter->code[2];
+			can->acode[3] = afilter->code[3];
+
+			can->amask[0] = afilter->mask[0];
+			can->amask[1] = afilter->mask[1];
+			can->amask[2] = afilter->mask[2];
+			can->amask[3] = afilter->mask[3];
+
+			can->single_mode = ( afilter->single_mode ) ? 1 : 0;
+
+			/* Acceptance filter is written to hardware
+			 * when starting.
+			 */
+			/* pelican_set_accept(can,can->acode,can->amask);*/
+			break;
+
+		case OCCAN_IOC_SET_BLK_MODE:
+			can->rxblk = (unsigned int)ioarg->buffer & OCCAN_BLK_MODE_RX;
+			can->txblk = ((unsigned int)ioarg->buffer & OCCAN_BLK_MODE_TX) >> 1;
+			break;
+
+		case OCCAN_IOC_START:
+			if ( can->started )
+				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; -- Is set in pelican_start due to interrupt may occur before we 
+			 * get here.
+			 */
+			break;
+
+		case OCCAN_IOC_STOP:
+			if ( !can->started )
+				return RTEMS_RESOURCE_IN_USE; /* EBUSY */
+			pelican_stop(can);
+			can->started = 0;
+			break;
+
+		default:
+			return RTEMS_NOT_DEFINED;
+	}
+	return RTEMS_SUCCESSFUL;
+}
+
+void occan_interrupt(void *arg)
+{
+	occan_priv *can = arg;
+	unsigned char iflags;
+	pelican_regs *regs = can->regs;
+	CANMsg *msg;
+	int signal_rx=0, signal_tx=0;
+	unsigned char tmp, errcode, arbcode;
+	int tx_error_cnt,rx_error_cnt;
+	SPIN_ISR_IRQFLAGS(irqflags);
+
+	if ( !can->started )
+		return; /* Spurious Interrupt, do nothing */
+
+	SPIN_LOCK(&can->devlock, irqflags);
+	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
+
+		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
+			 */
+
+			/* get empty (or make room) message */
+			msg = occan_fifo_put_claim(can->rxfifo,1);
+			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(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(can, &regs->msg.rx_eff.data[tmp]);
+				}
+				/*
+				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(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(can, &regs->msg.rx_sff.data[tmp]);
+				}
+				/*
+				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 */
+			WRITE_REG(can, &regs->cmd, PELICAN_CMD_RELRXBUF);
+
+			/* make message available to the user */
+			occan_fifo_put(can->rxfifo);
+
+			/* bump stat counters */
+			can->stats.rx_msgs++;
+
+			/* signal the semaphore only once */
+			signal_rx = 1;
+		}
+
+		if ( iflags & PELICAN_IF_TX ) {
+
+			/* there is room in tx fifo of HW */
+
+			if ( !occan_fifo_empty(can->txfifo) ){
+				/* send 1 more messages */
+				msg = occan_fifo_claim_get(can->txfifo);
+
+				if ( pelican_send(can,msg) ){
+					/* ERROR! We got an TX interrupt telling us
+					 * tx fifo is empty, yet it is not.
+					 *
+					 * Complain about this max 10 times
+					 */
+					if ( can->stats.tx_buf_error < 10 ){
+						printk("OCCAN: got TX interrupt but TX fifo in not empty (%d)\n\r",can->stats.tx_buf_error);
+					}
+					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);
+
+				/* bump stat counters */
+				can->stats.tx_msgs++;
+
+				/* wake any sleeping thread waiting for "fifo not full" */
+				signal_tx = 1;
+			}
+		}
+
+		if ( iflags & PELICAN_IF_ERRW ){
+			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) ){
+				/* in Error Active Warning area or BUS OFF */
+				can->status |= OCCAN_STATUS_WARN;
+
+				/* check reset bit for reset mode */
+				if ( READ_REG(can, &regs->mode) & PELICAN_MOD_RESET ){
+					/* in reset mode ==> bus off */
+					can->status |= OCCAN_STATUS_ERR_BUSOFF | OCCAN_STATUS_RESET;
+
+					/***** pelican_stop(can) ******
+					 * turn off interrupts
+					 * enter reset mode (HW already done that for us)
+					 */
+					WRITE_REG(can, &regs->inten,0);
+
+					/* Indicate that we are not started any more.
+					 * This will make write/read return with EBUSY
+					 * on read/write attempts.
+					 *
+					 * User must issue a ioctl(START) to get going again.
+					 */
+					can->started = 0;
+
+					/* signal any waiting read/write threads, so that they
+					 * can handle the bus error.
+					 */
+					signal_rx = 1;
+					signal_tx = 1;
+
+					/* ingnore any old pending interrupt */
+					break;
+				}
+
+			}else{
+				/* not in Upper Error Active area any more */
+				can->status &= ~(OCCAN_STATUS_WARN);
+			}
+			can->stats.err_warn++;
+		}
+
+		if ( iflags & PELICAN_IF_DOVR){
+			can->status |= OCCAN_STATUS_OVERRUN;
+			can->stats.err_dovr++;
+			DBG("OCCAN_INT: DOVR\n\r");
+		}
+
+		if ( iflags & PELICAN_IF_ERRP){
+			/* Let the error counters decide what kind of
+			 * interrupt it was. In/Out of EPassive area.
+			 */
+			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;
+			}else{
+				can->status &= ~(OCCAN_STATUS_ERR_PASSIVE);
+			}
+
+			/* increase Error Passive In/out interrupt counter */
+			can->stats.err_errp++;
+		}
+
+		if ( iflags & PELICAN_IF_ARB){
+			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);
+		}
+
+		if ( iflags & PELICAN_IF_BUS){
+			/* Some kind of BUS error, only used for
+			 * statistics. Error Register is decoded
+			 * and put into can->stats.
+			 */
+			errcode = READ_REG(can, &regs->errcode);
+			switch( errcode & PELICAN_ECC_CODE ){
+				case PELICAN_ECC_CODE_BIT:
+					can->stats.err_bus_bit++;
+					break;
+				case PELICAN_ECC_CODE_FORM:
+					can->stats.err_bus_form++;
+					break;
+				case PELICAN_ECC_CODE_STUFF:
+					can->stats.err_bus_stuff++;
+					break;
+				case PELICAN_ECC_CODE_OTHER:
+					can->stats.err_bus_other++;
+					break;
+			}
+
+			/* Get Direction (TX/RX) */
+			if ( errcode & PELICAN_ECC_DIR ){
+				can->stats.err_bus_rx++;
+			}else{
+				can->stats.err_bus_tx++;
+			}
+
+			/* Get Segment in frame that went wrong */
+			can->stats.err_bus_segs[errcode & PELICAN_ECC_SEG]++;
+
+			/* total number of bus errors */
+			can->stats.err_bus++;
+		}
+	}
+	SPIN_UNLOCK(&can->devlock, irqflags);
+
+	/* signal Binary semaphore, messages available! */
+	if ( signal_rx ){
+		rtems_semaphore_release(can->rxsem);
+	}
+
+	if ( signal_tx ){
+		rtems_semaphore_release(can->txsem);
+	}
+}
+
+/*******************************************************************************
+ * FIFO IMPLEMENTATION
+ */
+
+static occan_fifo *occan_fifo_create(int cnt)
+{
+	occan_fifo *fifo;
+	fifo = grlib_calloc(1, sizeof(*fifo)+cnt*sizeof(CANMsg));
+	if ( fifo ){
+		fifo->cnt = cnt;
+		fifo->base = &fifo->fifoarea[0];
+		fifo->tail = fifo->head = fifo->base;
+	}
+	return fifo;
+}
+
+static void occan_fifo_free(occan_fifo *fifo)
+{
+	if ( fifo )
+		free(fifo);
+}
+
+static int occan_fifo_full(occan_fifo *fifo)
+{
+	return fifo->full;
+}
+
+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)
+{
+	if ( !fifo )
+		return NULL;
+
+	if ( occan_fifo_full(fifo) ){
+
+		if ( !force )
+			return NULL;
+
+		/* all buffers already used ==> overwrite the oldest */
+		fifo->ovcnt++;
+		occan_fifo_get(fifo);
+	}
+
+	return fifo->head;
+}
+
+/* Stage 2 - increment indexes */
+static void occan_fifo_put(occan_fifo *fifo)
+{
+	if ( occan_fifo_full(fifo) )
+		return;
+
+	/* wrap around */
+	fifo->head = (fifo->head >= &fifo->base[fifo->cnt-1])? fifo->base : fifo->head+1;
+
+	if ( fifo->head == fifo->tail )
+		fifo->full = 1;
+}
+
+static CANMsg *occan_fifo_claim_get(occan_fifo *fifo)
+{
+	if ( occan_fifo_empty(fifo) )
+		return NULL;
+
+	/* return oldest message */
+	return fifo->tail;
+}
+
+
+static void occan_fifo_get(occan_fifo *fifo)
+{
+	if ( !fifo )
+		return;
+
+	if ( occan_fifo_empty(fifo) )
+		return;
+
+	/* increment indexes */
+	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;
+}
+
+/******************************************************************************/
diff --git a/bsps/shared/grlib/can/satcan.c b/bsps/shared/grlib/can/satcan.c
new file mode 100644
index 0000000000..c6d58aaed4
--- /dev/null
+++ b/bsps/shared/grlib/can/satcan.c
@@ -0,0 +1,716 @@
+/*
+ *  SatCAN FPGA driver
+ * 
+ *  COPYRIGHT (c) 2008.
+ *  Cobham Gaisler AB.
+ *
+ *  The license and distribution terms for this file may be
+ *  found in the file LICENSE in this distribution or at
+ *  http://www.rtems.org/license/LICENSE.
+ */
+
+#include <rtems/libio.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+#include <bsp.h>
+#include <rtems/bspIo.h> /* printk */
+
+#include <grlib/satcan.h>
+#include <grlib/ambapp.h>
+
+#include <grlib/grlib_impl.h>
+
+#ifndef GAISLER_SATCAN
+#define GAISLER_SATCAN 0x080
+#endif
+
+#if !defined(SATCAN_DEVNAME) 
+ #undef SATCAN_DEVNAME
+ #define SATCAN_DEVNAME "/dev/satcan"
+#endif
+
+/* Enable debug output? */
+/* #define DEBUG */ 
+
+#ifdef DEBUG
+#define DBG(x...) printk(x)
+#else
+#define DBG(x...) 
+#endif
+
+
+/* Defines related to DMA */
+#define ALIGN_2KMEM 32*1024
+#define ALIGN_8KMEM 128*1024
+
+#define OFFSET_2K_LOW_POS 15
+#define OFFSET_8K_LOW_POS 17
+
+#define DMA_2K_DATA_SELECT (1 << 14)
+#define DMA_8K_DATA_SELECT (1 << 16)
+
+#define DMA_2K_DATA_OFFSET 16*1024
+#define DMA_8K_DATA_OFFSET 64*1024
+
+/* Core register structures and defines */
+
+/* Indexes to SatCAN registers in satcan array are declared in satcan.h*/
+/* Fields for some of the SatCAN FPGA registers */
+
+/* CmdReg0 */
+#define CAN_TODn_Int_sel   (1 << 5)
+
+/* CmdReg1 */
+#define Sel_2k_8kN         (1 << 0)
+
+/* Read FIFO */
+#define FIFO_Full          (1 << 8)
+#define FIFO_Empty         (1 << 9)
+
+/* DMA Ch_Enable */
+#define DMA_AutoInitDmaTx  (1 << 3)
+#define DMA_EnTx2          (1 << 2)
+#define DMA_EnTx1          (1 << 1)
+#define DMA_EnRx           (1 << 0)
+
+/* SatCAN wrapper register fields */
+#define CTRL_BT_P     9
+#define CTRL_NODENO_P 5
+#define CTRL_DIS      (1 << 2)
+#define CTRL_DPS_P    1
+#define CTRL_RST      (1 << 0)
+
+#define IRQ_AHB       (1 << 8)
+#define IRQ_PPS       (1 << 7)
+#define IRQ_M5        (1 << 6)
+#define IRQ_M4        (1 << 5)
+#define IRQ_M3        (1 << 4)
+#define IRQ_M2        (1 << 3)
+#define IRQ_M1        (1 << 2)
+#define IRQ_SYNC      (1 << 1)
+#define IRQ_CAN       (1 << 0)
+
+#define MSK_AHB       (1 << 8)
+#define MSK_PPS       (1 << 7)
+#define MSK_M5        (1 << 6)
+#define MSK_M4        (1 << 5)
+#define MSK_M3        (1 << 4)
+#define MSK_M2        (1 << 3)
+#define MSK_M1        (1 << 2)
+#define MSK_SYNC      (1 << 1)
+#define MSK_CAN       (1 << 0)
+
+
+
+struct satcan_regs {
+	volatile unsigned int satcan[32];
+	volatile unsigned int ctrl;
+	volatile unsigned int irqpend;
+	volatile unsigned int irqmask;
+	volatile unsigned int membase;
+};
+
+
+struct satcan_priv {
+	/* config */
+	void           *dmaptr;
+	unsigned char  *alptr;
+	satcan_config  *cfg;
+	
+	/* driver state */
+	rtems_id       devsem;
+	rtems_id       txsem;
+	int            open;
+	int            txactive;
+	int            dmaen;
+	int            doff;
+	rtems_interval timeout;
+	int            dmamode;
+};
+
+static struct satcan_regs *regs;
+static struct satcan_priv *priv;
+
+static rtems_device_driver satcan_ioctl(rtems_device_major_number major, rtems_device_minor_number minor, void *arg);
+static rtems_device_driver satcan_write(rtems_device_major_number major, rtems_device_minor_number minor, void *arg);
+static rtems_device_driver satcan_read(rtems_device_major_number major, rtems_device_minor_number minor, void *arg);
+static rtems_device_driver satcan_close(rtems_device_major_number major, rtems_device_minor_number minor, void *arg);
+static rtems_device_driver satcan_open(rtems_device_major_number major, rtems_device_minor_number minor, void *arg);
+static rtems_device_driver satcan_initialize(rtems_device_major_number major, rtems_device_minor_number unused, void *arg);
+
+
+/*
+ * almalloc: allocate memory area of size sz aligned on sz boundary 
+ * alptr: Utilized to return aligned pointer
+ * ptr:   Unaligned pointer
+ * sz:    Size of memory area
+ */
+static void almalloc(unsigned char **alptr, void **ptr, int sz)
+{
+  *ptr = rtems_calloc(1,2*sz);
+  *alptr = (unsigned char *) (((int)*ptr+sz) & ~(sz-1));
+}
+
+static rtems_isr satcan_interrupt_handler(rtems_vector_number v)
+{
+	unsigned int irq;
+	unsigned int fifo;
+	
+	irq = regs->irqpend;
+	
+	if (irq & IRQ_AHB && priv->cfg->ahb_irq_callback) {
+		priv->cfg->ahb_irq_callback();
+	}
+	if (irq & IRQ_PPS && priv->cfg->pps_irq_callback) {
+		priv->cfg->pps_irq_callback();
+	}
+	if (irq & IRQ_M5 && priv->cfg->m5_irq_callback) {
+		priv->cfg->m5_irq_callback();
+	}
+	if (irq & IRQ_M4 && priv->cfg->m4_irq_callback) {
+		priv->cfg->m4_irq_callback();
+	}
+	if (irq & IRQ_M3 && priv->cfg->m3_irq_callback) {
+		priv->cfg->m3_irq_callback();
+	}
+	if (irq & IRQ_M2 && priv->cfg->m2_irq_callback) {
+		priv->cfg->m2_irq_callback();
+	}
+	if (irq & IRQ_M1 && priv->cfg->m1_irq_callback) {
+		priv->cfg->m1_irq_callback();
+	}
+	if (irq & IRQ_SYNC && priv->cfg->sync_irq_callback) {
+		priv->cfg->sync_irq_callback();
+	}
+	if (irq & IRQ_CAN) {
+		fifo = regs->satcan[SATCAN_FIFO];
+		if (!(fifo & FIFO_Empty) && priv->txactive &&
+		    (((fifo & 0xff) == SATCAN_IRQ_EOD1) || ((fifo & 0xff) == SATCAN_IRQ_EOD2))) { 
+			rtems_semaphore_release(priv->txsem);
+		}
+		if (priv->cfg->can_irq_callback)
+			priv->cfg->can_irq_callback(fifo);
+	}
+}
+
+
+
+static rtems_device_driver satcan_ioctl(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
+{
+	rtems_libio_ioctl_args_t *ioarg = (rtems_libio_ioctl_args_t*)arg;
+	int *value;
+	rtems_interval *timeout;
+	satcan_regmod *regmod;
+
+	DBG("SatCAN:  IOCTL %d\n\r", ioarg->command);
+
+	ioarg->ioctl_return = 0;
+	switch(ioarg->command) {
+	case SATCAN_IOC_DMA_2K:
+		DBG("SatCAN: ioctl: setting 2K DMA mode\n\r");
+		free(priv->dmaptr);
+		almalloc(&priv->alptr, &priv->dmaptr, ALIGN_2KMEM);
+		if (priv->dmaptr == NULL) {
+			printk("SatCAN: Failed to allocate DMA memory\n\r");
+			return RTEMS_NO_MEMORY;
+		}
+
+		regs->membase = (unsigned int)priv->alptr;
+		regs->satcan[SATCAN_RAM_BASE] = (unsigned int)priv->alptr >> OFFSET_2K_LOW_POS;
+		regs->satcan[SATCAN_CMD1] = regs->satcan[SATCAN_CMD1] | Sel_2k_8kN;
+		break;
+
+	case SATCAN_IOC_DMA_8K:
+		DBG("SatCAN: ioctl: setting 8K DMA mode\n\r");
+		free(priv->dmaptr);
+		almalloc(&priv->alptr, &priv->dmaptr, ALIGN_8KMEM);
+		if (priv->dmaptr == NULL) {
+			printk("SatCAN: Failed to allocate DMA memory\n\r");
+			return RTEMS_NO_MEMORY;
+		}
+		
+		regs->membase = (unsigned int)priv->alptr;
+		regs->satcan[SATCAN_RAM_BASE] = (unsigned int)priv->alptr >> OFFSET_8K_LOW_POS;
+		regs->satcan[SATCAN_CMD1] = regs->satcan[SATCAN_CMD1] & ~Sel_2k_8kN;
+		break;
+		
+	case SATCAN_IOC_GET_REG:
+		/* Get regmod structure from argument */
+		regmod = (satcan_regmod*)ioarg->buffer;
+		DBG("SatCAN: ioctl: getting register %d\n\r", regmod->reg);
+		if (regmod->reg < 0)
+			return RTEMS_INVALID_NAME;
+		else if (regmod->reg <= SATCAN_FILTER_STOP)
+			regmod->val = regs->satcan[regmod->reg];
+		else if (regmod->reg == SATCAN_WCTRL)
+			regmod->val = regs->ctrl;
+		else if (regmod->reg == SATCAN_WIPEND)
+			regmod->val = regs->irqpend;
+		else if (regmod->reg == SATCAN_WIMASK)
+			regmod->val = regs->irqmask;
+		else if (regmod->reg == SATCAN_WAHBADDR)
+			regmod->val = regs->membase;
+		else
+			return RTEMS_INVALID_NAME;
+		break;
+
+	case SATCAN_IOC_SET_REG:
+		/* Get regmod structure from argument */
+		regmod = (satcan_regmod*)ioarg->buffer;
+		DBG("SatCAN: ioctl: setting register %d, value %x\n\r", 
+		    regmod->reg, regmod->val);
+		if (regmod->reg < 0)
+			return RTEMS_INVALID_NAME;
+		else if (regmod->reg <= SATCAN_FILTER_STOP)
+			regs->satcan[regmod->reg] = regmod->val;
+		else if (regmod->reg == SATCAN_WCTRL)
+			regs->ctrl = regmod->val;
+		else if (regmod->reg == SATCAN_WIPEND)
+			regs->irqpend = regmod->val;
+		else if (regmod->reg == SATCAN_WIMASK)
+			regs->irqmask = regmod->val;
+		else if (regmod->reg == SATCAN_WAHBADDR)
+			regs->membase = regmod->val;
+		else
+			return RTEMS_INVALID_NAME;
+		break;
+
+	case SATCAN_IOC_OR_REG:
+		/* Get regmod structure from argument */
+		regmod = (satcan_regmod*)ioarg->buffer;
+		DBG("SatCAN: ioctl: or:ing register %d, with value %x\n\r",
+		    regmod->reg, regmod->val);
+		if (regmod->reg < 0)
+			return RTEMS_INVALID_NAME;
+		else if (regmod->reg <= SATCAN_FILTER_STOP)
+			regs->satcan[regmod->reg] |= regmod->val;
+		else if (regmod->reg == SATCAN_WCTRL)
+			regs->ctrl |= regmod->val;
+		else if (regmod->reg == SATCAN_WIPEND)
+			regs->irqpend |= regmod->val;
+		else if (regmod->reg == SATCAN_WIMASK)
+			regs->irqmask |= regmod->val;
+		else if (regmod->reg == SATCAN_WAHBADDR)
+			regs->membase |= regmod->val;
+		else
+			return RTEMS_INVALID_NAME;
+		break;
+
+	case SATCAN_IOC_AND_REG:
+		/* Get regmod structure from argument */
+		regmod = (satcan_regmod*)ioarg->buffer;
+		DBG("SatCAN: ioctl: masking register %d, with value %x\n\r",
+		    regmod->reg, regmod->val);
+		if (regmod->reg < 0)
+			return RTEMS_INVALID_NAME;
+		else if (regmod->reg <= SATCAN_FILTER_STOP)
+			regs->satcan[regmod->reg] &= regmod->val;
+		else if (regmod->reg == SATCAN_WCTRL)
+			regs->ctrl &= regmod->val;
+		else if (regmod->reg == SATCAN_WIPEND)
+			regs->irqpend &= regmod->val;
+		else if (regmod->reg == SATCAN_WIMASK)
+			regs->irqmask &= regmod->val;
+		else if (regmod->reg == SATCAN_WAHBADDR)
+			regs->membase &= regmod->val;
+		else
+			return RTEMS_INVALID_NAME;
+		break;
+
+	case SATCAN_IOC_EN_TX1_DIS_TX2:
+		priv->dmaen = SATCAN_DMA_ENABLE_TX1;
+		break;
+
+	case SATCAN_IOC_EN_TX2_DIS_TX1:
+		priv->dmaen = SATCAN_DMA_ENABLE_TX2;
+		break;
+
+	case SATCAN_IOC_GET_DMA_MODE:
+		value = (int*)ioarg->buffer;
+		*value = priv->dmamode;
+		break;
+		
+	case SATCAN_IOC_SET_DMA_MODE:
+		value = (int*)ioarg->buffer;
+		if (*value != SATCAN_DMA_MODE_USER && *value != SATCAN_DMA_MODE_SYSTEM) {
+			DBG("SatCAN: ioctl: invalid DMA mode\n\r");
+			return RTEMS_INVALID_NAME;
+		}
+		priv->dmamode = *value;
+		break;
+
+	case SATCAN_IOC_ACTIVATE_DMA:
+		if (priv->dmamode != SATCAN_DMA_MODE_USER) {
+			DBG("SatCAN: ioctl: ACTIVATE_DMA: not in user mode\n\r");
+			return RTEMS_INVALID_NAME;
+		}
+		value = (int*)ioarg->buffer;
+		if (*value != SATCAN_DMA_ENABLE_TX1 && *value != SATCAN_DMA_ENABLE_TX2) {
+			DBG("SatCAN: ioctl: ACTIVATE_DMA: Illegal channel\n\r");
+			return RTEMS_INVALID_NAME;
+		}
+		regs->satcan[SATCAN_DMA] |= *value << 1;
+		break;
+
+	case SATCAN_IOC_DEACTIVATE_DMA:
+		if (priv->dmamode != SATCAN_DMA_MODE_USER) {
+			DBG("SatCAN: ioctl: DEACTIVATE_DMA: not in user mode\n\r");
+			return RTEMS_INVALID_NAME;
+		}
+		value = (int*)ioarg->buffer;
+		if (*value != SATCAN_DMA_ENABLE_TX1 && *value != SATCAN_DMA_ENABLE_TX2) {
+			DBG("SatCAN: ioctl: DEACTIVATE_DMA: Illegal channel\n\r");
+			return RTEMS_INVALID_NAME;
+		}
+		regs->satcan[SATCAN_DMA] &= ~(*value << 1);
+		break;	
+
+	case SATCAN_IOC_GET_DOFFSET:
+		value = (int*)ioarg->buffer;
+		*value = priv->doff;
+		break;
+	
+	case SATCAN_IOC_SET_DOFFSET:
+		value = (int*)ioarg->buffer;
+		priv->doff = *value;
+		break;
+		
+	case SATCAN_IOC_GET_TIMEOUT:
+		timeout = (rtems_interval*)ioarg->buffer;
+		*timeout = priv->timeout;
+		break;
+
+	case SATCAN_IOC_SET_TIMEOUT:
+		timeout = (rtems_interval*)ioarg->buffer;
+		priv->timeout = *timeout;
+		break;
+
+	default:
+		return RTEMS_NOT_DEFINED;
+	}
+
+	return RTEMS_SUCCESSFUL;
+}
+
+static rtems_device_driver satcan_write(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
+{
+	int i;
+	int doff;
+	int msgindex;
+	int messages;
+	rtems_libio_rw_args_t *rw_args=(rtems_libio_rw_args_t *) arg;
+	satcan_msg *msgs;
+	rtems_status_code status;
+	
+	DBG("SatCAN: Writing %d bytes from %p\n\r",rw_args->count,rw_args->buffer);
+
+	if ((rw_args->count < sizeof(satcan_msg)) || (!rw_args->buffer)) {
+		DBG("SatCAN: write: returning EINVAL\n\r");
+		return RTEMS_INVALID_NAME; /* EINVAL */
+	}
+
+	messages = rw_args->count / sizeof(satcan_msg);
+	msgs = (satcan_msg*)rw_args->buffer;
+
+	/* Check that size matches any number of satcan_msg */
+	if (rw_args->count % sizeof(satcan_msg)) {
+		DBG("SatCAN: write: count can not be evenly divided with satcan_msg size\n\r");
+		return RTEMS_INVALID_NAME; /* EINVAL */
+	}
+
+
+	/* DMA channel must be set if we are in system DMA mode */
+	DBG("SatCAN: write: dma channel select is %x\n\r", priv->dmaen);
+	if (!priv->dmaen && priv->dmamode == SATCAN_DMA_MODE_SYSTEM)
+		return RTEMS_INVALID_NAME; /* EINVAL */
+
+	/* DMA must not be active */
+	if (regs->satcan[SATCAN_DMA] & (DMA_EnTx1 | DMA_EnTx2 | DMA_AutoInitDmaTx)) {
+		DBG("SatCAN: write: DMA was active\n\r");
+		rw_args->bytes_moved = 0;
+		return RTEMS_IO_ERROR; /* EIO */
+	}		
+
+	doff = regs->satcan[SATCAN_CMD1] & Sel_2k_8kN ? DMA_2K_DATA_OFFSET : DMA_8K_DATA_OFFSET;
+
+	for (msgindex = 0; msgindex < messages; msgindex++) {
+		/* Place header in DMA area */
+		for (i = 0; i < SATCAN_HEADER_SIZE; i++) {
+			priv->alptr[priv->doff+8*msgindex+i] = msgs[msgindex].header[i];
+		}
+
+		/* Place data in DMA area */
+		for (i = 0; i < SATCAN_PAYLOAD_SIZE; i++)
+			priv->alptr[priv->doff+doff+8*msgindex+i] = msgs[msgindex].payload[i];
+	}
+
+	if ((priv->dmaen & SATCAN_DMA_ENABLE_TX1) ||  priv->dmamode == SATCAN_DMA_MODE_USER) {
+		regs->satcan[SATCAN_DMA_TX_1_CUR] = 0;
+		regs->satcan[SATCAN_DMA_TX_1_END] = messages<<3; 
+	}
+
+	if ((priv->dmaen & SATCAN_DMA_ENABLE_TX2) || priv->dmamode == SATCAN_DMA_MODE_USER) {
+		regs->satcan[SATCAN_DMA_TX_2_CUR] = 0;
+		regs->satcan[SATCAN_DMA_TX_2_END] = messages<<3; 
+	}
+
+	/* If we are in DMA user mode we are done here, otherwise we block */
+	if (priv->dmamode == SATCAN_DMA_MODE_SYSTEM) {
+		priv->txactive = 1;
+
+		/* Enable DMA */
+		regs->satcan[SATCAN_DMA] |= priv->dmaen << 1;
+		
+		/* Wait for TX interrupt */
+		status = rtems_semaphore_obtain(priv->txsem, RTEMS_WAIT, priv->timeout);
+		
+		priv->txactive = 0;
+
+		/* Disable activated Tx DMA */
+		regs->satcan[SATCAN_DMA] &= ~(priv->dmaen << 1);
+
+		if (status != RTEMS_SUCCESSFUL) {
+			rw_args->bytes_moved = 0;
+			return status;
+		}
+	}
+
+	rw_args->bytes_moved = rw_args->count;
+	
+	return RTEMS_SUCCESSFUL;
+}
+
+static rtems_device_driver satcan_read(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
+{
+	char *buf;
+	int i;
+	int canid;
+	int messages;
+	rtems_libio_rw_args_t *rw_args = (rtems_libio_rw_args_t*)arg;
+	satcan_msg *ret;
+
+	/* Check that there is room for the return */
+	if (rw_args->count < sizeof(satcan_msg)) {
+		DBG("SatCAN: read: length of buffer must be at least %d, current is %d\n\r",
+		    sizeof(satcan_msg) + sizeof(int), rw_args->count);
+		return RTEMS_INVALID_NAME; /* -EINVAL */
+	}
+	
+	/* Check that size matches any number of satcan_msg */
+	if (rw_args->count % sizeof(satcan_msg)) {
+		DBG("SatCAN: read: count can not be evenly divided with satcan_msg size\n\r");
+		return RTEMS_INVALID_NAME; /* EINVAL */
+	}
+	
+	messages = rw_args->count / sizeof(satcan_msg);
+	ret = (satcan_msg*)rw_args->buffer;
+
+	DBG("SatCAN: read: reading %d messages to %p\n\r", messages, ret);
+	
+	for (i = 0; i < messages; i++) {
+		canid = (ret[i].header[1] << 8) | ret[i].header[0];
+	
+		/* Copy message header from DMA header area to buffer */
+		buf = (char*)((int)priv->alptr | (canid << 3));
+		memcpy(ret[i].header, buf, SATCAN_HEADER_SIZE);
+
+		DBG("SatCAN: read: copied header from %p to %p\n\r", buf, ret[i].header);
+
+		/* Clear New Message Marker */
+		buf[SATCAN_HEADER_NMM_POS] = 0;
+		
+		/* Copy message payload from DMA data area to buffer */
+		buf = (char*)((int)buf | 
+			      (regs->satcan[SATCAN_CMD1] & Sel_2k_8kN ? DMA_2K_DATA_SELECT : DMA_8K_DATA_SELECT));
+		memcpy(ret[i].payload, buf, SATCAN_PAYLOAD_SIZE);
+	
+		DBG("SatCAN: read: copied payload from %p to %p\n\r", buf, ret[i].payload);
+	}
+	rw_args->bytes_moved = rw_args->count;
+
+	return RTEMS_SUCCESSFUL;
+}
+
+
+static rtems_device_driver satcan_close(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
+{
+	DBG("SatCAN: Closing %d\n\r",minor);
+
+	if (priv->open) {
+		regs->irqmask = 0;
+		regs->satcan[SATCAN_INT_EN] = 0;
+		regs->satcan[SATCAN_RX] = 0;
+		regs->satcan[SATCAN_DMA] = 0;
+		priv->open = 0;
+		priv->dmaen = 0;
+		priv->doff = 0;
+		priv->timeout = RTEMS_NO_TIMEOUT;
+		priv->dmamode = SATCAN_DMA_MODE_SYSTEM;
+	}
+
+	return RTEMS_SUCCESSFUL;
+}
+
+
+static rtems_device_driver satcan_open(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
+{
+	DBG("SatCAN: Opening %d\n\r",minor);
+	
+	rtems_semaphore_obtain(priv->devsem,RTEMS_WAIT, RTEMS_NO_TIMEOUT);
+	if (priv->open) {
+		rtems_semaphore_release(priv->devsem);
+		return RTEMS_RESOURCE_IN_USE; /* EBUSY */
+	}
+	priv->open = 1;
+	rtems_semaphore_release(priv->devsem);
+
+	/* Enable AHB and CAN IRQs in wrapper and EOD1, EOD2 and CAN critical  IRQs in SatCAN core */
+	regs->irqmask = MSK_AHB | MSK_CAN;
+	regs->satcan[SATCAN_INT_EN] = ((1 << SATCAN_IRQ_EOD1) | (1 << SATCAN_IRQ_EOD2) |
+				       (1 << SATCAN_IRQ_CRITICAL));
+
+	/* Select can_int as IRQ source */
+	regs->satcan[SATCAN_CMD0] = CAN_TODn_Int_sel;
+	/* CAN RX DMA Enable */
+	regs->satcan[SATCAN_DMA] = 1;
+	/* CAN RX Enable */
+	regs->satcan[SATCAN_RX] = 1;
+
+	DBG("SatCAN: Opening %d success\n\r",minor);
+
+	return RTEMS_SUCCESSFUL;
+}
+
+static rtems_device_driver satcan_initialize(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
+{
+	struct ambapp_ahb_info d;
+	char fs_name[20];
+	rtems_status_code status;
+
+	DBG("SatCAN: Initialize..\n\r");
+
+	strcpy(fs_name, SATCAN_DEVNAME);
+
+	/* Find core and initialize register pointer */
+	if (!ambapp_find_ahbslv(&ambapp_plb, VENDOR_GAISLER, GAISLER_SATCAN, &d)) {
+		printk("SatCAN: Failed to find SatCAN core\n\r");
+		return -1;
+	}
+
+	status = rtems_io_register_name(fs_name, major, minor);
+	if (RTEMS_SUCCESSFUL != status)
+		rtems_fatal_error_occurred(status);
+
+	regs = (struct satcan_regs*)d.start[0];
+		
+	/* Set node number and DPS */
+	regs->ctrl |= ((priv->cfg->nodeno & 0xf) << 5) | (priv->cfg->dps << 1);
+
+	/* Reset core */
+	regs->ctrl |= CTRL_RST;
+
+	/* Allocate DMA area */
+	almalloc(&priv->alptr, &priv->dmaptr, ALIGN_2KMEM);
+	if (priv->dmaptr == NULL) {
+		printk("SatCAN: Failed to allocate DMA memory\n\r");
+		free(priv->cfg);
+		free(priv);
+		return -1; 
+	}
+	
+	/* Wait until core reset has completed */
+	while (regs->ctrl & CTRL_RST)
+		;
+
+	/* Initialize core registers, default is 2K messages */
+	regs->membase = (unsigned int)priv->alptr;
+	regs->satcan[SATCAN_RAM_BASE] = (unsigned int)priv->alptr >> 15;
+	
+	DBG("regs->membase = %x\n\r", (unsigned int)priv->alptr);
+	DBG("regs->satcan[SATCAN_RAM_BASE] = %x\n\r", (unsigned int)priv->alptr >> 15);
+
+	status = rtems_semaphore_create(
+		rtems_build_name('S', 'd', 'v', '0'),
+		1,
+		RTEMS_FIFO | RTEMS_SIMPLE_BINARY_SEMAPHORE | RTEMS_NO_INHERIT_PRIORITY | \
+		RTEMS_NO_PRIORITY_CEILING,
+		0, 
+		&priv->devsem);
+	if (status != RTEMS_SUCCESSFUL) {
+		printk("SatCAN: Failed to create dev semaphore (%d)\n\r", status);
+		free(priv->cfg);
+		free(priv);
+		return RTEMS_UNSATISFIED;
+	}
+	status = rtems_semaphore_create(
+		rtems_build_name('S', 't', 'x', '0'),
+		0, 
+		RTEMS_FIFO | RTEMS_SIMPLE_BINARY_SEMAPHORE | RTEMS_NO_INHERIT_PRIORITY | \
+		RTEMS_NO_PRIORITY_CEILING,
+		0, 
+		&priv->txsem);
+	if (status != RTEMS_SUCCESSFUL) {
+	  printk("SatCAN: Failed to create tx semaphore (%d)\n\r", status);
+	  free(priv->cfg);
+	  free(priv);
+	  return RTEMS_UNSATISFIED;
+	}
+
+	priv->txactive = 0;
+	priv->open = 0;
+	priv->dmaen = 0;
+	priv->doff = 0;
+	priv->timeout = RTEMS_NO_TIMEOUT;
+	priv->dmamode = SATCAN_DMA_MODE_SYSTEM;
+
+	/* Register interrupt handler */
+	set_vector(satcan_interrupt_handler, d.irq+0x10, 2);
+
+	return RTEMS_SUCCESSFUL;
+}
+
+
+
+#define SATCAN_DRIVER_TABLE_ENTRY { satcan_initialize, satcan_open, satcan_close, satcan_read, satcan_write, satcan_ioctl }
+
+static rtems_driver_address_table satcan_driver = SATCAN_DRIVER_TABLE_ENTRY;
+
+int satcan_register(satcan_config *conf)
+{
+	rtems_status_code r;
+	rtems_device_major_number m;
+
+	DBG("SatCAN: satcan_register called\n\r");
+
+	/* Create private structure */
+	if ((priv = grlib_malloc(sizeof(*priv))) == NULL) {
+		printk("SatCAN driver could not allocate memory for priv structure\n\r");
+		return -1;
+	}
+
+	DBG("SatCAN: Creating local copy of config structure\n\r");
+	if ((priv->cfg = grlib_malloc(sizeof(*priv->cfg))) == NULL) {
+		printk("SatCAN driver could not allocate memory for cfg structure\n\r");
+		return 1;
+	}
+	memcpy(priv->cfg, conf, sizeof(satcan_config));
+
+	if ((r = rtems_io_register_driver(0, &satcan_driver, &m)) == RTEMS_SUCCESSFUL) {
+		DBG("SatCAN driver successfully registered, major: %d\n\r", m);
+	} else {
+		switch(r) {
+		case RTEMS_TOO_MANY:
+			printk("SatCAN rtems_io_register_driver failed: RTEMS_TOO_MANY\n\r"); break;
+		case RTEMS_INVALID_NUMBER:  
+			printk("SatCAN rtems_io_register_driver failed: RTEMS_INVALID_NUMBER\n\r"); break;
+		case RTEMS_RESOURCE_IN_USE:
+			printk("SatCAN rtems_io_register_driver failed: RTEMS_RESOURCE_IN_USE\n\r"); break;
+		default:
+			printk("SatCAN rtems_io_register_driver failed\n\r");
+		}
+		return 1;
+	}
+
+	return 0;
+}