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

1 files changed, 859 insertions, 0 deletions

diff --git a/c/src/lib/libbsp/sparc/shared/1553/b1553rt.c b/c/src/lib/libbsp/sparc/shared/1553/b1553rt.c
new file mode 100644
index 0000000000..3dfb40309c
--- /dev/null
+++ b/c/src/lib/libbsp/sparc/shared/1553/b1553rt.c
@@ -0,0 +1,859 @@
+/*
+ *  B1553RT driver implmenetation
+ *
+ *  COPYRIGHT (c) 2009.
+ *  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 <bsp.h>
+#include <rtems/libio.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+#include <assert.h>
+#include <ctype.h>
+#include <rtems/bspIo.h>
+
+#include <drvmgr/drvmgr.h>
+#include <b1553rt.h>
+#include <ambapp.h>
+#include <drvmgr/ambapp_bus.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) )
+
+#if 0
+#define DBG(x...) printk(x)
+#else
+#define DBG(x...) 
+#endif
+
+#ifdef FUNCDEBUG
+#define FUNCDBG(x...) printk(x)
+#else
+#define FUNCDBG(x...) 
+#endif
+
+#define READ_DMA(address) _READ16((unsigned int)address)
+
+static __inline__ unsigned short _READ16(unsigned int addr) {
+    unsigned short tmp;
+    asm(" lduha [%1]1, %0 "
+        : "=r"(tmp)
+        : "r"(addr)
+	);
+    return tmp;
+}
+
+static rtems_device_driver rt_initialize(rtems_device_major_number major, rtems_device_minor_number minor, void *arg);
+static rtems_device_driver rt_open(rtems_device_major_number major, rtems_device_minor_number minor, void *arg);
+static rtems_device_driver rt_close(rtems_device_major_number major, rtems_device_minor_number minor, void *arg);
+static rtems_device_driver rt_read(rtems_device_major_number major, rtems_device_minor_number minor, void *arg);
+static rtems_device_driver rt_write(rtems_device_major_number major, rtems_device_minor_number minor, void *arg);
+static rtems_device_driver rt_control(rtems_device_major_number major, rtems_device_minor_number minor, void *arg);
+
+#define RT_DRIVER_TABLE_ENTRY { rt_initialize, rt_open, rt_close, rt_read, rt_write, rt_control }
+
+static rtems_driver_address_table b1553rt_driver = RT_DRIVER_TABLE_ENTRY;
+
+typedef struct { 
+
+    struct drvmgr_dev *dev; /* Driver manager device */
+    char devName[32];     /* Device Name */
+
+    struct rt_reg *regs;
+    unsigned int ctrl_copy; /* Local copy of config register */
+
+    unsigned int cfg_freq;
+
+    unsigned int memarea_base;
+    unsigned int memarea_base_remote;
+
+    volatile unsigned short *mem;
+
+    /* Received events waiting to be read */
+    struct rt_msg *rt_event;
+    unsigned int head, tail;
+
+    int rx_blocking;
+
+    rtems_id rx_sem, tx_sem, dev_sem;
+    int minor;
+    int irqno;
+
+#ifdef DEBUG    		
+    unsigned int log[EVENT_QUEUE_SIZE*4];
+    unsigned int log_i;
+#endif
+
+    unsigned int status;
+    rtems_id event_id; /* event that may be signalled upon errors, needs to be set through ioctl command RT_SET_EVENTID */
+
+} rt_priv;
+
+static void b1553rt_interrupt(void *arg);
+static rtems_device_driver rt_init(rt_priv *rt);
+
+#define OFS(ofs) (((unsigned int)&ofs & 0x1ffff)>>1)
+
+static int b1553rt_driver_io_registered = 0;
+static rtems_device_major_number b1553rt_driver_io_major = 0;
+
+/******************* Driver manager interface ***********************/
+
+/* Driver prototypes */
+int b1553rt_register_io(rtems_device_major_number *m);
+int b1553rt_device_init(rt_priv *pDev);
+
+int b1553rt_init2(struct drvmgr_dev *dev);
+int b1553rt_init3(struct drvmgr_dev *dev);
+int b1553rt_remove(struct drvmgr_dev *dev);
+
+struct drvmgr_drv_ops b1553rt_ops = 
+{
+    .init = {NULL, b1553rt_init2, b1553rt_init3, NULL},
+    .remove = b1553rt_remove,
+    .info = NULL
+};
+
+struct amba_dev_id b1553rt_ids[] = 
+{
+    {VENDOR_GAISLER, GAISLER_B1553RT},
+    {0, 0}		/* Mark end of table */
+};
+
+struct amba_drv_info b1553rt_drv_info =
+{
+    {
+	DRVMGR_OBJ_DRV,				/* Driver */
+        NULL,					/* Next driver */
+        NULL,					/* Device list */
+        DRIVER_AMBAPP_GAISLER_B1553RT_ID,	/* Driver ID */
+        "B1553RT_DRV",				/* Driver Name */
+        DRVMGR_BUS_TYPE_AMBAPP,			/* Bus Type */
+        &b1553rt_ops,
+        NULL,					/* Funcs */
+        0,					/* No devices yet */
+	0,
+
+    },
+    &b1553rt_ids[0]
+};
+
+void b1553rt_register_drv (void)
+{
+    DBG("Registering B1553RT driver\n");
+    drvmgr_drv_register(&b1553rt_drv_info.general);
+}
+
+int b1553rt_init2(struct drvmgr_dev *dev)
+{
+    rt_priv *priv;
+
+    DBG("B1553RT[%d] on bus %s\n", dev->minor_drv, dev->parent->dev->name);
+    priv = dev->priv = malloc(sizeof(rt_priv));
+    if ( !priv )
+        return DRVMGR_NOMEM;
+    memset(priv, 0, sizeof(*priv));
+    priv->dev = dev;
+
+    /* This core will not find other cores, so we wait for init2() */
+
+    return DRVMGR_OK;
+}
+
+int b1553rt_init3(struct drvmgr_dev *dev)
+{
+    rt_priv *priv;
+    char prefix[32];
+    rtems_status_code status;
+
+    priv = dev->priv;
+
+    /* Do initialization */
+
+    if ( b1553rt_driver_io_registered == 0) {
+        /* Register the I/O driver only once for all cores */
+        if ( b1553rt_register_io(&b1553rt_driver_io_major) ) {
+            /* Failed to register I/O driver */
+            dev->priv = NULL;
+            return DRVMGR_FAIL;
+        }
+
+        b1553rt_driver_io_registered = 1;
+    }
+
+    /* I/O system registered and initialized 
+     * Now we take care of device initialization.
+     */
+
+    if ( b1553rt_device_init(priv) ) {
+        return DRVMGR_FAIL;
+    }
+
+    /* Get Filesystem name prefix */
+    prefix[0] = '\0';
+    if ( drvmgr_get_dev_prefix(dev, prefix) ) {
+        /* Failed to get prefix, make sure of a unique FS name
+         * by using the driver minor.
+         */
+        sprintf(priv->devName, "/dev/b1553rt%d", dev->minor_drv);
+    } else {
+        /* Got special prefix, this means we have a bus prefix
+         * And we should use our "bus minor"
+         */
+        sprintf(priv->devName, "/dev/%sb1553rt%d", prefix, dev->minor_bus);
+    }
+
+    /* Register Device */
+    status = rtems_io_register_name(priv->devName, b1553rt_driver_io_major, dev->minor_drv);
+    if (status != RTEMS_SUCCESSFUL) {
+        return DRVMGR_FAIL;
+    }
+
+    return DRVMGR_OK;
+}
+
+int b1553rt_remove(struct drvmgr_dev *dev)
+{
+    /* Stop more tasks to open driver */
+
+    /* Throw out all tasks using this driver */
+
+    /* Unregister I/O node */
+
+    /* Unregister and disable Interrupt */
+
+    /* Free device memory */
+
+    /* Return sucessfully */
+
+    return DRVMGR_FAIL;
+}
+
+/******************* Driver Implementation ***********************/
+
+int b1553rt_register_io(rtems_device_major_number *m)
+{
+    rtems_status_code r;
+
+    if ((r = rtems_io_register_driver(0, &b1553rt_driver, m)) == RTEMS_SUCCESSFUL) {
+        DBG("B1553RT driver successfully registered, major: %d\n", *m);
+    } else {
+        switch(r) {
+        case RTEMS_TOO_MANY:
+            printk("B1553RT rtems_io_register_driver failed: RTEMS_TOO_MANY\n");
+            return -1;
+        case RTEMS_INVALID_NUMBER:  
+            printk("B1553RT rtems_io_register_driver failed: RTEMS_INVALID_NUMBER\n");
+            return -1;
+        case RTEMS_RESOURCE_IN_USE:
+            printk("B1553RT rtems_io_register_driver failed: RTEMS_RESOURCE_IN_USE\n");
+            return -1;
+        default:
+            printk("B1553RT rtems_io_register_driver failed\n");
+            return -1;
+        }
+    }
+    return 0;
+}
+
+int b1553rt_device_init(rt_priv *pDev)
+{
+    struct amba_dev_info *ambadev;
+    struct ambapp_core *pnpinfo;
+    union drvmgr_key_value *value;
+    unsigned int mem;
+    unsigned int sys_freq_hz;
+
+    /* Get device information from AMBA PnP information */
+    ambadev = (struct amba_dev_info *)pDev->dev->businfo;
+    if ( ambadev == NULL ) {
+        return -1;
+    }
+    pnpinfo = &ambadev->info;
+    pDev->irqno = pnpinfo->irq;
+    pDev->regs = (struct rt_reg *)pnpinfo->apb_slv->start;
+    pDev->minor = pDev->dev->minor_drv;
+
+#ifdef DEBUG
+    pDev->log_i = 0;	
+    memset(pDev->log,0,sizeof(pDev->log));
+    printf("LOG: 0x%x\n", &pDev->log[0]);
+    printf("LOG_I: 0x%x\n", &pDev->log_i);
+#endif	
+
+    /* Get memory configuration from bus resources */
+    value = drvmgr_dev_key_get(pDev->dev, "dmaBaseAdr", KEY_TYPE_POINTER);
+    if (value)
+        mem = (unsigned int)value->ptr;
+
+    if (value && (mem & 1)) {
+        /* Remote address, address as RT looks at it. */
+
+        /* Translate the base address into an address that the the CPU can understand */
+        pDev->memarea_base = mem & ~1;
+        drvmgr_translate_check(pDev->dev, DMAMEM_TO_CPU,
+                                (void *)pDev->memarea_base_remote,
+                                (void **)&pDev->memarea_base,
+                                4 * 1024);
+    } else {
+        if (!value) {
+            /* Use dynamically allocated memory,
+             * 4k DMA memory + 4k for alignment 
+             */
+            mem = (char *)malloc(4 * 1024 * 2);
+            if ( !mem ){
+                printk("RT: Failed to allocate HW memory\n\r");
+                return -1;
+            }
+            /* align memory to 4k boundary */
+            pDev->memarea_base = (mem + 0xfff) & ~0xfff;
+        } else {
+            pDev->memarea_base = mem;
+        }
+
+        /* Translate the base address into an address that the RT core can understand */
+        drvmgr_translate_check(pDev->dev, CPUMEM_TO_DMA,
+                               (void *)pDev->memarea_base,
+                               (void **)&pDev->memarea_base_remote,
+                               4 * 1024);
+    }
+
+    /* clear the used memory */
+    memset((char *)pDev->memarea_base, 0, 4 * 1024);
+
+    /* Set base address of all descriptors */
+    pDev->memarea_base = (unsigned int)mem;
+    pDev->mem = (volatile unsigned short *)pDev->memarea_base;
+
+    pDev->rt_event = NULL;
+
+    /* The RT is always clocked at the same frequency as the bus 
+     * If the frequency doesnt match it is defaulted to 24MHz, 
+     * user can always override it.
+     */
+    pDev->cfg_freq = RT_FREQ_24MHZ;
+
+    /* Get frequency in Hz */
+    if ( drvmgr_freq_get(pDev->dev, DEV_APB_SLV, &sys_freq_hz) == 0 ) {
+        if ( sys_freq_hz == 20000000 ) {
+            pDev->cfg_freq = RT_FREQ_20MHZ;
+        } else if ( sys_freq_hz == 16000000 ) {
+            pDev->cfg_freq = RT_FREQ_16MHZ;
+        } else if ( sys_freq_hz == 12000000 ) {
+            pDev->cfg_freq = RT_FREQ_12MHZ;
+        }
+    }
+
+    value = drvmgr_dev_key_get(pDev->dev, "coreFreq", KEY_TYPE_INT);
+    if ( value ) {
+        pDev->cfg_freq = value->i & RT_FREQ_MASK;
+    }
+
+    /* RX Semaphore created with count = 0 */
+    if ( rtems_semaphore_create(rtems_build_name('R', 'T', '0', '0' + pDev->minor),
+                                0,
+                                RTEMS_FIFO|RTEMS_SIMPLE_BINARY_SEMAPHORE|RTEMS_NO_INHERIT_PRIORITY|RTEMS_LOCAL|RTEMS_NO_PRIORITY_CEILING, 
+                                0,
+                                &pDev->rx_sem) != RTEMS_SUCCESSFUL ) {
+        printk("RT: Failed to create rx semaphore\n");
+        return RTEMS_INTERNAL_ERROR;
+    }
+
+    /* Device Semaphore created with count = 1 */
+    if ( rtems_semaphore_create(rtems_build_name('R', 'T', '0', '0' + pDev->minor),
+                                1,
+                                RTEMS_FIFO|RTEMS_SIMPLE_BINARY_SEMAPHORE|RTEMS_NO_INHERIT_PRIORITY|RTEMS_LOCAL|RTEMS_NO_PRIORITY_CEILING, 
+                                0,
+                                &pDev->dev_sem) != RTEMS_SUCCESSFUL ){
+        printk("RT: Failed to create device semaphore\n");
+        return RTEMS_INTERNAL_ERROR;
+    }
+
+    /* Default to RT-mode */
+    rt_init(pDev);
+
+    return 0;
+}
+
+static int odd_parity(unsigned int data)
+{
+    unsigned int i=0;
+
+    while(data)
+    {
+        i++;
+        data &= (data - 1);
+    } 
+
+    return !(i&1);
+}
+
+static void start_operation(rt_priv *rt)
+{
+
+}
+
+static void stop_operation(rt_priv *rt)
+{
+
+}
+
+static void set_extmdata_en(rt_priv *rt, int extmdata)
+{
+    if ( extmdata )
+        extmdata = 1;    
+    rt->ctrl_copy = (rt->ctrl_copy & ~(1<<16)) | (extmdata<<16);
+    rt->regs->ctrl = rt->ctrl_copy;
+}
+
+static void set_vector_word(rt_priv *rt, unsigned short vword)
+{
+    rt->regs->vword = vword;
+}
+
+/* Set clock speed */
+static void set_clkspd(rt_priv *rt, int spd)
+{
+    rt->ctrl_copy = (rt->ctrl_copy & ~0xC0) | (spd<<6);
+    rt->regs->ctrl = rt->ctrl_copy;
+    asm volatile("nop"::);
+    rt->regs->ctrl = rt->ctrl_copy | (1<<20);
+}
+
+static void set_rtaddr(rt_priv *rt, int addr)
+{
+    rt->ctrl_copy = (rt->ctrl_copy & ~0x3F00) | (addr << 8) | (odd_parity(addr)<<13);
+    rt->regs->ctrl = rt->ctrl_copy;
+}
+
+static void set_broadcast_en(rt_priv *rt, int data)
+{
+    rt->ctrl_copy = (rt->ctrl_copy & ~0x40000) | (data<<18);
+    rt->regs->ctrl = rt->ctrl_copy;
+}
+
+static rtems_device_driver rt_init(rt_priv *rt)
+{
+    rt->rx_blocking = 1;
+
+    if ( rt->rt_event )
+        free(rt->rt_event);
+    rt->rt_event = NULL;
+
+    rt->rt_event = (struct rt_msg *) malloc(EVENT_QUEUE_SIZE*sizeof(struct rt_msg));
+
+    if (rt->rt_event == NULL) {
+        DBG("RT driver failed to allocated memory.");
+        return RTEMS_NO_MEMORY;
+    }
+
+    rt->ctrl_copy = rt->regs->ctrl & 0x3F00; /* Keep rtaddr and rtaddrp */
+    rt->ctrl_copy |= 0x3C0D0; /* broadcast disabled, extmdata=1, writetsw = writecmd = 1 */
+    rt->regs->ctrl = rt->ctrl_copy; 
+
+    /* Set Clock speed */
+    set_clkspd(rt, rt->cfg_freq);
+
+    rt->regs->addr = rt->memarea_base_remote;
+    rt->regs->ipm  = 0x70000; /* Enable RT RX, MEM Failure and AHB Error interrupts */
+
+    DBG("B1553RT DMA_AREA: 0x%x\n", (unsigned int)rt->mem);
+
+    return RTEMS_SUCCESSFUL;
+}
+
+
+static rtems_device_driver rt_initialize(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
+{
+    return RTEMS_SUCCESSFUL;
+}
+
+static rtems_device_driver rt_open(rtems_device_major_number major, rtems_device_minor_number minor, void *arg) {
+    rt_priv *rt;
+    struct drvmgr_dev *dev;
+
+    FUNCDBG("rt_open\n");
+
+    if ( drvmgr_get_dev(&b1553rt_drv_info.general, minor, &dev) ) {
+        DBG("Wrong minor %d\n", minor);
+        return RTEMS_UNSATISFIED;
+    }
+    rt = (rt_priv *)dev->priv;
+
+    if (rtems_semaphore_obtain(rt->dev_sem, RTEMS_NO_WAIT, RTEMS_NO_TIMEOUT) != RTEMS_SUCCESSFUL) {
+        DBG("rt_open: resource in use\n");
+        return RTEMS_RESOURCE_IN_USE; /* EBUSY */
+    }
+
+    /* Set defaults */
+    rt->event_id = 0;
+
+    start_operation(rt);
+
+    /* Register interrupt routine */
+    if (drvmgr_interrupt_register(rt->dev, 0, "b1553rt", b1553rt_interrupt, rt)) {
+        rtems_semaphore_release(rt->dev_sem);
+        return -1;
+    }
+
+
+    return RTEMS_SUCCESSFUL;
+}
+ 
+static rtems_device_driver rt_close(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
+{
+    rt_priv *rt;
+    struct drvmgr_dev *dev;
+
+    FUNCDBG("rt_close");
+
+    if ( drvmgr_get_dev(&b1553rt_drv_info.general, minor, &dev) ) {
+        return RTEMS_UNSATISFIED;
+    }
+    rt = (rt_priv *)dev->priv;
+
+    drvmgr_interrupt_unregister(rt->dev, 0, b1553rt_interrupt, rt);
+
+    stop_operation(rt);
+    rtems_semaphore_release(rt->dev_sem);
+
+    return RTEMS_SUCCESSFUL;
+}
+ 
+static int get_messages(rt_priv *rt, void *buf, unsigned int msg_count)
+{
+
+    struct rt_msg *dest = (struct rt_msg *) buf;
+    int count = 0;
+
+    if (rt->head == rt->tail) {
+        return 0;
+    }
+
+    do {
+
+        DBG("rt read - head: %d, tail: %d\n", rt->head, rt->tail);
+        dest[count++] = rt->rt_event[INDEX(rt->tail++)];
+
+    } while (rt->head != rt->tail && count < msg_count);
+
+    return count;
+
+}
+static rtems_device_driver rt_read(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
+{
+    rtems_libio_rw_args_t *rw_args;
+    int count = 0;
+    rt_priv *rt;
+    struct drvmgr_dev *dev;
+
+    if ( drvmgr_get_dev(&b1553rt_drv_info.general, minor, &dev) ) {
+        return RTEMS_UNSATISFIED;
+    }
+    rt = (rt_priv *)dev->priv;
+
+    rw_args = (rtems_libio_rw_args_t *) arg;
+
+    FUNCDBG("rt_read [%i,%i]: buf: 0x%x, len: %i\n",major, minor, (unsigned int)rw_args->buffer, rw_args->count);
+
+    while ( (count = get_messages(rt,rw_args->buffer, rw_args->count)) == 0 ) {
+
+        if (rt->rx_blocking) {
+            rtems_semaphore_obtain(rt->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 rt_write(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
+{
+    rtems_libio_rw_args_t *rw_args;
+    struct rt_msg *source;
+    rt_priv *rt;
+    struct drvmgr_dev *dev;
+    unsigned int descriptor, suba, wc;
+
+    if ( drvmgr_get_dev(&b1553rt_drv_info.general, minor, &dev) ) {
+        return RTEMS_UNSATISFIED;
+    }
+    rt = (rt_priv *)dev->priv;
+
+    rw_args = (rtems_libio_rw_args_t *) arg;
+
+    if ( rw_args->count != 1 ) {
+        return RTEMS_INVALID_NAME;
+    }
+
+    source = (struct rt_msg *) rw_args->buffer;
+
+    descriptor = source[0].desc & 0x7F;
+    suba       = descriptor-32;
+    wc         = source[0].miw >> 11;
+    wc         = wc ? wc : 32;
+
+    FUNCDBG("rt_write [%i,%i]: buf: 0x%x\n",major, minor, (unsigned int)rw_args->buffer);
+
+    memcpy((void *)&rt->mem[0x400 + suba*32], &source[0].data[0], wc*2);
+
+    rw_args->bytes_moved = 1; 
+
+    return RTEMS_SUCCESSFUL;
+
+}
+
+static rtems_device_driver rt_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;
+
+    rt_priv *rt;
+    struct drvmgr_dev *dev;
+
+    FUNCDBG("rt_control[%d]: [%i,%i]\n", minor, major, minor);
+
+    if ( drvmgr_get_dev(&b1553rt_drv_info.general, minor, &dev) ) {
+        return RTEMS_UNSATISFIED;
+    }
+    rt = (rt_priv *)dev->priv;
+
+    if (!ioarg) {
+        DBG("rt_control: invalid argument\n");
+        return RTEMS_INVALID_NAME;
+    }
+
+    ioarg->ioctl_return = 0;
+    switch (ioarg->command) {
+
+    case RT_SET_ADDR:   
+        set_rtaddr(rt, data[0]);
+        break;
+
+    case RT_SET_BCE:
+        set_broadcast_en(rt, data[0]);
+        break;
+
+    case RT_SET_VECTORW:
+        set_vector_word(rt, data[0]);
+        break;
+
+    case RT_SET_EXTMDATA:
+        set_extmdata_en(rt, data[0]);
+        break;
+
+    case RT_RX_BLOCK: 
+        rt->rx_blocking     = data[0];   
+        break;
+
+    case RT_CLR_STATUS:
+        rt->status = 0;
+        break;
+
+    case RT_GET_STATUS: /* copy status */
+        if ( !ioarg->buffer )
+            return RTEMS_INVALID_NAME;
+            
+        *(unsigned int *)ioarg->buffer = rt->status;
+        break;
+
+    case RT_SET_EVENTID:
+        rt->event_id = (rtems_id)ioarg->buffer;
+        break;
+
+    default:
+        return RTEMS_NOT_IMPLEMENTED;
+    }
+
+    return RTEMS_SUCCESSFUL;
+}
+
+static void b1553rt_interrupt(void *arg)
+{
+    rt_priv *rt = arg;
+    unsigned short descriptor;
+    int signal_event=0, wake_rx_task=0;
+    unsigned int event_status=0;
+    unsigned int wc, irqv, cmd, tsw, suba, tx, miw, i;
+    unsigned int ipend;
+
+    #define SET_ERROR_DESCRIPTOR(descriptor) (event_status = (event_status & 0x0000ffff) | descriptor<<16)
+    ipend = rt->regs->ipm;
+
+    if (ipend == 0) {
+      /* IRQ mask has been cleared, we must have been reset */
+      /* Restore ctrl registers */
+      rt->regs->ctrl = rt->ctrl_copy;
+      rt->regs->addr = rt->memarea_base_remote;
+      rt->regs->ipm = 0x70000;
+      /* Send reset mode code event */
+      if (rt->head - rt->tail != EVENT_QUEUE_SIZE) {
+	miw = (8<<11);
+	descriptor = 64 + 32 + 8;
+	rt->rt_event[INDEX(rt->head)].miw = miw;
+	rt->rt_event[INDEX(rt->head)].time = 0;
+	rt->rt_event[INDEX(rt->head)].desc = descriptor;
+	rt->head++;
+      }
+    }
+
+    if ( ipend & 0x1 ) {
+      /* RT IRQ */
+      if (rt->head - rt->tail != EVENT_QUEUE_SIZE) {
+
+        irqv = rt->regs->irq;
+        cmd  = irqv >> 7;
+        wc   = cmd & 0x1F;       /* word count / mode code  */
+        suba = irqv & 0x1F;      /* sub address (0-31) */
+        tx   = (irqv >> 5) & 1;   
+ 
+        /* read status word */
+        tsw = READ_DMA(&rt->mem[tx*0x3E0+suba]); 
+
+        /* Build Message Information Word (B1553BRM-style) */
+        miw = (wc<<11) | (tsw&RT_TSW_BUS)>>4 | !(tsw&RT_TSW_OK)>>7 | (tsw&RT_TSW_ILL)>>5 | 
+            (tsw&RT_TSW_PAR)>>5 | (tsw&RT_TSW_MAN)>>7;
+
+        descriptor = (tx << 5) | suba;
+
+        /* Mode codes */
+        if (suba == 0 || suba == 31) {
+            descriptor = 64 + (tx*32) + wc;
+        }
+          
+        /* Data received or transmitted */
+        if (descriptor < 64) {
+            wc = wc ? wc : 32;   /* wc = 0 means 32 words transmitted */
+        }
+        /* RX Mode code */
+        else if (descriptor < 96) {
+            wc = (wc>>4);
+        }
+        /* TX Mode code */
+        else if (descriptor < 128) {
+            wc = (wc>>4);
+        }
+
+        /* Copy to event queue */
+        rt->rt_event[INDEX(rt->head)].miw  = miw;
+        rt->rt_event[INDEX(rt->head)].time = 0;
+
+        for (i = 0; i < wc; i++) {
+            rt->rt_event[INDEX(rt->head)].data[i] = READ_DMA(&rt->mem[tx*0x400 + suba*32 + i]);
+        }
+        rt->rt_event[INDEX(rt->head)].desc = descriptor;
+        rt->head++;
+
+
+        /* Handle errors */
+        if ( tsw & RT_TSW_ILL){
+            FUNCDBG("RT: RT_ILLCMD\n\r");
+            rt->status |= RT_ILLCMD_IRQ;
+            event_status |= RT_ILLCMD_IRQ;
+            SET_ERROR_DESCRIPTOR(descriptor);
+            signal_event=1;
+        }
+            
+        if ( !(tsw & RT_TSW_OK) ) {
+            FUNCDBG("RT: RT_MERR_IRQ\n\r");
+            rt->status |= RT_MERR_IRQ;
+            event_status |= RT_MERR_IRQ;
+            SET_ERROR_DESCRIPTOR(descriptor);
+            signal_event=1;
+        }
+	
+      }
+      else {
+        /* Indicate overrun */
+        rt->rt_event[INDEX(rt->head)].desc |= 0x8000;
+      }
+    }
+
+    if ( ipend & 0x2 ) {
+      /* Memory failure IRQ */
+      FUNCDBG("B1553RT: Memory failure\n");
+			event_status |= RT_DMAF_IRQ;
+			signal_event=1;
+    }
+
+    if ( ipend & 0x4 ) {
+      /* AHB Error */
+      FUNCDBG("B1553RT: AHB ERROR\n");
+			event_status |= RT_DMAF_IRQ;
+			signal_event=1;
+    }
+
+#ifdef DEBUG            
+    rt->log[rt->log_i++ % EVENT_QUEUE_SIZE] = descriptor; 
+    rt->log[rt->log_i++ % EVENT_QUEUE_SIZE] = cmd;
+    rt->log[rt->log_i++ % EVENT_QUEUE_SIZE] = miw;
+    rt->log[rt->log_i++ % EVENT_QUEUE_SIZE] = tsw;
+#endif
+
+    wake_rx_task = 1;
+
+    /* Wake any blocked rx thread only on receive interrupts */
+    if ( wake_rx_task ) {
+        rtems_semaphore_release(rt->rx_sem);
+    }
+
+    /* Copy current mask to status mask */
+    if ( event_status ) {
+        if ( event_status & 0xffff0000 )
+            rt->status &= 0x0000ffff;
+        rt->status |= event_status;
+    }
+
+    /* signal event once */
+    if ( signal_event && (rt->event_id != 0) ) {
+        rtems_event_send(rt->event_id, event_status);
+    }
+
+}
+
+void b1553rt_print_dev(struct drvmgr_dev *dev, int options)
+{
+    rt_priv *pDev = dev->priv;
+    struct amba_dev_info *devinfo;
+
+    devinfo = (struct amba_dev_info *)pDev->dev->businfo;
+
+    /* Print */
+    printf("--- B1553RT[%d] %s ---\n", pDev->minor, pDev->devName);
+    printf(" REGS:            0x%x\n", (unsigned int)pDev->regs);
+    printf(" IRQ:             %d\n", pDev->irqno);
+
+}
+
+void b1553rt_print(int options)
+{
+    struct amba_drv_info *drv = &b1553rt_drv_info;
+    struct drvmgr_dev *dev;
+
+    dev = drv->general.dev;
+    while(dev) {
+        b1553rt_print_dev(dev, options);
+        dev = dev->next_in_drv;
+    }
+}