LEON: added new drivers to the LEON2/LEON3 BSPs

Most drivers use the Driver Manager for device probing, they
work on AMBA-over-PCI systems if PCI is big-endian.

New APIs:
 * GPIO Library, interfaced to GRGPIO
 * GENIRQ, Generic interrupt service implementation helper

New GRLIB Drivers:
 * ACTEL 1553 RT, user interface is similar to 1553 BRM driver
 * GR1553 (1553 BC, RT and BM core)
 * AHBSTAT (AHB error status core)
 * GRADCDAC (Core interfacing to ADC/DAC hardware)
 * GRGPIO (GPIO port accessed from GPIO Library)
 * MCTRL (Memory controller settings configuration)
 * GRETH (10/100/1000 Ethernet driver using Driver manager)
 * GRPWM (Pulse Width Modulation core)
 * SPICTRL (SPI master interface)
 * GRSPW_ROUTER (SpaceWire Router AMBA configuration interface)
 * GRCTM (SpaceCraft on-board Time Management core)
 * SPWCUC (Time distribution over SpaceWire)
 * GRTC (SpaceCraft up-link Tele core)
 * GRTM (SpaceCraft down-link Tele Metry core)

GR712RC ASIC specific interfaces:
 * GRASCS
 * CANMUX (select between OCCAN and SATCAN)
 * SATCAN
 * SLINK

2 files changed, 911 insertions, 0 deletions

