2007-09-06 Daniel Hellstrom <daniel@gaisler.com>

	New drivers: PCI, b1553BRM, SpaceWire(GRSPW), CAN (GRCAN,OC_CAN),
	Raw UART.
	* shared/1553/b1553brm.c, shared/1553/b1553brm_pci.c,
	shared/1553/b1553brm_rasta.c, shared/can/grcan.c,
	shared/can/grcan_rasta.c, shared/can/occan.c, shared/can/occan_pci.c,
	shared/spw/grspw.c, shared/spw/grspw_pci.c, shared/spw/grspw_rasta.c,
	shared/uart/apbuart.c, shared/uart/apbuart_pci.c,
	shared/uart/apbuart_rasta.c: New files missed in previous commit.

14 files changed, 8310 insertions, 0 deletions

diff --git a/c/src/lib/libbsp/sparc/ChangeLog b/c/src/lib/libbsp/sparc/ChangeLog
index a8ec7f5029..7ce4bf2f29 100644
--- a/c/src/lib/libbsp/sparc/ChangeLog
+++ b/c/src/lib/libbsp/sparc/ChangeLog
@@ -1,5 +1,16 @@
 2007-09-06	Daniel Hellstrom <daniel@gaisler.com>
 
+	New drivers: PCI, b1553BRM, SpaceWire(GRSPW), CAN (GRCAN,OC_CAN),
+	Raw UART.
+	* shared/1553/b1553brm.c, shared/1553/b1553brm_pci.c,
+	shared/1553/b1553brm_rasta.c, shared/can/grcan.c,
+	shared/can/grcan_rasta.c, shared/can/occan.c, shared/can/occan_pci.c,
+	shared/spw/grspw.c, shared/spw/grspw_pci.c, shared/spw/grspw_rasta.c,
+	shared/uart/apbuart.c, shared/uart/apbuart_pci.c,
+	shared/uart/apbuart_rasta.c: New files missed in previous commit.
+
+2007-09-06	Daniel Hellstrom <daniel@gaisler.com>
+
 	* Makefile.am: Add the following new drivers: PCI, b1553BRM,
 	SpaceWire(GRSPW), CAN (GRCAN,OC_CAN), Raw UART.
 	* shared/include/apbuart.h, shared/include/apbuart_pci.h,
