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.

1 files changed, 1706 insertions, 0 deletions

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();
+}