diff --git a/c/src/lib/libbsp/sparc/shared/can/canmux.c b/c/src/lib/libbsp/sparc/shared/can/canmux.c
new file mode 100644
index 0000000000..0884d911f5
--- /dev/null
+++ b/c/src/lib/libbsp/sparc/shared/can/canmux.c
@@ -0,0 +1,197 @@
+/*
+ *  CAN_MUX driver. Present in GR712RC.
+ *
+ *  COPYRIGHT (c) 2008.
+ *  Cobham Gaisler AB.
+ *
+ *  The license and distribution terms for this file may be
+ *  found in the file LICENSE in this distribution or at
+ *  http://www.rtems.com/license/LICENSE.
+ */
+
+#include <rtems/libio.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+#include <bsp.h>
+#include <rtems/bspIo.h> /* printk */
+
+#include <canmux.h>
+#include <ambapp.h>
+
+#ifndef GAISLER_CANMUX
+#define GAISLER_CANMUX 0x081
+#endif
+
+#if !defined(CANMUX_DEVNAME) 
+ #undef CANMUX_DEVNAME
+ #define CANMUX_DEVNAME "/dev/canmux"
+#endif
+
+/* Enable debug output? */
+/* #define DEBUG */
+
+#ifdef DEBUG
+#define DBG(x...) printk(x)
+#else
+#define DBG(x...) 
+#endif
+
+#define BUSA_SELECT (1 << 0)
+#define BUSB_SELECT (1 << 1)
+
+struct canmux_priv {
+	volatile unsigned int *muxreg;
+	rtems_id devsem;
+	int open;
+};
+
+static struct canmux_priv *priv;
+
+static rtems_device_driver canmux_ioctl(rtems_device_major_number major, rtems_device_minor_number minor, void *arg);
+static rtems_device_driver canmux_write(rtems_device_major_number major, rtems_device_minor_number minor, void *arg);
+static rtems_device_driver canmux_read(rtems_device_major_number major, rtems_device_minor_number minor, void *arg);
+static rtems_device_driver canmux_close(rtems_device_major_number major, rtems_device_minor_number minor, void *arg);
+static rtems_device_driver canmux_open(rtems_device_major_number major, rtems_device_minor_number minor, void *arg);
+static rtems_device_driver canmux_initialize(rtems_device_major_number major, rtems_device_minor_number unused, void *arg);
+
+
+static rtems_device_driver canmux_ioctl(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
+{
+	rtems_libio_ioctl_args_t *ioarg = (rtems_libio_ioctl_args_t*)arg;
+	
+	DBG("CAN_MUX:  IOCTL %d\n\r", ioarg->command);
+
+	ioarg->ioctl_return = 0;
+	switch(ioarg->command) {
+	case CANMUX_IOC_BUSA_SATCAN: *priv->muxreg &= ~BUSA_SELECT; break;
+	case CANMUX_IOC_BUSA_OCCAN1: *priv->muxreg |= BUSA_SELECT;  break;
+	case CANMUX_IOC_BUSB_SATCAN: *priv->muxreg &= ~BUSB_SELECT; break;
+	case CANMUX_IOC_BUSB_OCCAN2: *priv->muxreg |= BUSB_SELECT; break;
+	default: return RTEMS_NOT_DEFINED;
+	}
+
+	return RTEMS_SUCCESSFUL;
+}
+
+static rtems_device_driver canmux_write(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
+{
+	rtems_libio_rw_args_t *rw_args=(rtems_libio_rw_args_t*)arg;
+	
+	rw_args->bytes_moved = 0;
+
+	return RTEMS_SUCCESSFUL;
+}
+
+static rtems_device_driver canmux_read(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
+{
+	rtems_libio_rw_args_t *rw_args = (rtems_libio_rw_args_t*)arg;
+	
+	rw_args->bytes_moved = 0;
+	
+	return RTEMS_SUCCESSFUL;
+}
+
+
+static rtems_device_driver canmux_close(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
+{
+	DBG("CAN_MUX: Closing %d\n\r",minor);
+
+	priv->open = 0;
+	return RTEMS_SUCCESSFUL;
+}
+
+
+static rtems_device_driver canmux_open(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
+{
+	DBG("CAN_MUX: Opening %d\n\r",minor);
+	
+	rtems_semaphore_obtain(priv->devsem,RTEMS_WAIT, RTEMS_NO_TIMEOUT);
+	if (priv->open) {
+		rtems_semaphore_release(priv->devsem);
+		return RTEMS_RESOURCE_IN_USE; /* EBUSY */
+	}
+	priv->open = 1;
+	rtems_semaphore_release(priv->devsem);
+
+	DBG("CAN_MUX: Opening %d success\n\r",minor);
+
+	return RTEMS_SUCCESSFUL;
+}
+
+static rtems_device_driver canmux_initialize(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
+{
+	struct ambapp_apb_info d;
+	char fs_name[20];
+	rtems_status_code status;
+
+	DBG("CAN_MUX: Initialize..\n\r");
+
+	strcpy(fs_name, CANMUX_DEVNAME);
+	
+	/* Find core and initialize register pointer */
+	if (!ambapp_find_apbslv(&ambapp_plb, VENDOR_GAISLER, GAISLER_CANMUX, &d)) {
+		printk("CAN_MUX: Failed to find CAN_MUX core\n\r");
+		return -1;
+	}
+	
+	status = rtems_io_register_name(fs_name, major, minor);
+	if (RTEMS_SUCCESSFUL != status)
+		rtems_fatal_error_occurred(status);
+
+	/* Create private structure */
+	if ((priv = malloc(sizeof(struct canmux_priv))) == NULL) {
+		printk("CAN_MUX driver could not allocate memory for priv structure\n\r");
+		return -1;
+	}
+	
+	priv->muxreg = (unsigned int*)d.start;
+
+	status = rtems_semaphore_create(
+		rtems_build_name('M', 'd', 'v', '0'),
+		1,
+		RTEMS_FIFO | RTEMS_SIMPLE_BINARY_SEMAPHORE | RTEMS_NO_INHERIT_PRIORITY | \
+		RTEMS_NO_PRIORITY_CEILING,
+		0, 
+		&priv->devsem);
+	if (status != RTEMS_SUCCESSFUL) {
+		printk("CAN_MUX: Failed to create dev semaphore (%d)\n\r", status);
+		free(priv);
+		return RTEMS_UNSATISFIED;
+	}
+	
+	priv->open = 0;
+
+	return RTEMS_SUCCESSFUL;
+}
+
+
+#define CANMUX_DRIVER_TABLE_ENTRY { canmux_initialize, canmux_open, canmux_close, canmux_read, canmux_write, canmux_ioctl }
+
+static rtems_driver_address_table canmux_driver = CANMUX_DRIVER_TABLE_ENTRY;
+
+int canmux_register(void)
+{
+	rtems_status_code r;
+	rtems_device_major_number m;
+
+	DBG("CAN_MUX: canmux_register called\n\r");
+
+	if ((r = rtems_io_register_driver(0, &canmux_driver, &m)) == RTEMS_SUCCESSFUL) {
+		DBG("CAN_MUX driver successfully registered, major: %d\n\r", m);
+	} else {
+		switch(r) {
+		case RTEMS_TOO_MANY:
+			printk("CAN_MUX rtems_io_register_driver failed: RTEMS_TOO_MANY\n\r"); break;
+		case RTEMS_INVALID_NUMBER:  
+			printk("CAN_MUX rtems_io_register_driver failed: RTEMS_INVALID_NUMBER\n\r"); break;
+		case RTEMS_RESOURCE_IN_USE:
+			printk("CAN_MUX rtems_io_register_driver failed: RTEMS_RESOURCE_IN_USE\n\r"); break;
+		default:
+			printk("CAN_MUX rtems_io_register_driver failed\n\r");
+		}
+		return 1;
+	}
+
+	return 0;
+}
diff --git a/c/src/lib/libbsp/sparc/shared/can/satcan.c b/c/src/lib/libbsp/sparc/shared/can/satcan.c
new file mode 100644
index 0000000000..36c6bb9f5d
--- /dev/null
+++ b/c/src/lib/libbsp/sparc/shared/can/satcan.c
@@ -0,0 +1,714 @@
+/*
+ *  SatCAN FPGA driver
+ * 
+ *  COPYRIGHT (c) 2008.
+ *  Cobham Gaisler AB.
+ *
+ *  The license and distribution terms for this file may be
+ *  found in the file LICENSE in this distribution or at
+ *  http://www.rtems.com/license/LICENSE.
+ */
+
+#include <rtems/libio.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+#include <bsp.h>
+#include <rtems/bspIo.h> /* printk */
+
+#include <satcan.h>
+#include <ambapp.h>
+
+#ifndef GAISLER_SATCAN
+#define GAISLER_SATCAN 0x080
+#endif
+
+#if !defined(SATCAN_DEVNAME) 
+ #undef SATCAN_DEVNAME
+ #define SATCAN_DEVNAME "/dev/satcan"
+#endif
+
+/* Enable debug output? */
+/* #define DEBUG */ 
+
+#ifdef DEBUG
+#define DBG(x...) printk(x)
+#else
+#define DBG(x...) 
+#endif
+
+
+/* Defines related to DMA */
+#define ALIGN_2KMEM 32*1024
+#define ALIGN_8KMEM 128*1024
+
+#define OFFSET_2K_LOW_POS 15
+#define OFFSET_8K_LOW_POS 17
+
+#define DMA_2K_DATA_SELECT (1 << 14)
+#define DMA_8K_DATA_SELECT (1 << 16)
+
+#define DMA_2K_DATA_OFFSET 16*1024
+#define DMA_8K_DATA_OFFSET 64*1024
+
+/* Core register structures and defines */
+
+/* Indexes to SatCAN registers in satcan array are declared in satcan.h*/
+/* Fields for some of the SatCAN FPGA registers */
+
+/* CmdReg0 */
+#define CAN_TODn_Int_sel   (1 << 5)
+
+/* CmdReg1 */
+#define Sel_2k_8kN         (1 << 0)
+
+/* Read FIFO */
+#define FIFO_Full          (1 << 8)
+#define FIFO_Empty         (1 << 9)
+
+/* DMA Ch_Enable */
+#define DMA_AutoInitDmaTx  (1 << 3)
+#define DMA_EnTx2          (1 << 2)
+#define DMA_EnTx1          (1 << 1)
+#define DMA_EnRx           (1 << 0)
+
+/* SatCAN wrapper register fields */
+#define CTRL_BT_P     9
+#define CTRL_NODENO_P 5
+#define CTRL_DIS      (1 << 2)
+#define CTRL_DPS_P    1
+#define CTRL_RST      (1 << 0)
+
+#define IRQ_AHB       (1 << 8)
+#define IRQ_PPS       (1 << 7)
+#define IRQ_M5        (1 << 6)
+#define IRQ_M4        (1 << 5)
+#define IRQ_M3        (1 << 4)
+#define IRQ_M2        (1 << 3)
+#define IRQ_M1        (1 << 2)
+#define IRQ_SYNC      (1 << 1)
+#define IRQ_CAN       (1 << 0)
+
+#define MSK_AHB       (1 << 8)
+#define MSK_PPS       (1 << 7)
+#define MSK_M5        (1 << 6)
+#define MSK_M4        (1 << 5)
+#define MSK_M3        (1 << 4)
+#define MSK_M2        (1 << 3)
+#define MSK_M1        (1 << 2)
+#define MSK_SYNC      (1 << 1)
+#define MSK_CAN       (1 << 0)
+
+
+
+struct satcan_regs {
+	volatile unsigned int satcan[32];
+	volatile unsigned int ctrl;
+	volatile unsigned int irqpend;
+	volatile unsigned int irqmask;
+	volatile unsigned int membase;
+};
+
+
+struct satcan_priv {
+	/* config */
+	void           *dmaptr;
+	unsigned char  *alptr;
+	satcan_config  *cfg;
+	
+	/* driver state */
+	rtems_id       devsem;
+	rtems_id       txsem;
+	int            open;
+	int            txactive;
+	int            dmaen;
+	int            doff;
+	rtems_interval timeout;
+	int            dmamode;
+};
+
+static struct satcan_regs *regs;
+static struct satcan_priv *priv;
+
+static rtems_device_driver satcan_ioctl(rtems_device_major_number major, rtems_device_minor_number minor, void *arg);
+static rtems_device_driver satcan_write(rtems_device_major_number major, rtems_device_minor_number minor, void *arg);
+static rtems_device_driver satcan_read(rtems_device_major_number major, rtems_device_minor_number minor, void *arg);
+static rtems_device_driver satcan_close(rtems_device_major_number major, rtems_device_minor_number minor, void *arg);
+static rtems_device_driver satcan_open(rtems_device_major_number major, rtems_device_minor_number minor, void *arg);
+static rtems_device_driver satcan_initialize(rtems_device_major_number major, rtems_device_minor_number unused, void *arg);
+
+
+/*
+ * almalloc: allocate memory area of size sz aligned on sz boundary 
+ * alptr: Utilized to return aligned pointer
+ * ptr:   Unaligned pointer
+ * sz:    Size of memory area
+ */
+static void almalloc(unsigned char **alptr, void **ptr, int sz)
+{
+  *ptr = calloc(1,2*sz);
+  *alptr = (unsigned char *) (((int)*ptr+sz) & ~(sz-1));
+}
+
+static rtems_isr satcan_interrupt_handler(rtems_vector_number v)
+{
+	unsigned int irq;
+	unsigned int fifo;
+	
+	irq = regs->irqpend;
+	
+	if (irq & IRQ_AHB && priv->cfg->ahb_irq_callback) {
+		priv->cfg->ahb_irq_callback();
+	}
+	if (irq & IRQ_PPS && priv->cfg->pps_irq_callback) {
+		priv->cfg->pps_irq_callback();
+	}
+	if (irq & IRQ_M5 && priv->cfg->m5_irq_callback) {
+		priv->cfg->m5_irq_callback();
+	}
+	if (irq & IRQ_M4 && priv->cfg->m4_irq_callback) {
+		priv->cfg->m4_irq_callback();
+	}
+	if (irq & IRQ_M3 && priv->cfg->m3_irq_callback) {
+		priv->cfg->m3_irq_callback();
+	}
+	if (irq & IRQ_M2 && priv->cfg->m2_irq_callback) {
+		priv->cfg->m2_irq_callback();
+	}
+	if (irq & IRQ_M1 && priv->cfg->m1_irq_callback) {
+		priv->cfg->m1_irq_callback();
+	}
+	if (irq & IRQ_SYNC && priv->cfg->sync_irq_callback) {
+		priv->cfg->sync_irq_callback();
+	}
+	if (irq & IRQ_CAN) {
+		fifo = regs->satcan[SATCAN_FIFO];
+		if (!(fifo & FIFO_Empty) && priv->txactive &&
+		    (((fifo & 0xff) == SATCAN_IRQ_EOD1) || ((fifo & 0xff) == SATCAN_IRQ_EOD2))) { 
+			rtems_semaphore_release(priv->txsem);
+		}
+		if (priv->cfg->can_irq_callback)
+			priv->cfg->can_irq_callback(fifo);
+	}
+}
+
+
+
+static rtems_device_driver satcan_ioctl(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
+{
+	rtems_libio_ioctl_args_t *ioarg = (rtems_libio_ioctl_args_t*)arg;
+	int *value;
+	rtems_interval *timeout;
+	satcan_regmod *regmod;
+
+	DBG("SatCAN:  IOCTL %d\n\r", ioarg->command);
+
+	ioarg->ioctl_return = 0;
+	switch(ioarg->command) {
+	case SATCAN_IOC_DMA_2K:
+		DBG("SatCAN: ioctl: setting 2K DMA mode\n\r");
+		free(priv->dmaptr);
+		almalloc(&priv->alptr, &priv->dmaptr, ALIGN_2KMEM);
+		if (priv->dmaptr == NULL) {
+			printk("SatCAN: Failed to allocate DMA memory\n\r");
+			return RTEMS_NO_MEMORY;
+		}
+
+		regs->membase = (unsigned int)priv->alptr;
+		regs->satcan[SATCAN_RAM_BASE] = (unsigned int)priv->alptr >> OFFSET_2K_LOW_POS;
+		regs->satcan[SATCAN_CMD1] = regs->satcan[SATCAN_CMD1] | Sel_2k_8kN;
+		break;
+
+	case SATCAN_IOC_DMA_8K:
+		DBG("SatCAN: ioctl: setting 8K DMA mode\n\r");
+		free(priv->dmaptr);
+		almalloc(&priv->alptr, &priv->dmaptr, ALIGN_8KMEM);
+		if (priv->dmaptr == NULL) {
+			printk("SatCAN: Failed to allocate DMA memory\n\r");
+			return RTEMS_NO_MEMORY;
+		}
+		
+		regs->membase = (unsigned int)priv->alptr;
+		regs->satcan[SATCAN_RAM_BASE] = (unsigned int)priv->alptr >> OFFSET_8K_LOW_POS;
+		regs->satcan[SATCAN_CMD1] = regs->satcan[SATCAN_CMD1] & ~Sel_2k_8kN;
+		break;
+		
+	case SATCAN_IOC_GET_REG:
+		/* Get regmod structure from argument */
+		regmod = (satcan_regmod*)ioarg->buffer;
+		DBG("SatCAN: ioctl: getting register %d\n\r", regmod->reg);
+		if (regmod->reg < 0)
+			return RTEMS_INVALID_NAME;
+		else if (regmod->reg <= SATCAN_FILTER_STOP)
+			regmod->val = regs->satcan[regmod->reg];
+		else if (regmod->reg == SATCAN_WCTRL)
+			regmod->val = regs->ctrl;
+		else if (regmod->reg == SATCAN_WIPEND)
+			regmod->val = regs->irqpend;
+		else if (regmod->reg == SATCAN_WIMASK)
+			regmod->val = regs->irqmask;
+		else if (regmod->reg == SATCAN_WAHBADDR)
+			regmod->val = regs->membase;
+		else
+			return RTEMS_INVALID_NAME;
+		break;
+
+	case SATCAN_IOC_SET_REG:
+		/* Get regmod structure from argument */
+		regmod = (satcan_regmod*)ioarg->buffer;
+		DBG("SatCAN: ioctl: setting register %d, value %x\n\r", 
+		    regmod->reg, regmod->val);
+		if (regmod->reg < 0)
+			return RTEMS_INVALID_NAME;
+		else if (regmod->reg <= SATCAN_FILTER_STOP)
+			regs->satcan[regmod->reg] = regmod->val;
+		else if (regmod->reg == SATCAN_WCTRL)
+			regs->ctrl = regmod->val;
+		else if (regmod->reg == SATCAN_WIPEND)
+			regs->irqpend = regmod->val;
+		else if (regmod->reg == SATCAN_WIMASK)
+			regs->irqmask = regmod->val;
+		else if (regmod->reg == SATCAN_WAHBADDR)
+			regs->membase = regmod->val;
+		else
+			return RTEMS_INVALID_NAME;
+		break;
+
+	case SATCAN_IOC_OR_REG:
+		/* Get regmod structure from argument */
+		regmod = (satcan_regmod*)ioarg->buffer;
+		DBG("SatCAN: ioctl: or:ing register %d, with value %x\n\r",
+		    regmod->reg, regmod->val);
+		if (regmod->reg < 0)
+			return RTEMS_INVALID_NAME;
+		else if (regmod->reg <= SATCAN_FILTER_STOP)
+			regs->satcan[regmod->reg] |= regmod->val;
+		else if (regmod->reg == SATCAN_WCTRL)
+			regs->ctrl |= regmod->val;
+		else if (regmod->reg == SATCAN_WIPEND)
+			regs->irqpend |= regmod->val;
+		else if (regmod->reg == SATCAN_WIMASK)
+			regs->irqmask |= regmod->val;
+		else if (regmod->reg == SATCAN_WAHBADDR)
+			regs->membase |= regmod->val;
+		else
+			return RTEMS_INVALID_NAME;
+		break;
+
+	case SATCAN_IOC_AND_REG:
+		/* Get regmod structure from argument */
+		regmod = (satcan_regmod*)ioarg->buffer;
+		DBG("SatCAN: ioctl: masking register %d, with value %x\n\r",
+		    regmod->reg, regmod->val);
+		if (regmod->reg < 0)
+			return RTEMS_INVALID_NAME;
+		else if (regmod->reg <= SATCAN_FILTER_STOP)
+			regs->satcan[regmod->reg] &= regmod->val;
+		else if (regmod->reg == SATCAN_WCTRL)
+			regs->ctrl &= regmod->val;
+		else if (regmod->reg == SATCAN_WIPEND)
+			regs->irqpend &= regmod->val;
+		else if (regmod->reg == SATCAN_WIMASK)
+			regs->irqmask &= regmod->val;
+		else if (regmod->reg == SATCAN_WAHBADDR)
+			regs->membase &= regmod->val;
+		else
+			return RTEMS_INVALID_NAME;
+		break;
+
+	case SATCAN_IOC_EN_TX1_DIS_TX2:
+		priv->dmaen = SATCAN_DMA_ENABLE_TX1;
+		break;
+
+	case SATCAN_IOC_EN_TX2_DIS_TX1:
+		priv->dmaen = SATCAN_DMA_ENABLE_TX2;
+		break;
+
+	case SATCAN_IOC_GET_DMA_MODE:
+		value = (int*)ioarg->buffer;
+		*value = priv->dmamode;
+		break;
+		
+	case SATCAN_IOC_SET_DMA_MODE:
+		value = (int*)ioarg->buffer;
+		if (*value != SATCAN_DMA_MODE_USER && *value != SATCAN_DMA_MODE_SYSTEM) {
+			DBG("SatCAN: ioctl: invalid DMA mode\n\r");
+			return RTEMS_INVALID_NAME;
+		}
+		priv->dmamode = *value;
+		break;
+
+	case SATCAN_IOC_ACTIVATE_DMA:
+		if (priv->dmamode != SATCAN_DMA_MODE_USER) {
+			DBG("SatCAN: ioctl: ACTIVATE_DMA: not in user mode\n\r");
+			return RTEMS_INVALID_NAME;
+		}
+		value = (int*)ioarg->buffer;
+		if (*value != SATCAN_DMA_ENABLE_TX1 && *value != SATCAN_DMA_ENABLE_TX2) {
+			DBG("SatCAN: ioctl: ACTIVATE_DMA: Illegal channel\n\r");
+			return RTEMS_INVALID_NAME;
+		}
+		regs->satcan[SATCAN_DMA] |= *value << 1;
+		break;
+
+	case SATCAN_IOC_DEACTIVATE_DMA:
+		if (priv->dmamode != SATCAN_DMA_MODE_USER) {
+			DBG("SatCAN: ioctl: DEACTIVATE_DMA: not in user mode\n\r");
+			return RTEMS_INVALID_NAME;
+		}
+		value = (int*)ioarg->buffer;
+		if (*value != SATCAN_DMA_ENABLE_TX1 && *value != SATCAN_DMA_ENABLE_TX2) {
+			DBG("SatCAN: ioctl: DEACTIVATE_DMA: Illegal channel\n\r");
+			return RTEMS_INVALID_NAME;
+		}
+		regs->satcan[SATCAN_DMA] &= ~(*value << 1);
+		break;	
+
+	case SATCAN_IOC_GET_DOFFSET:
+		value = (int*)ioarg->buffer;
+		*value = priv->doff;
+		break;
+	
+	case SATCAN_IOC_SET_DOFFSET:
+		value = (int*)ioarg->buffer;
+		priv->doff = *value;
+		break;
+		
+	case SATCAN_IOC_GET_TIMEOUT:
+		timeout = (rtems_interval*)ioarg->buffer;
+		*timeout = priv->timeout;
+		break;
+
+	case SATCAN_IOC_SET_TIMEOUT:
+		timeout = (rtems_interval*)ioarg->buffer;
+		priv->timeout = *timeout;
+		break;
+
+	default:
+		return RTEMS_NOT_DEFINED;
+	}
+
+	return RTEMS_SUCCESSFUL;
+}
+
+static rtems_device_driver satcan_write(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
+{
+	int i;
+	int doff;
+	int msgindex;
+	int messages;
+	rtems_libio_rw_args_t *rw_args=(rtems_libio_rw_args_t *) arg;
+	satcan_msg *msgs;
+	rtems_status_code status;
+	
+	DBG("SatCAN: Writing %d bytes from %p\n\r",rw_args->count,rw_args->buffer);
+
+	if ((rw_args->count < sizeof(satcan_msg)) || (!rw_args->buffer)) {
+		DBG("SatCAN: write: returning EINVAL\n\r");
+		return RTEMS_INVALID_NAME; /* EINVAL */
+	}
+
+	messages = rw_args->count / sizeof(satcan_msg);
+	msgs = (satcan_msg*)rw_args->buffer;
+
+	/* Check that size matches any number of satcan_msg */
+	if (rw_args->count % sizeof(satcan_msg)) {
+		DBG("SatCAN: write: count can not be evenly divided with satcan_msg size\n\r");
+		return RTEMS_INVALID_NAME; /* EINVAL */
+	}
+
+
+	/* DMA channel must be set if we are in system DMA mode */
+	DBG("SatCAN: write: dma channel select is %x\n\r", priv->dmaen);
+	if (!priv->dmaen && priv->dmamode == SATCAN_DMA_MODE_SYSTEM)
+		return RTEMS_INVALID_NAME; /* EINVAL */
+
+	/* DMA must not be active */
+	if (regs->satcan[SATCAN_DMA] & (DMA_EnTx1 | DMA_EnTx2 | DMA_AutoInitDmaTx)) {
+		DBG("SatCAN: write: DMA was active\n\r");
+		rw_args->bytes_moved = 0;
+		return RTEMS_IO_ERROR; /* EIO */
+	}		
+
+	doff = regs->satcan[SATCAN_CMD1] & Sel_2k_8kN ? DMA_2K_DATA_OFFSET : DMA_8K_DATA_OFFSET;
+
+	for (msgindex = 0; msgindex < messages; msgindex++) {
+		/* Place header in DMA area */
+		for (i = 0; i < SATCAN_HEADER_SIZE; i++) {
+			priv->alptr[priv->doff+8*msgindex+i] = msgs[msgindex].header[i];
+		}
+
+		/* Place data in DMA area */
+		for (i = 0; i < SATCAN_PAYLOAD_SIZE; i++)
+			priv->alptr[priv->doff+doff+8*msgindex+i] = msgs[msgindex].payload[i];
+	}
+
+	if ((priv->dmaen & SATCAN_DMA_ENABLE_TX1) ||  priv->dmamode == SATCAN_DMA_MODE_USER) {
+		regs->satcan[SATCAN_DMA_TX_1_CUR] = 0;
+		regs->satcan[SATCAN_DMA_TX_1_END] = messages<<3; 
+	}
+
+	if ((priv->dmaen & SATCAN_DMA_ENABLE_TX2) || priv->dmamode == SATCAN_DMA_MODE_USER) {
+		regs->satcan[SATCAN_DMA_TX_2_CUR] = 0;
+		regs->satcan[SATCAN_DMA_TX_2_END] = messages<<3; 
+	}
+
+	/* If we are in DMA user mode we are done here, otherwise we block */
+	if (priv->dmamode == SATCAN_DMA_MODE_SYSTEM) {
+		priv->txactive = 1;
+
+		/* Enable DMA */
+		regs->satcan[SATCAN_DMA] |= priv->dmaen << 1;
+		
+		/* Wait for TX interrupt */
+		status = rtems_semaphore_obtain(priv->txsem, RTEMS_WAIT, priv->timeout);
+		
+		priv->txactive = 0;
+
+		/* Disable activated Tx DMA */
+		regs->satcan[SATCAN_DMA] &= ~(priv->dmaen << 1);
+
+		if (status != RTEMS_SUCCESSFUL) {
+			rw_args->bytes_moved = 0;
+			return status;
+		}
+	}
+
+	rw_args->bytes_moved = rw_args->count;
+	
+	return RTEMS_SUCCESSFUL;
+}
+
+static rtems_device_driver satcan_read(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
+{
+	char *buf;
+	int i;
+	int canid;
+	int messages;
+	rtems_libio_rw_args_t *rw_args = (rtems_libio_rw_args_t*)arg;
+	satcan_msg *ret;
+
+	/* Check that there is room for the return */
+	if (rw_args->count < sizeof(satcan_msg)) {
+		DBG("SatCAN: read: length of buffer must be at least %d, current is %d\n\r",
+		    sizeof(satcan_msg) + sizeof(int), rw_args->count);
+		return RTEMS_INVALID_NAME; /* -EINVAL */
+	}
+	
+	/* Check that size matches any number of satcan_msg */
+	if (rw_args->count % sizeof(satcan_msg)) {
+		DBG("SatCAN: read: count can not be evenly divided with satcan_msg size\n\r");
+		return RTEMS_INVALID_NAME; /* EINVAL */
+	}
+	
+	messages = rw_args->count / sizeof(satcan_msg);
+	ret = (satcan_msg*)rw_args->buffer;
+
+	DBG("SatCAN: read: reading %d messages to %p\n\r", messages, ret);
+	
+	for (i = 0; i < messages; i++) {
+		canid = (ret[i].header[1] << 8) | ret[i].header[0];
+	
+		/* Copy message header from DMA header area to buffer */
+		buf = (char*)((int)priv->alptr | (canid << 3));
+		memcpy(ret[i].header, buf, SATCAN_HEADER_SIZE);
+
+		DBG("SatCAN: read: copied header from %p to %p\n\r", buf, ret[i].header);
+
+		/* Clear New Message Marker */
+		buf[SATCAN_HEADER_NMM_POS] = 0;
+		
+		/* Copy message payload from DMA data area to buffer */
+		buf = (char*)((int)buf | 
+			      (regs->satcan[SATCAN_CMD1] & Sel_2k_8kN ? DMA_2K_DATA_SELECT : DMA_8K_DATA_SELECT));
+		memcpy(ret[i].payload, buf, SATCAN_PAYLOAD_SIZE);
+	
+		DBG("SatCAN: read: copied payload from %p to %p\n\r", buf, ret[i].payload);
+	}
+	rw_args->bytes_moved = rw_args->count;
+
+	return RTEMS_SUCCESSFUL;
+}
+
+
+static rtems_device_driver satcan_close(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
+{
+	DBG("SatCAN: Closing %d\n\r",minor);
+
+	if (priv->open) {
+		regs->irqmask = 0;
+		regs->satcan[SATCAN_INT_EN] = 0;
+		regs->satcan[SATCAN_RX] = 0;
+		regs->satcan[SATCAN_DMA] = 0;
+		priv->open = 0;
+		priv->dmaen = 0;
+		priv->doff = 0;
+		priv->timeout = RTEMS_NO_TIMEOUT;
+		priv->dmamode = SATCAN_DMA_MODE_SYSTEM;
+	}
+
+	return RTEMS_SUCCESSFUL;
+}
+
+
+static rtems_device_driver satcan_open(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
+{
+	DBG("SatCAN: Opening %d\n\r",minor);
+	
+	rtems_semaphore_obtain(priv->devsem,RTEMS_WAIT, RTEMS_NO_TIMEOUT);
+	if (priv->open) {
+		rtems_semaphore_release(priv->devsem);
+		return RTEMS_RESOURCE_IN_USE; /* EBUSY */
+	}
+	priv->open = 1;
+	rtems_semaphore_release(priv->devsem);
+
+	/* Enable AHB and CAN IRQs in wrapper and EOD1, EOD2 and CAN critical  IRQs in SatCAN core */
+	regs->irqmask = MSK_AHB | MSK_CAN;
+	regs->satcan[SATCAN_INT_EN] = ((1 << SATCAN_IRQ_EOD1) | (1 << SATCAN_IRQ_EOD2) |
+				       (1 << SATCAN_IRQ_CRITICAL));
+
+	/* Select can_int as IRQ source */
+	regs->satcan[SATCAN_CMD0] = CAN_TODn_Int_sel;
+	/* CAN RX DMA Enable */
+	regs->satcan[SATCAN_DMA] = 1;
+	/* CAN RX Enable */
+	regs->satcan[SATCAN_RX] = 1;
+
+	DBG("SatCAN: Opening %d success\n\r",minor);
+
+	return RTEMS_SUCCESSFUL;
+}
+
+static rtems_device_driver satcan_initialize(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
+{
+	struct ambapp_ahb_info d;
+	char fs_name[20];
+	rtems_status_code status;
+
+	DBG("SatCAN: Initialize..\n\r");
+
+	strcpy(fs_name, SATCAN_DEVNAME);
+
+	/* Find core and initialize register pointer */
+	if (!ambapp_find_ahbslv(&ambapp_plb, VENDOR_GAISLER, GAISLER_SATCAN, &d)) {
+		printk("SatCAN: Failed to find SatCAN core\n\r");
+		return -1;
+	}
+
+	status = rtems_io_register_name(fs_name, major, minor);
+	if (RTEMS_SUCCESSFUL != status)
+		rtems_fatal_error_occurred(status);
+
+	regs = (struct satcan_regs*)d.start[0];
+		
+	/* Set node number and DPS */
+	regs->ctrl |= ((priv->cfg->nodeno & 0xf) << 5) | (priv->cfg->dps << 1);
+
+	/* Reset core */
+	regs->ctrl |= CTRL_RST;
+
+	/* Allocate DMA area */
+	almalloc(&priv->alptr, &priv->dmaptr, ALIGN_2KMEM);
+	if (priv->dmaptr == NULL) {
+		printk("SatCAN: Failed to allocate DMA memory\n\r");
+		free(priv->cfg);
+		free(priv);
+		return -1; 
+	}
+	
+	/* Wait until core reset has completed */
+	while (regs->ctrl & CTRL_RST)
+		;
+
+	/* Initialize core registers, default is 2K messages */
+	regs->membase = (unsigned int)priv->alptr;
+	regs->satcan[SATCAN_RAM_BASE] = (unsigned int)priv->alptr >> 15;
+	
+	DBG("regs->membase = %x\n\r", (unsigned int)priv->alptr);
+	DBG("regs->satcan[SATCAN_RAM_BASE] = %x\n\r", (unsigned int)priv->alptr >> 15);
+
+	status = rtems_semaphore_create(
+		rtems_build_name('S', 'd', 'v', '0'),
+		1,
+		RTEMS_FIFO | RTEMS_SIMPLE_BINARY_SEMAPHORE | RTEMS_NO_INHERIT_PRIORITY | \
+		RTEMS_NO_PRIORITY_CEILING,
+		0, 
+		&priv->devsem);
+	if (status != RTEMS_SUCCESSFUL) {
+		printk("SatCAN: Failed to create dev semaphore (%d)\n\r", status);
+		free(priv->cfg);
+		free(priv);
+		return RTEMS_UNSATISFIED;
+	}
+	status = rtems_semaphore_create(
+		rtems_build_name('S', 't', 'x', '0'),
+		0, 
+		RTEMS_FIFO | RTEMS_SIMPLE_BINARY_SEMAPHORE | RTEMS_NO_INHERIT_PRIORITY | \
+		RTEMS_NO_PRIORITY_CEILING,
+		0, 
+		&priv->txsem);
+	if (status != RTEMS_SUCCESSFUL) {
+	  printk("SatCAN: Failed to create tx semaphore (%d)\n\r", status);
+	  free(priv->cfg);
+	  free(priv);
+	  return RTEMS_UNSATISFIED;
+	}
+
+	priv->txactive = 0;
+	priv->open = 0;
+	priv->dmaen = 0;
+	priv->doff = 0;
+	priv->timeout = RTEMS_NO_TIMEOUT;
+	priv->dmamode = SATCAN_DMA_MODE_SYSTEM;
+
+	/* Register interrupt handler */
+	set_vector(satcan_interrupt_handler, d.irq+0x10, 2);
+
+	return RTEMS_SUCCESSFUL;
+}
+
+
+
+#define SATCAN_DRIVER_TABLE_ENTRY { satcan_initialize, satcan_open, satcan_close, satcan_read, satcan_write, satcan_ioctl }
+
+static rtems_driver_address_table satcan_driver = SATCAN_DRIVER_TABLE_ENTRY;
+
+int satcan_register(satcan_config *conf)
+{
+	rtems_status_code r;
+	rtems_device_major_number m;
+
+	DBG("SatCAN: satcan_register called\n\r");
+
+	/* Create private structure */
+	if ((priv = malloc(sizeof(struct satcan_priv))) == NULL) {
+		printk("SatCAN driver could not allocate memory for priv structure\n\r");
+		return -1;
+	}
+
+	DBG("SatCAN: Creating local copy of config structure\n\r");
+	if ((priv->cfg = malloc(sizeof(satcan_config))) == NULL) {
+		printk("SatCAN driver could not allocate memory for cfg structure\n\r");
+		return 1;
+	}
+	memcpy(priv->cfg, conf, sizeof(satcan_config));
+
+	if ((r = rtems_io_register_driver(0, &satcan_driver, &m)) == RTEMS_SUCCESSFUL) {
+		DBG("SatCAN driver successfully registered, major: %d\n\r", m);
+	} else {
+		switch(r) {
+		case RTEMS_TOO_MANY:
+			printk("SatCAN rtems_io_register_driver failed: RTEMS_TOO_MANY\n\r"); break;
+		case RTEMS_INVALID_NUMBER:  
+			printk("SatCAN rtems_io_register_driver failed: RTEMS_INVALID_NUMBER\n\r"); break;
+		case RTEMS_RESOURCE_IN_USE:
+			printk("SatCAN rtems_io_register_driver failed: RTEMS_RESOURCE_IN_USE\n\r"); break;
+		default:
+			printk("SatCAN rtems_io_register_driver failed\n\r");
+		}
+		return 1;
+	}
+
+	return 0;
+}