diff --git a/c/src/lib/libbsp/sparc/shared/1553/b1553brm.c b/c/src/lib/libbsp/sparc/shared/1553/b1553brm.c
new file mode 100644
index 0000000000..f60cfa5771
--- /dev/null
+++ b/c/src/lib/libbsp/sparc/shared/1553/b1553brm.c
@@ -0,0 +1,1367 @@
+/*
+ *  BRM driver  
+ *
+ *  COPYRIGHT (c) 2006.
+ *  Gaisler Research.
+ *
+ *  The license and distribution terms for this file may be
+ *  found in the file LICENSE in this distribution or at
+ *  http://www.rtems.com/license/LICENSE.
+ *
+ */
+
+/********** Set defaults **********/
+
+/* basic bus/interface of device,
+ * Default to direct accessed AMBA bus.
+ */
+#ifndef B1553BRM_NO_AMBA
+ #define B1553BRM_AMBA
+ #undef B1553BRM_PCI
+#endif
+
+/* default name to /dev/brm */
+#if !defined(B1553BRM_DEVNAME) || !defined(B1553BRM_DEVNAME_NO) 
+ #undef B1553BRM_DEVNAME
+ #undef B1553BRM_DEVNAME_NO
+ #define B1553BRM_DEVNAME "/dev/brm0"
+ #define B1553BRM_DEVNAME_NO(devstr,no) ((devstr)[8]='0'+(no))
+#endif
+
+#ifndef B1553BRM_PREFIX
+ #define B1553BRM_PREFIX(name) b1553brm##name
+#endif
+
+/* default to no translation */
+#ifndef B1553BRM_ADR_TO
+ #define memarea_to_hw(x) ((unsigned int)(x))
+#endif
+
+#ifndef B1553BRM_REG_INT
+ #define B1553BRM_REG_INT(handler,irqno,arg) set_vector(handler,(irqno)+0x10,1)
+#endif
+
+/* default to 128K memory layout */
+#if !defined(DMA_MEM_16K)
+ #define DMA_MEM_128K
+#endif
+
+#include <bsp.h>
+#include <rtems/libio.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+#include <assert.h>
+#include <sched.h>
+#include <ctype.h>
+#include <rtems/bspIo.h>
+
+#include <b1553brm.h>
+#include <ambapp.h>
+
+/* Uncomment for debug output */
+/*#define DEBUG 1
+#define FUNCDEBUG 1*/
+#undef DEBUG
+#undef FUNCDEBUG
+
+/* EVENT_QUEUE_SIZE sets the size of the event queue
+ */
+#define EVENT_QUEUE_SIZE           1024  
+
+
+#define INDEX(x) ( x&(EVENT_QUEUE_SIZE-1) )
+
+#ifdef DEBUG
+#define DBG(x...) printk(x)
+#else
+#define DBG(x...) 
+#endif
+
+#ifdef FUNCDEBUG
+#define FUNCDBG(x...) printk(x)
+#else
+#define FUNCDBG(x...) 
+#endif
+
+#define READ_REG(address) _BRM_REG_READ16((unsigned int)address)
+#define READ_DMA(address) _BRM_REG_READ16((unsigned int)address)
+static __inline__ unsigned short _BRM_REG_READ16(unsigned int addr) {
+	unsigned short tmp;
+	asm(" lduha [%1]1, %0 "
+	  : "=r"(tmp)
+	  : "r"(addr)
+	);
+	return tmp;
+}
+
+static rtems_device_driver brm_initialize(rtems_device_major_number major, rtems_device_minor_number minor, void *arg);
+static rtems_device_driver brm_open(rtems_device_major_number major, rtems_device_minor_number minor, void *arg);
+static rtems_device_driver brm_close(rtems_device_major_number major, rtems_device_minor_number minor, void *arg);
+static rtems_device_driver brm_read(rtems_device_major_number major, rtems_device_minor_number minor, void *arg);
+static rtems_device_driver brm_write(rtems_device_major_number major, rtems_device_minor_number minor, void *arg);
+static rtems_device_driver brm_control(rtems_device_major_number major, rtems_device_minor_number minor, void *arg);
+
+#define BRM_DRIVER_TABLE_ENTRY { brm_initialize, brm_open, brm_close, brm_read, brm_write, brm_control }
+
+static rtems_driver_address_table brm_driver = BRM_DRIVER_TABLE_ENTRY;
+
+struct msg {
+	unsigned short miw;
+	unsigned short time;
+	unsigned short data[32];
+};
+#if defined(DMA_MEM_128K)
+struct circ_buf {
+	struct msg msgs[9];
+};
+#elif defined(DMA_MEM_16K)
+/* two message queue */
+struct circ_buf_2 {
+	struct msg msgs[2];
+};
+/* one message queue */
+struct circ_buf_1 {
+	struct msg msgs[1];
+};
+#endif
+
+typedef struct { 
+
+    unsigned int memarea_base;
+    struct brm_reg *regs;
+
+    /* BRM descriptors */
+    struct desc_table {
+
+        volatile unsigned short ctrl;        
+        volatile unsigned short top;
+        volatile unsigned short cur;
+        volatile unsigned short bot;
+
+    } *desc;
+
+    volatile unsigned short *mem;
+		/* bc mem struct */
+		struct {
+			/* BC Descriptors */
+			struct {
+				unsigned short ctrl; /* control */
+				unsigned short cw1;  /* Command word 1*/
+				unsigned short cw2;  /* Command word 1*/
+				unsigned short dptr; /* Data pointer in halfword offset from bcmem */
+				unsigned short tsw[2]; /* status word 1 & 2 */
+				unsigned short ba;     /* branch address */
+				unsigned short timer;  /* timer value */
+			} descs[128]; /* 2k (1024 half words) */
+			
+			/* message data */
+			struct {
+				unsigned short data[32]; /* 1 message's data */
+			} msg_data[128]; /* 8k */
+
+#if defined(DMA_MEM_128K)
+			/* offset to last 64bytes of 128k */
+			unsigned short unused[(64*1024-(128*8+128*32))-16*2];
+#elif defined(DMA_MEM_16K)
+			unsigned short unused[(8*1024-(128*8+128*32))-16*2];
+#endif
+			/* interrupt log at 64 bytes from end */
+			struct {
+				unsigned short iiw;
+				unsigned short iaw;
+			} irq_logs[16];
+		} *bcmem;
+
+#if defined(DMA_MEM_128K)
+		/* Memory structure of a RT being inited, just used
+		 * for RT initialization.
+		 *
+		 * *mesgs[32] fit each minimally 8 messages per sub address.
+		 */
+		struct {
+			/* RX Sub Address descriptors */
+			struct desc_table rxsubs[32];
+			/* TX Sub Address descriptors */
+			struct desc_table txsubs[32];
+			/* RX mode code descriptors */
+			struct desc_table rxmodes[32];
+			/* TX mode code descriptors */
+			struct desc_table txmodes[32];
+			
+			/* RX Sub Address messages */
+			struct circ_buf rxsuba_msgs[32];
+			/* TX Sub Address messages */
+			struct circ_buf txsuba_msgs[32];
+			/* RX Mode Code messages */
+			struct circ_buf rxmode_msgs[32];
+			/* RX Mode Code messages */
+			struct circ_buf txmode_msgs[32];
+			
+			
+			/* offset to last 64bytes of 128k: tot-used-needed */
+			unsigned short unused[(64*1024-(4*32*4+4*32*9*34))-16*2];
+			
+			/* interrupt log at 64 bytes from end */
+			struct {
+				unsigned short iiw;
+				unsigned short iaw;
+			} irq_logs[16];
+		} *rtmem;
+#elif defined(DMA_MEM_16K)
+		/* Memory structure of a RT being inited, just used
+		 * for RT initialization.
+		 *
+		 * circ_buf_2 *mesgs[32] fit each minimally 2 messages per queue.
+		 * circ_buf_1 *mesgs[32] fit each minimally 1 messages per queue.
+		 */
+		struct {
+			/* RX Sub Address descriptors */
+			struct desc_table rxsubs[32];
+			/* TX Sub Address descriptors */
+			struct desc_table txsubs[32];
+			/* RX mode code descriptors */
+			struct desc_table rxmodes[32];
+			/* TX mode code descriptors */
+			struct desc_table txmodes[32];
+			
+			/* RX Sub Address messages */
+			struct circ_buf_2 rxsuba_msgs[32];
+			/* TX Sub Address messages */
+			struct circ_buf_2 txsuba_msgs[32];
+			/* RX Mode Code messages */
+			struct circ_buf_2 rxmode_msgs[32];
+			/* RX Mode Code messages */
+			struct circ_buf_1 txmode_msgs[32];
+			
+			
+			/* offset to last 64bytes of 16k: tot-used-needed */
+			unsigned short unused[8*1024 -(4*32*4 +3*32*2*34 +1*32*1*34) -16*2];
+			
+			/* interrupt log at 64 bytes from end */
+			struct {
+				unsigned short iiw;
+				unsigned short iaw;
+			} irq_logs[16];
+		} *rtmem;
+#else
+	#error You must define one DMA_MEM_???K
+#endif
+
+    /* Interrupt log list */
+    struct irq_log_list {
+        volatile unsigned short iiw;
+        volatile unsigned short iaw;
+    }  *irq_log;
+    unsigned int irq;
+
+    /* Received events waiting to be read */
+    struct rt_msg *rt_event;
+    struct bm_msg *bm_event;
+
+    unsigned int head, tail;
+
+    unsigned int last_read[128];
+    unsigned int written[32];
+
+    struct bc_msg *cur_list;
+
+    int tx_blocking, rx_blocking;
+
+    rtems_id rx_sem, tx_sem, dev_sem;
+		int minor;
+		int irqno;
+		unsigned int mode;
+#ifdef DEBUG    		
+		unsigned int log[EVENT_QUEUE_SIZE*4];
+		unsigned int log_i;
+#endif
+		
+		rtems_id event_id; /* event that may be signalled upon errors, needs to be set through ioctl command BRM_SET_EVENTID */
+		unsigned int status;
+		int bc_list_fail;
+} brm_priv;
+
+static int brm_cores;
+static unsigned int allbrm_memarea;
+static brm_priv *brms;
+static amba_confarea_type *amba_bus;
+static unsigned int	allbrm_cfg_clksel;
+static unsigned int allbrm_cfg_clkdiv;
+static unsigned int allbrm_cfg_freq;
+
+static void brm_interrupt(brm_priv *brm);
+static void b1553brm_interrupt_handler(rtems_vector_number v);
+
+#define OFS(ofs) (((unsigned int)&ofs & 0x1ffff)>>1)
+
+static int odd_parity(unsigned int data) {
+    unsigned int i=0;
+
+    while(data)
+    {
+        i++;
+        data &= (data - 1);
+    } 
+
+    return !(i&1);
+}
+
+
+static void start_operation(brm_priv *brm) {
+	unsigned int ctrl = READ_REG(&brm->regs->ctrl);
+	brm->regs->ctrl = ctrl | 0x8000;
+}
+
+static void stop_operation(brm_priv *brm) {
+	unsigned int ctrl = READ_REG(&brm->regs->ctrl);
+  brm->regs->ctrl = ctrl & ~0x8000;
+}
+static int is_executing(brm_priv *brm) {
+	unsigned int ctrl = READ_REG(&brm->regs->ctrl);
+	return ((ctrl>>15) & 1);
+}
+
+#ifdef LEON3
+#ifndef DONT_DEF_RAMON
+int brm_register_leon3_ramon_fpga(void){
+	/* Clock div & Clock sel is NOT available.
+	 * The BRM is always clocked with 24MHz.
+	 * 3 in BRM enhanced register will select 24MHz
+	 */
+	return b1553brm_register(&amba_conf,0,0,3);
+}
+
+int brm_register_leon3_ramon_asic(void){
+	/* Clock div & Clock sel is available.
+	 * Clkdiv only matter when clksel is 1.
+	 * clksel=2, clkdiv=don't care, brm_frq=24MHz
+	 *
+	 * 3 in BRM enhanced register will select 24MHz
+	 */
+	return b1553brm_register(&amba_conf,2,0,3);
+}
+#endif
+#endif
+
+int B1553BRM_PREFIX(_register)(amba_confarea_type *bus, unsigned int clksel, unsigned int clkdiv, unsigned int brm_freq)
+{
+    rtems_status_code r;
+    rtems_device_major_number m;
+
+    FUNCDBG("brm_register:\n\r");
+		
+		/* save amba bus pointer */
+		amba_bus = bus;
+		if ( !bus ){
+			printk("brm_register: bus is NULL\n\r");
+			return 1;
+		}
+			
+#ifdef B1553BRM_LOCAL_MEM
+		allbrm_memarea = B1553BRM_LOCAL_MEM_ADR;
+#else
+		allbrm_memarea = NULL;
+#endif
+		
+		/* Save clksel, clkdiv and brm_freq for later use */
+		allbrm_cfg_clksel = clksel & CLKSEL_MASK;
+		allbrm_cfg_clkdiv = clkdiv & CLKDIV_MASK;
+		allbrm_cfg_freq = brm_freq & BRM_FREQ_MASK;
+		
+    if ((r = rtems_io_register_driver(0, &brm_driver, &m)) == RTEMS_SUCCESSFUL) {
+        DBG("BRM: driver successfully registered, major: %d\n",m);
+
+    } else {
+        switch(r) {
+        case RTEMS_TOO_MANY:
+            printk("BRM rtems_io_register_driver failed: RTEMS_TOO_MANY\n"); break;
+        case RTEMS_INVALID_NUMBER:
+            printk("BRM rtems_io_register_driver failed: RTEMS_INVALID_NUMBER\n"); break;
+        case RTEMS_RESOURCE_IN_USE:
+            printk("BRM rtems_io_register_driver failed: RTEMS_RESOURCE_IN_USE\n"); break;
+        default:
+           printk("BRM rtems_io_register_driver failed\n");
+        }
+				return 1;
+    }
+		return 0;
+}
+
+static void clr_int_logs(struct irq_log_list *logs){
+	int i;
+	for(i=0; i<16; i++){
+		logs[i].iiw = 0xffff;
+		logs[i].iaw = 0x0;
+	}
+}
+
+static rtems_device_driver rt_init(brm_priv *brm) {
+	unsigned int i;
+
+	brm->head = brm->tail = 0;
+	brm->rx_blocking = brm->tx_blocking = 1;
+
+  if ( brm->bm_event )
+		free(brm->bm_event);
+	brm->bm_event = NULL;
+
+  if ( brm->rt_event )
+		free(brm->rt_event);
+	
+	brm->bcmem = NULL;
+	brm->rtmem = (void *)brm->mem;
+
+	brm->rt_event = (struct rt_msg *) malloc(EVENT_QUEUE_SIZE*sizeof(struct rt_msg));
+  
+	if (brm->rt_event == NULL) {
+		DBG("BRM driver failed to allocated memory.");
+		return RTEMS_NO_MEMORY;
+	}
+
+	brm->irq_log = &brm->rtmem->irq_logs[0];
+
+	brm->regs->ctrl	  = 0x1912;  /* enable both buses, circular 1 bufmode, broadcast, interrupt log */
+	brm->regs->oper	  = 0x0900;  /* configure as RT, with addr 1 */
+	brm->regs->imask	 = BRM_RT_ILLCMD_IRQ|BRM_SUBAD_IRQ|BRM_TAPF_IRQ|BRM_DMAF_IRQ|BRM_WRAPF_IRQ|BRM_MERR_IRQ;
+	brm->regs->dpoint	= 0;
+	brm->regs->ipoint	= OFS(brm->rtmem->irq_logs[0]);
+	brm->regs->enhanced  = 0x0000 | allbrm_cfg_freq;  /* BRM clocked with freq = 12,16,20 or 24MHz */
+	brm->regs->w_ctrl	= (allbrm_cfg_clksel<<9) | (allbrm_cfg_clkdiv<<5) | 1;
+	brm->regs->w_irqctrl = 6;
+	brm->regs->w_ahbaddr = (unsigned int) memarea_to_hw(brm->memarea_base);
+		
+	clr_int_logs(brm->irq_log);
+
+	/* Legalize all commands */
+	for (i = 0; i < 16; i++) {
+		brm->regs->rt_cmd_leg[i] = 0;
+	}
+
+	/* Init descriptor table 
+	 * 
+	 * Each circular buffer has room for 8 messages with up to 34 (32 data + miw + time) words (16b) in each.
+	 * The buffers must separated by 34 words.
+	 */
+
+ 
+	/* RX Sub-address 0 - 31 */
+	for (i = 0; i < 32; i++) {
+		brm->rtmem->rxsubs[i].ctrl = 0x00E0;				/* Interrupt: INTX, IWA, and IBRD */
+		brm->rtmem->rxsubs[i].top  = OFS(brm->rtmem->rxsuba_msgs[i]);		     /* Top address */
+		brm->rtmem->rxsubs[i].cur  = OFS(brm->rtmem->rxsuba_msgs[i]);		     /* Current address */
+		brm->rtmem->rxsubs[i].bot  = OFS(brm->rtmem->rxsuba_msgs[i+1]) - sizeof(struct msg)/2; /* Bottom address */	
+		brm->last_read[i] = OFS(brm->rtmem->rxsuba_msgs[i]);
+	}
+	/* TX Sub-address 0 - 31 */
+	for (i = 0; i < 32; i++) {
+		brm->rtmem->txsubs[i].ctrl  = 0x0060;					/* Interrupt: IWA and IBRD */
+		brm->rtmem->txsubs[i].top   = OFS(brm->rtmem->txsuba_msgs[i]);			  /* Top address */
+		brm->rtmem->txsubs[i].cur   = OFS(brm->rtmem->txsuba_msgs[i]);			  /* Current address */
+		brm->rtmem->txsubs[i].bot   = OFS(brm->rtmem->txsuba_msgs[i+1]) - sizeof(struct msg)/2;	/* Bottom address */
+		brm->last_read[i+32]  = OFS(brm->rtmem->txsuba_msgs[i]);
+		brm->written[i] = OFS(brm->rtmem->txsuba_msgs[i]);
+	}
+	/* RX mode code 0 - 31 */
+	for (i = 0; i < 32; i++) {
+		brm->rtmem->rxmodes[i].ctrl = 0x00E0;					/* Interrupt: INTX, IWA, and IBRD */
+		brm->rtmem->rxmodes[i].top  = OFS(brm->rtmem->rxmode_msgs[i]);			  /* Top address */
+		brm->rtmem->rxmodes[i].cur  = OFS(brm->rtmem->rxmode_msgs[i]);			  /* Current address */
+		brm->rtmem->rxmodes[i].bot  = OFS(brm->rtmem->rxmode_msgs[i+1]) - sizeof(struct msg)/2;	/* Bottom address */
+		brm->last_read[i+64] = OFS(brm->rtmem->rxmode_msgs[i]);
+	}   
+	/* TX mode code 0 - 31 */
+	for (i = 0; i < 32; i++) {
+		brm->rtmem->txmodes[i].ctrl = 0x0060;					/* Interrupt: IWA and IBRD */
+		brm->rtmem->txmodes[i].top  = OFS(brm->rtmem->txmode_msgs[i]);			  /* Top address */
+		brm->rtmem->txmodes[i].cur  = OFS(brm->rtmem->txmode_msgs[i]);			  /* Current address */
+		brm->rtmem->txmodes[i].bot  = OFS(brm->rtmem->txmode_msgs[i+1]) - sizeof(struct msg)/2;	/* Bottom address */
+		brm->last_read[i+96] = OFS(brm->rtmem->txmode_msgs[i]);
+	}
+	
+	brm->mode = BRM_MODE_RT;
+
+	return RTEMS_SUCCESSFUL;
+}
+
+static rtems_device_driver bc_init(brm_priv *brm){
+
+  if ( brm->bm_event )
+		free(brm->bm_event);
+	brm->bm_event = NULL;
+
+  if ( brm->rt_event )
+		free(brm->rt_event);
+	brm->rt_event = NULL;
+	
+	brm->bcmem = (void *)brm->mem;
+	brm->rtmem = NULL;
+	brm->irq_log = &brm->bcmem->irq_logs[0];
+	
+	brm->head = brm->tail = 0;
+	brm->rx_blocking = brm->tx_blocking = 1;
+	
+	brm->regs->ctrl	  = 0x0006;  /* ping pong enable and enable interrupt log */
+	brm->regs->oper	  = 0x0800;  /* configure as BC */
+	brm->regs->imask	 = BRM_EOL_IRQ|BRM_BC_ILLCMD_IRQ|BRM_ILLOP_IRQ|BRM_DMAF_IRQ|BRM_WRAPF_IRQ|BRM_MERR_IRQ;
+	brm->regs->dpoint	= 0;
+	printk("Set BC interrupt log: 0x%lx, 0x%lx, 0x%lx\n",OFS(brm->bcmem->irq_logs[0]),&brm->bcmem->irq_logs[0],brm->bcmem);
+	brm->regs->ipoint	= OFS(brm->bcmem->irq_logs[0]);
+	brm->regs->enhanced  = 0x0000 | (allbrm_cfg_freq&0x3);  /* freq = 24 */
+	brm->regs->w_ctrl	= (allbrm_cfg_clksel<<9) | (allbrm_cfg_clkdiv<<5) | 1;
+	brm->regs->w_irqctrl = 6;
+	brm->regs->w_ahbaddr = (unsigned int) memarea_to_hw(brm->memarea_base);
+	
+	clr_int_logs(brm->irq_log);
+	
+	brm->mode = BRM_MODE_BC;
+	
+	return RTEMS_SUCCESSFUL;
+}
+
+static rtems_device_driver bm_init(brm_priv *brm) {
+
+
+	brm->head = brm->tail = 0;
+	brm->rx_blocking = brm->tx_blocking = 1;
+
+  if ( brm->rt_event )
+		free(brm->rt_event);
+	brm->rt_event = NULL;
+
+  if ( brm->bm_event )
+		free(brm->bm_event);
+	
+	brm->bcmem = NULL;
+	brm->rtmem = NULL;
+	
+	brm->bm_event	 = (struct bm_msg *) malloc(EVENT_QUEUE_SIZE*sizeof(struct bm_msg));
+ 
+	if (brm->bm_event == NULL) {
+		DBG("BRM driver failed to allocated memory.");
+		return RTEMS_NO_MEMORY;
+	}
+
+	/* end of 16K, fits all current modes (128K, 16K) */
+	brm->irq_log = &brm->mem[8*1024-16*2];
+
+	brm->regs->ctrl	  = 0x0006;  /* ping pong enable and enable interrupt log */
+	brm->regs->oper	  = 0x0A00;  /* configure as BM */
+	brm->regs->imask	= BRM_MBC_IRQ|BRM_MERR_IRQ|BRM_DMAF_IRQ|BRM_MERR_IRQ;
+	brm->regs->dpoint	= 0;
+	brm->regs->ipoint	= OFS(brm->mem[8*1024-16*2]);
+	brm->regs->mcpoint   = 0;	   /* Command pointer */
+	brm->regs->mdpoint   = 0x100;   /* Data pointer */
+	brm->regs->mbc	   = 1;	   /* Block count */
+	brm->regs->enhanced  = 0x0000 | (allbrm_cfg_freq&0x3);  /* freq = 24 */
+	brm->regs->w_ctrl	= (allbrm_cfg_clksel<<9) | (allbrm_cfg_clkdiv<<5) | 1;
+	brm->regs->w_irqctrl = 6;
+	brm->regs->w_ahbaddr = (unsigned int) memarea_to_hw(brm->memarea_base);
+	
+	clr_int_logs(brm->irq_log);
+	
+	brm->mode = BRM_MODE_BM;
+	
+	return RTEMS_SUCCESSFUL;
+}
+
+
+static rtems_device_driver brm_initialize(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
+{
+	rtems_status_code status;
+	int dev_cnt;
+	char fs_name[20];
+	brm_priv *brm;
+	amba_ahb_device ambadev;
+	char *mem;
+   
+	FUNCDBG("brm_initialize\n");
+
+	brm_cores = 0;
+	strcpy(fs_name,B1553BRM_DEVNAME);
+	
+	/* Find all BRM devices */
+	dev_cnt = amba_get_number_ahbslv_devices(amba_bus,VENDOR_GAISLER,GAISLER_BRM);
+	if ( dev_cnt < 1 ){
+		/* Failed to find any CAN cores! */
+		printk("BRM: Failed to find any BRM cores\n\r");
+		return -1;
+	}
+		
+	/* allocate & zero memory for the brm devices */
+	brms = malloc(sizeof(*brms)*dev_cnt);
+	if ( !brms ){
+		printk("BRM: Failed to allocate SW memory\n\r");
+		return -1;
+	}
+	memset(brms,0,sizeof(*brms)*dev_cnt);	
+	
+	/* Allocate memory for all device's descriptors, 
+	 * they must be aligned to a XXX byte boundary.
+	 */
+	#define BRM_DESCS_PER_CTRL 128
+	if ( allbrm_memarea ){
+		mem = allbrm_memarea;
+	}else{
+ 		/* sizeof(struct desc_table) * BRM_DESCS_PER_CTRL * dev_cnt */
+		mem = malloc( (128*1024) * (dev_cnt+1)); /* 128k per core + 128k for alignment */
+		if ( !mem ){
+			free(brms);
+			printk("BRM: Failed to allocate HW memory\n\r");
+			return -1;
+		}
+	
+		/* align memory to 128k boundary */
+		mem = (char *)(((unsigned int)mem+0x1ffff) & ~0x1ffff);
+	}
+
+	/* clear the used memory */
+	memset(mem,0,(128*1024) * dev_cnt);
+
+	/* initialize each brm device, one at a time */
+	for(minor=0; minor<dev_cnt; minor++){
+		brm = &brms[minor];
+		
+		/* Get AMBA AHB device info from Plug&Play */
+		amba_find_next_ahbslv(amba_bus,VENDOR_GAISLER,GAISLER_BRM,&ambadev,minor);
+		
+		/* Copy Basic HW info */
+		brm->regs = (void *)ambadev.start[0];
+		brm->irqno = ambadev.irq;
+		brm->minor = minor;
+		brm->irq = 0;
+#ifdef DEBUG
+		brm->log_i = 0;	
+		memset(brm->log,0,sizeof(brm->log));
+#endif
+		
+		/* Set unique name */
+		B1553BRM_DEVNAME_NO(fs_name,minor);
+    
+    DBG("Registering BRM core at [0x%x] irq %d, minor %d as %s\n",brm->regs,brm->irqno,minor,fs_name);
+		
+		/* Bind filesystem name to device number (minor) */
+		status = rtems_io_register_name(fs_name, major, minor);
+    if (status != RTEMS_SUCCESSFUL)
+			rtems_fatal_error_occurred(status);
+	
+		/* RX Semaphore created with count = 0 */
+		if ( rtems_semaphore_create(rtems_build_name('B', 'M', 'R', '0'+minor),
+		         0,
+		         RTEMS_FIFO|RTEMS_SIMPLE_BINARY_SEMAPHORE|RTEMS_NO_INHERIT_PRIORITY|RTEMS_LOCAL|RTEMS_NO_PRIORITY_CEILING, 
+		         0,
+		         &brm->rx_sem) != RTEMS_SUCCESSFUL ){
+      printk("BRM: Failed to create rx semaphore\n");
+	  	return RTEMS_INTERNAL_ERROR;
+    }
+
+			/* TX Semaphore created with count = 1 */
+		if ( rtems_semaphore_create(rtems_build_name('B', 'M', 'T', '0'+minor),
+		         1,
+		         RTEMS_FIFO|RTEMS_SIMPLE_BINARY_SEMAPHORE|RTEMS_NO_INHERIT_PRIORITY|RTEMS_LOCAL|RTEMS_NO_PRIORITY_CEILING, 
+		         0,
+		         &brm->tx_sem) != RTEMS_SUCCESSFUL ){
+      printk("BRM: Failed to create tx semaphore\n");
+	  	return RTEMS_INTERNAL_ERROR;
+    }
+
+		/* Device Semaphore created with count = 1 */
+		if ( rtems_semaphore_create(rtems_build_name('B', 'M', 'D', '0'+minor),
+		         1,
+		         RTEMS_FIFO|RTEMS_SIMPLE_BINARY_SEMAPHORE|RTEMS_NO_INHERIT_PRIORITY|RTEMS_LOCAL|RTEMS_NO_PRIORITY_CEILING, 
+		         0,
+		         &brm->dev_sem) != RTEMS_SUCCESSFUL ){
+      printk("BRM: Failed to create device semaphore\n");
+	  	return RTEMS_INTERNAL_ERROR;
+    }
+
+	
+		/* Set base address of all descriptors */
+		brm->memarea_base = (unsigned int)&mem[(128*1024) * minor];
+		brm->desc = (struct desc_table *) brm->memarea_base;
+		brm->mem = (volatile unsigned short *) brm->memarea_base;
+ 	  brm->irq_log	= (void *)(brm->memarea_base + (0xFFE0<<1)); /* last 64byte */
+		
+		brm->bm_event = NULL;
+		brm->rt_event = NULL;
+		
+		/* Sel clock so that we can write to BRM's registers */
+		brm->regs->w_ctrl = (allbrm_cfg_clksel<<9) | (allbrm_cfg_clkdiv<<5);
+		/* Reset BRM core */
+		brm->regs->w_ctrl = 1<<10 | READ_REG(&brm->regs->w_ctrl);
+		
+		/* Register interrupt handler */
+		B1553BRM_REG_INT(B1553BRM_PREFIX(_interrupt_handler), brm->irqno, brm);
+		
+		rt_init(brm);
+		
+		DBG("BRM: LOG: 0x%lx, 0x%lx\n\r",brm->log,brm);
+	}
+	
+	/* save number of BRM cores found */
+	brm_cores = dev_cnt;
+	
+	DBG("BRM initialisation done.\n");
+	
+	return RTEMS_SUCCESSFUL;
+}
+
+static rtems_device_driver brm_open(rtems_device_major_number major, rtems_device_minor_number minor, void *arg) {
+		brm_priv *brm;
+	
+    FUNCDBG("brm_open\n");
+
+    if (minor >= brm_cores) {
+        DBG("Wrong minor %d\n", minor);
+        return RTEMS_UNSATISFIED; /* ENODEV */
+    }
+		
+		brm = &brms[minor];
+   
+    if (rtems_semaphore_obtain(brm->dev_sem, RTEMS_NO_WAIT, RTEMS_NO_TIMEOUT) != RTEMS_SUCCESSFUL) {
+        DBG("brm_open: resource in use\n");
+        return RTEMS_RESOURCE_IN_USE; /* EBUSY */
+    }
+		
+		/* Set defaults */
+		brm->event_id = 0;
+    
+    start_operation(brm);
+ 
+    return RTEMS_SUCCESSFUL;
+}
+ 
+static rtems_device_driver brm_close(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
+{
+	brm_priv *brm = &brms[minor];
+	FUNCDBG("brm_close");
+
+	stop_operation(brm);
+	rtems_semaphore_release(brm->dev_sem);
+
+	return RTEMS_SUCCESSFUL;
+}
+ 
+static int get_rt_messages(brm_priv *brm, void *buf, unsigned int msg_count) {
+
+    struct rt_msg *dest = (struct rt_msg *) buf;
+    int count = 0;
+
+    if (brm->head == brm->tail) {
+        return 0;
+    }
+
+    do {
+
+        DBG("rt read - head: %d, tail: %d\n", brm->head, brm->tail);
+        dest[count++] = brm->rt_event[INDEX(brm->tail++)];
+
+    } while (brm->head != brm->tail && count < msg_count);
+
+    return count;
+
+}
+
+static int get_bm_messages(brm_priv *brm, void *buf, unsigned int msg_count) {
+
+    struct bm_msg *dest = (struct bm_msg *) buf;
+    int count = 0;
+
+    if (brm->head == brm->tail) {
+        return 0;
+    }
+
+    do {
+
+        DBG("bm read - head: %d, tail: %d\n", brm->head, brm->tail);
+        dest[count++] = brm->bm_event[INDEX(brm->tail++)];
+
+    } while (brm->head != brm->tail && count < msg_count);
+
+    return count;
+
+}
+
+static rtems_device_driver brm_read(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
+{
+    rtems_libio_rw_args_t *rw_args;
+    int count = 0;
+		brm_priv *brm = &brms[minor];
+    int (*get_messages)(brm_priv *brm, void *buf, unsigned int count);
+		
+		if ( ! (brm->mode & (BRM_MODE_RT | BRM_MODE_BM)) ){
+			return RTEMS_INVALID_NAME;
+		}
+  
+    rw_args = (rtems_libio_rw_args_t *) arg;
+    
+    if ( ((READ_REG(&brm->regs->oper)>>8) & 3) == 1 ) { /* RT */
+        get_messages = get_rt_messages;
+    }
+    else { /* BM */
+        get_messages = get_bm_messages;
+    }
+
+
+    FUNCDBG("brm_read [%i,%i]: buf: 0x%x len: %i\n",major, minor, (unsigned int)rw_args->buffer,rw_args->count);
+
+    while ( (count=get_messages(brm,rw_args->buffer, rw_args->count)) == 0 ) {
+
+        if (brm->rx_blocking) {
+            rtems_semaphore_obtain(brm->rx_sem, RTEMS_WAIT, RTEMS_NO_TIMEOUT);
+        }
+        else {
+            /* Translates to EBUSY */
+            return RTEMS_RESOURCE_IN_USE;
+        }
+
+    }
+
+    rw_args->bytes_moved = count;
+    return RTEMS_SUCCESSFUL;
+  
+}
+
+
+static rtems_device_driver brm_write(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
+{
+    rtems_libio_rw_args_t *rw_args;
+    struct rt_msg *source;
+    unsigned int count=0, current, next, descriptor, wc, suba;
+		brm_priv *brm = &brms[minor];
+  
+		if ( ! (brm->mode & BRM_MODE_RT) ){
+			return RTEMS_INVALID_NAME;
+		}
+	
+    rw_args = (rtems_libio_rw_args_t *) arg;
+    source = (struct rt_msg *) rw_args->buffer;
+
+    FUNCDBG("brm_write [%i,%i]: buf: 0x%x len: %i\n",major, minor, (unsigned int)rw_args->buffer,rw_args->count);
+  
+    do {
+
+        descriptor = source[count].desc & 0x7F;
+        suba       = descriptor-32;
+        wc         = source[count].miw >> 11;
+        wc = wc ? wc : 32;
+
+        /* Only subaddress transmission is allowed with write */
+        if (descriptor < 32 || descriptor >= 64)
+            return RTEMS_INVALID_NAME;
+
+        current = brm->desc[descriptor].cur; 
+        next = brm->written[suba] + 2 + wc;
+
+        if (brm->written[suba] < current) {
+            
+            if (next > current) {
+
+                /* No room in transmission buffer */ 
+
+                if (brm->tx_blocking && count == 0) {
+                    rtems_semaphore_obtain(brm->tx_sem, RTEMS_WAIT, RTEMS_NO_TIMEOUT);
+                }
+								else if ( brm->tx_blocking && (count != 0) ){
+									/* return the number of messages sent so far */
+									break;
+								}
+                else {	
+                    /* Translates to posix EBUSY */
+                    return RTEMS_RESOURCE_IN_USE;
+                }
+            }
+        }
+
+        memcpy((void *)&brm->mem[brm->written[suba]], &source[count], (2+wc)*2);
+        
+        count++;
+
+        if (next >= brm->desc[descriptor].bot) {
+            next = brm->desc[descriptor].top;
+        }
+        brm->written[suba] = next;
+
+    }  while (count < rw_args->count);
+  
+    rw_args->bytes_moved = count; 
+
+    if (count >= 0) {
+        return RTEMS_SUCCESSFUL;
+    }
+    return RTEMS_UNSATISFIED;
+}
+
+static rtems_device_driver brm_control(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
+{
+    
+    unsigned int i=0;
+    unsigned short ctrl, oper, cw1, cw2;
+    rtems_libio_ioctl_args_t *ioarg = (rtems_libio_ioctl_args_t *) arg;
+    unsigned int *data = ioarg->buffer;
+    struct bc_msg *cmd_list = (struct bc_msg *) ioarg->buffer;
+		brm_priv *brm = &brms[minor];
+		rtems_device_driver ret;
+		int len;
+    
+    FUNCDBG("brm_control[%d]: [%i,%i]\n",minor,major, minor);
+  
+    if (!ioarg) {
+        DBG("brm_control: invalid argument\n");
+        return RTEMS_INVALID_NAME;
+    }
+
+    ioarg->ioctl_return = 0;
+    switch(ioarg->command) {
+
+    case BRM_SET_MODE:
+			if ( data[0] > 2 )
+				return RTEMS_INVALID_NAME;
+      stop_operation(brm);
+        if (data[0] == 0) {
+            ret = bc_init(brm);
+        }
+        else if (data[0] == 1) { 
+            ret = rt_init(brm);				
+        }  
+        else if (data[0] == 2) { 
+            ret = bm_init(brm);						
+        }else
+					ret = RTEMS_INVALID_NAME;
+
+				if ( ret != RTEMS_SUCCESSFUL)
+							return ret;
+				
+        if ( brm->mode & (BRM_MODE_RT | BRM_MODE_BM ) )
+					start_operation(brm);
+        break;
+
+    case BRM_SET_BUS:
+        stop_operation(brm);
+				ctrl = READ_REG(&brm->regs->ctrl);
+        ctrl &= 0xE7FF;                               /* Clear bit 12-11 ...      */
+        ctrl |= (data[0]&0x3)<<11;                    /* ... OR in new bus status */
+				brm->regs->ctrl = ctrl;
+        start_operation(brm);
+        break;
+
+    case BRM_SET_MSGTO:
+        stop_operation(brm);
+        ctrl = READ_REG(&brm->regs->ctrl);
+				ctrl &= 0xFDFF;                               /* Clear bit 9 ...          */
+        ctrl |= (data[0]&1)<<9;                       /* ... OR in new MSGTO      */
+				brm->regs->ctrl = ctrl;
+        start_operation(brm);
+        break;
+
+    case BRM_SET_RT_ADDR:   
+        stop_operation(brm);
+				oper = READ_REG(&brm->regs->oper);
+        oper &= 0x03FF;                               /* Clear bit 15-10 ...      */
+        oper |= (data[0]&0x1f)<<11;                   /* ... OR in new address    */
+        oper |= odd_parity(data[0]&0x1f)<<10;         /* ... OR in parity         */
+				brm->regs->oper = oper;
+        start_operation(brm);
+        break;
+ 
+    case BRM_SET_STD:   
+        stop_operation(brm);
+				ctrl = READ_REG(&brm->regs->ctrl);
+        ctrl &= 0xFF7F;                               /* Clear bit 7 ...           */
+        ctrl |= (data[0]&1)<<7;                       /* ... OR in new ABSTD (1=A) */
+				brm->regs->ctrl = ctrl;
+        start_operation(brm);
+        break;
+
+    case BRM_SET_BCE:
+        stop_operation(brm);
+				ctrl = READ_REG(&brm->regs->ctrl);
+        ctrl &= 0xFFEF;                               /* Clear bit 4 ...           */
+        ctrl |= (data[0]&1)<<4;                       /* ... OR in new BCE         */
+				brm->regs->ctrl = ctrl;
+        start_operation(brm);
+     break;
+
+    case BRM_TX_BLOCK:
+        brm->tx_blocking     = data[0];      
+        break;
+
+    case BRM_RX_BLOCK: 
+        brm->rx_blocking     = data[0];   
+        break; 
+
+    case BRM_DO_LIST:
+
+        if ( brm->mode != BRM_MODE_BC ){
+          return RTEMS_INVALID_NAME;
+				}
+
+        /* Check if we are bus controller */
+        if ( ((READ_REG(&brm->regs->oper)>>8) & 3) != 0 ) {
+            return RTEMS_INVALID_NAME;
+        }
+
+        /* Already processing list? */
+        if (is_executing(brm)) {
+            return RTEMS_RESOURCE_IN_USE;
+        }
+
+				/* clear any earlier releases */
+        rtems_semaphore_obtain(brm->tx_sem, RTEMS_NO_WAIT, RTEMS_NO_TIMEOUT);
+
+				brm->bc_list_fail = 0;
+        brm->cur_list = cmd_list;
+				brm->regs->dpoint = 0;
+
+        i = 0;
+        while ( (cmd_list[i].ctrl & BC_EOL) == 0) {
+
+            ctrl = (4<<12) | (((cmd_list[i].ctrl&BC_BUSA)==BC_BUSA)<<9) | (((cmd_list[i].ctrl&BC_RTRT)==BC_RTRT)<<8);
+
+            if (cmd_list[i].ctrl&BC_RTRT) {
+                cw1 = (cmd_list[i].rtaddr[0]<<11) | (0<<10) | (cmd_list[i].subaddr[0]<<5) | (cmd_list[i].wc & 0x1f); /* receive cw */
+                cw2 = (cmd_list[i].rtaddr[1]<<11) | (1<<10) | (cmd_list[i].subaddr[1]<<5) | (cmd_list[i].wc & 0x1f); /* transmit cw */
+            }
+            else {
+                cw1 = (cmd_list[i].rtaddr[0]<<11) | (((cmd_list[i].ctrl&BC_TR)==BC_TR)<<10) | (cmd_list[i].subaddr[0]<<5) | (cmd_list[i].wc&0x1f);
+								cw2 = 0;
+            }
+
+
+            /* Set up command block */
+            brm->bcmem->descs[i].ctrl = ctrl;
+            brm->bcmem->descs[i].cw1 = cw1;
+            brm->bcmem->descs[i].cw2 = cw2;
+						/* data pointer:
+						 * (&brm->bcmem->msg_data[i].data[0] & 0x1ffff) / 2 
+						 */
+            brm->bcmem->descs[i].dptr = 1024+i*32;  /* data pointer */
+						brm->bcmem->descs[i].tsw[0] = 0;
+						brm->bcmem->descs[i].tsw[1] = 0;
+						brm->bcmem->descs[i].ba = 0;
+						brm->bcmem->descs[i].timer = 0;
+
+            memcpy((void *)&brm->bcmem->msg_data[i].data[0], &cmd_list[i].data[0], cmd_list[i].wc*2);
+          
+            i++;
+        }
+
+        brm->bcmem->descs[i].ctrl = 0; /* end of list */
+
+        start_operation(brm);        
+
+        break;  
+
+    case BRM_LIST_DONE:
+
+        if ( brm->mode != BRM_MODE_BC ){
+          return RTEMS_INVALID_NAME;
+				}
+				
+				/* Check if we are bus controller */
+        if ( ((READ_REG(&brm->regs->oper)>>8) & 3) != 0 ) {
+            return RTEMS_INVALID_NAME;
+        }
+    
+        if (is_executing(brm)) {
+
+            data[0] = 0;
+            if (brm->tx_blocking) {
+                rtems_semaphore_obtain(brm->tx_sem, RTEMS_WAIT, RTEMS_NO_TIMEOUT);
+                data[0] = 1;
+								if ( brm->bc_list_fail ){
+									return RTEMS_INVALID_NAME;
+								}
+            }else{
+							return RTEMS_RESOURCE_IN_USE;
+						}
+            
+            
+        }
+        else {
+            data[0] = 1; /* done */
+        }
+
+        /* copy finished list results back into bc_msg array */
+        i = 0;
+        while ( (brm->cur_list[i].ctrl & BC_EOL) == 0) {
+
+            if (READ_DMA(&brm->bcmem->descs[i].ctrl) & 1) {
+                brm->cur_list[i].ctrl |= 0x8000; /* Set BAME */ 
+            } 
+            if (brm->cur_list[i].ctrl & BC_TR) {
+                /* RT Transmit command, copy received data */
+								len = brm->cur_list[i].wc;
+								while( len-- > 0){
+									brm->cur_list[i].data[len] = READ_DMA(&brm->bcmem->msg_data[i].data[len]);
+								}
+            }
+            brm->cur_list[i].tsw[0] = READ_DMA(&brm->bcmem->descs[i].tsw[0]);
+            brm->cur_list[i].tsw[1] = READ_DMA(&brm->bcmem->descs[i].tsw[1]);
+
+            i++;
+        }
+        break;
+
+
+    case BRM_CLR_STATUS:
+			brm->status = 0;
+			break;
+		
+    case BRM_GET_STATUS: /* copy status */
+
+			if ( !ioarg->buffer )
+				return RTEMS_INVALID_NAME;
+			
+			*(unsigned int *)ioarg->buffer = brm->status;
+			break;
+    
+		case BRM_SET_EVENTID:
+			brm->event_id = (rtems_id)ioarg->buffer;
+			break;
+
+    default:
+        return RTEMS_NOT_DEFINED;
+    }
+    return RTEMS_SUCCESSFUL;
+}
+
+static void b1553brm_interrupt_handler(rtems_vector_number v){
+	int i;
+	/* find minor */
+	for(i=0; i<brm_cores; i++){
+		if ( (brms[i].irqno+0x10) == v ){
+			brm_interrupt(&brms[i]);
+			return;
+		}
+	}
+}
+
+static void brm_interrupt(brm_priv *brm) {
+  unsigned short descriptor, current, pending, miw, wc, tmp;
+	unsigned short msgadr, iaw, iiw;
+	int len;
+	int signal_event=0;
+	unsigned int event_status;
+	#define SET_ERROR_DESCRIPTOR(descriptor) (event_status = (event_status & 0x0000ffff) | descriptor<<16);
+ 		
+	while( (iiw=READ_REG(&brm->irq_log[brm->irq].iiw)) != 0xffff ){
+		iaw=READ_REG(&brm->irq_log[brm->irq].iaw);
+		
+		/* indicate that the interrupt log entry has been processed */
+		brm->irq_log[brm->irq].iiw = 0xffff;
+
+		/* Interpret interrupt log entry  */
+    descriptor = iaw >> 2;
+    pending    = iiw;
+    brm->irq = (brm->irq + 1) % 16;
+		
+		/* Clear the log so that we */
+		
+	
+    /* Subaddress accessed irq (RT only) 
+     *
+     * Can be either a receive or transmit command
+     * as well as a mode code.
+     */
+    if (pending & BRM_SUBAD_IRQ) {
+        			
+        /* Pointer to next free message in circular buffer */
+        current = READ_DMA(&brm->desc[descriptor].cur);
+
+        while  ( (msgadr=brm->last_read[descriptor]) != current) {
+
+            /* Get word count */
+            miw = READ_DMA(&brm->mem[msgadr]);
+            wc  = miw >> 11;
+
+            /* Data received */
+            if (descriptor < 32) {
+                wc = wc ? wc : 32;
+            }
+            /* Data transmitted */ 
+            else if (descriptor < 64) {
+                wc = wc ? wc : 32;  
+                rtems_semaphore_release(brm->tx_sem);
+            }
+            /* RX Mode code */
+            else if (descriptor < 96) {
+                wc = (wc>>4);
+            }  
+            /* TX Mode code */
+            else if (descriptor < 128) {
+                wc = (wc>>4);
+            }
+
+#ifdef DEBUG            
+            brm->log[brm->log_i++ % EVENT_QUEUE_SIZE] = (descriptor << 16) | wc; 
+            brm->log[brm->log_i++ % EVENT_QUEUE_SIZE] = current;
+            brm->log[brm->log_i++ % EVENT_QUEUE_SIZE] = msgadr;
+#endif
+
+            /* If there is room in the event queue, copy the event there */
+            if (brm->head - brm->tail != EVENT_QUEUE_SIZE) {
+        
+                /* Copy to event queue */
+								brm->rt_event[INDEX(brm->head)].miw = READ_DMA(&brm->mem[msgadr]);
+								brm->rt_event[INDEX(brm->head)].time = READ_DMA(&brm->mem[msgadr+1]);
+								len = wc;
+								while( len-- > 0){
+									brm->rt_event[INDEX(brm->head)].data[len] = READ_DMA(&brm->mem[msgadr+2+len]);
+								}
+                brm->rt_event[INDEX(brm->head)].desc = descriptor;
+                brm->head++;
+
+            }
+            else {
+                /* Indicate overrun */
+                brm->rt_event[INDEX(brm->head)].desc |= 0x8000;
+            }
+
+            msgadr += (2+wc);
+
+            if (msgadr >= brm->desc[descriptor].bot) {
+                msgadr = brm->desc[descriptor].top;
+            }
+						brm->last_read[descriptor] = msgadr;
+
+#ifdef DEBUG
+            brm->log[brm->log_i++ % EVENT_QUEUE_SIZE] = msgadr;
+#endif
+
+            /* Wake any blocked rx thread */
+            rtems_semaphore_release(brm->rx_sem);
+  
+        }
+        
+    }
+
+    if (pending & BRM_EOL_IRQ) {  
+        rtems_semaphore_release(brm->tx_sem);
+    }
+
+    if (pending & BRM_BC_ILLCMD_IRQ) {
+				brm->bc_list_fail = 1;
+        rtems_semaphore_release(brm->tx_sem);
+				SET_ERROR_DESCRIPTOR(descriptor);
+				FUNCDBG("BRM: ILLCMD IRQ\n\r");
+    }
+
+    /* Monitor irq */
+    if (pending & BRM_MBC_IRQ) { 
+			
+        stop_operation(brm);
+        brm->regs->mbc = 1;
+        start_operation(brm);
+        
+        /* If there is room in the event queue, copy the event there */
+        if (brm->head - brm->tail != EVENT_QUEUE_SIZE) {
+            
+            /* Copy to event queue */
+         
+            brm->bm_event[INDEX(brm->head)].miw  =  READ_DMA(&brm->mem[0]);
+            brm->bm_event[INDEX(brm->head)].cw1  =  READ_DMA(&brm->mem[1]);
+            brm->bm_event[INDEX(brm->head)].cw2  =  READ_DMA(&brm->mem[2]);
+            brm->bm_event[INDEX(brm->head)].sw1  =  READ_DMA(&brm->mem[4]);
+            brm->bm_event[INDEX(brm->head)].sw2  =  READ_DMA(&brm->mem[5]);
+            brm->bm_event[INDEX(brm->head)].time =  READ_DMA(&brm->mem[6]);
+
+            len = 32;
+            while ( len-- ){
+              brm->bm_event[INDEX(brm->head)].data[len] =  READ_DMA(&brm->mem[0x100+len]);
+              len--;
+              brm->bm_event[INDEX(brm->head)].data[len] =  READ_DMA(&brm->mem[0x100+len]);
+              len--;
+              brm->bm_event[INDEX(brm->head)].data[len] =  READ_DMA(&brm->mem[0x100+len]);
+              len--;
+              brm->bm_event[INDEX(brm->head)].data[len] =  READ_DMA(&brm->mem[0x100+len]);
+            }
+/*            memcpy((void *)brm->bm_event[INDEX(brm->head)].data, &brm->mem[0x100], 32);*/
+
+#ifdef DEBUG            
+            brm->log[brm->log_i++ % EVENT_QUEUE_SIZE] = READ_REG(&brm->regs->mbc);
+            brm->log[brm->log_i++ % EVENT_QUEUE_SIZE] = READ_DMA(&brm->mem[0]);
+            brm->log[brm->log_i++ % EVENT_QUEUE_SIZE] = READ_DMA(&brm->mem[1]);
+            brm->log[brm->log_i++ % EVENT_QUEUE_SIZE] = READ_DMA(&brm->mem[4]);
+#endif
+
+            brm->head++;
+            
+        }
+        else {
+            /* Indicate overrun */
+            brm->rt_event[INDEX(brm->head)].miw |= 0x8000;
+        }
+        
+        /* Wake any blocking thread */
+        rtems_semaphore_release(brm->rx_sem);
+
+    }
+		
+		/* The reset of the interrupts 
+		 * cause a event to be signalled
+		 * so that user can handle error.
+		 */
+		if ( pending & BRM_RT_ILLCMD_IRQ){
+			FUNCDBG("BRM: BRM_RT_ILLCMD_IRQ\n\r");
+			brm->status |= BRM_RT_ILLCMD_IRQ;
+			event_status |= BRM_RT_ILLCMD_IRQ;
+			SET_ERROR_DESCRIPTOR(descriptor);
+			signal_event=1;
+		}
+		
+		if ( pending & BRM_ILLOP_IRQ){
+			FUNCDBG("BRM: BRM_ILLOP_IRQ\n\r");
+			brm->bc_list_fail = 1;
+			rtems_semaphore_release(brm->tx_sem);
+			event_status |= BRM_ILLOP_IRQ;
+			SET_ERROR_DESCRIPTOR(descriptor);			
+			signal_event=1;
+		}
+		
+		if ( pending & BRM_MERR_IRQ){
+			FUNCDBG("BRM: BRM_MERR_IRQ\n\r");
+			event_status |= BRM_MERR_IRQ;
+			SET_ERROR_DESCRIPTOR(descriptor);
+			signal_event=1;
+		}
+	    /* Clear Block Accessed Bit */ 
+    tmp = READ_REG(&brm->desc[descriptor].ctrl);
+    brm->desc[descriptor].ctrl = tmp & ~0x10;
+		
+	} /* While */
+	
+	/* clear interrupt flags & handle Hardware errors */
+	pending = READ_REG(&brm->regs->ipend);
+	
+	if ( pending & BRM_DMAF_IRQ){
+		FUNCDBG("BRM: BRM_DMAF_IRQ\n\r");
+		event_status |= BRM_DMAF_IRQ;
+		signal_event=1;
+	}
+	
+	if ( pending & BRM_WRAPF_IRQ){
+		FUNCDBG("BRM: BRM_WRAPF_IRQ\n\r");
+		event_status |= BRM_WRAPF_IRQ;
+		signal_event=1;
+	}
+	
+	if ( pending & BRM_TAPF_IRQ){
+		FUNCDBG("BRM: BRM_TAPF_IRQ\n\r");
+		event_status |= BRM_TAPF_IRQ;
+		signal_event=1;
+	}
+	
+	/* Copy current mask to status mask */
+	if ( event_status ){
+		if ( event_status & 0xffff0000 )
+			brm->status &= 0x0000ffff;
+		brm->status |= event_status;
+	}
+	
+	/* signal event once */
+	if ( signal_event && (brm->event_id!=0) ){
+		rtems_event_send(brm->event_id, event_status);
+	}
+   
+}
diff --git a/c/src/lib/libbsp/sparc/shared/1553/b1553brm_pci.c b/c/src/lib/libbsp/sparc/shared/1553/b1553brm_pci.c
new file mode 100644
index 0000000000..82e09e8abe
--- /dev/null
+++ b/c/src/lib/libbsp/sparc/shared/1553/b1553brm_pci.c
@@ -0,0 +1,132 @@
+/* Select PCI driver */
+#define B1553BRM_NO_AMBA
+#define B1553BRM_PCI
+
+#undef B1553BRM_MAXDEVS
+
+/* Use only 16K memory */
+#define DMA_MEM_16K
+
+/* Malloced memory or
+ * Card local memory
+ */
+#define B1553BRM_LOCAL_MEM
+
+#define DONT_DEF_RAMON
+
+/* memory must be aligned to a 128k boundary */
+unsigned int brmpci_memarea_address;
+#define B1553BRM_LOCAL_MEM_ADR brmpci_memarea_address
+
+/* We have custom address tranlation for HW addresses */
+#define B1553BRM_ADR_TO
+
+/* No custom MEMAREA=>CPU used since BRM Core work with offsets 
+ * in it's descriptors.
+ */
+#undef B1553BRM_ADR_FROM
+
+/* Set registered device name */
+#define B1553BRM_DEVNAME "/dev/brmpci0"
+#define B1553BRM_DEVNAME_NO(devstr,no) ((devstr)[11]='0'+(no))
+
+/* Any non-static function will begin with */
+#define B1553BRM_PREFIX(name) b1553brmpci##name
+
+/* do nothing, assume that the interrupt handler is called 
+ * setup externally calling b1553_interrupt_handler.
+ */
+#define B1553BRM_REG_INT(handler,irq,arg) \
+ if ( b1553brm_pci_int_reg ) \
+   b1553brm_pci_int_reg(handler,irq,arg);
+
+
+#ifdef B1553BRM_ADR_TO
+/* Translate a address within the Memory Region (memarea) into an Hardware
+ * device address. This address is put into hardware registers or descriptors
+ * so that the hardware can access the Memory Region.
+ * Example:
+ * An local AMBA access at 0xe0000000 will translate into PCI address 0x40000000,
+ * the PCI address 0x40000000 will translate into CPU-AMBA address 0x40000000.
+ */
+unsigned int brmpci_hw_address;
+static inline unsigned int memarea_to_hw(unsigned int addr) {
+		/* don't translate? */
+		if ( brmpci_hw_address == 0xffffffff )
+			return addr;
+    return ((addr & 0x000fffff) | brmpci_hw_address);
+}
+#endif
+
+/* not used since BRM Core work with offsets */
+#ifdef B1553BRM_ADR_FROM
+unsigned int brmpci_cpu_access_address;
+static inline unsigned int hw_to_cpu(unsigned int addr) {
+		/* don't translate? */
+		if ( brmpci_cpu_address == 0xffffffff )
+			return addr;
+    return ((addr & 0x0fffffff) | brmpci_cpu_address);
+}
+#endif
+
+void (*b1553brm_pci_int_reg)(void *handler, int irq, void *arg) = 0;
+
+static void b1553brmpci_interrupt_handler(int irq, void *arg);
+
+#include "b1553brm.c"
+
+/*
+ *
+ * memarea     = preallocated memory somewhere, pointer to start of memory.
+ * hw_address  = how to translate a memarea address into an HW device AMBA address.
+ */
+
+int b1553brm_pci_register(
+ amba_confarea_type *bus, 
+ unsigned int clksel, 
+ unsigned int clkdiv, 
+ unsigned int brm_freq,
+ unsigned int memarea,
+ unsigned int hw_address
+ )
+{
+	/* Setup configuration */
+	
+	/* if zero malloc will be used */
+	brmpci_memarea_address = memarea;
+
+	brmpci_hw_address = hw_address;
+
+#ifdef B1553BRM_ADR_FROM
+	brmpci_cpu_address = memarea & 0xf0000000;
+#endif
+	
+	/* Register the driver */
+	return B1553BRM_PREFIX(_register)(bus,clksel,clkdiv,brm_freq);
+}
+
+/* Call this from PCI interrupt handler 
+ * irq = the irq number of the HW device local to that IRQMP controller
+ * 
+ */
+static void b1553brmpci_interrupt_handler(int irq, void *arg){
+	brm_interrupt(arg);
+}
+
+#if 0
+int b1553brm_pci_interrupt_handler(int irqmask){
+	int i;
+	unsigned int mask=0;
+	/* find minor */
+	for(i=0; i<brm_cores; i++){
+		if ( (1<<brms[i].irqno) & irqmask ){
+			mask |= 1<<brms[i].irqno;
+			brm_interrupt(&brms[i]);
+			/* more interrupts to scan for? */
+			if ( irqmask & ~mask )
+				return mask; /* handled */
+		}
+	}
+	return mask;
+}
+#endif
diff --git a/c/src/lib/libbsp/sparc/shared/1553/b1553brm_rasta.c b/c/src/lib/libbsp/sparc/shared/1553/b1553brm_rasta.c
new file mode 100644
index 0000000000..45e619fb0d
--- /dev/null
+++ b/c/src/lib/libbsp/sparc/shared/1553/b1553brm_rasta.c
@@ -0,0 +1,115 @@
+/* Select PCI driver */
+#define B1553BRM_NO_AMBA
+#define B1553BRM_PCI
+
+#undef B1553BRM_MAXDEVS
+
+/* Use only 16K memory */
+#define DMA_MEM_128K
+
+/* Malloced memory or
+ * Card local memory
+ */
+#define B1553BRM_LOCAL_MEM
+
+#define DONT_DEF_RAMON
+
+/* memory must be aligned to a 128k boundary */
+unsigned int brmrasta_memarea_address;
+#define B1553BRM_LOCAL_MEM_ADR brmrasta_memarea_address
+
+/* We have custom address tranlation for HW addresses */
+#define B1553BRM_ADR_TO
+
+/* No custom MEMAREA=>CPU used since BRM Core work with offsets 
+ * in it's descriptors.
+ */
+#undef B1553BRM_ADR_FROM
+
+/* Set registered device name */
+#define B1553BRM_DEVNAME "/dev/brmrasta0"
+#define B1553BRM_DEVNAME_NO(devstr,no) ((devstr)[13]='0'+(no))
+
+/* Any non-static function will begin with */
+#define B1553BRM_PREFIX(name) b1553brmrasta##name
+
+/* do nothing, assume that the interrupt handler is called 
+ * setup externally calling b1553_interrupt_handler.
+ */
+#define B1553BRM_REG_INT(handler,irq,arg) \
+ if ( b1553brm_rasta_int_reg ) \
+   b1553brm_rasta_int_reg(handler,irq,arg);
+
+
+#ifdef B1553BRM_ADR_TO
+/* Translate a address within the Memory Region (memarea) into an Hardware
+ * device address. This address is put into hardware registers or descriptors
+ * so that the hardware can access the Memory Region.
+ * Example:
+ * An local AMBA access at 0xe0000000 will translate into PCI address 0x40000000,
+ * the PCI address 0x40000000 will translate into CPU-AMBA address 0x40000000.
+ */
+unsigned int brmrasta_hw_address;
+static inline unsigned int memarea_to_hw(unsigned int addr) {
+		/* don't translate? */
+		if ( brmrasta_hw_address == 0xffffffff )
+			return addr;
+    return ((addr & 0x0fffffff) | brmrasta_hw_address);
+}
+#endif
+
+/* not used since BRM Core work with offsets */
+#ifdef B1553BRM_ADR_FROM
+unsigned int brmrasta_cpu_access_address;
+static inline unsigned int hw_to_cpu(unsigned int addr) {
+		/* don't translate? */
+		if ( brmrasta_cpu_address == 0xffffffff )
+			return addr;
+    return ((addr & 0x0fffffff) | brmrasta_cpu_address);
+}
+#endif
+
+void (*b1553brm_rasta_int_reg)(void *handler, int irq, void *arg) = 0;
+
+static void b1553brmrasta_interrupt_handler(int irq, void *arg);
+
+#include "b1553brm.c"
+
+/*
+ *
+ * memarea     = preallocated memory somewhere, pointer to start of memory.
+ * hw_address  = how to translate a memarea address into an HW device AMBA address.
+ */
+
+int b1553brm_rasta_register(
+ amba_confarea_type *bus, 
+ unsigned int clksel, 
+ unsigned int clkdiv, 
+ unsigned int brm_freq,
+ unsigned int memarea,
+ unsigned int hw_address
+ )
+{
+	/* Setup configuration */
+	
+	/* if zero the malloc will be used */
+	brmrasta_memarea_address = memarea;
+
+	brmrasta_hw_address = hw_address;
+
+#ifdef B1553BRM_ADR_FROM
+	brmrasta_cpu_address = memarea & 0xf0000000;
+#endif
+	
+	/* Register the driver */
+	return B1553BRM_PREFIX(_register)(bus,clksel,clkdiv,brm_freq);
+}
+
+/* Call this from RASTA interrupt handler 
+ * irq = the irq number of the HW device local to that IRQMP controller
+ * 
+ */
+static void b1553brmrasta_interrupt_handler(int irq, void *arg){
+	brm_interrupt(arg);
+}
+
diff --git a/c/src/lib/libbsp/sparc/shared/can/grcan.c b/c/src/lib/libbsp/sparc/shared/can/grcan.c
new file mode 100644
index 0000000000..2482414165
--- /dev/null
+++ b/c/src/lib/libbsp/sparc/shared/can/grcan.c
@@ -0,0 +1,1706 @@
+/*
+ *  GRCAN driver  
+ *
+ *  COPYRIGHT (c) 2007.
+ *  Gaisler Research.
+ *
+ *  The license and distribution terms for this file may be
+ *  found in the file LICENSE in this distribution or at
+ *  http://www.rtems.com/license/LICENSE.
+ *
+ *
+ *  2007-06-13, Daniel Hellstrom <daniel@gaisler.com>
+ *    New driver in sparc shared directory. Parts taken
+ *    from rasta grhcan driver.
+ *
+ */
+
+#include <bsp.h>
+#include <rtems/libio.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+#include <assert.h>
+#include <sched.h>
+#include <ctype.h>
+#include <rtems/bspIo.h>
+
+#include <grcan.h>
+#include <ambapp.h>
+#include <pci.h>
+
+#define WRAP_AROUND_TX_MSGS 1
+#define WRAP_AROUND_RX_MSGS 2
+#define GRCAN_MSG_SIZE sizeof(struct grcan_msg)
+#define BLOCK_SIZE (16*4)
+
+/* 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_GLOBAL_DISABLE
+ #define IRQ_GLOBAL_DISABLE() sparc_disable_interrupts()
+#endif
+
+#ifndef IRQ_GLOBAL_ENABLE
+ #define IRQ_GLOBAL_ENABLE() sparc_enable_interrupts()
+#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_PREFIX
+ #define GRCAN_PREFIX(name) grcan##name
+#endif
+
+#ifndef MEMAREA_TO_HW
+  #define MEMAREA_TO_HW(x) (x)
+#endif
+
+/* default name to /dev/grcan0 */
+#if !defined(GRCAN_DEVNAME) || !defined(GRCAN_DEVNAME_NO)
+ #undef GRCAN_DEVNAME
+ #undef GRCAN_DEVNAME_NO
+ #define GRCAN_DEVNAME "/dev/grcan0"
+ #define GRCAN_DEVNAME_NO(devstr,no) ((devstr)[10]='0'+(no))
+#endif
+
+#ifndef GRCAN_REG_INT
+ #define GRCAN_REG_INT(handler,irqno,arg) set_vector(handler,irqno+0x10,1)
+#endif
+
+#ifndef GRCAN_DEFAULT_BAUD
+  /* default to 500kbits/s */
+  #define GRCAN_DEFAULT_BAUD 500000
+#endif
+
+/* Uncomment for debug output */
+/****************** DEBUG Definitions ********************/
+#define DBG_IOCTRL 1
+#define DBG_TX 2
+#define DBG_RX 4
+
+#define DEBUG_FLAGS (DBG_IOCTRL | DBG_RX | DBG_TX )
+/*#define DEBUG
+#define DEBUGFUNCS*/
+
+#include <debug_defs.h>
+
+/*
+#ifdef DEBUG
+#include <debug_defs.h>
+#else
+void silentdbg_init(void);
+void silentdbg_printf(char *fmt, ...);
+void silentdbg_int_printf(char *fmt, ...);
+void silentdbg_get_buf(char *buf, int max);
+int silentdbg_print_buf(int max);
+ extern int DEBUG_printf(const char *fmt, ...);
+ #define DBG(fmt, args...)    do { silentdbg_printf(" : %03d @ %18s()]:" fmt , __LINE__,__FUNCTION__,## args); } while(0)
+ #define DBG2(fmt)            do { silentdbg_printf(" : %03d @ %18s()]:" fmt , __LINE__,__FUNCTION__); } while(0) 
+ #define DBGC(c,fmt, args...) do { if (DEBUG_FLAGS & c) { silentdbg_printf(" : %03d @ %18s()]:" fmt , __LINE__,__FUNCTION__,## args);  }} while(0)
+ #ifdef DEBUGFUNCS
+  #define FUNCDBG()         do { silentdbg_printf("%s\n\r",__FUNCTION__); } while(0)
+  #define FUNCDBG_INT()
+ #else
+  #define FUNCDBG()
+  #define FUNCDBG_INT()     do { silentdbg_int_printf("%s\n\r",__FUNCTION__); } while(0)
+ #endif
+ 
+#endif
+*/
+/*********************************************************/
+
+/* grcan needs to have it buffers aligned to 1k boundaries */
+#define BUFFER_ALIGNMENT_NEEDS 1024
+
+#ifdef STATICALLY_ALLOCATED_TX_BUFFER
+static unsigned int tx_circbuf[GRCAN_MAX_CORES][TX_BUF_SIZE] 
+	__attribute__ ((aligned(BUFFER_ALIGNMENT_NEEDS)));
+#define STATIC_TX_BUF_SIZE TX_BUF_SIZE
+#define STATIC_TX_BUF_ADDR(core) (&tx_circbuf[(core)][0])
+#endif
+
+#ifdef STATICALLY_ALLOCATED_RX_BUFFER
+static unsigned int rx_circbuf[GRCAN_MAX_CORES][RX_BUF_SIZE] 
+	__attribute__ ((aligned(BUFFER_ALIGNMENT_NEEDS)));
+#define STATIC_RX_BUF_SIZE RX_BUF_SIZE
+#define STATIC_RX_BUF_ADDR(core) (&rx_circbuf[(core)][0])
+#endif
+
+/* 
+ * If USE_AT697_RAM is defined the RAM on the AT697 board will be used for DMA buffers (but rx message queue is always in AT697 ram).
+ * USE_AT697_DMA specifies whether the messages will be fetched using DMA or PIO.
+ *
+ * RASTA_PCI_BASE is the base address of the GRPCI AHB slave
+ * 
+ * GRCAN_BUF_SIZE must be set to the size (in bytes) of the GRCAN DMA buffers.
+ *
+ * RX_QUEUE_SIZE defines the number of messages that fits in the  RX message queue. On RX interrupts the messages in the DMA buffer 
+ * are copied into the message queue (using dma if the rx buf is not in the AT697 ram).
+ */
+
+/*#define USE_AT697_RAM              1      */
+#define USE_AT697_DMA              1     
+#define RASTA_PCI_BASE             0xe0000000   
+#define GRCAN_BUF_SIZE            4096      
+#define RX_QUEUE_SIZE              1024         
+
+#define INDEX(x) ( x&(RX_QUEUE_SIZE-1) )
+
+/* pa(x) 
+ *
+ * x: address in AT697 address space
+ * 
+ * returns the address in the RASTA address space that can be used to access x with dma.
+ * 
+*/
+#ifdef USE_AT697_RAM
+static inline unsigned int pa(unsigned int addr) {
+    return ((addr & 0x0fffffff) | RASTA_PCI_BASE);   
+}
+#else
+static inline unsigned int pa(unsigned int addr) {
+    return ((addr & 0x0fffffff) | 0x40000000);        
+}
+#endif
+
+struct grcan_msg {
+    unsigned int head[2];
+    unsigned char data[8];
+};
+
+struct grcan_config {
+	struct grcan_timing timing;
+	struct grcan_selection selection;
+	int abort;
+	int silent;
+};
+
+struct grcan_priv {
+  unsigned int baseaddr, ram_base;
+  struct grcan_regs *regs;
+  int irq;
+  int minor;
+  int open;
+  int started;
+  unsigned int channel;
+  int flushing;
+  unsigned int corefreq_hz;
+  
+  /* Circular DMA buffers */
+  void *_rx;
+  void *_tx;
+  struct grcan_msg *rx;
+  struct grcan_msg *tx;
+  unsigned int rxbuf_size;    /* requested RX buf size in bytes */
+  unsigned int txbuf_size;    /* requested TX buf size in bytes */
+
+  int txblock, rxblock;
+  int txcomplete, rxcomplete;
+  int txerror, rxerror;
+  
+  struct grcan_filter sfilter;
+  struct grcan_filter afilter;
+  int config_changed; /* 0=no changes, 1=changes ==> a Core reset is needed */
+  struct grcan_config config;
+  struct grcan_stats stats;
+  
+  rtems_id rx_sem, tx_sem, txempty_sem, dev_sem;
+};
+
+static int grcan_core_cnt;
+struct grcan_priv *grcans;
+static amba_confarea_type *amba_bus;
+static unsigned int ram_base;
+struct grcan_device_info *grcan_cores;
+static int grcan_core_cnt;
+
+static rtems_device_driver grcan_initialize(rtems_device_major_number  major, rtems_device_minor_number  minor, void *arg);
+static rtems_device_driver grcan_open(rtems_device_major_number major, rtems_device_minor_number minor, void *arg);
+static rtems_device_driver grcan_close(rtems_device_major_number major, rtems_device_minor_number minor, void *arg);
+static rtems_device_driver grcan_read(rtems_device_major_number major, rtems_device_minor_number minor, void *arg);
+static rtems_device_driver grcan_write(rtems_device_major_number major, rtems_device_minor_number minor, void *arg);
+static rtems_device_driver grcan_ioctl(rtems_device_major_number major, rtems_device_minor_number minor, void *arg);
+
+#define GRCAN_DRIVER_TABLE_ENTRY { grcan_initialize, grcan_open, grcan_close, grcan_read, grcan_write, grcan_ioctl }
+
+static unsigned int grcan_hw_read_try(
+  struct grcan_priv *pDev,
+  struct grcan_regs *regs,
+  CANMsg *buffer,
+  int max);
+
+static unsigned 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);
+
+#ifndef GRCAN_DONT_DECLARE_IRQ_HANDLER
+static rtems_isr grcan_interrupt_handler(rtems_vector_number v);
+#endif
+
+static void grcan_interrupt(struct grcan_priv *pDev);
+
+#ifdef GRCAN_REG_BYPASS_CACHE
+#define READ_REG(address) _grcan_read_nocache((unsigned int)(address))
+#else
+#define READ_REG(address) (*(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) (*(unsigned int *)(address))
+#define READ_DMA_BYTE(address) (*(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
+
+
+static rtems_driver_address_table grcan_driver = GRCAN_DRIVER_TABLE_ENTRY;
+
+static void __inline__ grcan_hw_reset(struct grcan_regs *regs)
+{
+	regs->ctrl = GRCAN_CTRL_RESET;
+}
+
+static rtems_device_driver grcan_start(struct grcan_priv *pDev)
+{
+  unsigned int tmp;
+  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 = MEMAREA_TO_HW((unsigned int)pDev->rx);
+  pDev->regs->rx0size = pDev->rxbuf_size;
+  
+  /* Setup Transmitter */
+  pDev->regs->tx0addr = MEMAREA_TO_HW((unsigned int)pDev->tx);
+  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 */
+  IRQ_GLOBAL_DISABLE();
+  IRQ_UNMASK(pDev->irq+GRCAN_IRQ_TXSYNC);
+  IRQ_UNMASK(pDev->irq+GRCAN_IRQ_RXSYNC);
+  IRQ_UNMASK(pDev->irq+GRCAN_IRQ_IRQ);
+  IRQ_GLOBAL_ENABLE();
+  
+  /* Reset some software data */
+  /*pDev->txerror = 0;
+  pDev->rxerror = 0;*/
+
+  /* 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_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;
+  
+  /* Reset semaphores to the initial state and wakeing
+   * all threads waiting for an IRQ. The threads that 
+   * get woken up must check for RTEMS_UNSATISFIED in
+   * order to determine that they should return to
+   * user space with error status.
+   */
+  rtems_semaphore_flush(pDev->rx_sem);
+  rtems_semaphore_flush(pDev->tx_sem);
+  rtems_semaphore_flush(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;
+}
+
+static int grcan_hw_rx_ongoing(struct grcan_regs *regs)
+{
+  return READ_REG(&regs->rx0ctrl) & GRCAN_RXCTRL_ONGOING;
+};
+
+static int grcan_hw_tx_ongoing(struct grcan_regs *regs)
+{
+  return READ_REG(&regs->tx0ctrl) & GRCAN_TXCTRL_ONGOING;
+};
+
+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 */
+  struct grcan_timing *timing /* result is placed here */
+  )
+{
+  return -1; /* not implemented yet */
+}
+
+static unsigned 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,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]);
+      /* 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--;
+    }
+    /* Increment Hardware READ pointer (mark read byte as read)
+     * ! wait for registers to be safely re-configurable 
+     */
+    regs->rx0ctrl = 0; /* DISABLE RX CHANNEL */
+    i=0;
+    while( grcan_hw_rx_ongoing(regs) && (i<1000) ){
+      i++;
+    }
+    regs->rx0rd = (unsigned int)source-addr;
+    regs->rx0ctrl = GRCAN_RXCTRL_ENABLE; /* ENABLE_RX_CHANNEL */
+    return trunk_msg_cnt;
+  }
+  return 0;
+}
+
+static unsigned int grcan_hw_write_try(
+  struct grcan_priv *pDev,
+  struct grcan_regs *regs,
+  CANMsg *buffer,
+  int count
+  )
+{
+  unsigned int rp, wp, size, txmax, addr, 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--;
+  }
+  
+  /* Update write pointer 
+   * ! wait for registers to be safely re-configurable 
+   */
+  regs->tx0ctrl = 0; /* DISABLE TX CHANNEL */
+  i=0;
+  while( (grcan_hw_tx_ongoing(regs)) && i<1000 ){
+    i++;
+    printk("ongoing tx\n");
+  }
+  regs->tx0wr = (unsigned int)dest - addr; /* Update write pointer */
+  regs->tx0ctrl = GRCAN_TXCTRL_ENABLE; /* ENABLE_TX_CHANNEL */
+  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;
+  
+  FUNCDBG();
+  
+  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;
+
+  /*** block until receive IRQ received 
+   * Set up a valid IRQ point so that an IRQ is received 
+   * when one or more messages are received
+   */
+  IRQ_GLOBAL_DISABLE();
+  
+  /* 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;
+  }
+  IRQ_GLOBAL_ENABLE();
+    
+    /* Wait for IRQ to fire only if has been triggered */
+  if ( wait ){
+    if ( rtems_semaphore_obtain(pDev->rx_sem, RTEMS_WAIT, RTEMS_NO_TIMEOUT) == RTEMS_UNSATISFIED )
+      return -1; /* Device driver has been closed or stopped, return with error status */
+  }
+  
+  return 0;
+}
+
+/* Wait until min bytes available in TX circular buffer.
+ * The IRQ RxIrq is used to pin point the location of
+ * 
+ * min must be at least WRAP_AROUND_TX_BYTES bytes less 
+ * than max buffer for this algo to work.
+ *
+ */
+static int grcan_wait_txspace(
+  struct grcan_priv *pDev,
+  int min
+  )
+{
+  int wait;
+  unsigned int irq, rp, wp, size, space_left;
+  unsigned int irq_trunk;
+  
+  DBGC(DBG_TX,"\n");
+  /*FUNCDBG();*/
+  
+  size = READ_REG(&pDev->regs->tx0size);
+  wp = READ_REG(&pDev->regs->tx0wr);  
+
+  IRQ_GLOBAL_DISABLE();  
+  
+  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;
+  }
+  IRQ_GLOBAL_ENABLE();
+  
+  /* Wait for IRQ to fire only if it has been triggered */
+  if ( wait ){
+    if ( rtems_semaphore_obtain(pDev->tx_sem, RTEMS_WAIT, RTEMS_NO_TIMEOUT) ==
+         RTEMS_UNSATISFIED ){
+      /* Device driver has flushed us, this may be due to another thread has 
+       * closed the device, this is to avoid deadlock */
+      return -1;
+    }
+  }
+  
+  /* 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;
+  unsigned int rp, wp;
+  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 */
+    IRQ_GLOBAL_DISABLE();
+    /* 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;
+    }
+    IRQ_GLOBAL_ENABLE();
+    if ( !wait )
+      break;
+    
+    /* Wait for IRQ to wake us */
+    if ( rtems_semaphore_obtain(pDev->txempty_sem, RTEMS_WAIT, RTEMS_NO_TIMEOUT) ==
+         RTEMS_UNSATISFIED ) {
+      return -1;
+    }
+  }
+  return 0;
+}
+
+static int grcan_alloc_buffers(struct grcan_priv *pDev, int rx, int tx)
+{
+  FUNCDBG();
+  
+  if ( tx ) {
+#ifdef STATIC_TX_BUF_ADDR
+    pDev->_tx = STATIC_TX_BUF_ADDR(pDev->minor);
+    if ( pDev->txbuf_size > STATIC_TX_BUF_SIZE ){
+      pDev->txbuf_size = STATIC_TX_BUF_SIZE;
+      return -1;
+    }
+    /* Assume aligned buffer */
+    pDev->tx = (struct grcan_msg *)pDev->_tx;
+#else
+    pDev->_tx = malloc(pDev->txbuf_size + BUFFER_ALIGNMENT_NEEDS);
+    if ( !pDev->_tx )
+      return -1;
+
+    /* Align TX buffer */
+    pDev->tx = (struct grcan_msg *)
+               (((unsigned int)pDev->_tx + (BUFFER_ALIGNMENT_NEEDS-1)) & 
+               ~(BUFFER_ALIGNMENT_NEEDS-1));
+#endif
+  }
+  
+  if ( rx ) {
+#ifdef STATIC_RX_BUF_ADDR
+    pDev->_rx = STATIC_RX_BUF_ADDR(pDev->minor);
+    if ( pDev->rxbuf_size > STATIC_RX_BUF_SIZE ){
+      pDev->rxbuf_size = STATIC_RX_BUF_SIZE;
+      return -1;
+    }
+    /* Assume aligned buffer */
+    pDev->rx = (struct grcan_msg *)pDev->_rx;
+#else
+    pDev->_rx = malloc(pDev->rxbuf_size + BUFFER_ALIGNMENT_NEEDS);
+    if ( !pDev->_rx )
+      return -1;
+
+    /* Align TX buffer */
+    pDev->rx = (struct grcan_msg *) 
+               (((unsigned int)pDev->_rx + (BUFFER_ALIGNMENT_NEEDS-1)) & 
+               ~(BUFFER_ALIGNMENT_NEEDS-1));
+#endif
+  }
+  return 0;
+}
+
+static void grcan_free_buffers(struct grcan_priv *pDev, int rx, int tx)
+{
+  FUNCDBG();
+  
+#ifndef STATIC_TX_BUF_ADDR
+  if ( tx && pDev->_tx ){
+    free(pDev->_tx);
+    pDev->_tx = NULL;
+    pDev->tx = NULL;
+  }
+#endif
+#ifndef STATIC_RX_BUF_ADDR
+  if ( rx && pDev->_rx ){
+    free(pDev->_rx);
+    pDev->_rx = NULL;
+    pDev->rx = NULL;
+  }
+#endif
+}
+
+#if 0
+static char *almalloc(int sz)
+{
+  char *tmp;
+  tmp = calloc(1,2*sz);
+  tmp = (char *) (((int)tmp+sz) & ~(sz -1));
+  return(tmp);
+}
+#endif
+
+static rtems_device_driver grcan_initialize(
+  rtems_device_major_number major, 
+  rtems_device_minor_number unused,
+  void *arg
+  )
+{
+  int minor;
+  struct grcan_priv *pDev;
+  amba_apb_device dev;
+  rtems_status_code status;
+  char fs_name[20];
+  unsigned int sys_freq_hz;
+
+  printk("grcan_initialize()\n\r");
+
+  FUNCDBG();
+  
+  /* find GRCAN cores */
+  if ( !grcan_cores ) {    
+    grcan_core_cnt = amba_get_number_apbslv_devices(amba_bus,VENDOR_GAISLER,GAISLER_GRHCAN);
+    DBG("GRCAN: Using AMBA Plug&Play, found %d cores\n",grcan_core_cnt);
+    if ( grcan_core_cnt < 1 )
+      return RTEMS_UNSATISFIED;
+  }
+
+#ifdef GRCAN_MAX_CORENR
+  /* limit number of cores */
+  if ( grcan_core_cnt > GRCAN_MAX_CORENR )
+    grcan_core_cnt = GRCAN_MAX_CORENR;
+#endif
+
+  /* Allocate memory for cores */
+  grcans = malloc(grcan_core_cnt * sizeof(struct grcan_priv));
+  if ( !grcans )
+    return RTEMS_NO_MEMORY;
+  memset(grcans,0,grcan_core_cnt * sizeof(struct grcan_priv));
+
+  /* make a local copy of device name */
+  strcpy(fs_name,GRCAN_DEVNAME);
+
+  /* Detect System Frequency from initialized timer */
+#ifndef SYS_FREQ_HZ
+#if defined(LEON3)
+  /* LEON3: find timer address via AMBA Plug&Play info */
+  {
+    amba_apb_device gptimer;
+    LEON3_Timer_Regs_Map *tregs;
+
+    if (amba_find_apbslv (&amba_conf, VENDOR_GAISLER, GAISLER_GPTIMER, &gptimer)
+        == 1) {
+      tregs = (LEON3_Timer_Regs_Map *) gptimer.start;
+      sys_freq_hz = (tregs->scaler_reload + 1) * 1000 * 1000;
+      DBG("GRCAN: detected %dHZ system frequency\n\r", sys_freq_hz);
+    } else {
+      sys_freq_hz = 40000000;   /* Default to 40MHz */
+      printk("GRCAN: Failed to detect system frequency\n\r");
+    }
+  }
+#elif defined(LEON2)
+  /* LEON2: use hardcoded address to get to timer */
+  {
+    LEON_Register_Map *regs = (LEON_Register_Map *) 0x80000000;
+
+    sys_freq_hz = (regs->Scaler_Reload + 1) * 1000 * 1000;
+  }
+#else
+#error CPU not supported by driver
+#endif
+#else
+  /* Use hardcoded frequency */
+  sys_freq_hz = SYS_FREQ_HZ;
+#endif
+
+  for(minor=0; minor<grcan_core_cnt; minor++){
+
+    pDev = &grcans[minor];
+    pDev->minor = minor;
+    pDev->open = 0;
+    pDev->corefreq_hz = sys_freq_hz;
+    GRCAN_DEVNAME_NO(fs_name,minor);
+    
+    /* Find core address & IRQ */
+    if ( !grcan_cores ) {
+      amba_find_next_apbslv(amba_bus,VENDOR_GAISLER,GAISLER_GRHCAN,&dev,minor);
+      pDev->irq = dev.irq;
+      pDev->regs = (struct grcan_regs *)dev.start;
+    }else{
+      pDev->irq = grcan_cores[minor].irq;
+      pDev->regs = (struct grcan_regs *)grcan_cores[minor].base_address;
+    }
+    
+    DBG("Registering GRCAN core at [0x%x] irq %d, minor %d as %s\n",pDev->regs,pDev->irq,minor,fs_name);
+    printk("Registering GRCAN core at [0x%x] irq %d, minor %d as %s\n\r",pDev->regs,pDev->irq,minor,fs_name);
+    
+    status = rtems_io_register_name(fs_name, major, 0);
+    if (status != RTEMS_SUCCESSFUL)
+      rtems_fatal_error_occurred(status);
+    
+		/* Reset Hardware before attaching IRQ handler */
+    grcan_hw_reset(pDev->regs);
+				
+    /* Register interrupt handler */
+    GRCAN_REG_INT(GRCAN_PREFIX(_interrupt_handler), pDev->irq+GRCAN_IRQ_IRQ, pDev);
+    /*
+    GRCAN_REG_INT(grcan_interrupt_handler, pDev->irq+GRCAN_IRQ_TXSYNC, pDev);
+    GRCAN_REG_INT(grcan_interrupt_handler, pDev->irq+GRCAN_IRQ_RXSYNC, pDev);
+    */
+    
+    /* RX Semaphore created with count = 0 */
+    if ( rtems_semaphore_create(rtems_build_name('G', 'C', 'R', '0'+minor),
+        0,
+        RTEMS_FIFO|RTEMS_SIMPLE_BINARY_SEMAPHORE|RTEMS_NO_INHERIT_PRIORITY|\
+        RTEMS_LOCAL|RTEMS_NO_PRIORITY_CEILING, 
+        0,
+        &pDev->rx_sem) != RTEMS_SUCCESSFUL )
+      return RTEMS_INTERNAL_ERROR;
+
+    /* TX Semaphore created with count = 0 */
+    if ( rtems_semaphore_create(rtems_build_name('G', 'C', 'T', '0'+minor),
+        0,
+        RTEMS_FIFO|RTEMS_SIMPLE_BINARY_SEMAPHORE|RTEMS_NO_INHERIT_PRIORITY|\
+        RTEMS_LOCAL|RTEMS_NO_PRIORITY_CEILING, 
+        0,
+        &pDev->tx_sem) != RTEMS_SUCCESSFUL )
+      return RTEMS_INTERNAL_ERROR;
+
+    /* TX Empty Semaphore created with count = 0 */
+    if ( rtems_semaphore_create(rtems_build_name('G', 'C', 'E', '0'+minor),
+        0,
+        RTEMS_FIFO|RTEMS_SIMPLE_BINARY_SEMAPHORE|RTEMS_NO_INHERIT_PRIORITY|\
+        RTEMS_LOCAL|RTEMS_NO_PRIORITY_CEILING, 
+        0,
+        &pDev->txempty_sem) != RTEMS_SUCCESSFUL )
+      return RTEMS_INTERNAL_ERROR;
+
+    /* Device Semaphore created with count = 1 */
+    if ( rtems_semaphore_create(rtems_build_name('G', 'C', 'A', '0'+minor),
+        1,
+        RTEMS_FIFO|RTEMS_SIMPLE_BINARY_SEMAPHORE|RTEMS_NO_INHERIT_PRIORITY|\
+        RTEMS_LOCAL|RTEMS_NO_PRIORITY_CEILING,
+        0,
+        &pDev->dev_sem) != RTEMS_SUCCESSFUL )
+      return RTEMS_INTERNAL_ERROR;
+  }
+
+  return RTEMS_SUCCESSFUL;
+}
+
+static rtems_device_driver grcan_open(rtems_device_major_number major, rtems_device_minor_number minor, void *arg) {
+  struct grcan_priv *pDev;
+  rtems_device_driver ret;
+  
+  FUNCDBG();
+
+  if ( (minor < 0) || (minor>=grcan_core_cnt) ) {
+    DBG("Wrong minor %d\n", minor);
+    return RTEMS_INVALID_NUMBER;
+  }
+  
+  pDev = &grcans[minor];
+
+  /* Wait until we get semaphore */
+  if ( rtems_semaphore_obtain(pDev->dev_sem, RTEMS_WAIT, RTEMS_NO_TIMEOUT) !=
+       RTEMS_SUCCESSFUL ){
+    return RTEMS_INTERNAL_ERROR;
+  }
+  
+  /* is device busy/taken? */
+  if  ( pDev->open ) {
+    ret=RTEMS_RESOURCE_IN_USE;
+    goto out;
+  }
+
+  /* Mark device taken */
+  pDev->open = 1;
+
+  pDev->txblock = pDev->rxblock = 1;
+  pDev->txcomplete = pDev->rxcomplete = 0;
+  pDev->started = 0;
+  pDev->config_changed = 1;
+  pDev->config.silent = 0;
+  pDev->config.abort = 0;
+  pDev->config.selection.selection = 0;
+  pDev->config.selection.enable0 = 0;
+  pDev->config.selection.enable1 = 1;
+  pDev->flushing = 0;
+  pDev->rx = pDev->_rx = NULL;
+  pDev->tx = pDev->_tx = NULL;
+  pDev->txbuf_size = TX_BUF_SIZE;
+  pDev->rxbuf_size = RX_BUF_SIZE;
+  printk("Defaulting to rxbufsize: %d, txbufsize: %d\n",RX_BUF_SIZE,TX_BUF_SIZE);
+  
+  /* Default to accept all messages */
+  pDev->afilter.mask = 0x00000000;
+  pDev->afilter.code = 0x00000000;
+    
+  /* Default to disable sync messages (only trigger when id is set to all ones) */
+  pDev->sfilter.mask = 0xffffffff;
+  pDev->sfilter.code = 0x00000000;
+    
+  /* Calculate default timing register values */
+  grcan_calc_timing(GRCAN_DEFAULT_BAUD,pDev->corefreq_hz,&pDev->config.timing);
+  
+  if ( grcan_alloc_buffers(pDev,1,1) ) {
+    ret=RTEMS_NO_MEMORY;
+    goto out;
+  }
+  
+  /* Clear statistics */
+  memset(&pDev->stats,0,sizeof(struct grcan_stats));
+  
+  ret = RTEMS_SUCCESSFUL;
+out:
+  rtems_semaphore_release(pDev->dev_sem);
+  return ret;
+}
+
+static rtems_device_driver grcan_close(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
+{
+  struct grcan_priv *pDev = &grcans[minor];
+  
+  FUNCDBG();
+  
+  if ( pDev->started )
+    grcan_stop(pDev);
+  
+  grcan_hw_reset(pDev->regs);
+  
+  grcan_free_buffers(pDev,1,1);
+  
+  /* Mark Device as closed */
+  pDev->open = 0;
+
+  return RTEMS_SUCCESSFUL;
+}
+
+static rtems_device_driver grcan_read(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
+{
+  struct grcan_priv *pDev = &grcans[minor];
+  rtems_libio_rw_args_t *rw_args;  
+  CANMsg *dest;
+  unsigned int count, left;
+  int req_cnt;
+  
+  rw_args = (rtems_libio_rw_args_t *) arg;
+  dest = (CANMsg *) rw_args->buffer;
+  req_cnt = rw_args->count / sizeof(CANMsg);
+
+  FUNCDBG();
+  
+  if ( (!dest) || (req_cnt<1) )
+    return RTEMS_INVALID_NAME;
+  
+  if ( !pDev->started )
+    return RTEMS_RESOURCE_IN_USE;
+
+/*  FUNCDBG("grcan_read [%i,%i]: buf: 0x%x len: %i\n",major, minor, (unsigned int)rw_args->buffer,rw_args->count);*/
+  
+  count = grcan_hw_read_try(pDev,pDev->regs,dest,req_cnt);
+  if ( !( pDev->rxblock && pDev->rxcomplete && (count!=req_cnt) ) ){
+    if ( count > 0 ) {
+      /* Successfully received messages (at least one) */
+      rw_args->bytes_moved = count * sizeof(CANMsg);
+      return RTEMS_SUCCESSFUL;
+    }
+  
+    /* nothing read, shall we block? */
+    if ( !pDev->rxblock ) {
+      /* non-blocking mode */
+      rw_args->bytes_moved = 0;
+      return RTEMS_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;
+      }
+    }
+
+    if ( grcan_wait_rxdata(pDev,left) ) {
+      /* The wait has been aborted, probably due to 
+       * the device driver has been closed by another
+       * thread.
+       */
+      rw_args->bytes_moved = count * sizeof(CANMsg);
+      return RTEMS_UNSATISFIED;
+    }
+    
+    /* Try read bytes from circular buffer */
+    count += grcan_hw_read_try(
+      pDev,
+      pDev->regs,
+      dest+count,
+      req_cnt-count);
+  }
+  /* no need to unmask IRQ as IRQ Handler do that for us. */
+  rw_args->bytes_moved = count * sizeof(CANMsg);
+  return RTEMS_SUCCESSFUL;
+}
+ 
+static rtems_device_driver grcan_write(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
+{
+  struct grcan_priv *pDev = &grcans[minor];
+  rtems_libio_rw_args_t *rw_args;
+  CANMsg *source;
+  unsigned int count, left;
+  int req_cnt;
+  
+  DBGC(DBG_TX,"\n");
+  /*FUNCDBG();*/
+  
+  if ( !pDev->started || pDev->config.silent || pDev->flushing )
+    return RTEMS_RESOURCE_IN_USE;
+  
+  rw_args = (rtems_libio_rw_args_t *) arg;
+  req_cnt = rw_args->count / sizeof(CANMsg);
+  source = (CANMsg *) rw_args->buffer;
+  
+  /* check proper length and buffer pointer */
+  if (( req_cnt < 1) || (source == NULL) ){
+    return RTEMS_INVALID_NAME;
+  }
+   
+  count = grcan_hw_write_try(pDev,pDev->regs,source,req_cnt);
+  if ( !(pDev->txblock && pDev->txcomplete && (count!=req_cnt)) ) {
+    if ( count > 0 ) {
+      /* Successfully transmitted chars (at least one char) */
+      rw_args->bytes_moved = count * sizeof(CANMsg);
+      return RTEMS_SUCCESSFUL;
+    }
+  
+    /* nothing written, shall we block? */
+    if ( !pDev->txblock ) {
+      /* non-blocking mode */
+      rw_args->bytes_moved = 0;
+      return RTEMS_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 transmitt buffer 
+       * Why? We need some time to fill buffer before hw catches up.
+       */
+      if ( left > ((pDev->txbuf_size/GRCAN_MSG_SIZE)/2) ){
+        left = (pDev->txbuf_size/GRCAN_MSG_SIZE)/2;
+      }
+    }
+    
+    /* Wait until more room in transmit buffer */
+    if ( grcan_wait_txspace(pDev,left) ){
+      /* The wait has been aborted, probably due to 
+       * the device driver has been closed by another
+       * thread. To avoid deadlock we return directly
+       * with error status.
+       */
+      rw_args->bytes_moved = count * sizeof(CANMsg);
+      return RTEMS_UNSATISFIED;
+    }
+    
+    if ( pDev->txerror ){
+      /* Return number of bytes sent, compare write pointers */
+      pDev->txerror = 0;
+#if 0 
+#error HANDLE AMBA error
+#endif
+    }
+    
+    /* Try read bytes from circular buffer */
+    count += grcan_hw_write_try(
+      pDev,
+      pDev->regs,
+      source+count,
+      req_cnt-count);
+  }
+  /* no need to unmask IRQ as IRQ Handler do that for us. */
+  
+  rw_args->bytes_moved = count * sizeof(CANMsg);
+  return RTEMS_SUCCESSFUL;
+}
+
+static rtems_device_driver grcan_ioctl(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
+{
+  struct grcan_priv *pDev = &grcans[minor];
+  rtems_libio_ioctl_args_t *ioarg = (rtems_libio_ioctl_args_t *)arg;
+  unsigned int *data = ioarg->buffer;
+  struct grcan_timing timing;
+  unsigned int speed;
+  struct grcan_selection *selection;
+  int tmp,ret;
+  rtems_device_driver status;
+  struct grcan_stats *stats;
+  struct grcan_filter *filter;
+  
+  FUNCDBG();
+  
+  if (!ioarg)
+    return RTEMS_INVALID_NAME;
+
+  ioarg->ioctl_return = 0;
+  switch(ioarg->command) {
+    case GRCAN_IOC_START:
+      if ( pDev->started )
+        return RTEMS_RESOURCE_IN_USE; /* EBUSY */
+      
+      if ( (status=grcan_start(pDev)) != RTEMS_SUCCESSFUL ){
+        return status;
+      }
+      /* Read and write are now open... */
+      pDev->started = 1;
+      break;
+    
+    case GRCAN_IOC_STOP:
+      if ( !pDev->started )
+        return RTEMS_RESOURCE_IN_USE;
+      
+      grcan_stop(pDev);
+      pDev->started = 0;
+      break;
+    
+    case GRCAN_IOC_ISSTARTED:
+      if ( !pDev->started )
+        return RTEMS_RESOURCE_IN_USE;
+      break;
+    
+    case GRCAN_IOC_FLUSH:
+      if ( !pDev->started || pDev->flushing || pDev->config.silent )
+        return RTEMS_RESOURCE_IN_USE;
+      
+      pDev->flushing = 1;
+      tmp = grcan_tx_flush(pDev);
+      pDev->flushing = 0;
+      if ( tmp ) {
+        /* The wait has been aborted, probably due to 
+         * the device driver has been closed by another
+         * thread.
+         */
+         return RTEMS_UNSATISFIED;
+      }
+      break;
+    
+#if 0    
+    /* Set physical link */
+		case GRCAN_IOC_SET_LINK:
+#ifdef REDUNDANT_CHANNELS
+			if ( pDev->started )
+				return RTEMS_RESOURCE_IN_USE; /* EBUSY */
+			
+			/* switch HW channel */
+			pDev->channel = (unsigned int)ioargs->buffer;
+#else
+			return RTEMS_NOT_IMPLEMENTED;
+#endif
+			break;
+#endif
+
+    case GRCAN_IOC_SET_SILENT:
+      if ( pDev->started )
+        return RTEMS_RESOURCE_IN_USE;
+      pDev->config.silent = (int)ioarg->buffer;
+      pDev->config_changed = 1;
+      break;    
+
+    case GRCAN_IOC_SET_ABORT:
+      if ( pDev->started )
+        return RTEMS_RESOURCE_IN_USE;
+      pDev->config.abort = (int)ioarg->buffer;
+      /* This Configuration parameter doesn't need HurriCANe reset 
+       * ==> no pDev->config_changed = 1;
+       */
+      break;
+    
+    case GRCAN_IOC_SET_SELECTION:
+      if ( pDev->started )
+        return RTEMS_RESOURCE_IN_USE;
+
+      selection = (struct grcan_selection *)ioarg->buffer;
+      if ( !selection )
+        return RTEMS_INVALID_NAME;
+      
+      pDev->config.selection = *selection;
+      pDev->config_changed = 1;
+      break;
+       
+    case GRCAN_IOC_SET_RXBLOCK:
+      pDev->rxblock = (int)ioarg->buffer;
+      break;
+      
+    case GRCAN_IOC_SET_TXBLOCK:
+      pDev->txblock = (int)ioarg->buffer;
+      break;
+    
+    case GRCAN_IOC_SET_TXCOMPLETE:
+      pDev->txcomplete = (int)ioarg->buffer;
+      break;
+
+    case GRCAN_IOC_SET_RXCOMPLETE:
+      pDev->rxcomplete = (int)ioarg->buffer;
+      break;
+          
+    case GRCAN_IOC_GET_STATS:
+      stats = (struct grcan_stats *)ioarg->buffer;
+      if ( !stats )
+        return RTEMS_INVALID_NAME;
+      *stats = pDev->stats;
+      break;
+    
+    case GRCAN_IOC_CLR_STATS:
+      IRQ_GLOBAL_DISABLE();
+      memset(&pDev->stats,0,sizeof(struct grcan_stats));
+      IRQ_GLOBAL_ENABLE();
+      break;
+
+		case GRCAN_IOC_SET_SPEED:
+		
+			/* cannot change speed during run mode */
+			if ( pDev->started )
+				return RTEMS_RESOURCE_IN_USE; /* EBUSY */
+			
+			/* get speed rate from argument */
+			speed = (unsigned int)ioarg->buffer;
+			ret = grcan_calc_timing(pDev->corefreq_hz,speed,&timing);
+			if ( ret )
+				return  RTEMS_INVALID_NAME; /* EINVAL */
+			
+			/* save timing/speed */
+			pDev->config.timing = timing;
+      pDev->config_changed = 1;
+			break;
+    
+    case GRCAN_IOC_SET_BTRS:
+			/* Set BTR registers manually 
+			 * Read GRCAN/HurriCANe Manual.
+			 */
+			if ( pDev->started )
+				return RTEMS_RESOURCE_IN_USE; /* EBUSY */
+        
+			if ( !ioarg->buffer )
+        return RTEMS_INVALID_NAME;
+      
+			pDev->config.timing = *(struct grcan_timing *)ioarg->buffer;
+      pDev->config_changed = 1;
+			break;
+    
+    case GRCAN_IOC_SET_AFILTER:
+      filter = (struct grcan_filter *)ioarg->buffer;
+      if ( !filter ){
+        /* Disable filtering - let all messages pass */
+        pDev->afilter.mask = 0x0;
+        pDev->afilter.code = 0x0;
+      }else{
+        /* Save filter */
+        pDev->afilter = *filter;
+      }
+      /* Set hardware acceptance filter */      
+      grcan_hw_accept(pDev->regs,&pDev->afilter);
+      break;
+    
+    case GRCAN_IOC_SET_SFILTER:
+      filter = (struct grcan_filter *)ioarg->buffer;
+      if ( !filter ){
+        /* disable TX/RX SYNC filtering */
+        pDev->sfilter.mask = 0xffffffff; 
+        pDev->sfilter.mask = 0;
+        
+        /* disable Sync interrupt */
+        pDev->regs->imr = READ_REG(&pDev->regs->imr) & ~(GRCAN_RXSYNC_IRQ|GRCAN_TXSYNC_IRQ);
+      }else{
+        /* Save filter */
+        pDev->sfilter = *filter;
+        
+        /* Enable Sync interrupt */
+        pDev->regs->imr = READ_REG(&pDev->regs->imr) | (GRCAN_RXSYNC_IRQ|GRCAN_TXSYNC_IRQ);
+      }
+      /* Set Sync RX/TX filter */
+      grcan_hw_sync(pDev->regs,&pDev->sfilter);
+      break;
+
+    case GRCAN_IOC_GET_STATUS:
+      if ( !data )
+        return RTEMS_INVALID_NAME;
+      /* Read out the statsu register from the GRCAN core */
+      data[0] = READ_REG(&pDev->regs->stat);
+      break;
+  
+    default:
+      return RTEMS_NOT_DEFINED;
+  }
+  return RTEMS_SUCCESSFUL;
+}
+
+#ifndef GRCAN_DONT_DECLARE_IRQ_HANDLER
+/* Find what device caused the IRQ */
+static rtems_isr grcan_interrupt_handler(rtems_vector_number v)
+{
+  int minor=0;
+  while ( minor < grcan_core_cnt ){
+    if ( grcans[minor].irq == (v+0x10) ){
+      grcan_interrupt(&grcans[minor]);
+      break;
+    }
+  }
+}
+#endif
+
+/* Handle the IRQ */
+static void grcan_interrupt(struct grcan_priv *pDev)
+{
+  unsigned int status = READ_REG(&pDev->regs->pimsr);
+  unsigned int canstat = READ_REG(&pDev->regs->stat);
+  
+  /* Spurious IRQ call? */
+  if ( !status && !canstat )
+    return;
+  
+  FUNCDBG();
+  
+  /* Increment number of interrupts counter */
+  pDev->stats.ints++;
+  
+  if ( (status & GRCAN_ERR_IRQ) || (canstat & GRCAN_STAT_PASS) ){
+    /* Error-Passive interrupt */
+    pDev->stats.passive_cnt++;
+  }
+
+  if ( (status & GRCAN_OFF_IRQ) || (canstat & GRCAN_STAT_OFF) ){
+    /* Bus-off condition interrupt 
+     * The link is brought down by hardware, we wake all threads
+     * that is blocked in read/write calls and stop futher calls
+     * to read/write until user has called ioctl(fd,START,0).
+     */
+     pDev->started = 0;
+     grcan_stop(pDev); /* this mask all IRQ sources */
+     status=0x1ffff; /* clear all interrupts */
+     goto out;
+  }
+  
+  if ( (status & GRCAN_OR_IRQ) || (canstat & GRCAN_STAT_OR) ){
+    /* Over-run during reception interrupt */
+    pDev->stats.overrun_cnt++;
+  }
+  
+  if ( (status & GRCAN_RXAHBERR_IRQ) ||
+       (status & GRCAN_TXAHBERR_IRQ) ||
+       (canstat & GRCAN_STAT_AHBERR) ){
+    /* RX or Tx AHB Error interrupt */
+    printk("AHBERROR: status: 0x%x, canstat: 0x%x\n",status,canstat);
+    pDev->stats.ahberr_cnt++;
+  }
+  
+  if ( status & GRCAN_TXLOSS_IRQ ) {
+    pDev->stats.txloss_cnt++;
+  }
+  
+  if ( status & GRCAN_RXIRQ_IRQ ){
+    /* RX IRQ pointer interrupt */
+    /*printk("RxIrq 0x%x\n",status);*/
+    pDev->regs->imr = READ_REG(&pDev->regs->imr) & ~GRCAN_RXIRQ_IRQ;
+    rtems_semaphore_release(pDev->rx_sem);
+  }
+  
+  if ( status & GRCAN_TXIRQ_IRQ ){
+    /* TX IRQ pointer interrupt */
+    pDev->regs->imr = READ_REG(&pDev->regs->imr) & ~GRCAN_TXIRQ_IRQ;
+    rtems_semaphore_release(pDev->tx_sem);
+  }
+  
+  if ( status & GRCAN_TXSYNC_IRQ ){
+    /* TxSync message transmitted interrupt */
+    pDev->stats.txsync_cnt++;
+  }
+  
+  if ( status & GRCAN_RXSYNC_IRQ ){
+    /* RxSync message received interrupt */
+    pDev->stats.rxsync_cnt++;
+  }
+  
+  if ( status & GRCAN_TXEMPTY_IRQ ){
+    pDev->regs->imr = READ_REG(&pDev->regs->imr) & ~GRCAN_TXEMPTY_IRQ;
+    rtems_semaphore_release(pDev->txempty_sem);
+  }
+  
+out:  
+  /* Clear IRQs */
+  pDev->regs->picr = status;
+}
+
+static int grcan_register_internal(void)
+{
+  rtems_status_code r;
+  rtems_device_major_number m;
+  
+  if ((r = rtems_io_register_driver(0, &grcan_driver, &m)) != 
+       RTEMS_SUCCESSFUL) {
+    switch(r) {
+      case RTEMS_TOO_MANY:
+        DBG2("failed RTEMS_TOO_MANY\n"); 
+        break;
+      case RTEMS_INVALID_NUMBER:
+        DBG2("failed RTEMS_INVALID_NUMBER\n"); 
+        break;
+      case RTEMS_RESOURCE_IN_USE:
+        DBG2("failed RTEMS_RESOURCE_IN_USE\n"); 
+        break;
+      default:
+        DBG("failed %i\n",r); 
+        break;
+    }
+    return 1;
+  }
+  DBG("Registered GRCAN on major %d\n",m);
+  return 0;
+}
+
+
+/* Use custom addresses and IRQs to find hardware */
+int GRCAN_PREFIX(_register_abs)(struct grcan_device_info *devices, int dev_cnt)
+{
+  FUNCDBG();
+  
+  if ( !devices || (dev_cnt<0) )
+    return 1;
+  grcan_cores = devices;
+  grcan_core_cnt = dev_cnt;
+
+  amba_bus = NULL;
+	return grcan_register_internal();
+}
+
+/* Use prescanned AMBA Plug&Play information to find all GRCAN cores */
+int GRCAN_PREFIX(_register)(amba_confarea_type *abus)
+{
+  FUNCDBG();
+  
+  if ( !abus )
+    return 1;
+  amba_bus = abus;
+  grcan_cores = NULL;
+  grcan_core_cnt = 0;
+  return grcan_register_internal();
+}
diff --git a/c/src/lib/libbsp/sparc/shared/can/grcan_rasta.c b/c/src/lib/libbsp/sparc/shared/can/grcan_rasta.c
new file mode 100644
index 0000000000..5f6701fa4c
--- /dev/null
+++ b/c/src/lib/libbsp/sparc/shared/can/grcan_rasta.c
@@ -0,0 +1,99 @@
+
+#include <rasta.h>
+
+/* PCI frequency */
+#define SYS_FREQ_HZ 33000000
+
+/*#define USE_AT697_RAM              1      */
+
+/* memarea_to_hw(x) 
+ *
+ * x: address in AT697 address space
+ * 
+ * returns the address in the RASTA address space that can be used to access x with dma.
+ * 
+*/
+#ifdef USE_AT697_RAM
+static inline unsigned int memarea_to_hw(unsigned int addr) {
+    return ((addr & 0x0fffffff) | RASTA_PCI_BASE);   
+}
+#else
+static inline unsigned int memarea_to_hw(unsigned int addr) {
+    return ((addr & 0x0fffffff) | RASTA_LOCAL_SRAM);        
+}
+#endif
+
+#define MEMAREA_TO_HW(x) memarea_to_hw(x)
+
+#define IRQ_CLEAR_PENDING(irqno)
+#define IRQ_UNMASK(irqno) 
+#define IRQ_MASK(irqno) 
+
+#define IRQ_GLOBAL_DISABLE() sparc_disable_interrupts()
+#define IRQ_GLOBAL_ENABLE() sparc_enable_interrupts()
+
+#define GRCAN_REG_INT(handler,irqno,arg) \
+  if ( grcan_rasta_int_reg ) \
+    grcan_rasta_int_reg(handler,irqno,arg);
+
+void (*grcan_rasta_int_reg)(void *handler, int irq, void *arg) = 0;
+
+#define GRCAN_PREFIX(name) grcan_rasta##name
+
+/* We provide our own handler */
+#define GRCAN_DONT_DECLARE_IRQ_HANDLER
+
+#define GRCAN_REG_BYPASS_CACHE
+#define GRCAN_DMA_BYPASS_CACHE
+
+#define GRCAN_MAX_CORES 1
+
+/* Custom Statically allocated memory */
+#undef STATICALLY_ALLOCATED_TX_BUFFER
+#undef STATICALLY_ALLOCATED_RX_BUFFER
+
+#define STATIC_TX_BUF_SIZE 4096
+#define STATIC_RX_BUF_SIZE 4096
+#define TX_BUF_SIZE 4096
+#define RX_BUF_SIZE 4096
+
+#define STATIC_TX_BUF_ADDR(core) \
+	(grcan_rasta_rambase+(core)*(STATIC_TX_BUF_SIZE+STATIC_RX_BUF_SIZE))
+
+#define STATIC_RX_BUF_ADDR(core) \
+	(grcan_rasta_rambase+(core)*(STATIC_TX_BUF_SIZE+STATIC_RX_BUF_SIZE)+STATIC_RX_BUF_SIZE)
+  
+  
+#define GRCAN_DEVNAME "/dev/grcan0"
+#define GRCAN_DEVNAME_NO(devstr,no) ((devstr)[10]='0'+(no))
+
+static int grcan_rasta_calc_memoffs(int maxcores, int corenum, unsigned int *mem_base, unsigned int *mem_end, unsigned int *bdtable_base);
+
+void grcan_rasta_interrupt_handler(int irq, void *pDev);
+
+unsigned int grcan_rasta_rambase;
+
+#include "grcan.c"
+
+
+int grcan_rasta_ram_register(amba_confarea_type *abus, int rambase)
+{
+  grcan_rasta_rambase = rambase;
+
+  return GRCAN_PREFIX(_register)(abus);
+}
+#if 0
+static void grcan_rasta_interrupt_handler(int v)
+{
+  /* We know there is always only one GRCAN core in a RASTA chip... */
+  grcan_interrupt(&grcans[0]);
+  /*
+	struct grcan_priv *pDev = arg;
+	grcan_interrupt(pDev);
+  */
+}
+#endif
+void GRCAN_PREFIX(_interrupt_handler)(int irq, void *pDev)
+{
+  grcan_interrupt(pDev);
+}
diff --git a/c/src/lib/libbsp/sparc/shared/can/occan.c b/c/src/lib/libbsp/sparc/shared/can/occan.c
new file mode 100644
index 0000000000..3227957d9d
--- /dev/null
+++ b/c/src/lib/libbsp/sparc/shared/can/occan.c
@@ -0,0 +1,1922 @@
+/*  OC_CAN driver
+ *
+ *  COPYRIGHT (c) 2007.
+ *  Gaisler Research.
+ *
+ *  The license and distribution terms for this file may be
+ *  found in the file LICENSE in this distribution or at
+ *  http://www.rtems.com/license/LICENSE.
+ *
+ *  Author: Daniel Hellström, Gaisler Research AB, www.gaisler.com
+ */
+
+#include <rtems/libio.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+#include <bsp.h>
+#include <rtems/bspIo.h> /* printk */
+
+#include <leon.h>
+#include <ambapp.h> 
+#include <occan.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
+
+#ifndef OCCAN_PREFIX
+ #define OCCAN_PREFIX(name) occan##name
+#endif
+
+#if !defined(OCCAN_DEVNAME) || !defined(OCCAN_DEVNAME_NO)
+ #undef OCCAN_DEVNAME
+ #undef OCCAN_DEVNAME_NO
+ #define OCCAN_DEVNAME "/dev/occan0"
+ #define OCCAN_DEVNAME_NO(devstr,no) ((devstr)[10]='0'+(no))
+#endif
+
+#ifndef OCCAN_REG_INT
+	#define OCCAN_REG_INT(handler,irq,arg) set_vector(handler,irq+0x10,1)
+#endif
+
+/* Default to 40MHz system clock */
+/*#ifndef SYS_FREQ_HZ
+ #define SYS_FREQ_HZ 40000000
+#endif*/
+
+#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;
+	char fifoarea[0];
+} occan_fifo;
+
+/* PELICAN */
+#ifdef OCCAN_BYTE_REGS
+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;
+} pelican_regs;
+#else
+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];
+} pelican_regs;
+#endif
+
+#define MAX_TSEG1 7
+#define MAX_TSEG2 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 {
+	/* hardware shortcuts */	
+	pelican_regs *regs;
+	int irq;
+	occan_speed_regs timing;
+	int channel; /* 0=default, 1=second bus */
+	int single_mode;
+	
+	/* driver state */
+	rtems_id devsem;
+	rtems_id txsem;
+	rtems_id rxsem;
+	int open;
+	int started;
+	int rxblk;
+	int txblk;
+	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);
+
+/**** 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 int pelican_speed_auto(occan_priv *priv);
+static void pelican_init(occan_priv *priv);
+static void pelican_open(occan_priv *priv);
+static void pelican_close(occan_priv *priv);
+static int pelican_start(occan_priv *priv);
+static void pelican_stop(occan_priv *priv);
+static void pelican_exit(occan_priv *priv);
+static int pelican_send(occan_priv *can, CANMsg *msg);
+static void pelican_set_accept(occan_priv *priv, unsigned char *acode, unsigned char *amask);
+static void occan_interrupt(occan_priv *can);
+static void pelican_regadr_print(pelican_regs *regs);
+
+/***** 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);
+static void occan_interrupt_handler(rtems_vector_number v);
+
+static int can_cores;
+static occan_priv *cans;
+static amba_confarea_type *amba_bus;
+static unsigned int sys_freq_hz;
+
+
+/* Read byte bypassing */
+
+#ifdef OCCAN_DONT_BYPASS_CACHE
+ #define READ_REG(address) (*(unsigned char *)(address))
+#else
+ /* Bypass cache */
+ #define READ_REG(address) _OCCAN_REG_READ((unsigned int)(address))
+ static __inline__ unsigned char _OCCAN_REG_READ(unsigned int addr) {
+        unsigned char tmp;
+        asm(" lduba [%1]1, %0 "
+            : "=r"(tmp)
+            : "r"(addr)
+           );
+        return tmp;
+	}
+#endif
+
+#define WRITE_REG(address,data) (*(unsigned char *)(address) = (data))
+
+/* 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 void pelican_init(occan_priv *priv){
+	/* Reset core */
+	priv->regs->mode = PELICAN_MOD_RESET;
+	
+	/* wait for core to reset complete */
+	/*usleep(1);*/
+}
+
+static void pelican_open(occan_priv *priv){
+	unsigned char tmp;
+	int ret;
+
+	/* Set defaults */
+	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
+	 */
+	priv->regs->clkdiv = (1<<PELICAN_CDR_MODE_BITS) | (DEFAULT_CLKDIV & PELICAN_CDR_DIV);
+	
+	ret = occan_calc_speedregs(sys_freq_hz,priv->speed,&priv->timing);
+	if ( ret ){
+		/* failed to set speed for this system freq, try with 50K instead */
+		priv->speed = OCCAN_SPEED_50K;
+		occan_calc_speedregs(sys_freq_hz,priv->speed,&priv->timing);
+	}
+	
+	/* disable all interrupts */
+	priv->regs->inten = 0;
+	
+	/* clear pending interrupts by reading */
+	tmp = READ_REG(&priv->regs->intflags);
+}
+
+static void pelican_close(occan_priv *priv){
+
+	/* disable all interrupts */
+	priv->regs->inten = 0;
+	
+	priv->regs->mode = PELICAN_MOD_RESET;
+}
+
+static int pelican_start(occan_priv *priv){
+	unsigned char tmp;
+	/* Start HW communication */
+	
+	if ( !priv->rxfifo || !priv->txfifo )
+		return -1;
+
+	/* Clear status bits */
+	priv->status = 0;
+	
+	/* clear pending interrupts */
+	tmp = READ_REG(&priv->regs->intflags);
+	
+	/* clear error counters */
+	priv->regs->rx_err_cnt = 0;
+	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->regs->bustim0),READ_REG(&priv->regs->bustim1));
+	
+	/* Set default acceptance filter */
+	pelican_set_accept(priv,priv->acode,priv->amask);
+
+	/* turn on interrupts */
+	priv->regs->inten = PELICAN_IE_RX | PELICAN_IE_TX | PELICAN_IE_ERRW |
+	                    PELICAN_IE_ERRP | PELICAN_IE_BUS;
+	
+#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.
+	 */
+	priv->regs->mode =  (priv->single_mode << 3);
+	
+	return 0;
+}
+
+static void pelican_stop(occan_priv *priv){
+	/* stop HW */
+
+#ifdef DEBUG
+	/* print setup before stopping */
+	pelican_regs_print(priv->regs);
+	occan_stat_print(&priv->stats);
+#endif
+	
+	/* put core in reset mode */
+	priv->regs->mode = PELICAN_MOD_RESET;
+
+	/* turn off interrupts */
+	priv->regs->inten = 0;
+	
+	priv->status |= OCCAN_STATUS_RESET;
+}
+
+
+static void pelican_exit(occan_priv *priv){
+	/* reset core */
+	priv->regs->mode = PELICAN_MOD_RESET;
+}
+
+/* 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,status;
+	pelican_regs *regs = can->regs;
+	
+	/* is there room in send buffer? */
+	status = READ_REG(&regs->status);
+	if ( !(status & PELICAN_STAT_TXBUF) ){
+		/* tx fifo taken, we have to wait */
+		return -1;
+	}
+		
+	tmp = msg->len & 0xf;
+	if ( msg->rtr )
+		tmp |= 0x40;
+	
+	if ( msg->extended ){
+		/* Extended Frame */
+		regs->rx_fi_xff = 0x80 | tmp;
+		WRITE_REG(&regs->msg.tx_eff.id[0],(msg->id >> (5+8+8)) & 0xff);
+		WRITE_REG(&regs->msg.tx_eff.id[1],(msg->id >> (5+8)) & 0xff);
+		WRITE_REG(&regs->msg.tx_eff.id[2],(msg->id >> (5)) & 0xff);
+		WRITE_REG(&regs->msg.tx_eff.id[3],(msg->id << 3) & 0xf8);
+		tmp = msg->len;
+		while(tmp--){
+			WRITE_REG(&regs->msg.tx_eff.data[tmp],msg->data[tmp]);
+		}
+	}else{
+		/* Standard Frame */
+		regs->rx_fi_xff = tmp;
+		WRITE_REG(&regs->msg.tx_sff.id[0],(msg->id >> 3) & 0xff);
+		WRITE_REG(&regs->msg.tx_sff.id[1],(msg->id << 5) & 0xe0);
+		tmp = msg->len;
+		while(tmp--){
+			WRITE_REG(&regs->msg.tx_sff.data[tmp],msg->data[tmp]);
+		}
+	}
+	
+	/* let HW know of new message */
+	if ( msg->sshot ){
+		regs->cmd = PELICAN_CMD_TXREQ | PELICAN_CMD_ABORT;
+	}else{
+		/* normal case -- try resend until sent */
+		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 */
+	*acode0 = acode[0];
+	*acode1 = acode[1];
+	*acode2 = acode[2];
+	*acode3 = acode[3];
+
+	*amask0 = amask[0];
+	*amask1 = amask[1];
+	*amask2 = amask[2];
+	*amask3 = amask[3];
+}
+
+static void pelican_regs_print(pelican_regs *regs){
+	printk("--- PELICAN 0x%lx ---\n\r",(unsigned int)regs);
+	printk(" MODE: 0x%02x\n\r",READ_REG(&regs->mode));
+	printk(" CMD: 0x%02x\n\r",READ_REG(&regs->cmd));
+	printk(" STATUS: 0x%02x\n\r",READ_REG(&regs->status));
+	/*printk(" INTFLG: 0x%02x\n\r",READ_REG(&regs->intflags));*/
+	printk(" INTEN: 0x%02x\n\r",READ_REG(&regs->inten));
+	printk(" BTR0: 0x%02x\n\r",READ_REG(&regs->bustim0));
+	printk(" BTR1: 0x%02x\n\r",READ_REG(&regs->bustim1));
+	printk(" ARBCODE: 0x%02x\n\r",READ_REG(&regs->arbcode));
+	printk(" ERRCODE: 0x%02x\n\r",READ_REG(&regs->errcode));
+	printk(" ERRWARN: 0x%02x\n\r",READ_REG(&regs->errwarn));
+	printk(" RX_ERR_CNT: 0x%02x\n\r",READ_REG(&regs->rx_err_cnt));
+	printk(" TX_ERR_CNT: 0x%02x\n\r",READ_REG(&regs->tx_err_cnt));
+	if ( READ_REG(&regs->mode) & PELICAN_MOD_RESET ){
+		/* in reset mode it is possible to read acceptance filters */
+		printk(" ACR0: 0x%02x (0x%lx)\n\r",READ_REG(&regs->rx_fi_xff),&regs->rx_fi_xff);
+		printk(" ACR1: 0x%02x (0x%lx)\n\r",READ_REG(&regs->msg.rst_accept.code[0]),(unsigned int)&regs->msg.rst_accept.code[0]);
+		printk(" ACR1: 0x%02x (0x%lx)\n\r",READ_REG(&regs->msg.rst_accept.code[1]),(unsigned int)&regs->msg.rst_accept.code[1]);
+		printk(" ACR1: 0x%02x (0x%lx)\n\r",READ_REG(&regs->msg.rst_accept.code[2]),(unsigned int)&regs->msg.rst_accept.code[2]);
+		printk(" AMR0: 0x%02x (0x%lx)\n\r",READ_REG(&regs->msg.rst_accept.mask[0]),(unsigned int)&regs->msg.rst_accept.mask[0]);
+		printk(" AMR1: 0x%02x (0x%lx)\n\r",READ_REG(&regs->msg.rst_accept.mask[1]),(unsigned int)&regs->msg.rst_accept.mask[1]);
+		printk(" AMR2: 0x%02x (0x%lx)\n\r",READ_REG(&regs->msg.rst_accept.mask[2]),(unsigned int)&regs->msg.rst_accept.mask[2]);
+		printk(" AMR3: 0x%02x (0x%lx)\n\r",READ_REG(&regs->msg.rst_accept.mask[3]),(unsigned int)&regs->msg.rst_accept.mask[3]);
+		
+	}else{
+		printk(" RXFI_XFF: 0x%02x\n\r",READ_REG(&regs->rx_fi_xff));
+	}
+	printk(" RX_MSG_CNT: 0x%02x\n\r",READ_REG(&regs->rx_msg_cnt));
+	printk(" CLKDIV: 0x%02x\n\r",READ_REG(&regs->clkdiv));
+	printk("-------------------\n\r");
+}
+
+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");
+}
+
+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");
+}
+
+/* This function calculates BTR0 BTR1 values for a given bitrate.
+ * Heavily based on mgt_mscan_bitrate() from peak driver, which 
+ * in turn is based on work by Arnaud Westenberg.
+ *
+ * Set communication parameters.
+ * baud rate in Hz
+ * input clock frequency of can core in Hz (system frequency)
+ * sjw synchronization jump width (0-3) prescaled clock cycles
+ * sampl_pt sample point in % (0-100) sets (TSEG1+2)/(TSEG1+TSEG2+3)
+ *                                                               ratio
+ */
+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, best_rate=0, brp=0;
+	int tseg=0, tseg1=0, tseg2=0;
+	int sjw = 0;
+	int clock = clock_hz / 2;
+	int sampl_pt = 90;
+
+	if ( (rate<10000) || (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;
+			best_rate = clock/(brp*(1+tseg/2));
+		}
+	}
+
+	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->sjw = sjw;
+	result->brp = best_brp;
+	result->tseg1 = tseg1;
+	result->tseg2 = tseg2;
+*/
+	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;
+	
+	priv->regs->bustim0 = timing->btr0;
+	priv->regs->bustim1 = timing->btr1;
+	/*
+	priv->regs->bustim0 = (timing->sjw<<OCCAN_BUSTIM_SJW_BIT) | (timing->brp&OCCAN_BUSTIM_BRP);
+	priv->regs->bustim1 = (timing->sam<<7) | (timing->tseg2<<OCCAN_BUSTIM_TSEG2_BIT) | timing->tseg1;
+	*/
+	return 0;
+}
+
+#if 0
+static unsigned int pelican_speed_auto_steplist [] = {
+	OCCAN_SPEED_500K,
+	OCCAN_SPEED_250K,
+	OCCAN_SPEED_125K,
+	OCCAN_SPEED_75K,
+	OCCAN_SPEED_50K,
+	OCCAN_SPEED_25K,
+	OCCAN_SPEED_10K,
+	0
+};
+#endif
+
+static int pelican_speed_auto(occan_priv *priv){
+	return -1;
+	
+#if 0	
+	int i=0;
+	occan_speed_regs timing;
+	unsigned int speed;
+	unsigned char tmp;
+
+	while ( (speed=pelican_speed_auto_steplist[i]) > 0){
+		
+		/* Reset core */
+		priv->regs->mode = PELICAN_MOD_RESET;
+		
+		/* tell int handler about the auto speed detection test */
+		
+		
+		/* wait for a moment (10ms) */
+		/*usleep(10000);*/
+				
+		/* No acceptance filter */
+		pelican_set_accept(priv);
+		
+		/* calc timing params for this */
+		if ( occan_calc_speedregs(sys_freq_hz,speed,&timing) ){
+			/* failed to get good timings for this frequency
+			 * test with next
+			 */
+			 continue;
+		}
+		
+		timing.sam = 0;
+		
+		/* set timing params for this speed */
+		occan_set_speedregs(priv,&timing);
+			
+		/* Empty previous messages in hardware RX fifo */
+		/*
+		while( READ_REG(&priv->regs->) ){
+			
+		}
+		*/
+				
+		/* Clear pending interrupts */
+		tmp = READ_REG(&priv->regs->intflags);
+		
+		/* enable RX & ERR interrupt */
+		priv->regs->inten = 
+		
+		/* Get out of reset state */
+		priv->regs->mode = PELICAN_MOD_LISTEN;
+		
+		/* wait for frames or errors */
+		while(1){
+			/* sleep 10ms */
+			
+		}
+		
+	}
+#endif
+}
+
+
+static rtems_device_driver occan_initialize(rtems_device_major_number major, rtems_device_minor_number unused, void *arg){
+	int dev_cnt,minor,subcore_cnt,devi,subi,subcores;
+	amba_ahb_device ambadev;
+	occan_priv *can;
+	char fs_name[20];
+	rtems_status_code status;
+	
+	strcpy(fs_name,OCCAN_DEVNAME);
+	
+	/* find device on amba bus */
+	dev_cnt = amba_get_number_ahbslv_devices(amba_bus,VENDOR_GAISLER,GAISLER_OCCAN);
+	if ( dev_cnt < 1 ){
+		/* Failed to find any CAN cores! */
+		printk("OCCAN: Failed to find any CAN cores\n\r");
+		return -1;
+	}
+
+	/* Detect System Frequency from initialized timer */
+#ifndef SYS_FREQ_HZ
+#if defined(LEON3)
+	/* LEON3: find timer address via AMBA Plug&Play info */	
+	{
+		amba_apb_device gptimer;
+		LEON3_Timer_Regs_Map *tregs;
+		
+		if ( amba_find_apbslv(&amba_conf,VENDOR_GAISLER,GAISLER_GPTIMER,&gptimer) == 1 ){
+			tregs = (LEON3_Timer_Regs_Map *)gptimer.start;
+			sys_freq_hz = (tregs->scaler_reload+1)*1000*1000;
+			DBG("OCCAN: detected %dHZ system frequency\n\r",sys_freq_hz);
+		}else{
+			sys_freq_hz = 40000000; /* Default to 40MHz */
+			printk("OCCAN: Failed to detect system frequency\n\r");
+		}
+		
+	}
+#elif defined(LEON2)
+	/* LEON2: use hardcoded address to get to timer */
+	{
+		LEON_Register_Map *regs = (LEON_Register_Map *)0x80000000;
+		sys_freq_hz = (regs->Scaler_Reload+1)*1000*1000;
+	}
+#else
+  #error CPU not supported for OC_CAN driver
+#endif
+#else
+	/* Use hardcoded frequency */
+	sys_freq_hz = SYS_FREQ_HZ;
+#endif
+	
+	DBG("OCCAN: Detected %dHz system frequency\n\r",sys_freq_hz);
+	
+	/* OCCAN speciality:
+	 *  Mulitple cores are supported through the same amba AHB interface.
+	 *  The number of "sub cores" can be detected by decoding the AMBA
+	 *  Plug&Play version information. verion = ncores. A maximum of 8
+	 *  sub cores are supported, each separeated with 0x100 inbetween.
+	 *
+	 *  Now, lets detect sub cores.
+	 */
+	
+	for(subcore_cnt=devi=0; devi<dev_cnt; devi++){
+		amba_find_next_ahbslv(amba_bus,VENDOR_GAISLER,GAISLER_OCCAN,&ambadev,devi);
+		subcore_cnt += (ambadev.ver & 0x7)+1;
+	}
+	
+	printk("OCCAN: Found %d devs, totally %d sub cores\n\r",dev_cnt,subcore_cnt);
+	
+	/* allocate memory for cores */
+	can_cores = subcore_cnt;
+	cans = calloc(subcore_cnt*sizeof(occan_priv),1);
+	
+	minor=0;
+	for(devi=0; devi<dev_cnt; devi++){
+	
+		/* Get AHB device info */
+		amba_find_next_ahbslv(amba_bus,VENDOR_GAISLER,GAISLER_OCCAN,&ambadev,devi);
+		subcores = (ambadev.ver & 0x7)+1;
+		DBG("OCCAN: on dev %d found %d sub cores\n\r",devi,subcores);
+		
+		/* loop all subcores, at least 1 */
+		for(subi=0; subi<subcores; subi++){
+			can = &cans[minor];
+
+#ifdef OCCAN_BYTE_REGS
+			/* regs is byte regs */	
+			can->regs = (void *)ambadev.start[0] + OCCAN_NCORE_OFS*subi;
+#else
+			/* regs is word regs, accessed 0x100 from base address */
+			can->regs = (void *)(ambadev.start[0] + OCCAN_NCORE_OFS*subi+ OCCAN_WORD_REG_OFS);
+#endif
+			
+			/* remember IRQ number */
+			can->irq = ambadev.irq+subi;
+			
+			/* bind filesystem name to device */
+			OCCAN_DEVNAME_NO(fs_name,minor);
+			printk("OCCAN: Registering %s to [%d %d] @ 0x%lx irq %d\n\r",fs_name,major,minor,(unsigned int)can->regs,can->irq);
+			status = rtems_io_register_name(fs_name, major, minor);
+			if (RTEMS_SUCCESSFUL != status )
+				rtems_fatal_error_occurred(status);
+		
+			/* initialize software */
+			can->open = 0;
+			can->rxfifo = NULL;
+			can->txfifo = NULL;
+			status = rtems_semaphore_create(
+                        rtems_build_name('C', 'd', 'v', '0'+minor),
+                        1,
+                        RTEMS_FIFO | RTEMS_SIMPLE_BINARY_SEMAPHORE | RTEMS_NO_INHERIT_PRIORITY | \
+                        RTEMS_NO_PRIORITY_CEILING,
+                        0, 
+                        &can->devsem);
+			if ( status != RTEMS_SUCCESSFUL ){
+				printk("OCCAN: Failed to create dev semaphore for minor %d, (%d)\n\r",minor,status);
+				return RTEMS_UNSATISFIED;
+			}
+			status = rtems_semaphore_create(
+                        rtems_build_name('C', 't', 'x', '0'+minor),
+                        0, 
+                        RTEMS_FIFO | RTEMS_SIMPLE_BINARY_SEMAPHORE | RTEMS_NO_INHERIT_PRIORITY | \
+                        RTEMS_NO_PRIORITY_CEILING,
+                        0, 
+                        &can->txsem);
+			if ( status != RTEMS_SUCCESSFUL ){
+				printk("OCCAN: Failed to create tx semaphore for minor %d, (%d)\n\r",minor,status);
+				return RTEMS_UNSATISFIED;
+			}
+			status = rtems_semaphore_create(
+                        rtems_build_name('C', 'r', 'x', '0'+minor),
+                        0, 
+                        RTEMS_FIFO | RTEMS_SIMPLE_BINARY_SEMAPHORE | RTEMS_NO_INHERIT_PRIORITY | \
+                        RTEMS_NO_PRIORITY_CEILING,
+                        0,
+                        &can->rxsem);
+			if ( status != RTEMS_SUCCESSFUL ){
+				printk("OCCAN: Failed to create rx semaphore for minor %d, (%d)\n\r",minor,status);
+				return RTEMS_UNSATISFIED;
+			}
+				
+			/* hardware init/reset */
+			pelican_init(can);
+		
+			/* Setup interrupt handler for each channel */
+    	OCCAN_REG_INT(OCCAN_PREFIX(_interrupt_handler), can->irq, can);
+			
+			minor++;
+#ifdef DEBUG_PRINT_REGMAP
+			pelican_regadr_print(can->regs);
+#endif
+		}
+	}
+	
+	return RTEMS_SUCCESSFUL;
+}
+
+static rtems_device_driver occan_open(rtems_device_major_number major, rtems_device_minor_number minor, void *arg){
+	occan_priv *can;
+	
+	DBG("OCCAN: Opening %d\n\r",minor);
+	
+	if ( minor >= can_cores )
+		return RTEMS_UNSATISFIED; /* NODEV */
+	
+	/* get can device */
+	can = &cans[minor];
+	
+	/* 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);
+
+	/* 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 = &cans[minor];
+	
+	DBG("OCCAN: Closing %d\n\r",minor);
+	
+	/* stop if running */
+	if ( can->started )
+		pelican_stop(can);
+	
+	/* Enter Reset Mode */
+	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;
+	
+	return RTEMS_SUCCESSFUL;
+}
+
+static rtems_device_driver occan_read(rtems_device_major_number major, rtems_device_minor_number minor, void *arg){
+	occan_priv *can = &cans[minor];
+	rtems_libio_rw_args_t *rw_args=(rtems_libio_rw_args_t *) arg;
+	CANMsg *dstmsg, *srcmsg;
+	rtems_interrupt_level oldLevel;
+	int left;
+	
+	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 */
+		rtems_interrupt_disable(oldLevel);
+		
+		/* A bus off interrupt may have occured after checking can->started */
+		if ( can->status & OCCAN_STATUS_ERR_BUSOFF ){
+			rtems_interrupt_enable(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 */
+				rtems_interrupt_enable(oldLevel);
+				break;
+			}
+			
+			/* turn on interrupts again */
+			rtems_interrupt_enable(oldLevel);
+		
+			DBG("OCCAN: Waiting for RX int\n\r");
+		
+			/* wait for incomming 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 ){
+				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 */
+		rtems_interrupt_enable(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 = &cans[minor];
+	rtems_libio_rw_args_t *rw_args=(rtems_libio_rw_args_t *) arg;
+	CANMsg *msg,*fifo_msg;
+	rtems_interrupt_level oldLevel;
+	int left;
+	
+	DBG("OCCAN: Writing %d bytes from 0x%lx (%d)\n\r",rw_args->count,rw_args->buffer,sizeof(CANMsg));
+	
+	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 */
+	rtems_interrupt_disable(oldLevel);
+	
+	/* A bus off interrupt may have occured after checking can->started */
+	if ( can->status & OCCAN_STATUS_ERR_BUSOFF ){
+		rtems_interrupt_enable(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++;
+			
+			/* 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;
+			*/
+			rtems_interrupt_enable(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 ){
+				DBG("OCCAN: Blocking write 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 */
+			}
+			
+			rtems_interrupt_disable(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++;
+					
+					/* 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);
+	}
+	
+	rtems_interrupt_enable(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 = &cans[minor];
+	unsigned int speed;
+	rtems_libio_ioctl_args_t	*ioarg = (rtems_libio_ioctl_args_t *) arg;
+	struct occan_afilter *afilter;
+	occan_stats *dststats;
+	unsigned char btr0, btr1;
+	unsigned int rxcnt,txcnt;
+	
+	DBG("OCCAN: IOCTL %d\n\r",ioarg->command);
+	
+	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(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;
+			if ( can->started )
+				return RTEMS_RESOURCE_IN_USE; /* EBUSY */
+			
+			if ( (speed=pelican_speed_auto(can)) < 0 ){
+				/* failed */
+				return RTEMS_IO_ERROR;
+			}
+			
+			/* set new speed */
+			can->speed = speed;
+									
+			if ( (int *)ioarg->buffer ){
+				*(int *)ioarg->buffer = speed;
+			}
+			return RTEMS_SUCCESSFUL;
+			break;
+		
+		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;
+			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;
+}
+
+static void occan_interrupt(occan_priv *can){
+	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;
+	
+	can->stats.ints++;
+	
+	while ( (iflags = READ_REG(&can->regs->intflags)) != 0 ){
+		/* still interrupts to handle */
+		
+		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(&regs->rx_fi_xff);
+			msg->extended = tmp >> 7;
+			msg->rtr = (tmp >> 6) & 1;
+			msg->len = tmp = tmp & 0x0f;
+			
+			if ( msg->extended ){
+				/* extended message */
+				msg->id =  READ_REG(&regs->msg.rx_eff.id[0])<<(5+8+8) | 
+				           READ_REG(&regs->msg.rx_eff.id[1])<<(5+8) | 
+				           READ_REG(&regs->msg.rx_eff.id[2])<<5 | 
+				           READ_REG(&regs->msg.rx_eff.id[3])>>3;
+				while(tmp--){
+					msg->data[tmp] = READ_REG(&regs->msg.rx_eff.data[tmp]);
+				}
+				/*
+				msg->data[0] = READ_REG(&regs->msg.rx_eff.data[0]);
+				msg->data[1] = READ_REG(&regs->msg.rx_eff.data[1]);
+				msg->data[2] = READ_REG(&regs->msg.rx_eff.data[2]);
+				msg->data[3] = READ_REG(&regs->msg.rx_eff.data[3]);
+				msg->data[4] = READ_REG(&regs->msg.rx_eff.data[4]);
+				msg->data[5] = READ_REG(&regs->msg.rx_eff.data[5]);
+				msg->data[6] = READ_REG(&regs->msg.rx_eff.data[6]);
+				msg->data[7] = READ_REG(&regs->msg.rx_eff.data[7]);
+				*/
+			}else{
+				/* standard message */
+				msg->id =  READ_REG(&regs->msg.rx_sff.id[0])<<3 | 
+				           READ_REG(&regs->msg.rx_sff.id[1])>>5;
+				
+				while(tmp--){
+					msg->data[tmp] = READ_REG(&regs->msg.rx_sff.data[tmp]);
+				}
+				/*
+				msg->data[0] = READ_REG(&regs->msg.rx_sff.data[0]);
+				msg->data[1] = READ_REG(&regs->msg.rx_sff.data[1]);
+				msg->data[2] = READ_REG(&regs->msg.rx_sff.data[2]);
+				msg->data[3] = READ_REG(&regs->msg.rx_sff.data[3]);
+				msg->data[4] = READ_REG(&regs->msg.rx_sff.data[4]);
+				msg->data[5] = READ_REG(&regs->msg.rx_sff.data[5]);
+				msg->data[6] = READ_REG(&regs->msg.rx_sff.data[6]);
+				msg->data[7] = READ_REG(&regs->msg.rx_sff.data[7]);
+				*/
+			}
+			
+			/* Re-Enable RX buffer for a new message */
+			regs->cmd = READ_REG(&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++;
+				}
+				
+				/* 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(&regs->tx_err_cnt);
+			rx_error_cnt = READ_REG(&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(&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)
+					 */
+					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(&regs->tx_err_cnt);
+			rx_error_cnt = READ_REG(&regs->rx_err_cnt);
+			
+			if ( (tx_error_cnt > 127) || (tx_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(&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(&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++;
+		}
+	}
+	
+	/* signal Binary semaphore, messages available! */
+	if ( signal_rx ){
+		rtems_semaphore_release(can->rxsem);
+	}
+	
+	if ( signal_tx ){
+		rtems_semaphore_release(can->txsem);
+	}
+}
+
+static void occan_interrupt_handler(rtems_vector_number v){
+	int minor;
+	
+	/* convert to */
+  for(minor = 0; minor < can_cores; minor++) {
+  	if ( v == (cans[minor].irq+0x10) ) {
+			occan_interrupt(&cans[minor]);
+			return;
+		}
+	}
+}
+
+#define OCCAN_DRIVER_TABLE_ENTRY { occan_initialize, occan_open, occan_close, occan_read, occan_write, occan_ioctl }
+
+static rtems_driver_address_table occan_driver = OCCAN_DRIVER_TABLE_ENTRY;
+
+int OCCAN_PREFIX(_register)(amba_confarea_type *bus){
+	rtems_status_code r;
+	rtems_device_major_number m;
+	
+	amba_bus = bus;
+	if ( !bus )
+		return 1;
+
+  if ((r = rtems_io_register_driver(0, &occan_driver, &m)) == RTEMS_SUCCESSFUL) {
+		DBG("OCCAN driver successfully registered, major: %d\n\r", m);
+	}else{
+		switch(r) {
+			case RTEMS_TOO_MANY:
+				printk("OCCAN rtems_io_register_driver failed: RTEMS_TOO_MANY\n\r"); break;
+			case RTEMS_INVALID_NUMBER:  
+				printk("OCCAN rtems_io_register_driver failed: RTEMS_INVALID_NUMBER\n\r"); break;
+			case RTEMS_RESOURCE_IN_USE:
+				printk("OCCAN rtems_io_register_driver failed: RTEMS_RESOURCE_IN_USE\n\r"); break;
+			default:
+				printk("OCCAN rtems_io_register_driver failed\n\r");
+		}
+		return 1;
+	}
+	return 0;
+}
+
+
+/*******************************************************************************
+ * FIFO IMPLEMENTATION 
+ */
+
+static occan_fifo *occan_fifo_create(int cnt){
+	occan_fifo *fifo;
+	fifo = malloc(sizeof(occan_fifo)+cnt*sizeof(CANMsg));
+	if ( fifo ){
+		fifo->cnt = cnt;
+		fifo->full = 0;
+		fifo->ovcnt = 0;
+		fifo->base = (CANMsg *)&fifo->fifoarea[0];
+		fifo->tail = fifo->head = fifo->base;
+		/* clear CAN Messages */
+		memset(fifo->base,0,cnt * sizeof(CANMsg));
+	}
+	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;
+}
+/*******************************************************************************/
diff --git a/c/src/lib/libbsp/sparc/shared/can/occan_pci.c b/c/src/lib/libbsp/sparc/shared/can/occan_pci.c
new file mode 100644
index 0000000000..f68d04e9ca
--- /dev/null
+++ b/c/src/lib/libbsp/sparc/shared/can/occan_pci.c
@@ -0,0 +1,64 @@
+
+/* PCI cannot do byte accesses to addresses aligned byte wise
+ * Use alternative reg map.
+ */
+#define OCCAN_WORD_REGS
+
+/* Set registered device name */
+#define OCCAN_DEVNAME "/dev/occanpci0"
+#define OCCAN_DEVNAME_NO(devstr,no) ((devstr)[13]='0'+(no))
+ 
+/* Any non-static function will begin with */
+#define OCCAN_PREFIX(name) occanpci##name
+
+/* do nothing, assume that the interrupt handler is called 
+ * setup externally calling b1553_interrupt_handler.
+ */
+#define OCCAN_REG_INT(handler,irq,arg) \
+ if ( occan_pci_int_reg ) \
+   occan_pci_int_reg(handler,irq,arg);
+
+void (*occan_pci_int_reg)(void *handler, int irq, void *arg) = 0;
+
+void occanpci_interrupt_handler(int irq, void *arg);
+
+/* AMBA Bus is clocked using the PCI clock (33.3MHz) */
+#define SYS_FREQ_HZ 33333333
+
+/* Enable two redundant channels */
+#define REDUNDANT_CHANNELS 2
+
+#define OCCAN_SET_CHANNEL(priv,channel) occanpci_set_channel(priv,channel)
+
+#include "occan.c"
+
+/* Define method that sets redundant channel 
+ * The channel select register:
+ *  0x00 = byte regs
+ *  0x40 = channel select
+ *  0x80 = word regs
+ */
+static void inline occanpci_set_channel(occan_priv *priv, int channel){
+	unsigned int *chan_sel = (unsigned int *)(((unsigned int)priv->regs & ~0xff)+0x40);
+	if ( channel == 0 )
+	  *chan_sel = 0;
+	else
+		*chan_sel = 0xffffffff;
+}
+
+int occan_pci_register(amba_confarea_type *bus)
+{
+	/* Setup configuration */
+	
+	/* Register the driver */
+	return OCCAN_PREFIX(_register)(bus);
+}
+
+
+/* Call this from PCI interrupt handler 
+ * irq = the irq number of the HW device local to that IRQMP controller
+ * 
+ */
+void occanpci_interrupt_handler(int irq, void *arg){
+	occan_interrupt(arg);
+}
diff --git a/c/src/lib/libbsp/sparc/shared/spw/grspw.c b/c/src/lib/libbsp/sparc/shared/spw/grspw.c
new file mode 100644
index 0000000000..aa01be4854
--- /dev/null
+++ b/c/src/lib/libbsp/sparc/shared/spw/grspw.c
@@ -0,0 +1,1646 @@
+/*
+ *  This file contains the GRSPW SpaceWire Driver for LEON2 and LEON3.
+ *
+ *  COPYRIGHT (c) 2007
+ *  Gaisler Research.
+ *
+ *  The license and distribution terms for this file may be
+ *  found in the file LICENSE in this distribution or at
+ *  http://www.rtems.com/license/LICENSE.
+ *
+ * Changes:
+ * 
+ * 2007-07-12, Daniel Hellstrom <daniel@gaisler.com>
+ *  Fixed bug in TXBLOCK mode (normally called flush).
+ *
+ * 2007-05-28, Daniel Hellstrom <daniel@gaisler.com>
+ *  Changed register call from spacewire_register to
+ *  grspw_register. Added one parameter (the AMBA bus
+ *  pointer) for LEON2 and LEON3 PCI compability.
+ *  Typical LEON3 register: grspw_register(&amba_conf);
+ *
+ * 2007-05-28, Daniel Hellstrom <daniel@gaisler.com>
+ *  Changed errno return values, compatible with RASTA 
+ *  Spacewire driver
+ *
+ * 2007-05-25, Daniel Hellstrom <daniel@gaisler.com>
+ *  Changed name from /dev/spacewire,/dev/spacewire_b... 
+ *  to /dev/grspw0,/dev/grspw1...
+ *
+ * 2007-05-24, Daniel Hellstrom <daniel@gaisler.com>
+ *  Merged LEON3, LEON2 and RASTA driver to one - this.
+ *  The driver is included and configured from grspw_pci.c 
+ *  and grspw_rasta.c.
+ *
+ * 2007-05-23, Daniel Hellstrom <daniel@gaisler.com>
+ *  Changed open call, now one need to first call open
+ *  and then ioctl(fd,START,timeout) in order to setup
+ *  hardware for communication.
+ *  
+ * 2007-05-23, Daniel Hellstrom <daniel@gaisler.com>
+ *  Added ioctl(fd,SET_COREFREQ,freq_arg), the command
+ *  can autodetect the register values disconnect and
+ *  timer64. It is still possible to change them manually
+ *  by ioctl(fd,SET_{DISCONNECT,TIMER},arg).
+ *
+ */
+
+/* default name to /dev/grspw0 */
+#if !defined(GRSPW_DEVNAME) || !defined(GRSPW_DEVNAME_NO)
+ #undef GRSPW_DEVNAME
+ #undef GRSPW_DEVNAME_NO
+ #define GRSPW_DEVNAME "/dev/grspw0"
+ #define GRSPW_DEVNAME_NO(devstr,no) ((devstr)[10]='0'+(no))
+#endif
+
+#ifndef GRSPW_PREFIX
+ #define GRSPW_PREFIX(name) grspw##name
+#endif
+
+/* default to no translation */
+#ifndef GRSPW_ADR_TO
+ #define memarea_to_hw(x) ((unsigned int)(x))
+#endif
+#ifndef GRSPW_ADR_FROM
+ #define hw_to_memarea(x) ((unsigned int)(x))
+#endif
+
+#ifndef GRSPW_REG_INT
+ #define GRSPW_REG_INT(handler,irqno,arg) set_vector(handler,irqno+0x10,1)
+#endif
+
+#include <bsp.h>
+#include <rtems/libio.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+#include <assert.h>
+#include <sched.h>
+#include <ctype.h>
+#include <rtems/bspIo.h>
+#include <ambapp.h>
+#include <grspw.h>
+
+#define DBGSPW_IOCALLS 1
+#define DBGSPW_TX 2
+#define DBGSPW_RX 4
+#define DBGSPW_IOCTRL 1
+#define DBGSPW_DUMP 16
+#define DEBUG_SPACEWIRE_FLAGS (DBGSPW_IOCALLS | DBGSPW_TX | DBGSPW_RX )
+
+/* #define DEBUG_SPACEWIRE_ONOFF */
+ 
+#ifdef DEBUG_SPACEWIRE_ONOFF
+int DEBUG_printf(const char *fmt, ...);
+#define SPACEWIRE_DBG(fmt, args...)    do { { printk(" : %03d @ %18s()]:" fmt , __LINE__,__FUNCTION__,## args); }} while(0)
+#define SPACEWIRE_DBG2(fmt)            do { { printk(" : %03d @ %18s()]:" fmt , __LINE__,__FUNCTION__); }} while(0)
+#define SPACEWIRE_DBGC(c,fmt, args...) do { if (DEBUG_SPACEWIRE_FLAGS & c) { printk(" : %03d @ %18s()]:" fmt , __LINE__,__FUNCTION__,## args);  }} while(0)
+#else
+#define SPACEWIRE_DBG(fmt, args...)
+#define SPACEWIRE_DBG2(fmt, args...)
+#define SPACEWIRE_DBGC(c, fmt, args...)
+#endif
+
+typedef struct {
+   volatile unsigned int ctrl;
+   volatile unsigned int status;
+   volatile unsigned int nodeaddr;
+   volatile unsigned int clkdiv;
+   volatile unsigned int destkey;
+   volatile unsigned int time;
+   volatile unsigned int timer;
+   volatile unsigned int pad;
+   
+   volatile unsigned int dma0ctrl; 
+   volatile unsigned int dma0rxmax;
+   volatile unsigned int dma0txdesc;
+   volatile unsigned int dma0rxdesc;
+} LEON3_SPACEWIRE_Regs_Map;
+
+typedef struct {
+   volatile unsigned int ctrl;
+   volatile unsigned int addr;
+} SPACEWIRE_RXBD;
+
+typedef struct {
+   volatile unsigned int ctrl;
+   volatile unsigned int addr_header;
+   volatile unsigned int len;
+   volatile unsigned int addr_data;
+} SPACEWIRE_TXBD;
+
+#define SPACEWIRE_INIT_TIMEOUT 10
+#define SPACEWIRE_BDTABLE_SIZE 0x400
+#define SPACEWIRE_TXD_SIZE 1024
+#define SPACEWIRE_TXH_SIZE 64
+#define SPACEWIRE_RXPCK_SIZE 1024
+#define SPACEWIRE_TXBUFS_NR 64
+#define SPACEWIRE_RXBUFS_NR 128
+
+#define BUFMEM_PER_LINK (SPACEWIRE_TXBUFS_NR*(SPACEWIRE_TXD_SIZE+SPACEWIRE_TXH_SIZE) + SPACEWIRE_RXBUFS_NR*SPACEWIRE_RXPCK_SIZE)
+
+#define SPW_ALIGN(p,c) ((((unsigned int)(p))+((c)-1))&~((c)-1))
+
+typedef struct {
+   /* configuration parameters */ 
+   spw_config config;
+
+   unsigned int tx_all_in_use;
+   unsigned int tx_sent;
+   unsigned int tx_cur;
+   unsigned int rxcur;
+   unsigned int rxbufcur;
+   unsigned int txdbufsize;
+   unsigned int txhbufsize;
+   unsigned int rxbufsize;
+   unsigned int txbufcnt;
+   unsigned int rxbufcnt;
+
+   /* statistics */
+   spw_stats stat;
+   
+   char *ptr_rxbuf0;
+   char *ptr_txdbuf0;
+   char *ptr_txhbuf0;
+
+   unsigned int irq;
+   int minor;
+   int open;
+   int running;
+   unsigned int core_freq_khz;
+   
+
+   /* semaphores*/
+   rtems_id txsp;
+   rtems_id rxsp;
+   
+   SPACEWIRE_RXBD *rx;
+   SPACEWIRE_TXBD *tx;
+
+#ifdef GRSPW_STATIC_MEM
+   unsigned int membase, memend, mem_bdtable;
+#else
+   char _rxtable[SPACEWIRE_BDTABLE_SIZE*2];
+   char _txtable[SPACEWIRE_BDTABLE_SIZE*2];
+#endif
+
+   LEON3_SPACEWIRE_Regs_Map *regs;
+} GRSPW_DEV;
+
+static int spw_cores;
+static unsigned int sys_freq_khz;
+static GRSPW_DEV *grspw_devs;
+
+#ifdef GRSPW_DONT_BYPASS_CACHE
+#define _SPW_READ(address) (*(unsigned int *)(address))
+#define _MEM_READ(address) (*(unsigned char *)(address))
+#else
+static unsigned int _SPW_READ(void *addr) {
+        unsigned int tmp;
+        asm(" lda [%1]1, %0 "
+            : "=r"(tmp)
+            : "r"(addr)
+           );
+        return tmp;
+}
+
+static unsigned int _MEM_READ(void *addr) {
+        unsigned int tmp;
+        asm(" lduba [%1]1, %0 "
+            : "=r"(tmp)
+            : "r"(addr)
+           );
+        return tmp;        
+
+}
+#endif
+
+#define MEM_READ(addr) _MEM_READ((void *)(addr))
+#define SPW_READ(addr) _SPW_READ((void *)(addr))
+#define SPW_WRITE(addr,v) *addr=v
+
+#define SPW_REG(c,r) (c->regs->r)
+#define SPW_REG_CTRL(c) SPW_REG(c,ctrl)
+#define SPW_REG_STATUS(c) SPW_REG(c,status)
+#define SPW_REG_NODEADDR(c) SPW_REG(c,nodeaddr)
+
+#define SPW_CTRL_READ(c)      SPW_READ(&SPW_REG_CTRL(c))
+#define SPW_CTRL_WRITE(c,v)   SPW_WRITE(&SPW_REG_CTRL(c),v)
+#define SPW_STATUS_READ(c)    SPW_READ(&SPW_REG_STATUS(c))
+#define SPW_STATUS_WRITE(c,v) SPW_WRITE(&SPW_REG_STATUS(c),v)
+
+#define SPW_LINKSTATE(c) (((c) >> 21) & 0x7)
+
+#define SPACEWIRE_RXNR(c) ((c&~(SPACEWIRE_BDTABLE_SIZE-1))>>3)
+#define SPACEWIRE_TXNR(c) ((c&~(SPACEWIRE_BDTABLE_SIZE-1))>>4)
+
+#define SPW_RXBD_LENGTH 0x1ffffff
+#define SPW_RXBD_EN (1 << 25)
+#define SPW_RXBD_WR (1 << 26)
+#define SPW_RXBD_IE (1 << 27)
+
+#define SPW_RXBD_EEP (1 << 28)
+#define SPW_RXBD_EHC (1 << 29)
+#define SPW_RXBD_EDC (1 << 30)
+#define SPW_RXBD_ETR (1 << 31)
+
+#define SPW_RXBD_ERROR (SPW_RXBD_EEP | \
+                        SPW_RXBD_ETR)
+
+#define SPW_RXBD_RMAPERROR (SPW_RXBD_EHC | SPW_RXBD_EDC)
+
+#define SPW_TXBD_LENGTH 0xffffff
+
+#define SPW_TXBD_EN (1 << 12)
+#define SPW_TXBD_WR (1 << 13)
+#define SPW_TXBD_IE (1 << 14)
+#define SPW_TXBD_LE (1 << 15)
+
+#define SPW_TXBD_ERROR (SPW_TXBD_LE)
+
+#define SPW_CTRL_LINKDISABLED (1 << 0)
+#define SPW_CTRL_LINKSTART    (1 << 1)
+#define SPW_CTRL_AUTOSTART    (1 << 2)
+#define SPW_CTRL_IE           (1 << 3)
+#define SPW_CTRL_TI           (1 << 4)
+#define SPW_CTRL_PM           (1 << 5)
+#define SPW_CTRL_RESET        (1 << 6)
+#define SPW_CTRL_TQ           (1 << 8)
+#define SPW_CTRL_LI           (1 << 9)
+#define SPW_CTRL_TT           (1 << 10)
+#define SPW_CTRL_TR           (1 << 11)
+#define SPW_CTRL_RE           (1 << 16)
+#define SPW_CTRL_RD           (1 << 17)
+
+#define SPW_CTRL_RC           (1 << 29)
+#define SPW_CTRL_RX           (1 << 30)
+#define SPW_CTRL_RA           (1 << 31)
+
+#define SPW_STATUS_TO (1 << 0)
+#define SPW_STATUS_CE (1 << 1)
+#define SPW_STATUS_ER (1 << 2)
+#define SPW_STATUS_DE (1 << 3)
+#define SPW_STATUS_PE (1 << 4)
+#define SPW_STATUS_WE (1 << 6)
+#define SPW_STATUS_IA (1 << 7)
+#define SPW_STATUS_EE (1 << 8)
+
+#define SPW_DMACTRL_TXEN (1 << 0)
+#define SPW_DMACTRL_RXEN (1 << 1)
+#define SPW_DMACTRL_TXIE (1 << 2)
+#define SPW_DMACTRL_RXIE (1 << 3)
+#define SPW_DMACTRL_AI   (1 << 4)
+#define SPW_DMACTRL_PS   (1 << 5)
+#define SPW_DMACTRL_PR   (1 << 6)
+#define SPW_DMACTRL_TA   (1 << 7)
+#define SPW_DMACTRL_RA   (1 << 8)
+#define SPW_DMACTRL_AT   (1 << 9)
+#define SPW_DMACTRL_RX   (1 << 10)
+#define SPW_DMACTRL_RD   (1 << 11)
+#define SPW_DMACTRL_NS   (1 << 12)
+
+#define SPW_PREPAREMASK_TX (SPW_DMACTRL_RXEN | SPW_DMACTRL_RXIE | SPW_DMACTRL_PS | SPW_DMACTRL_TA | SPW_DMACTRL_RD | SPW_DMACTRL_NS)
+#define SPW_PREPAREMASK_RX (SPW_DMACTRL_TXEN | SPW_DMACTRL_TXIE | SPW_DMACTRL_AI | SPW_DMACTRL_PR | SPW_DMACTRL_RA)
+
+static int grspw_hw_init(GRSPW_DEV *pDev);
+static int grspw_hw_send(GRSPW_DEV *pDev, unsigned int hlen, unsigned char *hdr, unsigned int dlen, unsigned char *data);
+static int grspw_hw_receive(GRSPW_DEV *pDev,unsigned char *b,int c);
+static int grspw_hw_startup (GRSPW_DEV *pDev, int timeout);
+static int grspw_hw_stop (GRSPW_DEV *pDev, int rx, int tx);
+static void grspw_hw_wait_rx_inactive(GRSPW_DEV *pDev);
+static int grspw_hw_waitlink (GRSPW_DEV *pDev, int timeout);
+static void grspw_hw_reset(GRSPW_DEV *pDev);
+static void grspw_hw_read_config(GRSPW_DEV *pDev);
+
+static void check_rx_errors(GRSPW_DEV *pDev, int ctrl);
+static void grspw_rxnext(GRSPW_DEV *pDev);
+static void grspw_interrupt(GRSPW_DEV *pDev);
+
+static rtems_device_driver grspw_initialize(
+        rtems_device_major_number  major,
+        rtems_device_minor_number  minor,
+        void                    * arg
+        );
+
+static rtems_device_driver grspw_open(
+        rtems_device_major_number major,
+        rtems_device_minor_number minor,
+        void                    * arg
+        );
+
+static rtems_device_driver grspw_close(
+        rtems_device_major_number major,
+        rtems_device_minor_number minor,
+        void                    * arg
+        );
+
+static rtems_device_driver grspw_read(
+        rtems_device_major_number major,
+        rtems_device_minor_number minor,
+        void                    * arg
+        );
+
+static rtems_device_driver grspw_write(
+        rtems_device_major_number major,
+        rtems_device_minor_number minor,
+        void                    * arg
+        );
+
+static rtems_device_driver grspw_control(
+        rtems_device_major_number major,
+        rtems_device_minor_number minor,
+        void                    * arg
+        );
+
+#define GRSPW_DRIVER_TABLE_ENTRY \
+  { grspw_initialize, \
+    grspw_open, \
+    grspw_close, \
+    grspw_read, \
+    grspw_write, \
+    grspw_control }
+
+static rtems_driver_address_table grspw_driver = GRSPW_DRIVER_TABLE_ENTRY;
+static amba_confarea_type *amba_bus;
+
+int GRSPW_PREFIX(_register)(amba_confarea_type *bus)
+{
+        rtems_status_code r;
+        rtems_device_major_number m;
+        
+        /* Get System clock frequency */
+        sys_freq_khz = 0;
+        
+        amba_bus = bus;
+        
+        /* Auto Detect the GRSPW core frequency by assuming that the system frequency is
+         * is the same as the GRSPW core frequency.
+         */
+#ifndef SYS_FREQ_KHZ
+#ifdef LEON3
+	/* LEON3: find timer address via AMBA Plug&Play info */	
+	{
+		amba_apb_device gptimer;
+		LEON3_Timer_Regs_Map *tregs;
+		
+		if ( amba_find_apbslv(&amba_conf,VENDOR_GAISLER,GAISLER_GPTIMER,&gptimer) == 1 ){
+			tregs = (LEON3_Timer_Regs_Map *)gptimer.start;
+			sys_freq_khz = (tregs->scaler_reload+1)*1000;
+			SPACEWIRE_DBG("GRSPW: detected %dkHZ system frequency\n\r",sys_freq_khz);
+		}else{
+			sys_freq_khz = 40000; /* Default to 40MHz */
+			printk("GRSPW: Failed to detect system frequency\n\r");
+		}
+		
+	}
+#elif defined(LEON2)
+        /* LEON2: use hardcoded address to get to timer */
+        {
+                LEON_Register_Map *regs = (LEON_Register_Map *)0x80000000;
+                sys_freq_khz = (regs->Scaler_Reload+1)*1000;
+        }
+#else
+ #error CPU not supported by GRSPW driver
+#endif
+#else
+	/* Use hardcoded frequency */
+	sys_freq_khz = SYS_FREQ_KHZ;
+#endif        
+ 			
+        SPACEWIRE_DBG2("register driver\n");
+        if ((r = rtems_io_register_driver(0, &grspw_driver, &m)) == RTEMS_SUCCESSFUL) {
+                SPACEWIRE_DBG2("success\n");
+                return 0;
+        } else {
+                switch(r) {
+                        case RTEMS_TOO_MANY:
+                                SPACEWIRE_DBG2("failed RTEMS_TOO_MANY\n"); 
+                                break;
+                        case RTEMS_INVALID_NUMBER:
+                                SPACEWIRE_DBG2("failed RTEMS_INVALID_NUMBER\n"); 
+                                break;
+                        case RTEMS_RESOURCE_IN_USE:
+                                SPACEWIRE_DBG2("failed RTEMS_RESOURCE_IN_USE\n"); 
+                                break;
+                        default:
+                                SPACEWIRE_DBG("failed %i\n",r); 
+                                break;
+                }
+                return 1;
+        }
+        
+}
+
+/* Get a value at least 6.4us in number of clock cycles */
+static unsigned int grspw_calc_timer64(int freq_khz){
+	unsigned int timer64 = (freq_khz*64+9999)/10000;
+	return timer64 & 0xfff;
+}
+
+/* Get a value at least 850ns in number of clock cycles - 3 */
+static unsigned int grspw_calc_disconnect(int freq_khz){
+	unsigned int disconnect = ((freq_khz*85+99999)/100000) - 3;
+	return disconnect & 0x3ff;
+}
+
+#if 0
+static int grspw_buffer_alloc(GRSPW_DEV *pDev) {
+        if (pDev->ptr_rxbuf0) {
+                free(pDev->ptr_rxbuf0);
+        }
+        if (pDev->ptr_txdbuf0) {
+                free(pDev->ptr_txdbuf0);
+        }
+        if (pDev->ptr_txhbuf0) {
+                free(pDev->ptr_txhbuf0);
+        }
+        pDev->ptr_rxbuf0 = (char *) malloc(pDev->rxbufsize * pDev->rxbufcnt);
+        pDev->ptr_txdbuf0 = (char *) malloc(pDev->txdbufsize * pDev->txbufcnt);
+        pDev->ptr_txhbuf0 = (char *) malloc(pDev->txhbufsize * pDev->txbufcnt);
+        if ((pDev->ptr_rxbuf0 == NULL) || 
+            (pDev->ptr_txdbuf0 == NULL) || (pDev->ptr_txhbuf0 == NULL)) {
+                return 1;
+        } else {
+                return 0;
+        }
+}
+#endif
+
+static int grspw_buffer_alloc(GRSPW_DEV *pDev)
+{
+#ifndef GRSPW_STATIC_MEM
+        if (pDev->ptr_rxbuf0) {
+                free(pDev->ptr_rxbuf0);
+        }
+        if (pDev->ptr_txdbuf0) {
+                free(pDev->ptr_txdbuf0);
+        }
+        if (pDev->ptr_txhbuf0) {
+                free(pDev->ptr_txhbuf0);
+        }
+				
+        pDev->ptr_rxbuf0 = (char *) malloc(pDev->rxbufsize * pDev->rxbufcnt);
+        pDev->ptr_txdbuf0 = (char *) malloc(pDev->txdbufsize * pDev->txbufcnt);
+        pDev->ptr_txhbuf0 = (char *) malloc(pDev->txhbufsize * pDev->txbufcnt);
+        if ((pDev->ptr_rxbuf0 == NULL) || 
+            (pDev->ptr_txdbuf0 == NULL) || (pDev->ptr_txhbuf0 == NULL)) {
+                return 1;
+        } else {
+                return 0;
+        }
+#else
+        if ( (pDev->membase + pDev->rxbufsize*pDev->rxbufcnt + pDev->txdbufsize*pDev->txbufcnt) >= pDev->memend ) {
+                return -1;
+        }
+        pDev->ptr_rxbuf0  = (char *) pDev->membase;
+        pDev->ptr_txdbuf0 = pDev->ptr_rxbuf0 + pDev->rxbufsize * pDev->rxbufcnt;
+        pDev->ptr_txhbuf0 = pDev->ptr_txdbuf0 + pDev->txdbufsize * pDev->txbufcnt;
+        return 0;
+#endif
+
+}
+
+/*
+ *  Standard Interrupt handler
+ */
+static rtems_isr grspw_interrupt_handler(rtems_vector_number v) 
+{
+        int minor;
+
+        for(minor = 0; minor < spw_cores; minor++) {
+                if (v == (grspw_devs[minor].irq+0x10) ) {
+                        grspw_interrupt(&grspw_devs[minor]);
+                        break;
+                }
+        }
+}
+
+static void grspw_interrupt(GRSPW_DEV *pDev){
+        int dmactrl;
+        int status;
+        int ctrl;
+				
+        status = SPW_STATUS_READ(pDev);
+        SPW_STATUS_WRITE(pDev, SPW_STATUS_CE | SPW_STATUS_ER | SPW_STATUS_DE | SPW_STATUS_PE | SPW_STATUS_WE | SPW_STATUS_IA | SPW_STATUS_EE);
+        dmactrl = SPW_READ(&pDev->regs->dma0ctrl);
+        SPW_WRITE(&pDev->regs->dma0ctrl, dmactrl | SPW_DMACTRL_PR);
+        /* If linkinterrupts are enabled check if it was a linkerror irq and then send an event to the 
+           process set in the config */
+        if (pDev->config.link_err_irq) {
+                if (status & (SPW_STATUS_CE | SPW_STATUS_ER | SPW_STATUS_DE | SPW_STATUS_PE | SPW_STATUS_WE)) {
+                        rtems_event_send(pDev->config.event_id, SPW_LINKERR_EVENT);
+                        if (pDev->config.disable_err) {
+                                /* disable link*/
+                                SPW_CTRL_WRITE(pDev, (SPW_CTRL_READ(pDev) & 0xFFFFFFFC) | SPW_CTRL_LINKDISABLED);
+                                pDev->config.linkdisabled = 1;
+                                pDev->config.linkstart = 0;
+                                pDev->running = 0;
+                        }
+                }
+        }
+        if (status & SPW_STATUS_CE) {
+                pDev->stat.credit_err++;
+        }
+        if (status & SPW_STATUS_ER) {
+                pDev->stat.escape_err++;
+        }
+        if (status & SPW_STATUS_DE) {
+                pDev->stat.disconnect_err++;
+        }
+        if (status & SPW_STATUS_PE) {
+                pDev->stat.parity_err++;
+        }
+        if (status & SPW_STATUS_WE) {
+                pDev->stat.write_sync_err++;
+        }
+        if (status & SPW_STATUS_IA) {
+                pDev->stat.invalid_address++;
+        }
+        if (status & SPW_STATUS_EE) {
+                pDev->stat.early_ep++;
+        }
+                
+        /* Check for tx interrupts */
+        while( (pDev->tx_sent != pDev->tx_cur) || pDev->tx_all_in_use) {
+                /* Has this descriptor been sent? */
+                ctrl = SPW_READ((volatile void *)&pDev->tx[pDev->tx_sent].ctrl);
+                if ( ctrl & SPW_TXBD_EN ) {
+                        break;
+                }
+                /* Yes, increment status counters & tx_sent so we can use this descriptor to send more packets with */
+                pDev->stat.packets_sent++;
+                        
+                rtems_semaphore_release(pDev->txsp);
+                        
+                if ( ctrl & SPW_TXBD_LE ) {
+                        pDev->stat.tx_link_err++;
+                }
+                        
+                /* step to next descriptor */
+                pDev->tx_sent = (pDev->tx_sent + 1) % pDev->txbufcnt;
+                pDev->tx_all_in_use = 0; /* not all of the descriptors can be in use since we just freed one. */
+        }
+                
+        /* Check for rx interrupts */
+        if (dmactrl & SPW_DMACTRL_PR) {
+                rtems_semaphore_release(pDev->rxsp);
+        }
+}
+
+static rtems_device_driver grspw_initialize(
+        rtems_device_major_number  major,
+        rtems_device_minor_number  minor,
+        void                      *arg
+)
+{
+        rtems_status_code status;
+        int i=0;
+        char c;
+        GRSPW_DEV *pDev;
+        char console_name[20];
+				amba_apb_device dev;
+        SPACEWIRE_DBG2("spacewire driver initialization\n");        
+        
+        /* Copy device name */
+        strcpy(console_name,GRSPW_DEVNAME);
+        
+        /* Get the number of GRSPW cores */
+        i=0; spw_cores = 0;
+
+        /* get number of GRSPW cores */
+        spw_cores = amba_get_number_apbslv_devices(amba_bus,VENDOR_GAISLER,GAISLER_SPACEWIRE);
+#if 0
+				if ( spw_cores > SPACEWIRE_MAX_CORENR )
+					spw_cores = SPACEWIRE_MAX_CORENR;
+
+        while (i < amba_conf.apbslv.devnr) {
+                conf = amba_get_confword(amba_conf.apbslv, i, 0);
+                if ((amba_vendor(conf) == VENDOR_GAISLER) && (amba_device(conf) == GAISLER_SPACEWIRE))
+                        spw_cores++;
+                i++;
+        }
+#endif
+        
+        if ( spw_cores < 1 ){
+                /* No GRSPW cores around... */
+                return RTEMS_SUCCESSFUL;
+        }
+        
+        /* Allocate memory for all spacewire cores */
+        grspw_devs = (GRSPW_DEV *)malloc(spw_cores * sizeof(GRSPW_DEV));
+        
+        /* Zero out all memory */
+        memset(grspw_devs,0,spw_cores * sizeof(GRSPW_DEV));
+        
+        /* loop all found spacewire cores */
+        i = 0;
+        for(minor=0; minor<spw_cores; minor++){
+                /* Get device */
+                amba_find_next_apbslv(amba_bus,VENDOR_GAISLER,GAISLER_SPACEWIRE,&dev,minor);
+                
+                pDev = &grspw_devs[minor];
+                
+                pDev->regs = (LEON3_SPACEWIRE_Regs_Map *)dev.start;
+                pDev->irq = dev.irq;
+                pDev->minor = minor;
+                pDev->open = 0;
+
+                /* register interrupt routine */
+                GRSPW_REG_INT(GRSPW_PREFIX(_interrupt_handler), pDev->irq, pDev);
+                
+                SPACEWIRE_DBG("spacewire core at [0x%x]\n", (unsigned int) pDev->regs);
+
+                /* initialize the code with some resonable values,
+                   actual initialization is done later using ioctl(fd)
+                   on the opened device */
+                pDev->config.rxmaxlen = SPACEWIRE_RXPCK_SIZE;
+                pDev->txdbufsize = SPACEWIRE_TXD_SIZE;
+                pDev->txhbufsize = SPACEWIRE_TXH_SIZE;
+                pDev->rxbufsize = SPACEWIRE_RXPCK_SIZE;
+                pDev->txbufcnt = SPACEWIRE_TXBUFS_NR;
+                pDev->rxbufcnt = SPACEWIRE_RXBUFS_NR;
+                pDev->config.check_rmap_err = 0;
+                pDev->config.tx_blocking = 0;
+                pDev->config.tx_block_on_full = 0;
+                pDev->config.rx_blocking = 0;
+                pDev->config.disable_err = 0;
+                pDev->config.link_err_irq = 0; 
+                pDev->config.event_id = 0; 
+                
+                pDev->ptr_rxbuf0 = 0;
+                pDev->ptr_txdbuf0 = 0;
+                pDev->ptr_txhbuf0 = 0;
+								
+#ifdef GRSPW_STATIC_MEM
+                GRSPW_CALC_MEMOFS(spw_cores,minor,&pDev->membase,&pDev->memend,&pDev->mem_bdtable);
+#endif
+                
+                if (grspw_buffer_alloc(pDev)) 
+                        return RTEMS_NO_MEMORY;
+                
+        }
+  
+        /*  Register Device Names, /dev/grspw0, /dev/grspw1  ... */
+        for (i = 0; i < spw_cores; i++) {
+           GRSPW_DEVNAME_NO(console_name,i);
+           SPACEWIRE_DBG("registering minor %i as %s\n", i, console_name);
+           status = rtems_io_register_name( console_name, major, i);
+           if (status != RTEMS_SUCCESSFUL){
+              rtems_fatal_error_occurred(status);
+           }
+        }
+  
+        /* Initialize Hardware and semaphores*/
+        c = 'a';
+        for (i = 0; i < spw_cores; i++) {
+                pDev = &grspw_devs[i];
+                rtems_semaphore_create(
+                        rtems_build_name('T', 'x', 'S', c), 
+                        0, 
+                        RTEMS_FIFO | RTEMS_SIMPLE_BINARY_SEMAPHORE | RTEMS_NO_INHERIT_PRIORITY | \
+                        RTEMS_NO_PRIORITY_CEILING, 
+                        0, 
+                        &(pDev->txsp));
+                rtems_semaphore_create(
+                        rtems_build_name('R', 'x', 'S', c), 
+                        0, 
+                        RTEMS_FIFO | RTEMS_SIMPLE_BINARY_SEMAPHORE | RTEMS_NO_INHERIT_PRIORITY | \
+                        RTEMS_NO_PRIORITY_CEILING, 
+                        0, 
+                        &(pDev->rxsp));
+                c++;
+                grspw_hw_init(pDev);
+        }
+        
+        return RTEMS_SUCCESSFUL;
+}
+
+static rtems_device_driver grspw_open(
+        rtems_device_major_number major,
+        rtems_device_minor_number minor,
+        void                    * arg
+        ) 
+{
+        GRSPW_DEV *pDev;
+        SPACEWIRE_DBGC(DBGSPW_IOCALLS, "open [%i,%i]\n", major, minor);
+        if ( minor >= spw_cores ) {
+                SPACEWIRE_DBG("minor %i too big\n", minor);
+                return RTEMS_INVALID_NAME;
+        }
+        pDev = &grspw_devs[minor];
+        
+        if ( pDev->open )
+                return RTEMS_RESOURCE_IN_USE;
+        
+        /* Mark device open */
+        pDev->open = 1;
+        
+        pDev->stat.tx_link_err = 0;
+        pDev->stat.rx_rmap_header_crc_err = 0;
+        pDev->stat.rx_rmap_data_crc_err = 0;
+        pDev->stat.rx_eep_err = 0;
+        pDev->stat.rx_truncated = 0;
+        pDev->stat.parity_err = 0;
+        pDev->stat.escape_err = 0;
+        pDev->stat.credit_err = 0;
+        pDev->stat.write_sync_err = 0;
+        pDev->stat.disconnect_err = 0;
+        pDev->stat.early_ep = 0;
+        pDev->stat.invalid_address = 0;
+        pDev->stat.packets_sent = 0;
+        pDev->stat.packets_received = 0;
+        
+        pDev->running = 0;
+        pDev->core_freq_khz = 0;
+        
+        /* Reset Core */
+        grspw_hw_reset(pDev);
+        
+        /* Read default configuration */
+        grspw_hw_read_config(pDev);
+        
+        return RTEMS_SUCCESSFUL;
+}
+
+static rtems_device_driver grspw_close(
+        rtems_device_major_number major,
+        rtems_device_minor_number minor,
+        void                    * arg
+        )
+{        
+        GRSPW_DEV *pDev = &grspw_devs[minor];
+        
+        SPACEWIRE_DBGC(DBGSPW_IOCALLS, "close [%i,%i]\n", major, minor);        
+        rtems_semaphore_delete(pDev->txsp);
+        rtems_semaphore_delete(pDev->rxsp);
+
+        grspw_hw_stop(pDev,1,1);
+        
+        grspw_hw_reset(pDev);
+        
+        /* Mark device closed - not open */
+        pDev->open = 0;
+        
+        return RTEMS_SUCCESSFUL;
+}
+
+static rtems_device_driver grspw_read(
+        rtems_device_major_number major,
+        rtems_device_minor_number minor,
+        void                    * arg
+        )
+{
+        GRSPW_DEV *pDev = &grspw_devs[minor];
+        rtems_libio_rw_args_t *rw_args;
+        unsigned int count = 0;
+        rw_args = (rtems_libio_rw_args_t *) arg;
+        
+        /* is link up? */
+        if ( !pDev->running ) {
+                return RTEMS_INVALID_NAME;
+        }
+        
+        if ((rw_args->count < 1) || (rw_args->buffer == NULL)) {
+                return RTEMS_INVALID_NAME;
+        }
+        
+        SPACEWIRE_DBGC(DBGSPW_IOCALLS, "read  [%i,%i]: buf:0x%x len:%i \n", major, minor, (unsigned int)rw_args->buffer, rw_args->count);
+        
+        while ( (count = grspw_hw_receive(pDev, rw_args->buffer, rw_args->count)) == 0) {
+		/* wait a moment for any descriptors to get available 
+		 * 
+		 * Semaphore is signaled by interrupt handler
+		 */
+		if (pDev->config.rx_blocking) {
+			SPACEWIRE_DBG2("Rx blocking\n");
+			rtems_semaphore_obtain(pDev->rxsp, RTEMS_WAIT, RTEMS_NO_TIMEOUT);
+		} else {
+                        SPACEWIRE_DBG2("Rx non blocking\n");
+			return RTEMS_RESOURCE_IN_USE;
+		}
+	}
+        
+#ifdef DEBUG_SPACEWIRE_ONOFF  
+        if (DEBUG_SPACEWIRE_FLAGS & DBGSPW_DUMP) {
+                int k;
+                for (k = 0; k < count; k++){
+                        if (k % 16 == 0) {
+                                printf ("\n");
+                        }
+                        printf ("%.2x(%c) ", rw_args->buffer[k] & 0xff, isprint(rw_args->buffer[k] & 0xff) ? rw_args->buffer[k] & 0xff : ' ');
+                }
+                printf ("\n");
+        }
+#endif
+        
+        rw_args->bytes_moved = count;
+        return RTEMS_SUCCESSFUL;
+        
+}
+
+static rtems_device_driver grspw_write(
+        rtems_device_major_number major,
+        rtems_device_minor_number minor,
+        void                    * arg
+)
+{
+        GRSPW_DEV *pDev = &grspw_devs[minor];
+        rtems_libio_rw_args_t *rw_args;
+        rw_args = (rtems_libio_rw_args_t *) arg;
+        SPACEWIRE_DBGC(DBGSPW_IOCALLS, "write [%i,%i]: buf:0x%x len:%i\n", major, minor, (unsigned int)rw_args->buffer, rw_args->count);
+
+        /* is link up? */
+        if ( !pDev->running ) {
+                return RTEMS_INVALID_NAME;
+        }
+
+        if ((rw_args->count > pDev->txdbufsize) || (rw_args->count < 1) || (rw_args->buffer == NULL)) {
+                return RTEMS_INVALID_NAME;
+        }
+
+        while ((rw_args->bytes_moved = grspw_hw_send(pDev, 0, NULL, rw_args->count, rw_args->buffer)) == 0) {
+                if (pDev->config.tx_block_on_full == 1) { 
+                        SPACEWIRE_DBG2("Tx Block on full \n");
+                        rtems_semaphore_obtain(pDev->txsp, RTEMS_WAIT, RTEMS_NO_TIMEOUT);
+                } else {
+                        SPACEWIRE_DBG2("Tx non blocking return when full \n");
+                        return RTEMS_RESOURCE_IN_USE;
+                }
+        }
+        return RTEMS_SUCCESSFUL;
+}
+
+static rtems_device_driver grspw_control(
+        rtems_device_major_number major,
+        rtems_device_minor_number minor,
+        void                    * arg
+        )
+{
+        GRSPW_DEV *pDev = &grspw_devs[minor];
+        spw_ioctl_pkt_send *args;
+        spw_ioctl_packetsize *ps;
+        int status;
+        unsigned int tmp;
+        int timeout;
+        rtems_device_driver ret;
+        rtems_libio_ioctl_args_t	*ioarg = (rtems_libio_ioctl_args_t *) arg;
+        SPACEWIRE_DBGC(DBGSPW_IOCALLS, "ctrl [%i,%i]\n", major, minor);
+        
+        if (!ioarg)
+                return RTEMS_INVALID_NAME;
+
+        
+        ioarg->ioctl_return = 0;
+        switch(ioarg->command) {
+                case SPACEWIRE_IOCTRL_SET_NODEADDR:
+                        /*set node address*/
+                        SPACEWIRE_DBGC(DBGSPW_IOCTRL, "SPACEWIRE_IOCTRL_SET_NODEADDR %i\n", (unsigned int)ioarg->buffer);
+                        if ((unsigned int)ioarg->buffer > 255) {
+                                return RTEMS_INVALID_NAME;
+                        }
+                        SPW_WRITE(&pDev->regs->nodeaddr, (unsigned int)ioarg->buffer);  
+                        if (SPW_READ(&pDev->regs->nodeaddr) != (unsigned int)ioarg->buffer) {
+                                return RTEMS_IO_ERROR;
+                        }
+                        pDev->config.nodeaddr = (unsigned int) ioarg->buffer;
+                        break;
+                case SPACEWIRE_IOCTRL_SET_RXBLOCK:
+                        SPACEWIRE_DBGC(DBGSPW_IOCTRL, "SPACEWIRE_IOCTRL_SET_RXBLOCK %i \n", (unsigned int)ioarg->buffer);
+                        if ((unsigned int)ioarg->buffer > 1) {
+                                return RTEMS_INVALID_NAME;
+                        }
+                        pDev->config.rx_blocking = (unsigned int)ioarg->buffer;
+                        break;
+                case SPACEWIRE_IOCTRL_SET_DESTKEY:
+                        SPACEWIRE_DBGC(DBGSPW_IOCTRL,"SPACEWIRE_IOCTRL_SET_DESTKEY %i\n", (unsigned int)ioarg->buffer);
+                        if (!pDev->config.is_rmap) {
+                                return RTEMS_NOT_IMPLEMENTED;
+                        }
+                        if ((unsigned int)ioarg->buffer > 255) {
+                                return RTEMS_INVALID_NAME;
+                        }
+                        SPW_WRITE(&pDev->regs->destkey, (unsigned int)ioarg->buffer);
+                        if (SPW_READ(&pDev->regs->destkey) != (unsigned int)ioarg->buffer) {
+                                return RTEMS_IO_ERROR;
+                        }
+                        pDev->config.destkey = (unsigned int)ioarg->buffer;
+                        break;
+                case SPACEWIRE_IOCTRL_SET_CLKDIV:
+                        SPACEWIRE_DBGC(DBGSPW_IOCTRL,"SPACEWIRE_IOCTRL_SET_CLKDIV %i\n", (unsigned int)ioarg->buffer);
+                        if ((unsigned int)ioarg->buffer > 255) {
+                                return RTEMS_INVALID_NAME;
+                        }
+                        SPW_WRITE(&pDev->regs->clkdiv, (unsigned int)ioarg->buffer);
+                        if (SPW_READ(&pDev->regs->clkdiv) != (unsigned int)ioarg->buffer) {
+                                return RTEMS_IO_ERROR;
+                        }
+                        pDev->config.clkdiv = (unsigned int)ioarg->buffer;
+                        break;
+                case SPACEWIRE_IOCTRL_SET_TIMER:
+                        SPACEWIRE_DBGC(DBGSPW_IOCTRL,"SPACEWIRE_IOCTRL_SET_TIMER %i\n", (unsigned int)ioarg->buffer);
+                        if ((unsigned int)ioarg->buffer > 4095) {
+                                return RTEMS_INVALID_NAME;
+                        }
+                        SPW_WRITE(&pDev->regs->timer, (SPW_READ(&pDev->regs->timer) & 0xFFFFF000) | ((unsigned int)ioarg->buffer & 0xFFF));
+                        if ((SPW_READ(&pDev->regs->timer) & 0xFFF) != (unsigned int)ioarg->buffer) {
+                                return RTEMS_IO_ERROR;
+                        }
+                        pDev->config.timer = (unsigned int)ioarg->buffer;
+                        break;
+                case SPACEWIRE_IOCTRL_SET_DISCONNECT:
+                        SPACEWIRE_DBGC(DBGSPW_IOCTRL,"SPACEWIRE_IOCTRL_SET_DISCONNECT %i\n", (unsigned int)ioarg->buffer);
+                        if ((unsigned int)ioarg->buffer > 1023) {
+                                return RTEMS_INVALID_NAME;
+                        }
+                        SPW_WRITE(&pDev->regs->timer, (SPW_READ(&pDev->regs->timer) & 0xFFC00FFF) | (((unsigned int)ioarg->buffer & 0x3FF) << 12));
+                        if (((SPW_READ(&pDev->regs->timer) >> 12) & 0x3FF) != (unsigned int)ioarg->buffer) {
+                                return RTEMS_IO_ERROR;
+                        }
+                        pDev->config.disconnect = (unsigned int)ioarg->buffer;
+                        break;
+                case SPACEWIRE_IOCTRL_SET_PROMISCUOUS:        
+                        SPACEWIRE_DBGC(DBGSPW_IOCTRL,"SPACEWIRE_IOCTRL_SET_PROMISCUOUS %i \n", (unsigned int)ioarg->buffer);
+                        if ((unsigned int)ioarg->buffer > 1) {
+                                return RTEMS_INVALID_NAME;
+                        }
+                        SPW_CTRL_WRITE(pDev, SPW_CTRL_READ(pDev) | ((unsigned int)ioarg->buffer << 5));
+                        if (((SPW_CTRL_READ(pDev) >> 5) & 1) != (unsigned int)ioarg->buffer) {
+                                return RTEMS_IO_ERROR;
+                        }
+                        pDev->config.promiscuous = (unsigned int)ioarg->buffer;
+                        break;
+                case SPACEWIRE_IOCTRL_SET_RMAPEN:
+                        SPACEWIRE_DBGC(DBGSPW_IOCTRL,"SPACEWIRE_IOCTRL_SET_RMAPEN %i \n", (unsigned int)ioarg->buffer);
+                        if ((unsigned int)ioarg->buffer > 1) {
+                                return RTEMS_INVALID_NAME;
+                        }
+                        SPW_CTRL_WRITE(pDev, (SPW_STATUS_READ(pDev) & 0xFFFEFFFF) | ((unsigned int)ioarg->buffer << 16));
+                        if (((SPW_CTRL_READ(pDev) >> 16) & 1) != (unsigned int)ioarg->buffer) {
+                                return RTEMS_IO_ERROR;
+                        }
+                        pDev->config.rmapen = (unsigned int)ioarg->buffer;
+                        break;
+                case SPACEWIRE_IOCTRL_SET_RMAPBUFDIS: 
+                        SPACEWIRE_DBGC(DBGSPW_IOCTRL,"SPACEWIRE_IOCTRL_SET_RMAPBUFDIS %i \n", (unsigned int)ioarg->buffer);
+                        if ((unsigned int)ioarg->buffer > 1) {
+                                return RTEMS_INVALID_NAME;
+                        }
+                        SPW_CTRL_WRITE(pDev, (SPW_STATUS_READ(pDev) & 0xFFFDFFFF) | ((unsigned int)ioarg->buffer << 17));
+                        if (((SPW_CTRL_READ(pDev) >> 17) & 1) != (unsigned int)ioarg->buffer) {
+                                return RTEMS_IO_ERROR;
+                        }
+                        pDev->config.rmapbufdis = (unsigned int)ioarg->buffer;
+                        break;
+                case SPACEWIRE_IOCTRL_SET_CHECK_RMAP: 
+                        SPACEWIRE_DBGC(DBGSPW_IOCTRL,"SPACEWIRE_IOCTRL_SET_CHECK_RMAP %i \n", (unsigned int)ioarg->buffer);
+                        if ((unsigned int)ioarg->buffer > 1) {
+                                return RTEMS_INVALID_NAME;
+                        }
+                        pDev->config.check_rmap_err = (unsigned int)ioarg->buffer;
+                        break;
+                case SPACEWIRE_IOCTRL_SET_RM_PROT_ID: 
+                        SPACEWIRE_DBGC(DBGSPW_IOCTRL, "SPACEWIRE_IOCTRL_SET_RM_PROT_ID %i \n", (unsigned int)ioarg->buffer);
+                        if ((unsigned int)ioarg->buffer > 1) {
+                                return RTEMS_INVALID_NAME;
+                        }
+                        pDev->config.rm_prot_id = (unsigned int)ioarg->buffer;
+                        break;
+                case SPACEWIRE_IOCTRL_SET_TXBLOCK: 
+                        SPACEWIRE_DBGC(DBGSPW_IOCTRL, "SPACEWIRE_IOCTRL_SET_TXBLOCK %i \n", (unsigned int)ioarg->buffer);
+                        if ((unsigned int)ioarg->buffer > 1) {
+                                return RTEMS_INVALID_NAME;
+                        }
+                        pDev->config.tx_blocking = (unsigned int)ioarg->buffer;
+                        break;
+                case SPACEWIRE_IOCTRL_SET_TXBLOCK_ON_FULL: 
+                        SPACEWIRE_DBGC(DBGSPW_IOCTRL, "SPACEWIRE_IOCTRL_SET_TXBLOCK_ON_FULL %i \n", (unsigned int)ioarg->buffer);
+                        if ((unsigned int)ioarg->buffer > 1) {
+                                return RTEMS_INVALID_NAME;
+                        }
+                        pDev->config.tx_block_on_full = (unsigned int)ioarg->buffer;
+                        break;        
+                case SPACEWIRE_IOCTRL_SET_DISABLE_ERR: 
+                        SPACEWIRE_DBGC(DBGSPW_IOCTRL, "SPACEWIRE_IOCTRL_SET_DISABLE_ERR %i \n", (unsigned int)ioarg->buffer);
+                        if ((unsigned int)ioarg->buffer > 1) {
+                                return RTEMS_INVALID_NAME;
+                        }
+                        pDev->config.disable_err = (unsigned int)ioarg->buffer;
+                        break;
+                case SPACEWIRE_IOCTRL_SET_LINK_ERR_IRQ: 
+                        SPACEWIRE_DBGC(DBGSPW_IOCTRL, "SPACEWIRE_IOCTRL_SET_LINK_ERR_IRQ %i \n", (unsigned int)ioarg->buffer);
+                        SPACEWIRE_DBGC(DBGSPW_IOCTRL, "CTRL REG: %x\n", SPW_CTRL_READ(pDev));
+                        if ((unsigned int)ioarg->buffer > 1) {
+                                return RTEMS_INVALID_NAME;
+                        }
+                        SPW_CTRL_WRITE(pDev, (SPW_CTRL_READ(pDev) & 0xFFFFFDF7) | ((unsigned int)ioarg->buffer << 9) | (pDev->config.link_err_irq << 3));
+                        SPACEWIRE_DBGC(DBGSPW_IOCTRL, "CTRL REG: %x\n", SPW_CTRL_READ(pDev));
+                        if (((SPW_CTRL_READ(pDev) >> 9) & 1) != (unsigned int)ioarg->buffer) {
+                                return RTEMS_IO_ERROR;
+                        }
+                        pDev->config.link_err_irq = (unsigned int)ioarg->buffer;
+                        break;
+                case SPACEWIRE_IOCTRL_SET_EVENT_ID:
+                        SPACEWIRE_DBGC(DBGSPW_IOCTRL, "SPACEWIRE_IOCTRL_SET_EVENT_ID %i \n", (unsigned int)ioarg->buffer);
+                        pDev->config.event_id = (rtems_id)ioarg->buffer;
+                        SPACEWIRE_DBGC(DBGSPW_IOCTRL, "Event id: %i\n", pDev->config.event_id);
+                        break;
+                        
+                /* Change MAX Packet size by:
+                 *  - stop RX/TX (if on)
+                 *  - wait for hw to complete RX DMA (if on)
+                 *  - reallocate buffers with new size
+                 *  - tell hw about new size & start RX/TX again (if previously on)
+                 */
+                case SPACEWIRE_IOCTRL_SET_PACKETSIZE:
+                        if (ioarg->buffer == NULL)
+                                return RTEMS_INVALID_NAME;
+                        ps = (spw_ioctl_packetsize*) ioarg->buffer;
+                        SPACEWIRE_DBGC(DBGSPW_IOCTRL,"SPACEWIRE_IOCTRL_SET_RXPACKETSIZE %i \n", (unsigned int)ioarg->buffer);
+                        
+                        tmp = pDev->running;
+                        
+                        if ( pDev->running ){
+                                /* Stop RX */
+                                grspw_hw_stop(pDev,1,1); 
+                                
+                                /* If packetsize fails it is good to know if in running mode */
+                                pDev->running = 0;
+                                
+                                /* Wait for Receiver to finnish pending DMA transfers if any */
+                                grspw_hw_wait_rx_inactive(pDev);
+                        }
+                        
+                        /* Save new buffer sizes */
+                        pDev->rxbufsize = ps->rxsize;
+                        pDev->txdbufsize = ps->txdsize;
+                        pDev->txhbufsize = ps->txhsize;
+                        pDev->config.rxmaxlen = pDev->rxbufsize;
+
+                        /* Free previous buffers & allocate buffers with new size */
+                        if (grspw_buffer_alloc(pDev)) 
+                                return RTEMS_NO_MEMORY;
+                        
+                        /* if RX was actived before, we reactive it again */
+                        if ( tmp ) {
+                                if ( (status = grspw_hw_startup(pDev,-1)) != RTEMS_SUCCESSFUL ) {
+                                        return status;
+                                }
+                                pDev->running = 1;
+                        }
+#if 0
+                        /* Rewrite previous config which was wasted due to reset in hw_startup */
+                        SPW_WRITE(&pDev->regs->nodeaddr, pDev->config.nodeaddr);
+                        SPW_WRITE(&pDev->regs->destkey, pDev->config.destkey);
+                        SPW_WRITE(&pDev->regs->clkdiv, pDev->config.clkdiv);
+                        SPW_WRITE(&pDev->regs->timer, pDev->config.timer | ( (pDev->config.disconnect & 0x3FF) << 12) );
+                        SPW_CTRL_WRITE(pDev, (SPW_CTRL_READ(pDev) & !(SPW_CTRL_LINKSTART | SPW_CTRL_PM | SPW_CTRL_RE | SPW_CTRL_RD | SPW_CTRL_TT | SPW_CTRL_TR)) | \
+                                              (pDev->config.promiscuous << 5) | (pDev->config.rmapen << 16) | (pDev->config.rmapbufdis << 17) | \
+                                              (pDev->config.linkdisabled) | (pDev->config.linkstart << 1));
+#endif
+                        break;
+                case SPACEWIRE_IOCTRL_GET_CONFIG:
+                        if (ioarg->buffer == NULL)
+                                return RTEMS_INVALID_NAME;
+                        SPACEWIRE_DBG2("SPACEWIRE_IOCTRL_GET_CONFIG \n");
+                        (*(spw_config *)ioarg->buffer).nodeaddr = pDev->config.nodeaddr;
+                        (*(spw_config *)ioarg->buffer).destkey = pDev->config.destkey;
+                        (*(spw_config *)ioarg->buffer).clkdiv = pDev->config.clkdiv;
+                        (*(spw_config *)ioarg->buffer).rxmaxlen = pDev->config.rxmaxlen;
+                        (*(spw_config *)ioarg->buffer).timer = pDev->config.timer;
+                        (*(spw_config *)ioarg->buffer).disconnect = pDev->config.disconnect;
+                        (*(spw_config *)ioarg->buffer).promiscuous = pDev->config.promiscuous;
+                        (*(spw_config *)ioarg->buffer).rmapen = pDev->config.rmapen;
+                        (*(spw_config *)ioarg->buffer).rmapbufdis = pDev->config.rmapbufdis;
+                        (*(spw_config *)ioarg->buffer).check_rmap_err = pDev->config.check_rmap_err;
+                        (*(spw_config *)ioarg->buffer).rm_prot_id = pDev->config.rm_prot_id;
+                        (*(spw_config *)ioarg->buffer).tx_blocking = pDev->config.tx_blocking;
+                        (*(spw_config *)ioarg->buffer).disable_err = pDev->config.disable_err;
+                        (*(spw_config *)ioarg->buffer).link_err_irq = pDev->config.link_err_irq;
+                        (*(spw_config *)ioarg->buffer).event_id = pDev->config.event_id;
+                        (*(spw_config *)ioarg->buffer).is_rmap = pDev->config.is_rmap;
+                        (*(spw_config *)ioarg->buffer).is_rmapcrc = pDev->config.is_rmapcrc;
+                        (*(spw_config *)ioarg->buffer).is_rxunaligned = pDev->config.is_rxunaligned;
+                        (*(spw_config *)ioarg->buffer).linkdisabled = pDev->config.linkdisabled;
+                        (*(spw_config *)ioarg->buffer).linkstart = pDev->config.linkstart;
+                        (*(spw_config *)ioarg->buffer).rx_blocking = pDev->config.rx_blocking;
+                        (*(spw_config *)ioarg->buffer).tx_block_on_full = pDev->config.tx_block_on_full;
+                        break;
+                case SPACEWIRE_IOCTRL_GET_LINK_STATUS:
+                        SPACEWIRE_DBGC(DBGSPW_IOCTRL,"SPACEWIRE_IOCTRL_GET_STATUS=%i \n", (unsigned int)((SPW_STATUS_READ(pDev) >> 21) & 0x7));
+                        *(unsigned int *)ioarg->buffer = (unsigned int )((SPW_STATUS_READ(pDev) >> 21) & 0x7);
+                        break;
+                case SPACEWIRE_IOCTRL_GET_STATISTICS: 
+                        if (ioarg->buffer == NULL)
+                                return RTEMS_INVALID_NAME;
+                        SPACEWIRE_DBG2("SPACEWIRE_IOCTRL_GET_STATISTICS \n");
+                        (*(spw_stats *)ioarg->buffer).tx_link_err = pDev->stat.tx_link_err;
+                        (*(spw_stats *)ioarg->buffer).rx_rmap_header_crc_err = pDev->stat.rx_rmap_header_crc_err;
+                        (*(spw_stats *)ioarg->buffer).rx_rmap_data_crc_err = pDev->stat.rx_rmap_data_crc_err;
+                        (*(spw_stats *)ioarg->buffer).rx_eep_err =  pDev->stat.rx_eep_err;
+                        (*(spw_stats *)ioarg->buffer).rx_truncated = pDev->stat.rx_truncated;
+                        (*(spw_stats *)ioarg->buffer).parity_err = pDev->stat.parity_err;
+                        (*(spw_stats *)ioarg->buffer).escape_err = pDev->stat.escape_err;
+                        (*(spw_stats *)ioarg->buffer).credit_err = pDev->stat.credit_err;
+                        (*(spw_stats *)ioarg->buffer).write_sync_err = pDev->stat.write_sync_err;
+                        (*(spw_stats *)ioarg->buffer).disconnect_err = pDev->stat.disconnect_err;
+                        (*(spw_stats *)ioarg->buffer).early_ep = pDev->stat.early_ep;
+                        (*(spw_stats *)ioarg->buffer).invalid_address = pDev->stat.invalid_address;
+                        (*(spw_stats *)ioarg->buffer).packets_sent = pDev->stat.packets_sent;
+                        (*(spw_stats *)ioarg->buffer).packets_received = pDev->stat.packets_received;
+                        break;
+                case SPACEWIRE_IOCTRL_CLR_STATISTICS:
+                        SPACEWIRE_DBG2("SPACEWIRE_IOCTRL_CLR_STATISTICS \n");
+                        pDev->stat.tx_link_err = 0;
+                        pDev->stat.rx_rmap_header_crc_err = 0;
+                        pDev->stat.rx_rmap_data_crc_err = 0;
+                        pDev->stat.rx_eep_err = 0;
+                        pDev->stat.rx_truncated = 0;
+                        pDev->stat.parity_err = 0;
+                        pDev->stat.escape_err = 0;
+                        pDev->stat.credit_err = 0;
+                        pDev->stat.write_sync_err = 0;
+                        pDev->stat.disconnect_err = 0;
+                        pDev->stat.early_ep = 0;
+                        pDev->stat.invalid_address = 0;
+                        pDev->stat.packets_sent = 0;
+                        pDev->stat.packets_received = 0;
+                        break;
+                case SPACEWIRE_IOCTRL_SEND:
+                        if (ioarg->buffer == NULL)
+                                return RTEMS_INVALID_NAME;
+                        args = (spw_ioctl_pkt_send *)ioarg->buffer;
+                        args->sent = 0;
+                        
+                        /* is link up? */
+                        if ( !pDev->running ) {
+                                return RTEMS_INVALID_NAME;
+                        }
+        
+                        SPACEWIRE_DBGC(DBGSPW_IOCALLS, "write [%i,%i]: hlen: %i hbuf:0x%x dlen:%i dbuf:0x%x\n", major, minor, 
+                                       (unsigned int)args->hlen, (int)args->hdr,(unsigned int)args->dlen, (int)args->data);
+                        
+                        if ((args->hlen > pDev->txhbufsize) || (args->dlen > pDev->txdbufsize) || 
+                            ((args->hlen+args->dlen) < 1) || 
+                            ((args->hdr == NULL) && (args->hlen != 0)) || ((args->data == NULL) && (args->dlen != 0))) {
+                                return RTEMS_INVALID_NAME;
+                        }
+                        while ((args->sent = grspw_hw_send(pDev, args->hlen, args->hdr, args->dlen, args->data)) == 0) {
+                                if (pDev->config.tx_block_on_full == 1) { 
+                                        SPACEWIRE_DBG2("Tx Block on full \n");
+                                        rtems_semaphore_obtain(pDev->txsp, RTEMS_WAIT, RTEMS_NO_TIMEOUT);
+                                } else {
+                                        SPACEWIRE_DBG2("Tx non blocking return when full \n");
+                                        return RTEMS_RESOURCE_IN_USE;
+                                }
+                        }
+                        SPACEWIRE_DBGC(DBGSPW_IOCALLS, "Tx ioctl return: %i  \n", args->sent);
+                        break;
+
+                case SPACEWIRE_IOCTRL_LINKDISABLE:
+                        pDev->config.linkdisabled = 1;
+                        pDev->config.linkstart = 0;
+                        SPW_CTRL_WRITE(pDev, (SPW_CTRL_READ(pDev) & 0xFFFFFFFC) | 1);
+                        if ((SPW_CTRL_READ(pDev) & 3) != 1) {
+                                return RTEMS_IO_ERROR;
+                        }
+                        break;
+
+                case SPACEWIRE_IOCTRL_LINKSTART:
+                        pDev->config.linkdisabled = 0;
+                        pDev->config.linkstart = 1;
+                        SPW_CTRL_WRITE(pDev, (SPW_CTRL_READ(pDev) & 0xFFFFFFFC) | 2);
+                        if ((SPW_CTRL_READ(pDev) & 3) != 2) {
+                                return RTEMS_IO_ERROR;
+                        }
+                        break;
+                
+                /* Calculate timer register from GRSPW Core frequency 
+                 * Also possible to set disconnect and timer64 from
+                 *  - SPACEWIRE_IOCTRL_SET_DISCONNECT
+                 *  - SPACEWIRE_IOCTRL_SET_TIMER
+                 */
+                case SPACEWIRE_IOCTRL_SET_COREFREQ:
+                        pDev->core_freq_khz = (unsigned int)ioarg->buffer;
+                        if ( pDev->core_freq_khz == 0 ){
+                                /* Get GRSPW clock frequency from system clock.
+                                 * System clock has been read from timer inited
+                                 * by RTEMS loader (mkprom)
+                                 */
+                                 pDev->core_freq_khz = sys_freq_khz;
+                        }
+                        
+                        /* Calculate Timer64 & Disconnect */
+                        pDev->config.timer = grspw_calc_timer64(pDev->core_freq_khz);
+                        pDev->config.disconnect = grspw_calc_disconnect(pDev->core_freq_khz);
+                        
+                        /* Set Timer64 & Disconnect Register */
+                        SPW_WRITE(&pDev->regs->timer, 
+                                 (SPW_READ(&pDev->regs->timer) & 0xFFC00000) |
+                                 ((pDev->config.disconnect & 0x3FF)<<12) |
+                                 (pDev->config.timer & 0xFFF));
+                        
+                        /* Check that the registers were written successfully */
+                        tmp = SPW_READ(&pDev->regs->timer) & 0x003fffff;
+                        if ( ((tmp & 0xFFF) != pDev->config.timer) ||
+                             (((tmp >> 12) & 0x3FF) != pDev->config.disconnect) ) {
+                                return RTEMS_IO_ERROR;
+                        }
+                        break;
+                        
+                case SPACEWIRE_IOCTRL_START:
+                        if ( pDev->running ){
+                                return RTEMS_INVALID_NAME;
+                        }
+                        
+                        /* Get timeout from userspace
+                         *  timeout:
+                         *   ¤  -1           = Default timeout
+                         *   ¤  less than -1 = forever
+                         *   ¤  0            = no wait, proceed if link is up
+                         *   ¤  positive     = specifies number of system clock ticks that 
+                         *                     startup will wait for link to enter ready mode.
+                         */
+                        timeout = (int)ioarg->buffer;
+                        
+                        if ( (ret=grspw_hw_startup(pDev,timeout)) != RTEMS_SUCCESSFUL ) {
+                                return ret;
+                        }
+                        pDev->running = 1;
+                        break;
+                
+                case SPACEWIRE_IOCTRL_STOP:
+                        if ( !pDev->running ){
+                                return RTEMS_INVALID_NAME;
+                        }
+                        pDev->running = 0;
+                        
+                        /* Stop Receiver and transmitter */
+                        grspw_hw_stop(pDev,1,1);
+                        break;
+                
+                default:
+                        return RTEMS_NOT_IMPLEMENTED;
+        }
+                   
+        SPACEWIRE_DBGC(DBGSPW_IOCALLS, "SPW_IOCTRL Return\n");
+        return RTEMS_SUCCESSFUL;
+}
+
+/* ============================================================================== */
+
+static int grspw_set_rxmaxlen(GRSPW_DEV *pDev) {
+        unsigned int rxmax;
+        SPW_WRITE(&pDev->regs->dma0rxmax,pDev->config.rxmaxlen); /*set rx maxlength*/
+        rxmax = SPW_READ(&pDev->regs->dma0rxmax);
+        if (rxmax != pDev->config.rxmaxlen) {
+                return 0;
+        }
+        return 1;
+}
+
+static int grspw_hw_init(GRSPW_DEV *pDev) {
+        unsigned int ctrl;
+        
+        ctrl = SPW_CTRL_READ(pDev);
+
+#ifdef GRSPW_STATIC_MEM
+        pDev->rx = (SPACEWIRE_RXBD *) pDev->mem_bdtable;
+        pDev->tx = (SPACEWIRE_RXBD *) pDev->mem_bdtable + SPACEWIRE_BDTABLE_SIZE;
+#else
+        pDev->rx = (SPACEWIRE_RXBD *) SPW_ALIGN(&pDev->_rxtable, SPACEWIRE_BDTABLE_SIZE);
+        pDev->tx = (SPACEWIRE_TXBD *) SPW_ALIGN(&pDev->_txtable, SPACEWIRE_BDTABLE_SIZE);
+#endif
+        SPACEWIRE_DBG("hw_init [minor %i]\n", pDev->minor);
+        
+        pDev->config.is_rmap = ctrl & SPW_CTRL_RA;
+        pDev->config.is_rxunaligned = ctrl & SPW_CTRL_RX;
+        pDev->config.is_rmapcrc = ctrl & SPW_CTRL_RC;
+        return 0;
+}
+
+static int grspw_hw_waitlink (GRSPW_DEV *pDev, int timeout) 
+{
+        int j;
+        
+        if ( timeout == -1 ){
+                /* Wait default timeout */
+                timeout = SPACEWIRE_INIT_TIMEOUT;
+        }
+        
+        j=0;
+        while (SPW_LINKSTATE(SPW_STATUS_READ(pDev)) != 5) {
+                if ( timeout < -1 ) {
+                        /* wait forever */
+                }else if ( j >= timeout ){
+                        /* timeout reached, return fail */
+                        return 1;
+                }
+                
+                /* Sleep for 10 ticks */
+                rtems_task_wake_after(10);
+                j+=10;
+        }
+        return 0;
+}
+
+static void grspw_hw_reset(GRSPW_DEV *pDev)
+{
+        SPW_CTRL_WRITE(pDev, SPW_CTRL_RESET); /*reset core*/
+        SPW_STATUS_WRITE(pDev, SPW_STATUS_TO | SPW_STATUS_CE | SPW_STATUS_ER | SPW_STATUS_DE | SPW_STATUS_PE | 
+                         SPW_STATUS_WE | SPW_STATUS_IA | SPW_STATUS_EE); /*clear status*/
+        SPW_CTRL_WRITE(pDev, SPW_CTRL_LINKSTART); /*start link core*/
+}
+
+static void grspw_hw_read_config(GRSPW_DEV *pDev)
+{
+        unsigned int tmp;
+        
+        pDev->config.nodeaddr = 0xFF & SPW_READ(&pDev->regs->nodeaddr);
+        pDev->config.destkey = 0xFF & SPW_READ(&pDev->regs->destkey);
+        pDev->config.clkdiv = 0xFF & SPW_READ(&pDev->regs->clkdiv);
+        
+        tmp = SPW_CTRL_READ(pDev);
+        pDev->config.promiscuous = 1 & (tmp >> 5);
+        pDev->config.rmapen = 1 & (tmp >> 16);
+        pDev->config.rmapbufdis = 1 & (tmp >> 17);
+        pDev->config.is_rmap = 1 & (tmp >> 31);
+        pDev->config.is_rxunaligned = 1 & (tmp >> 30);
+        pDev->config.is_rmapcrc = 1 & (tmp >> 29);
+        pDev->config.linkdisabled = 1 & tmp;
+        pDev->config.linkstart = 1 & (tmp >> 1);
+        
+        tmp = SPW_READ(&pDev->regs->timer);
+        pDev->config.timer = 0xFFF & tmp;
+        pDev->config.disconnect = 0x3FF & (tmp >> 12);
+        
+        return;
+}
+
+/* timeout is given in ticks */
+static int grspw_hw_startup (GRSPW_DEV *pDev, int timeout)
+{
+        int i;
+        unsigned int dmactrl;
+        
+        SPW_WRITE(&pDev->regs->status, (SPW_STATUS_TO|SPW_STATUS_CE|SPW_STATUS_ER|SPW_STATUS_DE|SPW_STATUS_PE|SPW_STATUS_WE|SPW_STATUS_IA|SPW_STATUS_EE)); /*clear status*/
+        
+        if (grspw_hw_waitlink(pDev,timeout)) {
+                SPACEWIRE_DBG2("Device open. Link is not up\n");
+                return RTEMS_TIMEOUT;
+        }
+        
+        SPW_WRITE(&pDev->regs->dma0ctrl, SPW_DMACTRL_PS | SPW_DMACTRL_PR | SPW_DMACTRL_TA | SPW_DMACTRL_RA); /*clear status, set ctrl*/
+        
+        
+        if ((dmactrl = SPW_READ(&pDev->regs->dma0ctrl)) != 0) {
+                SPACEWIRE_DBG2("DMACtrl is not cleared\n");
+                return RTEMS_IO_ERROR;
+        }
+
+        /* prepare transmit buffers */
+        for (i = 0; i < pDev->txbufcnt; i++) {
+                pDev->tx[i].ctrl = 0;
+                pDev->tx[i].addr_header = memarea_to_hw(((unsigned int)&pDev->ptr_txhbuf0[0]) + (i * pDev->txhbufsize));
+                pDev->tx[i].addr_data = memarea_to_hw(((unsigned int)&pDev->ptr_txdbuf0[0]) + (i * pDev->txdbufsize));
+        }
+        pDev->tx_cur = 0;
+        pDev->tx_sent = 0;
+        pDev->tx_all_in_use = 0;
+        
+        /* prepare receive buffers */
+        for (i = 0; i < pDev->rxbufcnt; i++) {
+                if (i+1 == pDev->rxbufcnt) {
+                        pDev->rx[i].ctrl = SPW_RXBD_IE | SPW_RXBD_EN | SPW_RXBD_WR;
+                } else {
+                        pDev->rx[i].ctrl = SPW_RXBD_IE | SPW_RXBD_EN;
+                }
+                pDev->rx[i].addr = memarea_to_hw(((unsigned int)&pDev->ptr_rxbuf0[0]) + (i * pDev->rxbufsize));
+        }
+        pDev->rxcur = 0;
+        pDev->rxbufcur = -1;
+        grspw_set_rxmaxlen(pDev);
+        
+        SPW_WRITE(&pDev->regs->dma0txdesc, memarea_to_hw((unsigned int) pDev->tx));
+        SPW_WRITE(&pDev->regs->dma0rxdesc, memarea_to_hw((unsigned int) pDev->rx));
+        
+        /* start RX */
+        dmactrl = SPW_READ(&pDev->regs->dma0ctrl);
+        SPW_WRITE(&pDev->regs->dma0ctrl, (dmactrl & SPW_PREPAREMASK_RX) | SPW_DMACTRL_RD | SPW_DMACTRL_RXEN | SPW_DMACTRL_NS | SPW_DMACTRL_TXIE | SPW_DMACTRL_RXIE);
+        
+        SPACEWIRE_DBGC(DBGSPW_TX,"0x%x: setup complete\n", (unsigned int)pDev->regs);
+        return RTEMS_SUCCESSFUL;
+}
+
+/* Wait until the receiver is inactive */
+static void grspw_hw_wait_rx_inactive(GRSPW_DEV *pDev)
+{
+        while( SPW_READ(&pDev->regs->dma0ctrl) & SPW_DMACTRL_RX ){
+                /* switching may be needed: 
+                 *  - low frequency GRSPW 
+                 *  - mega packet incoming
+                 */
+                rtems_task_wake_after(1);
+        }
+}
+
+/* Stop the rx or/and tx by disabling the receiver/transmitter */
+static int grspw_hw_stop (GRSPW_DEV *pDev, int rx, int tx) 
+{
+        unsigned int dmactrl;
+  
+        /* stop rx and/or tx */
+        dmactrl = SPW_READ(&pDev->regs->dma0ctrl);
+        if ( rx ) {
+                dmactrl &= ~(SPW_DMACTRL_RXEN|SPW_DMACTRL_RXIE|SPW_DMACTRL_RD);
+        }
+        if ( tx ) {
+                dmactrl &= ~(SPW_DMACTRL_TXEN|SPW_DMACTRL_TXIE);
+        }
+        /*SPW_WRITE(&pDev->regs->dma0ctrl, (dmactrl & SPW_PREPAREMASK_RX) & ~(SPW_DMACTRL_RD | SPW_DMACTRL_RXEN) & ~(SPW_DMACTRL_TXEN));*/
+        
+        /* don't clear status flags */
+        dmactrl &= ~(SPW_DMACTRL_RA|SPW_DMACTRL_PR|SPW_DMACTRL_AI);
+        SPW_WRITE(&pDev->regs->dma0ctrl, dmactrl);
+        return RTEMS_SUCCESSFUL;
+}
+
+int grspw_hw_send(GRSPW_DEV *pDev, unsigned int hlen, unsigned char *hdr, unsigned int dlen, unsigned char *data) 
+{
+        
+        unsigned int dmactrl, ctrl;
+#ifdef DEBUG_SPACEWIRE_ONOFF
+				unsigned int k;
+#endif
+        rtems_interrupt_level level;
+        unsigned int cur = pDev->tx_cur;
+        char *txh = pDev->ptr_txhbuf0 + (cur * pDev->txhbufsize);
+        char *txd = pDev->ptr_txdbuf0 + (cur * pDev->txdbufsize);
+        
+        ctrl = SPW_READ((volatile void *)&pDev->tx[cur].ctrl);
+
+        if (ctrl & SPW_TXBD_EN) {
+                return 0;
+        }
+        
+        memcpy(&txd[0], data, dlen);
+        memcpy(&txh[0], hdr, hlen);
+
+#ifdef DEBUG_SPACEWIRE_ONOFF  
+        if (DEBUG_SPACEWIRE_FLAGS & DBGSPW_DUMP) {
+                for (k = 0; k < hlen; k++){
+                        if (k % 16 == 0) {
+                                printf ("\n");
+                        }
+                        printf ("%.2x(%c) ",txh[k] & 0xff,isprint(txh[k] & 0xff) ? txh[k] & 0xff : ' ');
+                }
+                printf ("\n");
+        }
+        if (DEBUG_SPACEWIRE_FLAGS & DBGSPW_DUMP) {
+                for (k = 0; k < dlen; k++){
+                        if (k % 16 == 0) {
+                                printf ("\n");
+                        }
+                        printf ("%.2x(%c) ",txd[k] & 0xff,isprint(txd[k] & 0xff) ? txd[k] & 0xff : ' ');
+                }
+                printf ("\n");
+        }
+#endif
+        
+        pDev->tx[cur].addr_header = memarea_to_hw((unsigned int)txh);
+        pDev->tx[cur].len = dlen;
+        pDev->tx[cur].addr_data = memarea_to_hw((unsigned int)txd);
+        if (pDev->tx_cur == (pDev->txbufcnt - 1) ) {
+                pDev->tx[cur].ctrl = SPW_TXBD_IE | SPW_TXBD_WR | SPW_TXBD_EN | hlen;
+        } else {
+                pDev->tx[cur].ctrl = SPW_TXBD_IE | SPW_TXBD_EN | hlen;
+        }
+          
+        dmactrl = SPW_READ(&pDev->regs->dma0ctrl);
+        SPW_WRITE(&pDev->regs->dma0ctrl, (dmactrl & SPW_PREPAREMASK_TX) | SPW_DMACTRL_TXEN | SPW_DMACTRL_TXIE);
+       
+        /* Update counters */
+        rtems_interrupt_disable(level);
+        
+        pDev->tx_cur = (pDev->tx_cur + 1) % pDev->txbufcnt;
+        if (pDev->tx_cur == pDev->tx_sent) {
+                pDev->tx_all_in_use = 1;
+        }
+        rtems_interrupt_enable(level);
+        
+        /* In blocking mode wait until message is sent */
+	if (pDev->config.tx_blocking) {
+		while ( SPW_READ(&pDev->regs->dma0ctrl) & SPW_DMACTRL_TXEN) {
+			/* if changed to blocking mode  */
+                        SPACEWIRE_DBGC(DBGSPW_TX, "Tx blocking\n");
+                        rtems_semaphore_obtain(pDev->txsp, RTEMS_WAIT, RTEMS_NO_TIMEOUT);
+		}
+	}
+        SPACEWIRE_DBGC(DBGSPW_TX, "0x%x: transmitted <%i> bytes\n", (unsigned int) pDev->regs, dlen+hlen);
+        return hlen + dlen;
+}
+
+static int grspw_hw_receive(GRSPW_DEV *pDev, unsigned char *b, int c) {
+        unsigned int len, rxlen, ctrl;
+        unsigned int cur; 
+        unsigned int tmp;
+        unsigned int dump_start_len;
+        int i;
+        char *rxb;  
+        
+        if ( pDev->config.promiscuous ) {
+		dump_start_len = 0; /* make sure address and prot can be read in promiscuous mode */
+	} else if (pDev->config.rm_prot_id) {
+		dump_start_len = 2; /* skip source address and protocol id */
+	} else {
+		dump_start_len = 1; /* default: skip only source address */
+	}
+        
+        rxlen = 0;
+        cur = pDev->rxcur;
+        rxb = pDev->ptr_rxbuf0 + (cur * pDev->rxbufsize);
+
+        SPACEWIRE_DBGC(DBGSPW_RX, "0x%x: waitin packet at pos %i\n", (unsigned int) pDev->regs, cur);
+        
+        ctrl = SPW_READ((volatile void *)&pDev->rx[cur].ctrl);
+        if (ctrl & SPW_RXBD_EN) {
+                return rxlen;
+        }
+        SPACEWIRE_DBGC(DBGSPW_RX, "checking packet\n");
+        
+        len = SPW_RXBD_LENGTH & ctrl;
+        if (!((ctrl & SPW_RXBD_ERROR) || (pDev->config.check_rmap_err && (ctrl & SPW_RXBD_RMAPERROR)))) {
+                if (pDev->rxbufcur == -1) {
+                        SPACEWIRE_DBGC(DBGSPW_RX, "incoming packet len %i\n", len);
+                        pDev->stat.packets_received++;
+                        pDev->rxbufcur = dump_start_len;
+                }
+                rxlen = tmp = len - pDev->rxbufcur;
+                SPACEWIRE_DBGC(DBGSPW_RX, "C %i\n", c);
+                SPACEWIRE_DBGC(DBGSPW_RX, "Dump %i\n", dump_start_len);
+                SPACEWIRE_DBGC(DBGSPW_RX, "Bufcur %i\n", pDev->rxbufcur);
+                SPACEWIRE_DBGC(DBGSPW_RX, "Rxlen %i\n", rxlen );
+                if (rxlen > c) {
+                        rxlen = c;
+                }
+                if (CPU_SPARC_HAS_SNOOPING) {
+                        memcpy(b, rxb+pDev->rxbufcur, rxlen);
+                } else {
+                        for(i = 0; i < rxlen; i++) {
+                                b[i] = MEM_READ(rxb+pDev->rxbufcur);
+                        }
+                }
+                
+                pDev->rxbufcur += rxlen;
+                if (c >= tmp) {
+                        SPACEWIRE_DBGC(DBGSPW_RX, "Next descriptor\n");
+                        grspw_rxnext(pDev);
+                }
+        } else {
+                check_rx_errors(pDev, ctrl);
+        }
+        return rxlen;
+}
+
+static void grspw_rxnext(GRSPW_DEV *pDev) 
+{
+        unsigned int dmactrl;
+        unsigned int cur = pDev->rxcur;
+        unsigned int ctrl = 0;
+        if (cur == (pDev->rxbufcnt - 1)) {
+                pDev->rx[cur].ctrl = ctrl | SPW_RXBD_EN | SPW_RXBD_IE | SPW_RXBD_WR;
+                cur = 0;
+        } else {
+                pDev->rx[cur].ctrl = ctrl | SPW_RXBD_EN | SPW_RXBD_IE;
+                cur++;
+        }
+                
+        pDev->rxcur = cur;
+        pDev->rxbufcur = -1;
+        
+        /* start RX */
+        dmactrl = SPW_READ(&pDev->regs->dma0ctrl);
+        SPW_WRITE(&pDev->regs->dma0ctrl, (dmactrl & SPW_PREPAREMASK_RX) | SPW_DMACTRL_RD | SPW_DMACTRL_RXEN | SPW_DMACTRL_RXIE | SPW_DMACTRL_NS);
+  
+}
+
+static void check_rx_errors(GRSPW_DEV *pDev, int ctrl) 
+{
+        if (ctrl & SPW_RXBD_EEP) {
+                pDev->stat.rx_eep_err++;
+        }
+        if (ctrl & SPW_RXBD_EHC) {
+                if (pDev->config.check_rmap_err) {
+                        pDev->stat.rx_rmap_header_crc_err++;
+                }
+        }  
+        if (ctrl & SPW_RXBD_EDC) {
+                if (pDev->config.check_rmap_err) {
+                        pDev->stat.rx_rmap_data_crc_err++;
+                }
+        }
+        if (ctrl & SPW_RXBD_ETR) {
+                pDev->stat.rx_truncated++;
+        }
+}
+
+
diff --git a/c/src/lib/libbsp/sparc/shared/spw/grspw_pci.c b/c/src/lib/libbsp/sparc/shared/spw/grspw_pci.c
new file mode 100644
index 0000000000..0f83146bdd
--- /dev/null
+++ b/c/src/lib/libbsp/sparc/shared/spw/grspw_pci.c
@@ -0,0 +1,126 @@
+/* Select PCI driver */
+#define GRSPW_PCI
+
+#undef GRSPW_MAXDEVS
+#undef DEBUG_SPACEWIRE_ONOFF
+
+/* Only Malloced memory supported
+ */
+#undef GRSPW_LOCAL_MEM
+
+/* memory must be aligned to a 128k boundary */
+unsigned int grspwpci_memarea_address;
+#define GRSPW_LOCAL_MEM_ADR grspwpci_memarea_address
+
+/* We have custom address tranlation for HW addresses */
+#define GRSPW_ADR_TO
+
+/* MEMAREA=>CPU used when reading descriptor buffer pointers,
+ * they need to be translated from adresses used by GRSPW HW
+ * into CPU readable addresses.
+ *
+ * NOT NEEDED AS GRSPW DRIVER USES INDEXES TO GET DESCRIPTOR
+ * DATA POINTER ADDRESSES.
+ */
+#undef GRSPW_ADR_FROM
+
+/* Set registered device name */
+#define GRSPW_DEVNAME "/dev/grspwpci0"
+#define GRSPW_DEVNAME_NO(devstr,no) ((devstr)[13]='0'+(no))
+
+/* Any non-static function will begin with */
+#define GRSPW_PREFIX(name) grspwpci##name 
+
+/* do nothing, assume that the interrupt handler is called 
+ * setup externally calling b1553_interrupt_handler.
+ */
+#define GRSPW_REG_INT(handler,irq,arg) \
+ if ( grspw_pci_int_reg ) \
+   grspw_pci_int_reg(handler,irq,arg);
+
+void (*grspw_pci_int_reg)(void *handler, int irq, void *arg) = 0;
+
+ 
+#ifdef GRSPW_ADR_TO
+/* Translate an address within the Memory Region (memarea) into an Hardware
+ * device address. This address is put into hardware registers or descriptors
+ * so that the hardware can access the Memory Region.
+ * Example:
+ * A local AMBA access at 0xe0000000 will translate into PCI address 0x40000000,
+ * the PCI address 0x40000000 will translate into LEON-AMBA address 0x40000000.
+ */
+unsigned int grspwpci_hw_address;
+static inline unsigned int memarea_to_hw(unsigned int addr) {
+		/* don't translate? */
+		if ( grspwpci_hw_address == 0xffffffff )
+			return addr;
+    return ((addr & 0x0fffffff) | grspwpci_hw_address);
+}
+#endif
+
+/* not used since BRM Core work with offsets */
+#ifdef GRSPW_ADR_FROM
+unsigned int grspwpci_cpu_access_address;
+static inline unsigned int hw_to_cpu(unsigned int addr) {
+		/* don't translate? */
+		if ( grspwpci_cpu_address == 0xffffffff )
+			return addr;
+    return ((addr & 0x0fffffff) | grspwpci_cpu_address);
+}
+#endif
+
+int grspwpci_interrupt_handler(int irq, void *arg);
+#include "grspw.c"
+
+/*
+ *
+ * memarea     = preallocated memory somewhere, pointer to start of memory.
+ * hw_address  = how to translate a memarea address into an HW device AMBA address.
+ */
+
+int grspw_pci_register(
+ amba_confarea_type *bus, 
+ unsigned int memarea,
+ unsigned int hw_address
+ )
+{
+	/* Setup configuration */
+	
+	/* if zero the malloc will be used */
+	grspwpci_memarea_address = memarea;
+
+	grspwpci_hw_address = hw_address;
+
+#ifdef GRSPW_ADR_FROM
+	grspwpci_cpu_address = memarea & 0xf0000000;
+#endif
+	
+	/* Register the driver */
+	return GRSPW_PREFIX(_register)(bus);
+}
+
+/* Call this from PCI interrupt handler 
+ * irq = the irq number of the HW device local to that IRQMP controller
+ * 
+ */
+int grspwpci_interrupt_handler(int irq, void *arg){
+	grspw_interrupt( (GRSPW_DEV *)arg );
+}
+
+#if 0
+int grspw_pci_interrupt_handler(int irqmask){
+	int i;
+	unsigned int mask=0;
+	/* find minor */
+	for(i=0; i<spw_cores; i++){
+		if ( (1<<SPW_PARAM(i).irq) & irqmask ){
+			mask |= 1<<SPW_PARAM(i).irq;
+			grspw_interrupt(i);
+			/* more interrupts to scan for? */
+			if ( irqmask & ~mask )
+				return mask; /* handled */
+		}
+	}
+	return mask;
+}
+#endif
diff --git a/c/src/lib/libbsp/sparc/shared/spw/grspw_rasta.c b/c/src/lib/libbsp/sparc/shared/spw/grspw_rasta.c
new file mode 100644
index 0000000000..646f86c6e1
--- /dev/null
+++ b/c/src/lib/libbsp/sparc/shared/spw/grspw_rasta.c
@@ -0,0 +1,153 @@
+/* Select PCI driver */
+#define GRSPW_PCI
+
+#undef GRSPW_MAXDEVS
+#undef DEBUG_SPACEWIRE_ONOFF
+/*#define DEBUG_SPACEWIRE_ONOFF*/
+/* 
+ * If USE_AT697_RAM is defined the RAM on the AT697 board will be used for DMA buffers (but rx message queue is always in AT697 ram).
+ * USE_AT697_DMA specifies whether the messages will be fetched using DMA or PIO.
+ *
+ * RASTA_PCI_BASE is the base address of the GRPCI AHB slave
+ * 
+  */
+
+#define USE_AT697_RAM              1
+#define USE_AT697_DMA              0
+#define RASTA_PCI_BASE             0xe0000000
+#define GRSPW_RASTA_MEM_OFF        0x21000
+
+/* Make GRSPW driver use malloced or static memory
+ */
+#ifdef USE_AT697_RAM
+#undef GRSPW_STATIC_MEM
+#else
+#define GRSPW_STATIC_MEM
+#define GRSPW_CALC_MEMOFS(maxcores,corenum,ptr_mem_base,ptr_mem_end,ptr_bdtable_base) \
+	grspw_rasta_calc_memoffs((maxcores),(corenum),(ptr_mem_base),(ptr_mem_end),(ptr_bdtable_base))
+#endif
+
+/* We have custom address tranlation for HW addresses */
+#define GRSPW_ADR_TO
+
+/* MEMAREA=>CPU used when reading descriptor buffer pointers,
+ * they need to be translated from adresses used by GRSPW HW
+ * into CPU readable addresses.
+ *
+ * NOT NEEDED AS GRSPW DRIVER USES INDEXES TO GET DESCRIPTOR
+ * DATA POINTER ADDRESSES.
+ */
+#undef GRSPW_ADR_FROM
+
+/* Set registered device name */
+#define GRSPW_DEVNAME "/dev/grspwrasta0"
+#define GRSPW_DEVNAME_NO(devstr,no) ((devstr)[15]='0'+(no))
+
+/* Any non-static function will begin with */
+#define GRSPW_PREFIX(name) grspwrasta##name 
+
+/* do nothing, assume that the interrupt handler is called 
+ * setup externally calling grspw_interrupt_handler.
+ */
+#define GRSPW_REG_INT(handler,irq,arg) \
+  if ( grspw_rasta_int_reg ) \
+    grspw_rasta_int_reg(handler,irq,arg);
+
+#define GRSPW_DONT_BYPASS_CACHE
+ 
+#ifdef GRSPW_ADR_TO
+/* Translate a address within the Memory Region (memarea) into an Hardware
+ * device address. This address is put into hardware registers or descriptors
+ * so that the hardware can access the Memory Region.
+ * Example:
+ * An local AMBA access at 0xe0000000 will translate into PCI address 0x40000000,
+ * the PCI address 0x40000000 will translate into CPU-AMBA address 0x40000000.
+ */
+static inline unsigned int memarea_to_hw(unsigned int addr) {
+    return ((addr & 0x0fffffff) | RASTA_PCI_BASE);
+}
+#endif
+
+void (*grspw_rasta_int_reg)(void *handler, int irq, void *arg) = 0;
+
+static int grspw_rasta_calc_memoffs(int maxcores, int corenum, unsigned int *mem_base, unsigned int *mem_end, unsigned int *bdtable_base);
+
+int grspw_rasta_interrupt_handler(unsigned int status);
+
+void grspwrasta_interrupt_handler(int irq, void *pDev);
+
+#include "grspw.c"
+
+unsigned int grspw_rasta_memarea_address;
+
+/* Register RASTA GRSPW driver.
+ *
+ * memarea     = preallocated memory somewhere, pointer to start of memory.
+ */
+
+int grspw_rasta_register(
+ amba_confarea_type *bus, 
+ unsigned int ram_base
+ )
+{
+	/* Setup configuration */
+	
+	/* if zero the malloc will be used */
+	grspw_rasta_memarea_address = ram_base + GRSPW_RASTA_MEM_OFF;
+
+	/* Register the driver */
+	return GRSPW_PREFIX(_register)(bus);
+}
+
+#if 0
+/* Call this from PCI interrupt handler, simply figures out
+ * which GRSPW core was responsible for the IRQ (may be multiple).
+ * v = status of the PCI/AMBA MCPU IRQ CTRL
+ */
+int grspw_rasta_interrupt_handler(unsigned int status)
+{
+        int minor;
+
+        for(minor = 0; minor < spw_cores; minor++) {
+                if (status & (1<<grspw_devs[minor].irq) ) {
+                        grspw_interrupt(&grspw_devs[minor]);
+                }
+        }
+}
+#endif
+
+void GRSPW_PREFIX(_interrupt_handler)(int irq, void *pDev)
+{
+  grspw_interrupt(pDev);
+}
+
+
+#ifdef GRSPW_STATIC_MEM
+/*
+ *  --------------------------------------- <-   
+ *  | Core1: BD TABLE 1 and 2             |
+ *  | Core2: BD TABLE 1 and 2             |
+ *  | Core3: BD TABLE 1 and 2             |
+ *  |-------------------------------------| 
+ *  | Core1: rx data buf + rx header buf  | 
+ *  | Core2: rx data buf + rx header buf  |
+ *  | Core3: rx data buf + rx header buf  |
+ *  ---------------------------------------
+ */
+static int grspw_rasta_calc_memoffs(int maxcores, int corenum, unsigned int *mem_base, unsigned int *mem_end, unsigned int *bdtable_base)
+{	
+	if ( maxcores > 3 )
+		return -1;
+	
+	if ( bdtable_base )
+		*bdtable_base = grspw_rasta_memarea_address + corenum*2*SPACEWIRE_BDTABLE_SIZE;
+	
+	if ( mem_base )
+		*mem_base = grspw_rasta_memarea_address + coremax*2*SPACEWIRE_BDTABLE_SIZE + corenum*BUFMEM_PER_LINK;
+	
+	if ( mem_end )
+		*mem_end = grspw_rasta_memarea_address + coremax*2*SPACEWIRE_BDTABLE_SIZE + (corenum+1)*BUFMEM_PER_LINK;
+	
+	return 0;
+}
+#endif
diff --git a/c/src/lib/libbsp/sparc/shared/uart/apbuart.c b/c/src/lib/libbsp/sparc/shared/uart/apbuart.c
new file mode 100644
index 0000000000..f204d96162
--- /dev/null
+++ b/c/src/lib/libbsp/sparc/shared/uart/apbuart.c
@@ -0,0 +1,885 @@
+/*
+ *  This file contains the driver for the APBUART serial port.
+ *  No console driver, only char driver.
+ *
+ *  COPYRIGHT (c) 2007.
+ *  Gaisler Research.
+ *
+ *  The license and distribution terms for this file may be
+ *  found in the file LICENSE in this distribution or at
+ *  http://www.rtems.com/license/LICENSE.
+ *
+ *
+ *  2007-07-11, Daniel Hellstrom <daniel@gaisler.com>
+ *    Added ioctl command APBUART_CLR_STATS
+ */
+
+#include <bsp.h>
+#include <rtems/libio.h>
+#include <stdlib.h>
+#include <assert.h>
+#include <rtems/bspIo.h>
+#include <string.h>
+
+#include <ambapp.h>
+#include <apbuart.h>
+
+#ifndef DEFAULT_TXBUF_SIZE
+ #define DEFAULT_TXBUF_SIZE 32
+#endif
+#ifndef DEFAULT_RXBUF_SIZE
+ #define DEFAULT_RXBUF_SIZE 32
+#endif
+
+#ifndef APBUART_PREFIX
+ #define APBUART_PREFIX(name) apbuart##name
+#endif
+
+#if !defined(APBUART_DEVNAME) || !defined(APBUART_DEVNAME_NO)
+ #undef APBUART_DEVNAME
+ #undef APBUART_DEVNAME_NO
+ #define APBUART_DEVNAME "/dev/apbuart0"
+ #define APBUART_DEVNAME_NO(devstr,no) ((devstr)[12]='0'+(no))
+#endif
+
+#ifndef APBUART_REG_INT
+	#define APBUART_REG_INT(handler,irq,arg) set_vector(handler,irq+0x10,1)
+#endif
+
+/* Default to 40MHz system clock */
+/*#ifndef SYS_FREQ_HZ
+ #define SYS_FREQ_HZ 40000000
+#endif*/
+
+typedef struct {
+	int size;
+	unsigned char *buf,
+	              *tail,
+		            *head,
+								*max;
+	int full; /* no more place in fifo */
+} apbuart_fifo;
+
+static apbuart_fifo *apbuart_fifo_create(int size);
+static void apbuart_fifo_free(apbuart_fifo *fifo);
+static inline int apbuart_fifo_isFull(apbuart_fifo *fifo);
+static inline int apbuart_fifo_isEmpty(apbuart_fifo *fifo);
+static int apbuart_fifo_put(apbuart_fifo *fifo, unsigned char c);
+static int apbuart_fifo_get(apbuart_fifo *fifo, unsigned char *c);
+static int inline apbuart_fifo_peek(apbuart_fifo *fifo, unsigned char **c);
+static void inline apbuart_fifo_skip(apbuart_fifo *fifo);
+
+static rtems_device_driver apbuart_initialize(rtems_device_major_number  major, rtems_device_minor_number  minor,  void *arg);
+static rtems_device_driver apbuart_open(rtems_device_major_number major, rtems_device_minor_number minor, void *arg);
+static rtems_device_driver apbuart_close(rtems_device_major_number major, rtems_device_minor_number minor, void *arg);
+static rtems_device_driver apbuart_read(rtems_device_major_number major, rtems_device_minor_number minor, void *arg);
+static rtems_device_driver apbuart_write(rtems_device_major_number major, rtems_device_minor_number minor, void *arg);
+static rtems_device_driver apbuart_control(rtems_device_major_number major, rtems_device_minor_number minor, void *arg);
+
+typedef struct {
+	ambapp_apb_uart *regs;
+	int irq;
+	int minor;
+	int scaler;
+	unsigned int baud;	
+
+	int txblk;    /* Make write block until at least 1 char has 
+	               * been put into software send fifo
+								 */
+	int tx_flush; /* Set this to block until all data has 
+	               * placed into the hardware send fifo 
+								 */
+	int rxblk;    /* Make read block until at least 1 char has 
+	               * been received (or taken from software fifo).
+								 */
+	int started;  /* Set to 1 when in running mode */	
+	
+	int ascii_mode; /* Set to 1 to make \n be printed as \r\n */
+	
+	/* TX/RX software FIFO Buffers */
+	apbuart_fifo *txfifo;
+	apbuart_fifo *rxfifo;
+	
+	apbuart_stats stats;
+	
+	rtems_id dev_sem;
+	rtems_id rx_sem;
+	rtems_id tx_sem;
+} apbuart_priv;
+
+static int dev_cnt;
+static apbuart_priv *apbuarts;
+static unsigned int sys_freq_hz;
+
+#define APBUART_DRIVER_TABLE_ENTRY { apbuart_initialize, apbuart_open, apbuart_close, apbuart_read, apbuart_write, apbuart_control }
+
+static rtems_driver_address_table apbuart_driver = APBUART_DRIVER_TABLE_ENTRY;
+static amba_confarea_type *amba_bus;
+
+static void apbuart_interrupt(apbuart_priv *uart);
+static void apbuart_interrupt_handler(rtems_vector_number v);
+static void apbuart_hw_close(apbuart_priv *uart);
+static void apbuart_hw_open(apbuart_priv *uart);
+
+/* Uncomment for debug output */
+/* #define DEBUG 1 
+ #define FUNCDEBUG 1 */
+
+#ifdef DEBUG
+#define DBG(x...) printk(x)
+#else
+#define DBG(x...) 
+#endif
+#ifdef FUNCDEBUG
+#define FUNCDBG(x...) printk(x)
+#else
+#define FUNCDBG(x...) 
+#endif
+
+#ifndef READ_REG
+	#define READ_REG(address) _APBUART_READ_REG((unsigned int)(address))
+  static __inline__ unsigned int _APBUART_READ_REG(unsigned int addr) {
+        unsigned int tmp;
+        asm(" lda [%1]1, %0 "
+            : "=r"(tmp)
+            : "r"(addr)
+           );
+        return tmp;
+	}
+#endif
+
+#if 0
+static int apbuart_outbyte_try(ambapp_apb_uart *regs,  unsigned char ch)
+{
+	if ( (READ_REG(&regs->status) & LEON_REG_UART_STATUS_THE) == 0 )
+		return -1; /* Failed */
+	
+	/* There is room in fifo, put ch in it */
+	regs->data = (unsigned int) ch;
+	return 0;
+}
+
+
+static int apbuart_inbyte_try(ambapp_apb_uart *regs)
+{
+	unsigned int status;
+	/* Clear errors if any */	
+	if ( (status=READ_REG(&regs->status)) & LEON_REG_UART_STATUS_ERR) {
+		regs->status = status & ~LEON_REG_UART_STATUS_ERR;
+	}
+	
+	/* Is Data available? */
+	if ( (READ_REG(&regs->status) & LEON_REG_UART_STATUS_DR) == 0 )
+		return -1; /* No data avail */
+	
+	/* Return Data */
+	return (int)READ_REG(&regs->data);
+}
+
+static int apbuart_write_support(apbuart_priv *uart, const char *buf, int len)
+{
+	int nwrite = 0;
+
+	while (nwrite < len) {
+		if ( apbuart_outbyte_try(minor, *buf++) ){
+			/* TX Fifo full */
+			
+		}
+		nwrite++;
+	}
+	return nwrite;
+}
+#endif
+
+static void apbuart_hw_open(apbuart_priv *uart){
+	unsigned int scaler;
+	
+	/* Calculate Baudrate */
+	if ( uart->scaler > 0 ) {
+		scaler = uart->scaler;
+	}else{
+		scaler = (((sys_freq_hz*10)/(uart->baud*8))-5)/10;
+	}
+	
+	/* Set new baud rate */
+	uart->regs->scaler = scaler;
+	
+	/* Enable receiver & Transmitter */
+	uart->regs->ctrl = APBUART_CTRL_RE | APBUART_CTRL_RF | APBUART_CTRL_RI | APBUART_CTRL_TI;
+}
+
+static void apbuart_hw_close(apbuart_priv *uart){
+	/* disable receiver & transmitter & all IRQs */
+	uart->regs->ctrl = 0;
+}
+
+/* interrupt handler */
+static void apbuart_interrupt_handler(rtems_vector_number v){
+	int minor;
+	
+	/* convert to */
+  for(minor = 0; minor < dev_cnt; minor++) {
+  	if ( v == (apbuarts[minor].irq+0x10) ) {
+			apbuart_interrupt(&apbuarts[minor]);
+			return;
+		}
+	}
+}
+
+/* The interrupt handler, taking care of the 
+ * APBUART hardware
+ */
+static void apbuart_interrupt(apbuart_priv *uart){
+	unsigned int status;
+	int empty;
+	unsigned char c, *next_char;
+	int signal;
+	
+	/* Clear & record any error */
+	status = READ_REG(&uart->regs->status);
+	if ( status & (APBUART_STATUS_OV|APBUART_STATUS_PE|APBUART_STATUS_FE) ){
+		/* Data overrun */
+		if ( status & APBUART_STATUS_OV ){
+			uart->stats.hw_dovr++;
+		}
+		/* Parity error */
+		if ( status & APBUART_STATUS_PE ){
+			uart->stats.hw_parity++;
+		}
+		/* Framing error */
+		if ( status & APBUART_STATUS_FE ){
+			uart->stats.hw_frame++;
+		}
+		uart->regs->status = status & ~(APBUART_STATUS_OV|APBUART_STATUS_PE|APBUART_STATUS_FE);
+	}
+	
+	/* Empty RX fifo into software fifo */
+	signal = 0;
+	while ( (status=READ_REG(&uart->regs->status)) & APBUART_STATUS_DR ){
+		c = READ_REG(&uart->regs->data);
+		if ( apbuart_fifo_isFull(uart->rxfifo) ){
+			uart->stats.sw_dovr++;
+      DBG("]");
+			break;
+		}
+		/* put into fifo */
+		apbuart_fifo_put(uart->rxfifo,c);
+    
+		/* bump RX counter */
+		uart->stats.rx_cnt++;
+    
+		signal = 1;
+	}
+	
+	/* Wake RX thread if any */
+	if ( signal )
+		rtems_semaphore_release(uart->rx_sem);
+	
+	/* If room in HW fifo and we got more chars to be sent */
+	if ( !(status & APBUART_STATUS_TF) ){
+		
+		if ( apbuart_fifo_isEmpty(uart->txfifo) ){
+			/* Turn off TX interrupt when no data is to be sent */
+			if ( status & APBUART_STATUS_TE ){
+				uart->regs->ctrl = READ_REG(&uart->regs->ctrl) & ~APBUART_CTRL_TF;
+				DBG("?");
+			}
+			return;
+		}
+		
+		/* signal when there will be more room in SW fifo */
+		if ( apbuart_fifo_isFull(uart->txfifo) )
+			signal = 1;
+		
+		do{
+			/* Put data into HW TX fifo */
+			apbuart_fifo_peek(uart->txfifo,&next_char);
+			c = *next_char;
+			if ( uart->ascii_mode && ( c == '\n') ){
+				uart->regs->data = '\n';
+				*next_char = '\r'; /* avoid sending mutiple '\n' or '\r' */
+			}else{
+				uart->regs->data = c;
+				apbuart_fifo_skip(uart->txfifo); /* remove sent char from fifo */
+			}
+			uart->regs->ctrl = READ_REG(&uart->regs->ctrl) | APBUART_CTRL_TE | APBUART_CTRL_TF;
+			DBG("!");
+		}while(!(empty=apbuart_fifo_isEmpty(uart->txfifo)) && 
+		       !((status=READ_REG(&uart->regs->status))&APBUART_STATUS_TF) );
+		
+		/* Wake userspace thread, on empty or full fifo
+		 * This makes tx_flush and block work.
+		 */
+		if ( signal || empty ){
+			rtems_semaphore_release(uart->tx_sem);
+		}
+	}
+}
+
+int APBUART_PREFIX(_register)(amba_confarea_type *bus) {
+	rtems_status_code r;
+	rtems_device_major_number m;
+
+	amba_bus = bus;
+
+	FUNCDBG("apbuart_register:\n");
+
+	if ((r = rtems_io_register_driver(0, &apbuart_driver, &m)) == RTEMS_SUCCESSFUL) {
+		DBG("APBUART driver successfully registered, major: %d\n", m);
+	} else {
+		switch(r) {
+		case RTEMS_TOO_MANY:
+			printk("APBUART rtems_io_register_driver failed: RTEMS_TOO_MANY\n"); return -1;
+		case RTEMS_INVALID_NUMBER:  
+			printk("APBUART rtems_io_register_driver failed: RTEMS_INVALID_NUMBER\n"); return -1;
+		case RTEMS_RESOURCE_IN_USE:
+			printk("APBUART rtems_io_register_driver failed: RTEMS_RESOURCE_IN_USE\n"); return -1;
+		default:
+		   printk("APBUART rtems_io_register_driver failed\n");
+			 return -1;
+		}
+	}
+	return 0;
+}
+
+static rtems_device_driver apbuart_initialize(rtems_device_major_number  major, rtems_device_minor_number  minor,  void *arg)
+{
+   
+	rtems_status_code status;
+	int i;
+	amba_apb_device dev;
+	char fs_name[20];
+
+	FUNCDBG("apbuart_initialize\n");
+
+	/* Find all APB UART devices */
+	dev_cnt = amba_get_number_apbslv_devices(amba_bus,VENDOR_GAISLER,GAISLER_APBUART);
+	if ( dev_cnt < 1 ){
+		/* Failed to find any CAN cores! */
+		printk("APBUART: Failed to find any APBUART cores\n\r");
+		return -1;
+	}
+	
+	strcpy(fs_name,APBUART_DEVNAME);
+	
+	DBG("Found %d APBUART(s)\n\r",dev_cnt);
+	
+	/* Allocate memory for device structures */
+	apbuarts = malloc(sizeof(apbuart_priv) * dev_cnt);
+	if ( !apbuarts ){
+		printk("APBUART: Failed to allocate SW memory\n\r");
+		return -1;
+	}
+  
+  memset(apbuarts,0,sizeof(sizeof(apbuart_priv) * dev_cnt));
+		
+	/* Detect System Frequency from initialized timer */
+#ifndef SYS_FREQ_HZ
+#if defined(LEON3)
+	/* LEON3: find timer address via AMBA Plug&Play info */	
+	{
+		amba_apb_device gptimer;
+		LEON3_Timer_Regs_Map *tregs;
+		
+		if ( amba_find_apbslv(&amba_conf,VENDOR_GAISLER,GAISLER_GPTIMER,&gptimer) == 1 ){
+			tregs = (LEON3_Timer_Regs_Map *)gptimer.start;
+			sys_freq_hz = (tregs->scaler_reload+1)*1000*1000;
+			DBG("APBUART: detected %dHZ system frequency\n\r",sys_freq_hz);
+		}else{
+			sys_freq_hz = 40000000; /* Default to 40MHz */
+			printk("APBUART: Failed to detect system frequency\n\r");
+		}
+		
+	}
+#elif defined(LEON2)
+	/* LEON2: use hardcoded address to get to timer */
+	{
+		LEON_Register_Map *regs = (LEON_Register_Map *)0x80000000;
+		sys_freq_hz = (regs->Scaler_Reload+1)*1000*1000;
+	}
+#else
+  #error CPU not supported for OC_CAN driver
+#endif
+#else
+	/* Use hardcoded frequency */
+	sys_freq_hz = SYS_FREQ_HZ;
+#endif
+		
+	for(i=0; i<dev_cnt; i++){
+		/* Get AMBA AHB device info from Plug&Play */
+		amba_find_next_apbslv(amba_bus,VENDOR_GAISLER,GAISLER_APBUART,&dev,i);
+		
+		printk("APBUART[%d]: at 0x%x irq %d (0x%x)\n\r",i,dev.start,dev.irq,(unsigned int)&apbuarts[i]);
+		
+		apbuarts[i].regs = (ambapp_apb_uart *)dev.start;
+		apbuarts[i].irq = dev.irq;
+		apbuarts[i].minor = i;
+		
+		/* Clear HW regs */
+		apbuarts[i].regs->status = 0;
+		apbuarts[i].regs->ctrl = 0;    
+		
+		/* Allocate default software buffers */
+		apbuarts[i].txfifo = apbuart_fifo_create(DEFAULT_TXBUF_SIZE);		
+		apbuarts[i].rxfifo = apbuart_fifo_create(DEFAULT_RXBUF_SIZE);
+		if ( !apbuarts[i].txfifo || !apbuarts[i].rxfifo )
+			rtems_fatal_error_occurred(RTEMS_NO_MEMORY);
+		
+		APBUART_DEVNAME_NO(fs_name,i);
+
+		/* Bind name to device */
+		DBG("APBUART[%d]: binding to name %s\n\r",i,fs_name);
+		status = rtems_io_register_name(fs_name, major, i);
+		if (status != RTEMS_SUCCESSFUL)
+			rtems_fatal_error_occurred(status);
+		
+		/* Setup interrupt handler for each channel */
+   	APBUART_REG_INT(APBUART_PREFIX(_interrupt_handler), apbuarts[i].irq, &apbuarts[i]);
+		
+		/* Device A Semaphore created with count = 1 */
+		if ( rtems_semaphore_create(rtems_build_name('A', 'U', 'D', '0'+i),
+		     1,
+		     RTEMS_FIFO|RTEMS_SIMPLE_BINARY_SEMAPHORE|RTEMS_NO_INHERIT_PRIORITY|RTEMS_LOCAL|RTEMS_NO_PRIORITY_CEILING, 
+		     0,
+		     &apbuarts[i].dev_sem) != RTEMS_SUCCESSFUL )
+			return RTEMS_INTERNAL_ERROR;
+		
+		if ( rtems_semaphore_create(rtems_build_name('A', 'U', 'T', '0'+i),
+		     1,
+		     RTEMS_FIFO|RTEMS_SIMPLE_BINARY_SEMAPHORE|RTEMS_NO_INHERIT_PRIORITY|RTEMS_LOCAL|RTEMS_NO_PRIORITY_CEILING, 
+		     0,
+		     &apbuarts[i].tx_sem) != RTEMS_SUCCESSFUL )
+			return RTEMS_INTERNAL_ERROR;
+				 
+		if ( rtems_semaphore_create(rtems_build_name('A', 'U', 'R', '0'+i),
+		     1,
+		     RTEMS_FIFO|RTEMS_SIMPLE_BINARY_SEMAPHORE|RTEMS_NO_INHERIT_PRIORITY|RTEMS_LOCAL|RTEMS_NO_PRIORITY_CEILING, 
+		     0,
+		     &apbuarts[i].rx_sem) != RTEMS_SUCCESSFUL )
+			return RTEMS_INTERNAL_ERROR;
+
+	}
+	return RTEMS_SUCCESSFUL;
+}
+
+static rtems_device_driver apbuart_open(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
+{  
+	apbuart_priv *uart;
+	
+	FUNCDBG("apbuart_open: major %d, minor %d\n", major, minor);
+
+	if ( (minor < 0) || (minor >= dev_cnt) ) {
+		DBG("Wrong minor %d\n", minor);
+		return RTEMS_INVALID_NAME;
+	}
+	
+	uart = &apbuarts[minor];
+
+	if (rtems_semaphore_obtain(uart->dev_sem, RTEMS_NO_WAIT, RTEMS_NO_TIMEOUT) != RTEMS_SUCCESSFUL) {
+		DBG("apbuart_open: resource in use\n");
+		return RTEMS_RESOURCE_IN_USE;
+	}
+	
+	/* Clear HW regs */
+	uart->regs->status = 0;
+	uart->regs->ctrl = 0;
+	
+	/* Set Defaults */
+	
+	/* 38400 baudrate */
+	uart->scaler = 0; /* use uart->baud */
+	uart->baud = 38400;
+	
+	/* Default to Blocking mode */
+	uart->txblk = 1;
+	uart->rxblk = 1;
+		
+	/* Default to no flush mode */
+	uart->tx_flush = 0;
+
+	/* non-ascii mode */
+	uart->ascii_mode = 0;
+  
+  /* not started */
+  uart->started = 0;
+	
+	if ( !uart->txfifo || (uart->txfifo->size!=DEFAULT_TXBUF_SIZE) ){
+		apbuart_fifo_free(uart->txfifo);
+		uart->txfifo = apbuart_fifo_create(DEFAULT_TXBUF_SIZE);
+	}
+	
+	if ( !uart->rxfifo || (uart->rxfifo->size!=DEFAULT_RXBUF_SIZE) ){
+		apbuart_fifo_free(uart->rxfifo);
+		uart->rxfifo = apbuart_fifo_create(DEFAULT_RXBUF_SIZE);
+	}
+	
+	if ( !uart->rxfifo || !uart->txfifo ){
+		/* Failed to get memory */
+		return RTEMS_NO_MEMORY;
+	}
+	
+	/* Now user must call ioctl(START,0) to begin */
+	
+	return RTEMS_SUCCESSFUL;
+}
+
+static rtems_device_driver apbuart_close(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
+{
+	apbuart_priv *uart = &apbuarts[minor];
+	
+	FUNCDBG("apbuart_close[%d]:\n",minor);
+	
+	apbuart_hw_close(uart);
+
+	/* Software state will be set when open is called again */
+	rtems_semaphore_release(uart->rx_sem);
+	rtems_semaphore_release(uart->tx_sem);
+	uart->started = 0;
+	
+	rtems_semaphore_release(uart->dev_sem);
+	
+	return RTEMS_SUCCESSFUL;
+}
+ 
+static rtems_device_driver apbuart_read(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
+{
+	rtems_libio_rw_args_t *rw_args;
+	unsigned int count = 0, oldLevel;
+	unsigned char *buf;
+	apbuart_priv *uart = &apbuarts[minor];
+	
+	rw_args = (rtems_libio_rw_args_t *) arg;
+
+	FUNCDBG("apbuart_read\n");
+	
+	buf = (unsigned char *)rw_args->buffer;
+	if ( (rw_args->count < 1) || !buf )
+		return RTEMS_INVALID_NAME; /* EINVAL */
+
+	rtems_interrupt_disable(oldLevel);
+	do {
+		if ( (unsigned int)uart < 0x40000000 ) {
+      printk("UART %x is screwed\n",uart);
+    }
+		/* Read from SW fifo */
+		if ( apbuart_fifo_get(uart->rxfifo,&buf[count]) != 0 ){			
+			/* non blocking or read at least 1 byte */
+			if ( (count > 0) || (!uart->rxblk) )
+				break; /* Return */
+			
+			rtems_interrupt_enable(oldLevel);
+
+			/* Block thread until a char is received */
+			rtems_semaphore_obtain(uart->rx_sem, RTEMS_WAIT, RTEMS_NO_TIMEOUT);
+      
+			rtems_interrupt_disable(oldLevel);
+			continue;
+		}
+		
+		/* Got char from SW FIFO */
+		count++;
+		
+	} while (count < rw_args->count );
+	
+	rtems_interrupt_enable(oldLevel);
+
+	rw_args->bytes_moved = count;
+	
+	if (count == 0)
+		return RTEMS_TIMEOUT; /* ETIMEDOUT should be EAGAIN/EWOULDBLOCK */
+
+	return RTEMS_SUCCESSFUL;
+}
+ 
+static rtems_device_driver apbuart_write(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
+{
+	rtems_libio_rw_args_t *rw_args;
+	unsigned int count, oldLevel, ctrl;
+	unsigned char *buf;
+	apbuart_priv *uart = &apbuarts[minor];
+	int direct=0;
+	
+   
+	rw_args = (rtems_libio_rw_args_t *) arg;
+  
+	FUNCDBG("apbuart_write\n");
+	
+	buf = rw_args->buffer;
+	
+	if ( rw_args->count < 1 || !buf )
+		return RTEMS_INVALID_NAME; /* EINVAL */ 
+	
+	count = 0;
+	rtems_interrupt_disable(oldLevel);
+	/* Do we need to start to send first char direct via HW
+	 * to get IRQ going.
+	 */
+	
+	ctrl = READ_REG(&uart->regs->ctrl);
+	if ( (ctrl & APBUART_CTRL_TF) == 0 ){
+		/* TX interrupt is disabled ==> 
+		 * SW FIFO is empty and,
+		 * HW FIFO empty
+		 */
+		uart->regs->ctrl = ctrl | APBUART_CTRL_TF;
+		if ( uart->ascii_mode && (buf[0] == '\n') ){
+			uart->regs->data = '\r';
+		}else{
+			uart->regs->data = buf[0];
+			count++;
+		}
+		uart->regs->ctrl = ctrl | APBUART_CTRL_TE | APBUART_CTRL_TF;
+		direct = 1;
+	}
+	
+	while( count < rw_args->count ) {
+		/* write to HW FIFO direct skipping SW FIFO */
+		if ( direct && ((READ_REG(&uart->regs->status) & APBUART_STATUS_TF) == 0) ){
+			uart->regs->data = buf[count];
+		}
+		/* write to SW FIFO */
+		else if ( apbuart_fifo_put(uart->txfifo,buf[count]) ){
+			direct = 0;
+			DBG("APBUART[%d]: write: SW FIFO Full\n\r",minor);
+			
+			/* is full, block? */
+			if ( ((count < 1) && uart->txblk) || uart->tx_flush ){
+			
+				rtems_interrupt_enable(oldLevel);
+				
+				rtems_semaphore_obtain(uart->tx_sem, RTEMS_WAIT, RTEMS_NO_TIMEOUT);
+				
+				rtems_interrupt_disable(oldLevel);
+			
+				/* Do we need to start to send first char direct via HW
+				 * to get IRQ going.
+				 */
+	
+				ctrl = READ_REG(&uart->regs->ctrl);
+				if ( (ctrl & APBUART_CTRL_TF) == 0 ){
+					/* TX interrupt is disabled ==> 
+					 * SW FIFO is empty and,
+					 * HW FIFO empty
+					 */
+					uart->regs->ctrl = ctrl | APBUART_CTRL_TF;
+					if ( uart->ascii_mode && (buf[count] == '\n') ){
+						uart->regs->data = '\r';
+					}else{
+						uart->regs->data = buf[count];
+						count++;
+					}
+					uart->regs->ctrl = ctrl | APBUART_CTRL_TF | APBUART_CTRL_TE;
+					direct = 1;
+				}
+				
+				continue;
+			}
+			/* don't block, return current status */
+			break;
+		}else{
+			direct = 0;
+		}
+		
+		count++;
+		
+	}
+
+	rtems_interrupt_enable(oldLevel);
+	
+	rw_args->bytes_moved = count;
+	
+	if (count == 0)
+		return RTEMS_TIMEOUT; /* ETIMEDOUT should be EAGAIN/EWOULDBLOCK */
+ 
+	return RTEMS_SUCCESSFUL;
+}
+
+static rtems_device_driver apbuart_control(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
+{
+	rtems_libio_ioctl_args_t *ioarg = (rtems_libio_ioctl_args_t *)arg;
+	unsigned int *data = ioarg->buffer;
+	apbuart_priv *uart = &apbuarts[minor];
+	int size;
+	unsigned int baudrate, blocking;
+	apbuart_stats *stats;
+
+	FUNCDBG("apbuart_control [%i,%i]\n",major, minor);
+  
+	if (!ioarg)
+		return RTEMS_INVALID_NAME;
+
+  ioarg->ioctl_return = 0;
+	switch(ioarg->command) {
+	
+	/* Enable Receiver & transmitter */
+	case APBUART_START:
+		if ( uart->started )
+			return RTEMS_INVALID_NAME;
+		apbuart_hw_open(uart);
+		uart->started = 1;
+		break;
+	
+	/* Close Receiver & transmitter */
+	case APBUART_STOP:
+		if ( !uart->started )
+			return RTEMS_INVALID_NAME;
+		apbuart_hw_close(uart);
+		uart->started = 0;
+		break;
+	
+	/* Set RX FIFO Software buffer length 
+	 * It is only possible to change buffer size in
+	 * non-running mode.
+	 */
+	case APBUART_SET_RXFIFO_LEN:
+		if ( uart->started )
+			return RTEMS_RESOURCE_IN_USE; /* EBUSY */
+	
+		size = (int)ioarg->buffer;
+		if ( size < 1 ) 
+			return RTEMS_INVALID_NAME; /* EINVAL */
+		
+		/* Free old buffer */
+		apbuart_fifo_free(uart->rxfifo);
+		
+		/* Allocate new buffer & init it */
+		uart->rxfifo = apbuart_fifo_create(size);
+		if ( !uart->rxfifo )
+			return RTEMS_NO_MEMORY;
+		break;
+
+	/* Set TX FIFO Software buffer length 
+	 * It is only possible to change buffer size 
+	 * while in non-running mode.
+	 */
+	case APBUART_SET_TXFIFO_LEN:
+		if ( uart->started )
+			return RTEMS_RESOURCE_IN_USE; /* EBUSY */
+		
+		size = (int)ioarg->buffer;
+		if ( size < 1 ) 
+			return RTEMS_INVALID_NAME; /* EINVAL */
+		
+		/* Free old buffer */
+		apbuart_fifo_free(uart->txfifo);
+		
+		/* Allocate new buffer & init it */
+		uart->txfifo = apbuart_fifo_create(size);
+		if ( !uart->txfifo )
+			return RTEMS_NO_MEMORY;
+		break;
+	
+	case APBUART_SET_BAUDRATE:
+		/* Set baud rate of */
+		baudrate = (int)ioarg->buffer;
+		if ( (baudrate < 1) || (baudrate > 115200) ){
+			return RTEMS_INVALID_NAME;
+		}
+		uart->scaler = 0; /* use uart->baud */
+		uart->baud = baudrate;
+		break;
+	
+	case APBUART_SET_SCALER:
+		/* use uart->scaler not uart->baud */
+		uart->scaler = data[0];
+		break;
+	
+	case APBUART_SET_BLOCKING:
+		blocking = (unsigned int)ioarg->buffer;
+		uart->rxblk = ( blocking & APBUART_BLK_RX );
+		uart->txblk = ( blocking & APBUART_BLK_TX );
+		uart->tx_flush = ( blocking & APBUART_BLK_FLUSH );
+		break;
+	
+	case APBUART_GET_STATS:
+		stats = (void *)ioarg->buffer;
+		if ( !stats )
+			return RTEMS_INVALID_NAME;
+		
+		/* Copy Stats */
+		*stats = uart->stats;
+		break;
+  
+  case APBUART_CLR_STATS:
+		/* Clear/reset Stats */
+    memset(&uart->stats,0,sizeof(uart->stats));
+		break;
+    
+	case APBUART_SET_ASCII_MODE:
+		uart->ascii_mode = (int)ioarg->buffer;
+		break;
+   
+	default:
+		return RTEMS_NOT_DEFINED;
+	}
+	return RTEMS_SUCCESSFUL;
+}
+
+
+/******************* APBUART FIFO implementation ***********************/
+
+static apbuart_fifo *apbuart_fifo_create(int size){
+	apbuart_fifo *fifo;
+	fifo = (apbuart_fifo *) malloc( size + sizeof(apbuart_fifo));
+	if ( fifo ){
+		/* Init fifo */
+		fifo->size = size;
+		fifo->buf = (char *)(fifo+1);
+		fifo->tail = fifo->buf;
+		fifo->head = fifo->buf;
+		fifo->max = &fifo->buf[size-1];
+		fifo->full=0;
+	}
+	return fifo;
+}
+
+static void apbuart_fifo_free(apbuart_fifo *fifo){
+	if ( fifo )
+		free(fifo);
+}
+
+static inline int apbuart_fifo_isFull(apbuart_fifo *fifo){
+	return fifo->full;
+}
+
+static inline int apbuart_fifo_isEmpty(apbuart_fifo *fifo){
+	if ( (fifo->head == fifo->tail) && !fifo->full )
+		return -1;
+	return 0;
+}
+
+static int apbuart_fifo_put(apbuart_fifo *fifo, unsigned char c){
+	if ( !fifo->full ){
+		*fifo->head = c;
+		fifo->head = (fifo->head >= fifo->max ) ? fifo->buf : fifo->head+1;
+		if ( fifo->head == fifo->tail )
+			fifo->full = -1;
+		return 0;
+	}
+	return -1;
+}
+
+static int apbuart_fifo_get(apbuart_fifo *fifo, unsigned char *c){
+	if ( apbuart_fifo_isEmpty(fifo) )
+		return -1;
+	if ( c )
+		*c = *fifo->tail;
+	fifo->tail = (fifo->tail >= fifo->max ) ? fifo->buf : fifo->tail+1;
+	fifo->full = 0;
+	return 0;
+}
+
+static int inline apbuart_fifo_peek(apbuart_fifo *fifo, unsigned char **c){
+	if ( apbuart_fifo_isEmpty(fifo) )
+		return -1;
+	if ( c )
+		*c = fifo->tail;
+	return 0;
+}
+
+static void inline apbuart_fifo_skip(apbuart_fifo *fifo){
+	if ( !apbuart_fifo_isEmpty(fifo) ){
+		fifo->tail = (fifo->tail >= fifo->max ) ? fifo->buf : fifo->tail+1;
+		fifo->full = 0;
+	}
+}
diff --git a/c/src/lib/libbsp/sparc/shared/uart/apbuart_pci.c b/c/src/lib/libbsp/sparc/shared/uart/apbuart_pci.c
new file mode 100644
index 0000000000..ad2effa065
--- /dev/null
+++ b/c/src/lib/libbsp/sparc/shared/uart/apbuart_pci.c
@@ -0,0 +1,42 @@
+
+#undef DEBUG
+
+/* Set registered device name */
+#define APBUART_DEVNAME "/dev/apbupci0"
+#define APBUART_DEVNAME_NO(devstr,no) ((devstr)[12]='0'+(no))
+
+/* Any non-static function will begin with */
+#define APBUART_PREFIX(name) apbuartpci##name
+
+/* do nothing, assume that the interrupt handler is called 
+ * setup externally calling apbuartpci_interrupt_handler.
+ */
+#define APBUART_REG_INT(handler,irq,arg) \
+ if ( apbuart_pci_int_reg ) \
+   apbuart_pci_int_reg(handler,irq,arg);
+
+void (*apbuart_pci_int_reg)(void *handler, int irq, void *arg) = 0;
+
+void apbuartpci_interrupt_handler(int irq, void *arg);
+
+/* AMBA Bus is clocked using the PCI clock (33.3MHz) */
+#define SYS_FREQ_HZ 33333333
+
+#include "apbuart.c"
+
+int apbuart_pci_register(amba_confarea_type *bus)
+{
+	/* Setup configuration */
+	
+	/* Register the driver */
+	return APBUART_PREFIX(_register)(bus);
+}
+
+
+/* Call this from PCI interrupt handler 
+ * irq = the irq number of the HW device local to that IRQMP controller
+ * 
+ */
+void apbuartpci_interrupt_handler(int irq, void *arg){
+	apbuart_interrupt(arg);
+}
diff --git a/c/src/lib/libbsp/sparc/shared/uart/apbuart_rasta.c b/c/src/lib/libbsp/sparc/shared/uart/apbuart_rasta.c
new file mode 100644
index 0000000000..a5c365eb77
--- /dev/null
+++ b/c/src/lib/libbsp/sparc/shared/uart/apbuart_rasta.c
@@ -0,0 +1,42 @@
+
+#undef DEBUG
+
+/* Set registered device name */
+#define APBUART_DEVNAME "/dev/apburasta0"
+#define APBUART_DEVNAME_NO(devstr,no) ((devstr)[14]='0'+(no))
+
+/* Any non-static function will begin with */
+#define APBUART_PREFIX(name) apbuartrasta##name
+
+/* do nothing, assume that the interrupt handler is called 
+ * setup externally calling apbuartrasta_interrupt_handler.
+ */
+#define APBUART_REG_INT(handler,irq,arg) \
+ if ( apbuart_rasta_int_reg ) \
+   apbuart_rasta_int_reg(handler,irq,arg);
+
+void (*apbuart_rasta_int_reg)(void *handler, int irq, void *arg) = 0;
+
+void apbuartrasta_interrupt_handler(int irq, void *arg);
+
+/* AMBA Bus is clocked using the RASTA internal clock (30MHz) */
+#define SYS_FREQ_HZ 30000000
+
+#include "apbuart.c"
+
+int apbuart_rasta_register(amba_confarea_type *bus)
+{
+	/* Setup configuration */
+	
+	/* Register the driver */
+	return APBUART_PREFIX(_register)(bus);
+}
+
+
+/* Call this from RASTA interrupt handler 
+ * irq = the irq number of the HW device local to that IRQMP controller
+ * 
+ */
+void apbuartrasta_interrupt_handler(int irq, void *arg){
+	apbuart_interrupt(arg);
+}