Back to home page

LXR
 
 

    

File indexing completed on 2025-05-11 08:24:05 

0001 /* SPDX-License-Identifier: BSD-2-Clause */
0002 
0003 /*
0004  *  B1553RT driver implmenetation
0005  *
0006  *  COPYRIGHT (c) 2009.
0007  *  Cobham Gaisler AB.
0008  *
0009  * Redistribution and use in source and binary forms, with or without
0010  * modification, are permitted provided that the following conditions
0011  * are met:
0012  * 1. Redistributions of source code must retain the above copyright
0013  *    notice, this list of conditions and the following disclaimer.
0014  * 2. Redistributions in binary form must reproduce the above copyright
0015  *    notice, this list of conditions and the following disclaimer in the
0016  *    documentation and/or other materials provided with the distribution.
0017  *
0018  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
0019  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
0020  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
0021  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
0022  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
0023  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
0024  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
0025  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
0026  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
0027  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
0028  * POSSIBILITY OF SUCH DAMAGE.
0029  */
0030 
0031 #include <bsp.h>
0032 #include <rtems/libio.h>
0033 #include <stdlib.h>
0034 #include <stdio.h>
0035 #include <string.h>
0036 #include <assert.h>
0037 #include <ctype.h>
0038 #include <rtems/bspIo.h>
0039 
0040 #include <drvmgr/drvmgr.h>
0041 #include <grlib/b1553rt.h>
0042 #include <grlib/ambapp.h>
0043 #include <grlib/ambapp_bus.h>
0044 
0045 #include <grlib/grlib_impl.h>
0046 
0047 /* Uncomment for debug output */
0048 /*#define DEBUG 1*/
0049 
0050 /*
0051   #define FUNCDEBUG 1*/
0052 /*#undef DEBUG*/
0053 #undef FUNCDEBUG
0054 
0055 /* EVENT_QUEUE_SIZE sets the size of the event queue
0056  */
0057 #define EVENT_QUEUE_SIZE           1024  
0058 
0059 
0060 #define INDEX(x) ( x&(EVENT_QUEUE_SIZE-1) )
0061 
0062 #if 0
0063 #define DBG(x...) printk(x)
0064 #else
0065 #define DBG(x...) 
0066 #endif
0067 
0068 #ifdef FUNCDEBUG
0069 #define FUNCDBG(x...) printk(x)
0070 #else
0071 #define FUNCDBG(x...) 
0072 #endif
0073 
0074 #define READ_DMA(address) grlib_read_uncached16((unsigned int)address)
0075 
0076 static rtems_device_driver rt_initialize(rtems_device_major_number major, rtems_device_minor_number minor, void *arg);
0077 static rtems_device_driver rt_open(rtems_device_major_number major, rtems_device_minor_number minor, void *arg);
0078 static rtems_device_driver rt_close(rtems_device_major_number major, rtems_device_minor_number minor, void *arg);
0079 static rtems_device_driver rt_read(rtems_device_major_number major, rtems_device_minor_number minor, void *arg);
0080 static rtems_device_driver rt_write(rtems_device_major_number major, rtems_device_minor_number minor, void *arg);
0081 static rtems_device_driver rt_control(rtems_device_major_number major, rtems_device_minor_number minor, void *arg);
0082 
0083 #define RT_DRIVER_TABLE_ENTRY { rt_initialize, rt_open, rt_close, rt_read, rt_write, rt_control }
0084 
0085 static rtems_driver_address_table b1553rt_driver = RT_DRIVER_TABLE_ENTRY;
0086 
0087 typedef struct { 
0088 
0089     struct drvmgr_dev *dev; /* Driver manager device */
0090     char devName[52];       /* Device Name */
0091 
0092     struct rt_reg *regs;
0093     unsigned int ctrl_copy; /* Local copy of config register */
0094 
0095     unsigned int cfg_freq;
0096 
0097     unsigned int memarea_base;
0098     unsigned int memarea_base_remote;
0099 
0100     volatile unsigned short *mem;
0101 
0102     /* Received events waiting to be read */
0103     struct rt_msg *rt_event;
0104     unsigned int head, tail;
0105 
0106     int rx_blocking;
0107 
0108     rtems_id rx_sem, tx_sem, dev_sem;
0109     int minor;
0110     int irqno;
0111 
0112 #ifdef DEBUG            
0113     unsigned int log[EVENT_QUEUE_SIZE*4];
0114     unsigned int log_i;
0115 #endif
0116 
0117     unsigned int status;
0118     rtems_id event_id; /* event that may be signalled upon errors, needs to be set through ioctl command RT_SET_EVENTID */
0119 
0120 } rt_priv;
0121 
0122 static void b1553rt_interrupt(void *arg);
0123 static rtems_device_driver rt_init(rt_priv *rt);
0124 
0125 #define OFS(ofs) (((unsigned int)&ofs & 0x1ffff)>>1)
0126 
0127 static int b1553rt_driver_io_registered = 0;
0128 static rtems_device_major_number b1553rt_driver_io_major = 0;
0129 
0130 /******************* Driver manager interface ***********************/
0131 
0132 /* Driver prototypes */
0133 int b1553rt_register_io(rtems_device_major_number *m);
0134 int b1553rt_device_init(rt_priv *pDev);
0135 
0136 int b1553rt_init2(struct drvmgr_dev *dev);
0137 int b1553rt_init3(struct drvmgr_dev *dev);
0138 int b1553rt_remove(struct drvmgr_dev *dev);
0139 
0140 struct drvmgr_drv_ops b1553rt_ops = 
0141 {
0142     .init = {NULL, b1553rt_init2, b1553rt_init3, NULL},
0143     .remove = b1553rt_remove,
0144     .info = NULL
0145 };
0146 
0147 struct amba_dev_id b1553rt_ids[] = 
0148 {
0149     {VENDOR_GAISLER, GAISLER_B1553RT},
0150     {0, 0}      /* Mark end of table */
0151 };
0152 
0153 struct amba_drv_info b1553rt_drv_info =
0154 {
0155     {
0156     DRVMGR_OBJ_DRV,             /* Driver */
0157         NULL,                   /* Next driver */
0158         NULL,                   /* Device list */
0159         DRIVER_AMBAPP_GAISLER_B1553RT_ID,   /* Driver ID */
0160         "B1553RT_DRV",              /* Driver Name */
0161         DRVMGR_BUS_TYPE_AMBAPP,         /* Bus Type */
0162         &b1553rt_ops,
0163         NULL,                   /* Funcs */
0164         0,                  /* No devices yet */
0165     0,
0166 
0167     },
0168     &b1553rt_ids[0]
0169 };
0170 
0171 void b1553rt_register_drv (void)
0172 {
0173     DBG("Registering B1553RT driver\n");
0174     drvmgr_drv_register(&b1553rt_drv_info.general);
0175 }
0176 
0177 int b1553rt_init2(struct drvmgr_dev *dev)
0178 {
0179     rt_priv *priv;
0180 
0181     DBG("B1553RT[%d] on bus %s\n", dev->minor_drv, dev->parent->dev->name);
0182     priv = dev->priv = grlib_calloc(1, sizeof(*priv));
0183     if ( !priv )
0184         return DRVMGR_NOMEM;
0185     priv->dev = dev;
0186 
0187     /* This core will not find other cores, so we wait for init2() */
0188 
0189     return DRVMGR_OK;
0190 }
0191 
0192 int b1553rt_init3(struct drvmgr_dev *dev)
0193 {
0194     rt_priv *priv;
0195     char prefix[32];
0196     rtems_status_code status;
0197 
0198     priv = dev->priv;
0199 
0200     /* Do initialization */
0201 
0202     if ( b1553rt_driver_io_registered == 0) {
0203         /* Register the I/O driver only once for all cores */
0204         if ( b1553rt_register_io(&b1553rt_driver_io_major) ) {
0205             /* Failed to register I/O driver */
0206             dev->priv = NULL;
0207             return DRVMGR_FAIL;
0208         }
0209 
0210         b1553rt_driver_io_registered = 1;
0211     }
0212 
0213     /* I/O system registered and initialized 
0214      * Now we take care of device initialization.
0215      */
0216 
0217     if ( b1553rt_device_init(priv) ) {
0218         return DRVMGR_FAIL;
0219     }
0220 
0221     /* Get Filesystem name prefix */
0222     prefix[0] = '\0';
0223     if ( drvmgr_get_dev_prefix(dev, prefix) ) {
0224         /* Failed to get prefix, make sure of a unique FS name
0225          * by using the driver minor.
0226          */
0227         sprintf(priv->devName, "/dev/b1553rt%d", dev->minor_drv);
0228     } else {
0229         /* Got special prefix, this means we have a bus prefix
0230          * And we should use our "bus minor"
0231          */
0232         sprintf(priv->devName, "/dev/%sb1553rt%d", prefix, dev->minor_bus);
0233     }
0234 
0235     /* Register Device */
0236     status = rtems_io_register_name(priv->devName, b1553rt_driver_io_major, dev->minor_drv);
0237     if (status != RTEMS_SUCCESSFUL) {
0238         return DRVMGR_FAIL;
0239     }
0240 
0241     return DRVMGR_OK;
0242 }
0243 
0244 int b1553rt_remove(struct drvmgr_dev *dev)
0245 {
0246     /* Stop more tasks to open driver */
0247 
0248     /* Throw out all tasks using this driver */
0249 
0250     /* Unregister I/O node */
0251 
0252     /* Unregister and disable Interrupt */
0253 
0254     /* Free device memory */
0255 
0256     /* Return sucessfully */
0257 
0258     return DRVMGR_FAIL;
0259 }
0260 
0261 /******************* Driver Implementation ***********************/
0262 
0263 int b1553rt_register_io(rtems_device_major_number *m)
0264 {
0265     rtems_status_code r;
0266 
0267     if ((r = rtems_io_register_driver(0, &b1553rt_driver, m)) == RTEMS_SUCCESSFUL) {
0268         DBG("B1553RT driver successfully registered, major: %d\n", *m);
0269     } else {
0270         switch(r) {
0271         case RTEMS_TOO_MANY:
0272             printk("B1553RT rtems_io_register_driver failed: RTEMS_TOO_MANY\n");
0273             return -1;
0274         case RTEMS_INVALID_NUMBER:  
0275             printk("B1553RT rtems_io_register_driver failed: RTEMS_INVALID_NUMBER\n");
0276             return -1;
0277         case RTEMS_RESOURCE_IN_USE:
0278             printk("B1553RT rtems_io_register_driver failed: RTEMS_RESOURCE_IN_USE\n");
0279             return -1;
0280         default:
0281             printk("B1553RT rtems_io_register_driver failed\n");
0282             return -1;
0283         }
0284     }
0285     return 0;
0286 }
0287 
0288 int b1553rt_device_init(rt_priv *pDev)
0289 {
0290     struct amba_dev_info *ambadev;
0291     struct ambapp_core *pnpinfo;
0292     union drvmgr_key_value *value;
0293     unsigned int mem;
0294     unsigned int sys_freq_hz;
0295 
0296     /* Get device information from AMBA PnP information */
0297     ambadev = (struct amba_dev_info *)pDev->dev->businfo;
0298     if ( ambadev == NULL ) {
0299         return -1;
0300     }
0301     pnpinfo = &ambadev->info;
0302     pDev->irqno = pnpinfo->irq;
0303     pDev->regs = (struct rt_reg *)pnpinfo->apb_slv->start;
0304     pDev->minor = pDev->dev->minor_drv;
0305 
0306 #ifdef DEBUG
0307     pDev->log_i = 0;    
0308     memset(pDev->log,0,sizeof(pDev->log));
0309     printf("LOG: 0x%x\n", &pDev->log[0]);
0310     printf("LOG_I: 0x%x\n", &pDev->log_i);
0311 #endif  
0312 
0313     /* Get memory configuration from bus resources */
0314     value = drvmgr_dev_key_get(pDev->dev, "dmaBaseAdr", DRVMGR_KT_POINTER);
0315     if (value)
0316         mem = (unsigned int)value->ptr;
0317 
0318     if (value && (mem & 1)) {
0319         /* Remote address, address as RT looks at it. */
0320 
0321         /* Translate the base address into an address that the the CPU can understand */
0322         pDev->memarea_base = mem & ~1;
0323         drvmgr_translate_check(pDev->dev, DMAMEM_TO_CPU,
0324                                 (void *)pDev->memarea_base_remote,
0325                                 (void **)&pDev->memarea_base,
0326                                 4 * 1024);
0327     } else {
0328         if (!value) {
0329             /* Use dynamically allocated memory,
0330              * 4k DMA memory + 4k for alignment 
0331              */
0332             mem = (unsigned int)grlib_malloc(4 * 1024 * 2);
0333             if ( !mem ){
0334                 printk("RT: Failed to allocate HW memory\n\r");
0335                 return -1;
0336             }
0337             /* align memory to 4k boundary */
0338             pDev->memarea_base = (mem + 0xfff) & ~0xfff;
0339         } else {
0340             pDev->memarea_base = mem;
0341         }
0342 
0343         /* Translate the base address into an address that the RT core can understand */
0344         drvmgr_translate_check(pDev->dev, CPUMEM_TO_DMA,
0345                                (void *)pDev->memarea_base,
0346                                (void **)&pDev->memarea_base_remote,
0347                                4 * 1024);
0348     }
0349 
0350     /* clear the used memory */
0351     memset((char *)pDev->memarea_base, 0, 4 * 1024);
0352 
0353     /* Set base address of all descriptors */
0354     pDev->memarea_base = (unsigned int)mem;
0355     pDev->mem = (volatile unsigned short *)pDev->memarea_base;
0356 
0357     pDev->rt_event = NULL;
0358 
0359     /* The RT is always clocked at the same frequency as the bus 
0360      * If the frequency doesnt match it is defaulted to 24MHz, 
0361      * user can always override it.
0362      */
0363     pDev->cfg_freq = RT_FREQ_24MHZ;
0364 
0365     /* Get frequency in Hz */
0366     if ( drvmgr_freq_get(pDev->dev, DEV_APB_SLV, &sys_freq_hz) == 0 ) {
0367         if ( sys_freq_hz == 20000000 ) {
0368             pDev->cfg_freq = RT_FREQ_20MHZ;
0369         } else if ( sys_freq_hz == 16000000 ) {
0370             pDev->cfg_freq = RT_FREQ_16MHZ;
0371         } else if ( sys_freq_hz == 12000000 ) {
0372             pDev->cfg_freq = RT_FREQ_12MHZ;
0373         }
0374     }
0375 
0376     value = drvmgr_dev_key_get(pDev->dev, "coreFreq", DRVMGR_KT_INT);
0377     if ( value ) {
0378         pDev->cfg_freq = value->i & RT_FREQ_MASK;
0379     }
0380 
0381     /* RX Semaphore created with count = 0 */
0382     if ( rtems_semaphore_create(rtems_build_name('R', 'T', '0', '0' + pDev->minor),
0383                                 0,
0384                                 RTEMS_FIFO|RTEMS_SIMPLE_BINARY_SEMAPHORE|RTEMS_NO_INHERIT_PRIORITY|RTEMS_LOCAL|RTEMS_NO_PRIORITY_CEILING, 
0385                                 0,
0386                                 &pDev->rx_sem) != RTEMS_SUCCESSFUL ) {
0387         printk("RT: Failed to create rx semaphore\n");
0388         return RTEMS_INTERNAL_ERROR;
0389     }
0390 
0391     /* Device Semaphore created with count = 1 */
0392     if ( rtems_semaphore_create(rtems_build_name('R', 'T', '0', '0' + pDev->minor),
0393                                 1,
0394                                 RTEMS_FIFO|RTEMS_SIMPLE_BINARY_SEMAPHORE|RTEMS_NO_INHERIT_PRIORITY|RTEMS_LOCAL|RTEMS_NO_PRIORITY_CEILING, 
0395                                 0,
0396                                 &pDev->dev_sem) != RTEMS_SUCCESSFUL ){
0397         printk("RT: Failed to create device semaphore\n");
0398         return RTEMS_INTERNAL_ERROR;
0399     }
0400 
0401     /* Default to RT-mode */
0402     rt_init(pDev);
0403 
0404     return 0;
0405 }
0406 
0407 static int odd_parity(unsigned int data)
0408 {
0409     unsigned int i=0;
0410 
0411     while(data)
0412     {
0413         i++;
0414         data &= (data - 1);
0415     } 
0416 
0417     return !(i&1);
0418 }
0419 
0420 static void start_operation(rt_priv *rt)
0421 {
0422 
0423 }
0424 
0425 static void stop_operation(rt_priv *rt)
0426 {
0427 
0428 }
0429 
0430 static void set_extmdata_en(rt_priv *rt, int extmdata)
0431 {
0432     if ( extmdata )
0433         extmdata = 1;    
0434     rt->ctrl_copy = (rt->ctrl_copy & ~(1<<16)) | (extmdata<<16);
0435     rt->regs->ctrl = rt->ctrl_copy;
0436 }
0437 
0438 static void set_vector_word(rt_priv *rt, unsigned short vword)
0439 {
0440     rt->regs->vword = vword;
0441 }
0442 
0443 /* Set clock speed */
0444 static void set_clkspd(rt_priv *rt, int spd)
0445 {
0446     rt->ctrl_copy = (rt->ctrl_copy & ~0xC0) | (spd<<6);
0447     rt->regs->ctrl = rt->ctrl_copy;
0448     asm volatile("nop"::);
0449     rt->regs->ctrl = rt->ctrl_copy | (1<<20);
0450 }
0451 
0452 static void set_rtaddr(rt_priv *rt, int addr)
0453 {
0454     rt->ctrl_copy = (rt->ctrl_copy & ~0x3F00) | (addr << 8) | (odd_parity(addr)<<13);
0455     rt->regs->ctrl = rt->ctrl_copy;
0456 }
0457 
0458 static void set_broadcast_en(rt_priv *rt, int data)
0459 {
0460     rt->ctrl_copy = (rt->ctrl_copy & ~0x40000) | (data<<18);
0461     rt->regs->ctrl = rt->ctrl_copy;
0462 }
0463 
0464 static rtems_device_driver rt_init(rt_priv *rt)
0465 {
0466     rt->rx_blocking = 1;
0467 
0468     if ( rt->rt_event )
0469         free(rt->rt_event);
0470     rt->rt_event = NULL;
0471 
0472     rt->rt_event = grlib_malloc(EVENT_QUEUE_SIZE*sizeof(*rt->rt_event));
0473 
0474     if (rt->rt_event == NULL) {
0475         DBG("RT driver failed to allocated memory.");
0476         return RTEMS_NO_MEMORY;
0477     }
0478 
0479     rt->ctrl_copy = rt->regs->ctrl & 0x3F00; /* Keep rtaddr and rtaddrp */
0480     rt->ctrl_copy |= 0x3C0D0; /* broadcast disabled, extmdata=1, writetsw = writecmd = 1 */
0481     rt->regs->ctrl = rt->ctrl_copy; 
0482 
0483     /* Set Clock speed */
0484     set_clkspd(rt, rt->cfg_freq);
0485 
0486     rt->regs->addr = rt->memarea_base_remote;
0487     rt->regs->ipm  = 0x70000; /* Enable RT RX, MEM Failure and AHB Error interrupts */
0488 
0489     DBG("B1553RT DMA_AREA: 0x%x\n", (unsigned int)rt->mem);
0490 
0491     return RTEMS_SUCCESSFUL;
0492 }
0493 
0494 
0495 static rtems_device_driver rt_initialize(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
0496 {
0497     return RTEMS_SUCCESSFUL;
0498 }
0499 
0500 static rtems_device_driver rt_open(rtems_device_major_number major, rtems_device_minor_number minor, void *arg) {
0501     rt_priv *rt;
0502     struct drvmgr_dev *dev;
0503 
0504     FUNCDBG("rt_open\n");
0505 
0506     if ( drvmgr_get_dev(&b1553rt_drv_info.general, minor, &dev) ) {
0507         DBG("Wrong minor %d\n", minor);
0508         return RTEMS_UNSATISFIED;
0509     }
0510     rt = (rt_priv *)dev->priv;
0511 
0512     if (rtems_semaphore_obtain(rt->dev_sem, RTEMS_NO_WAIT, RTEMS_NO_TIMEOUT) != RTEMS_SUCCESSFUL) {
0513         DBG("rt_open: resource in use\n");
0514         return RTEMS_RESOURCE_IN_USE; /* EBUSY */
0515     }
0516 
0517     /* Set defaults */
0518     rt->event_id = 0;
0519 
0520     start_operation(rt);
0521 
0522     /* Register interrupt routine */
0523     if (drvmgr_interrupt_register(rt->dev, 0, "b1553rt", b1553rt_interrupt, rt)) {
0524         rtems_semaphore_release(rt->dev_sem);
0525         return -1;
0526     }
0527 
0528 
0529     return RTEMS_SUCCESSFUL;
0530 }
0531  
0532 static rtems_device_driver rt_close(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
0533 {
0534     rt_priv *rt;
0535     struct drvmgr_dev *dev;
0536 
0537     FUNCDBG("rt_close");
0538 
0539     if ( drvmgr_get_dev(&b1553rt_drv_info.general, minor, &dev) ) {
0540         return RTEMS_UNSATISFIED;
0541     }
0542     rt = (rt_priv *)dev->priv;
0543 
0544     drvmgr_interrupt_unregister(rt->dev, 0, b1553rt_interrupt, rt);
0545 
0546     stop_operation(rt);
0547     rtems_semaphore_release(rt->dev_sem);
0548 
0549     return RTEMS_SUCCESSFUL;
0550 }
0551  
0552 static int get_messages(rt_priv *rt, void *buf, unsigned int msg_count)
0553 {
0554 
0555     struct rt_msg *dest = (struct rt_msg *) buf;
0556     int count = 0;
0557 
0558     if (rt->head == rt->tail) {
0559         return 0;
0560     }
0561 
0562     do {
0563 
0564         DBG("rt read - head: %d, tail: %d\n", rt->head, rt->tail);
0565         dest[count++] = rt->rt_event[INDEX(rt->tail++)];
0566 
0567     } while (rt->head != rt->tail && count < msg_count);
0568 
0569     return count;
0570 
0571 }
0572 static rtems_device_driver rt_read(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
0573 {
0574     rtems_libio_rw_args_t *rw_args;
0575     int count = 0;
0576     rt_priv *rt;
0577     struct drvmgr_dev *dev;
0578 
0579     if ( drvmgr_get_dev(&b1553rt_drv_info.general, minor, &dev) ) {
0580         return RTEMS_UNSATISFIED;
0581     }
0582     rt = (rt_priv *)dev->priv;
0583 
0584     rw_args = (rtems_libio_rw_args_t *) arg;
0585 
0586     FUNCDBG("rt_read [%i,%i]: buf: 0x%x, len: %i\n",major, minor, (unsigned int)rw_args->buffer, rw_args->count);
0587 
0588     while ( (count = get_messages(rt,rw_args->buffer, rw_args->count)) == 0 ) {
0589 
0590         if (rt->rx_blocking) {
0591             rtems_semaphore_obtain(rt->rx_sem, RTEMS_WAIT, RTEMS_NO_TIMEOUT);
0592         } else {
0593             /* Translates to EBUSY */
0594             return RTEMS_RESOURCE_IN_USE;
0595         }
0596     }
0597 
0598     rw_args->bytes_moved = count;
0599     return RTEMS_SUCCESSFUL;
0600 }
0601 
0602 static rtems_device_driver rt_write(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
0603 {
0604     rtems_libio_rw_args_t *rw_args;
0605     struct rt_msg *source;
0606     rt_priv *rt;
0607     struct drvmgr_dev *dev;
0608     unsigned int descriptor, suba, wc;
0609 
0610     if ( drvmgr_get_dev(&b1553rt_drv_info.general, minor, &dev) ) {
0611         return RTEMS_UNSATISFIED;
0612     }
0613     rt = (rt_priv *)dev->priv;
0614 
0615     rw_args = (rtems_libio_rw_args_t *) arg;
0616 
0617     if ( rw_args->count != 1 ) {
0618         return RTEMS_INVALID_NAME;
0619     }
0620 
0621     source = (struct rt_msg *) rw_args->buffer;
0622 
0623     descriptor = source[0].desc & 0x7F;
0624     suba       = descriptor-32;
0625     wc         = source[0].miw >> 11;
0626     wc         = wc ? wc : 32;
0627 
0628     FUNCDBG("rt_write [%i,%i]: buf: 0x%x\n",major, minor, (unsigned int)rw_args->buffer);
0629 
0630     memcpy((void *)&rt->mem[0x400 + suba*32], &source[0].data[0], wc*2);
0631 
0632     rw_args->bytes_moved = 1; 
0633 
0634     return RTEMS_SUCCESSFUL;
0635 
0636 }
0637 
0638 static rtems_device_driver rt_control(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
0639 {
0640     rtems_libio_ioctl_args_t *ioarg = (rtems_libio_ioctl_args_t *) arg;
0641     unsigned int *data;
0642 
0643     rt_priv *rt;
0644     struct drvmgr_dev *dev;
0645 
0646     FUNCDBG("rt_control[%d]: [%i,%i]\n", minor, major, minor);
0647 
0648     if ( drvmgr_get_dev(&b1553rt_drv_info.general, minor, &dev) ) {
0649         return RTEMS_UNSATISFIED;
0650     }
0651     rt = (rt_priv *)dev->priv;
0652 
0653     if (!ioarg) {
0654         DBG("rt_control: invalid argument\n");
0655         return RTEMS_INVALID_NAME;
0656     }
0657 
0658     data = ioarg->buffer;
0659 
0660     ioarg->ioctl_return = 0;
0661     switch (ioarg->command) {
0662 
0663     case RT_SET_ADDR:   
0664         set_rtaddr(rt, data[0]);
0665         break;
0666 
0667     case RT_SET_BCE:
0668         set_broadcast_en(rt, data[0]);
0669         break;
0670 
0671     case RT_SET_VECTORW:
0672         set_vector_word(rt, data[0]);
0673         break;
0674 
0675     case RT_SET_EXTMDATA:
0676         set_extmdata_en(rt, data[0]);
0677         break;
0678 
0679     case RT_RX_BLOCK: 
0680         rt->rx_blocking     = data[0];   
0681         break;
0682 
0683     case RT_CLR_STATUS:
0684         rt->status = 0;
0685         break;
0686 
0687     case RT_GET_STATUS: /* copy status */
0688         if ( !ioarg->buffer )
0689             return RTEMS_INVALID_NAME;
0690             
0691         *(unsigned int *)ioarg->buffer = rt->status;
0692         break;
0693 
0694     case RT_SET_EVENTID:
0695         rt->event_id = (rtems_id)ioarg->buffer;
0696         break;
0697 
0698     default:
0699         return RTEMS_NOT_IMPLEMENTED;
0700     }
0701 
0702     return RTEMS_SUCCESSFUL;
0703 }
0704 
0705 static void b1553rt_interrupt(void *arg)
0706 {
0707     rt_priv *rt = arg;
0708     unsigned short descriptor;
0709     int signal_event=0, wake_rx_task=0;
0710     unsigned int event_status=0;
0711     unsigned int wc, irqv, cmd, tsw, suba, tx, miw, i;
0712     unsigned int ipend;
0713 
0714     #define SET_ERROR_DESCRIPTOR(descriptor) (event_status = (event_status & 0x0000ffff) | descriptor<<16)
0715     ipend = rt->regs->ipm;
0716 
0717     if (ipend == 0) {
0718       /* IRQ mask has been cleared, we must have been reset */
0719       /* Restore ctrl registers */
0720       rt->regs->ctrl = rt->ctrl_copy;
0721       rt->regs->addr = rt->memarea_base_remote;
0722       rt->regs->ipm = 0x70000;
0723       /* Send reset mode code event */
0724       if (rt->head - rt->tail != EVENT_QUEUE_SIZE) {
0725     miw = (8<<11);
0726     descriptor = 64 + 32 + 8;
0727     rt->rt_event[INDEX(rt->head)].miw = miw;
0728     rt->rt_event[INDEX(rt->head)].time = 0;
0729     rt->rt_event[INDEX(rt->head)].desc = descriptor;
0730     rt->head++;
0731       }
0732     }
0733 
0734     if ( ipend & 0x1 ) {
0735       /* RT IRQ */
0736       if (rt->head - rt->tail != EVENT_QUEUE_SIZE) {
0737 
0738         irqv = rt->regs->irq;
0739         cmd  = irqv >> 7;
0740         wc   = cmd & 0x1F;       /* word count / mode code  */
0741         suba = irqv & 0x1F;      /* sub address (0-31) */
0742         tx   = (irqv >> 5) & 1;   
0743  
0744         /* read status word */
0745         tsw = READ_DMA(&rt->mem[tx*0x3E0+suba]); 
0746 
0747         /* Build Message Information Word (B1553BRM-style) */
0748         miw = (wc<<11) | (tsw&RT_TSW_BUS)>>4 | !(tsw&RT_TSW_OK)<<7 | (tsw&RT_TSW_ILL)>>5 | 
0749             (tsw&RT_TSW_PAR)>>5 | (tsw&RT_TSW_MAN)>>7;
0750 
0751         descriptor = (tx << 5) | suba;
0752 
0753         /* Mode codes */
0754         if (suba == 0 || suba == 31) {
0755             descriptor = 64 + (tx*32) + wc;
0756         }
0757           
0758         /* Data received or transmitted */
0759         if (descriptor < 64) {
0760             wc = wc ? wc : 32;   /* wc = 0 means 32 words transmitted */
0761         }
0762         /* RX Mode code */
0763         else if (descriptor < 96) {
0764             wc = (wc>>4);
0765         }
0766         /* TX Mode code */
0767         else if (descriptor < 128) {
0768             wc = (wc>>4);
0769         }
0770 
0771         /* Copy to event queue */
0772         rt->rt_event[INDEX(rt->head)].miw  = miw;
0773         rt->rt_event[INDEX(rt->head)].time = 0;
0774 
0775         for (i = 0; i < wc; i++) {
0776             rt->rt_event[INDEX(rt->head)].data[i] = READ_DMA(&rt->mem[tx*0x400 + suba*32 + i]);
0777         }
0778         rt->rt_event[INDEX(rt->head)].desc = descriptor;
0779         rt->head++;
0780 
0781 
0782         /* Handle errors */
0783         if ( tsw & RT_TSW_ILL){
0784             FUNCDBG("RT: RT_ILLCMD\n\r");
0785             rt->status |= RT_ILLCMD_IRQ;
0786             event_status |= RT_ILLCMD_IRQ;
0787             SET_ERROR_DESCRIPTOR(descriptor);
0788             signal_event=1;
0789         }
0790             
0791         if ( !(tsw & RT_TSW_OK) ) {
0792             FUNCDBG("RT: RT_MERR_IRQ\n\r");
0793             rt->status |= RT_MERR_IRQ;
0794             event_status |= RT_MERR_IRQ;
0795             SET_ERROR_DESCRIPTOR(descriptor);
0796             signal_event=1;
0797         }
0798     
0799       }
0800       else {
0801         /* Indicate overrun */
0802         rt->rt_event[INDEX(rt->head)].desc |= 0x8000;
0803       }
0804     }
0805 
0806     if ( ipend & 0x2 ) {
0807       /* Memory failure IRQ */
0808       FUNCDBG("B1553RT: Memory failure\n");
0809             event_status |= RT_DMAF_IRQ;
0810             signal_event=1;
0811     }
0812 
0813     if ( ipend & 0x4 ) {
0814       /* AHB Error */
0815       FUNCDBG("B1553RT: AHB ERROR\n");
0816             event_status |= RT_DMAF_IRQ;
0817             signal_event=1;
0818     }
0819 
0820 #ifdef DEBUG            
0821     rt->log[rt->log_i++ % EVENT_QUEUE_SIZE] = descriptor; 
0822     rt->log[rt->log_i++ % EVENT_QUEUE_SIZE] = cmd;
0823     rt->log[rt->log_i++ % EVENT_QUEUE_SIZE] = miw;
0824     rt->log[rt->log_i++ % EVENT_QUEUE_SIZE] = tsw;
0825 #endif
0826 
0827     wake_rx_task = 1;
0828 
0829     /* Wake any blocked rx thread only on receive interrupts */
0830     if ( wake_rx_task ) {
0831         rtems_semaphore_release(rt->rx_sem);
0832     }
0833 
0834     /* Copy current mask to status mask */
0835     if ( event_status ) {
0836         if ( event_status & 0xffff0000 )
0837             rt->status &= 0x0000ffff;
0838         rt->status |= event_status;
0839     }
0840 
0841     /* signal event once */
0842     if ( signal_event && (rt->event_id != 0) ) {
0843         rtems_event_send(rt->event_id, event_status);
0844     }
0845 
0846 }
0847 
0848 void b1553rt_print_dev(struct drvmgr_dev *dev, int options)
0849 {
0850     rt_priv *pDev = dev->priv;
0851 
0852     /* Print */
0853     printf("--- B1553RT[%d] %s ---\n", pDev->minor, pDev->devName);
0854     printf(" REGS:            0x%x\n", (unsigned int)pDev->regs);
0855     printf(" IRQ:             %d\n", pDev->irqno);
0856 
0857 }
0858 
0859 void b1553rt_print(int options)
0860 {
0861     struct amba_drv_info *drv = &b1553rt_drv_info;
0862     struct drvmgr_dev *dev;
0863 
0864     dev = drv->general.dev;
0865     while(dev) {
0866         b1553rt_print_dev(dev, options);
0867         dev = dev->next_in_drv;
0868     }
0869 }
[ Source navigation ] [ Diff markup ] [ Identifier search ] [ general search ]

This page was automatically generated by the 2.3.7 LXR engine.

The LXR team
Valid CSS 2.1! Valid HTML 4.01!