source: rtems/bsps/sparc/shared/1553/b1553rt.c @ 31720925

Last change on this file since 31720925 was 31720925, checked in by Sebastian Huber <sebastian.huber@…>, on Dec 22, 2018 at 6:13:44 AM

grlib: Move header files

Update #3678.

  • Property mode set to 100644
File size: 22.8 KB
Line 
1/*
2 *  B1553RT driver implmenetation
3 *
4 *  COPYRIGHT (c) 2009.
5 *  Cobham Gaisler AB.
6 *
7 *  The license and distribution terms for this file may be
8 *  found in the file LICENSE in this distribution or at
9 *  http://www.rtems.org/license/LICENSE.
10 */
11
12#include <bsp.h>
13#include <rtems/libio.h>
14#include <stdlib.h>
15#include <stdio.h>
16#include <string.h>
17#include <assert.h>
18#include <ctype.h>
19#include <rtems/bspIo.h>
20
21#include <drvmgr/drvmgr.h>
22#include <grlib/b1553rt.h>
23#include <grlib/ambapp.h>
24#include <grlib/ambapp_bus.h>
25
26#include <grlib/grlib_impl.h>
27
28/* Uncomment for debug output */
29/*#define DEBUG 1*/
30
31/*
32  #define FUNCDEBUG 1*/
33/*#undef DEBUG*/
34#undef FUNCDEBUG
35
36/* EVENT_QUEUE_SIZE sets the size of the event queue
37 */
38#define EVENT_QUEUE_SIZE           1024 
39
40
41#define INDEX(x) ( x&(EVENT_QUEUE_SIZE-1) )
42
43#if 0
44#define DBG(x...) printk(x)
45#else
46#define DBG(x...)
47#endif
48
49#ifdef FUNCDEBUG
50#define FUNCDBG(x...) printk(x)
51#else
52#define FUNCDBG(x...)
53#endif
54
55#define READ_DMA(address) _READ16((unsigned int)address)
56
57static __inline__ unsigned short _READ16(unsigned int addr) {
58    unsigned short tmp;
59    asm(" lduha [%1]1, %0 "
60        : "=r"(tmp)
61        : "r"(addr)
62        );
63    return tmp;
64}
65
66static rtems_device_driver rt_initialize(rtems_device_major_number major, rtems_device_minor_number minor, void *arg);
67static rtems_device_driver rt_open(rtems_device_major_number major, rtems_device_minor_number minor, void *arg);
68static rtems_device_driver rt_close(rtems_device_major_number major, rtems_device_minor_number minor, void *arg);
69static rtems_device_driver rt_read(rtems_device_major_number major, rtems_device_minor_number minor, void *arg);
70static rtems_device_driver rt_write(rtems_device_major_number major, rtems_device_minor_number minor, void *arg);
71static rtems_device_driver rt_control(rtems_device_major_number major, rtems_device_minor_number minor, void *arg);
72
73#define RT_DRIVER_TABLE_ENTRY { rt_initialize, rt_open, rt_close, rt_read, rt_write, rt_control }
74
75static rtems_driver_address_table b1553rt_driver = RT_DRIVER_TABLE_ENTRY;
76
77typedef struct { 
78
79    struct drvmgr_dev *dev; /* Driver manager device */
80    char devName[32];     /* Device Name */
81
82    struct rt_reg *regs;
83    unsigned int ctrl_copy; /* Local copy of config register */
84
85    unsigned int cfg_freq;
86
87    unsigned int memarea_base;
88    unsigned int memarea_base_remote;
89
90    volatile unsigned short *mem;
91
92    /* Received events waiting to be read */
93    struct rt_msg *rt_event;
94    unsigned int head, tail;
95
96    int rx_blocking;
97
98    rtems_id rx_sem, tx_sem, dev_sem;
99    int minor;
100    int irqno;
101
102#ifdef DEBUG                   
103    unsigned int log[EVENT_QUEUE_SIZE*4];
104    unsigned int log_i;
105#endif
106
107    unsigned int status;
108    rtems_id event_id; /* event that may be signalled upon errors, needs to be set through ioctl command RT_SET_EVENTID */
109
110} rt_priv;
111
112static void b1553rt_interrupt(void *arg);
113static rtems_device_driver rt_init(rt_priv *rt);
114
115#define OFS(ofs) (((unsigned int)&ofs & 0x1ffff)>>1)
116
117static int b1553rt_driver_io_registered = 0;
118static rtems_device_major_number b1553rt_driver_io_major = 0;
119
120/******************* Driver manager interface ***********************/
121
122/* Driver prototypes */
123int b1553rt_register_io(rtems_device_major_number *m);
124int b1553rt_device_init(rt_priv *pDev);
125
126int b1553rt_init2(struct drvmgr_dev *dev);
127int b1553rt_init3(struct drvmgr_dev *dev);
128int b1553rt_remove(struct drvmgr_dev *dev);
129
130struct drvmgr_drv_ops b1553rt_ops = 
131{
132    .init = {NULL, b1553rt_init2, b1553rt_init3, NULL},
133    .remove = b1553rt_remove,
134    .info = NULL
135};
136
137struct amba_dev_id b1553rt_ids[] = 
138{
139    {VENDOR_GAISLER, GAISLER_B1553RT},
140    {0, 0}              /* Mark end of table */
141};
142
143struct amba_drv_info b1553rt_drv_info =
144{
145    {
146        DRVMGR_OBJ_DRV,                         /* Driver */
147        NULL,                                   /* Next driver */
148        NULL,                                   /* Device list */
149        DRIVER_AMBAPP_GAISLER_B1553RT_ID,       /* Driver ID */
150        "B1553RT_DRV",                          /* Driver Name */
151        DRVMGR_BUS_TYPE_AMBAPP,                 /* Bus Type */
152        &b1553rt_ops,
153        NULL,                                   /* Funcs */
154        0,                                      /* No devices yet */
155        0,
156
157    },
158    &b1553rt_ids[0]
159};
160
161void b1553rt_register_drv (void)
162{
163    DBG("Registering B1553RT driver\n");
164    drvmgr_drv_register(&b1553rt_drv_info.general);
165}
166
167int b1553rt_init2(struct drvmgr_dev *dev)
168{
169    rt_priv *priv;
170
171    DBG("B1553RT[%d] on bus %s\n", dev->minor_drv, dev->parent->dev->name);
172    priv = dev->priv = grlib_calloc(1, sizeof(*priv));
173    if ( !priv )
174        return DRVMGR_NOMEM;
175    priv->dev = dev;
176
177    /* This core will not find other cores, so we wait for init2() */
178
179    return DRVMGR_OK;
180}
181
182int b1553rt_init3(struct drvmgr_dev *dev)
183{
184    rt_priv *priv;
185    char prefix[32];
186    rtems_status_code status;
187
188    priv = dev->priv;
189
190    /* Do initialization */
191
192    if ( b1553rt_driver_io_registered == 0) {
193        /* Register the I/O driver only once for all cores */
194        if ( b1553rt_register_io(&b1553rt_driver_io_major) ) {
195            /* Failed to register I/O driver */
196            dev->priv = NULL;
197            return DRVMGR_FAIL;
198        }
199
200        b1553rt_driver_io_registered = 1;
201    }
202
203    /* I/O system registered and initialized
204     * Now we take care of device initialization.
205     */
206
207    if ( b1553rt_device_init(priv) ) {
208        return DRVMGR_FAIL;
209    }
210
211    /* Get Filesystem name prefix */
212    prefix[0] = '\0';
213    if ( drvmgr_get_dev_prefix(dev, prefix) ) {
214        /* Failed to get prefix, make sure of a unique FS name
215         * by using the driver minor.
216         */
217        sprintf(priv->devName, "/dev/b1553rt%d", dev->minor_drv);
218    } else {
219        /* Got special prefix, this means we have a bus prefix
220         * And we should use our "bus minor"
221         */
222        sprintf(priv->devName, "/dev/%sb1553rt%d", prefix, dev->minor_bus);
223    }
224
225    /* Register Device */
226    status = rtems_io_register_name(priv->devName, b1553rt_driver_io_major, dev->minor_drv);
227    if (status != RTEMS_SUCCESSFUL) {
228        return DRVMGR_FAIL;
229    }
230
231    return DRVMGR_OK;
232}
233
234int b1553rt_remove(struct drvmgr_dev *dev)
235{
236    /* Stop more tasks to open driver */
237
238    /* Throw out all tasks using this driver */
239
240    /* Unregister I/O node */
241
242    /* Unregister and disable Interrupt */
243
244    /* Free device memory */
245
246    /* Return sucessfully */
247
248    return DRVMGR_FAIL;
249}
250
251/******************* Driver Implementation ***********************/
252
253int b1553rt_register_io(rtems_device_major_number *m)
254{
255    rtems_status_code r;
256
257    if ((r = rtems_io_register_driver(0, &b1553rt_driver, m)) == RTEMS_SUCCESSFUL) {
258        DBG("B1553RT driver successfully registered, major: %d\n", *m);
259    } else {
260        switch(r) {
261        case RTEMS_TOO_MANY:
262            printk("B1553RT rtems_io_register_driver failed: RTEMS_TOO_MANY\n");
263            return -1;
264        case RTEMS_INVALID_NUMBER: 
265            printk("B1553RT rtems_io_register_driver failed: RTEMS_INVALID_NUMBER\n");
266            return -1;
267        case RTEMS_RESOURCE_IN_USE:
268            printk("B1553RT rtems_io_register_driver failed: RTEMS_RESOURCE_IN_USE\n");
269            return -1;
270        default:
271            printk("B1553RT rtems_io_register_driver failed\n");
272            return -1;
273        }
274    }
275    return 0;
276}
277
278int b1553rt_device_init(rt_priv *pDev)
279{
280    struct amba_dev_info *ambadev;
281    struct ambapp_core *pnpinfo;
282    union drvmgr_key_value *value;
283    unsigned int mem;
284    unsigned int sys_freq_hz;
285
286    /* Get device information from AMBA PnP information */
287    ambadev = (struct amba_dev_info *)pDev->dev->businfo;
288    if ( ambadev == NULL ) {
289        return -1;
290    }
291    pnpinfo = &ambadev->info;
292    pDev->irqno = pnpinfo->irq;
293    pDev->regs = (struct rt_reg *)pnpinfo->apb_slv->start;
294    pDev->minor = pDev->dev->minor_drv;
295
296#ifdef DEBUG
297    pDev->log_i = 0;   
298    memset(pDev->log,0,sizeof(pDev->log));
299    printf("LOG: 0x%x\n", &pDev->log[0]);
300    printf("LOG_I: 0x%x\n", &pDev->log_i);
301#endif 
302
303    /* Get memory configuration from bus resources */
304    value = drvmgr_dev_key_get(pDev->dev, "dmaBaseAdr", DRVMGR_KT_POINTER);
305    if (value)
306        mem = (unsigned int)value->ptr;
307
308    if (value && (mem & 1)) {
309        /* Remote address, address as RT looks at it. */
310
311        /* Translate the base address into an address that the the CPU can understand */
312        pDev->memarea_base = mem & ~1;
313        drvmgr_translate_check(pDev->dev, DMAMEM_TO_CPU,
314                                (void *)pDev->memarea_base_remote,
315                                (void **)&pDev->memarea_base,
316                                4 * 1024);
317    } else {
318        if (!value) {
319            /* Use dynamically allocated memory,
320             * 4k DMA memory + 4k for alignment
321             */
322            mem = (unsigned int)grlib_malloc(4 * 1024 * 2);
323            if ( !mem ){
324                printk("RT: Failed to allocate HW memory\n\r");
325                return -1;
326            }
327            /* align memory to 4k boundary */
328            pDev->memarea_base = (mem + 0xfff) & ~0xfff;
329        } else {
330            pDev->memarea_base = mem;
331        }
332
333        /* Translate the base address into an address that the RT core can understand */
334        drvmgr_translate_check(pDev->dev, CPUMEM_TO_DMA,
335                               (void *)pDev->memarea_base,
336                               (void **)&pDev->memarea_base_remote,
337                               4 * 1024);
338    }
339
340    /* clear the used memory */
341    memset((char *)pDev->memarea_base, 0, 4 * 1024);
342
343    /* Set base address of all descriptors */
344    pDev->memarea_base = (unsigned int)mem;
345    pDev->mem = (volatile unsigned short *)pDev->memarea_base;
346
347    pDev->rt_event = NULL;
348
349    /* The RT is always clocked at the same frequency as the bus
350     * If the frequency doesnt match it is defaulted to 24MHz,
351     * user can always override it.
352     */
353    pDev->cfg_freq = RT_FREQ_24MHZ;
354
355    /* Get frequency in Hz */
356    if ( drvmgr_freq_get(pDev->dev, DEV_APB_SLV, &sys_freq_hz) == 0 ) {
357        if ( sys_freq_hz == 20000000 ) {
358            pDev->cfg_freq = RT_FREQ_20MHZ;
359        } else if ( sys_freq_hz == 16000000 ) {
360            pDev->cfg_freq = RT_FREQ_16MHZ;
361        } else if ( sys_freq_hz == 12000000 ) {
362            pDev->cfg_freq = RT_FREQ_12MHZ;
363        }
364    }
365
366    value = drvmgr_dev_key_get(pDev->dev, "coreFreq", DRVMGR_KT_INT);
367    if ( value ) {
368        pDev->cfg_freq = value->i & RT_FREQ_MASK;
369    }
370
371    /* RX Semaphore created with count = 0 */
372    if ( rtems_semaphore_create(rtems_build_name('R', 'T', '0', '0' + pDev->minor),
373                                0,
374                                RTEMS_FIFO|RTEMS_SIMPLE_BINARY_SEMAPHORE|RTEMS_NO_INHERIT_PRIORITY|RTEMS_LOCAL|RTEMS_NO_PRIORITY_CEILING, 
375                                0,
376                                &pDev->rx_sem) != RTEMS_SUCCESSFUL ) {
377        printk("RT: Failed to create rx semaphore\n");
378        return RTEMS_INTERNAL_ERROR;
379    }
380
381    /* Device Semaphore created with count = 1 */
382    if ( rtems_semaphore_create(rtems_build_name('R', 'T', '0', '0' + pDev->minor),
383                                1,
384                                RTEMS_FIFO|RTEMS_SIMPLE_BINARY_SEMAPHORE|RTEMS_NO_INHERIT_PRIORITY|RTEMS_LOCAL|RTEMS_NO_PRIORITY_CEILING, 
385                                0,
386                                &pDev->dev_sem) != RTEMS_SUCCESSFUL ){
387        printk("RT: Failed to create device semaphore\n");
388        return RTEMS_INTERNAL_ERROR;
389    }
390
391    /* Default to RT-mode */
392    rt_init(pDev);
393
394    return 0;
395}
396
397static int odd_parity(unsigned int data)
398{
399    unsigned int i=0;
400
401    while(data)
402    {
403        i++;
404        data &= (data - 1);
405    } 
406
407    return !(i&1);
408}
409
410static void start_operation(rt_priv *rt)
411{
412
413}
414
415static void stop_operation(rt_priv *rt)
416{
417
418}
419
420static void set_extmdata_en(rt_priv *rt, int extmdata)
421{
422    if ( extmdata )
423        extmdata = 1;   
424    rt->ctrl_copy = (rt->ctrl_copy & ~(1<<16)) | (extmdata<<16);
425    rt->regs->ctrl = rt->ctrl_copy;
426}
427
428static void set_vector_word(rt_priv *rt, unsigned short vword)
429{
430    rt->regs->vword = vword;
431}
432
433/* Set clock speed */
434static void set_clkspd(rt_priv *rt, int spd)
435{
436    rt->ctrl_copy = (rt->ctrl_copy & ~0xC0) | (spd<<6);
437    rt->regs->ctrl = rt->ctrl_copy;
438    asm volatile("nop"::);
439    rt->regs->ctrl = rt->ctrl_copy | (1<<20);
440}
441
442static void set_rtaddr(rt_priv *rt, int addr)
443{
444    rt->ctrl_copy = (rt->ctrl_copy & ~0x3F00) | (addr << 8) | (odd_parity(addr)<<13);
445    rt->regs->ctrl = rt->ctrl_copy;
446}
447
448static void set_broadcast_en(rt_priv *rt, int data)
449{
450    rt->ctrl_copy = (rt->ctrl_copy & ~0x40000) | (data<<18);
451    rt->regs->ctrl = rt->ctrl_copy;
452}
453
454static rtems_device_driver rt_init(rt_priv *rt)
455{
456    rt->rx_blocking = 1;
457
458    if ( rt->rt_event )
459        free(rt->rt_event);
460    rt->rt_event = NULL;
461
462    rt->rt_event = grlib_malloc(EVENT_QUEUE_SIZE*sizeof(*rt->rt_event));
463
464    if (rt->rt_event == NULL) {
465        DBG("RT driver failed to allocated memory.");
466        return RTEMS_NO_MEMORY;
467    }
468
469    rt->ctrl_copy = rt->regs->ctrl & 0x3F00; /* Keep rtaddr and rtaddrp */
470    rt->ctrl_copy |= 0x3C0D0; /* broadcast disabled, extmdata=1, writetsw = writecmd = 1 */
471    rt->regs->ctrl = rt->ctrl_copy; 
472
473    /* Set Clock speed */
474    set_clkspd(rt, rt->cfg_freq);
475
476    rt->regs->addr = rt->memarea_base_remote;
477    rt->regs->ipm  = 0x70000; /* Enable RT RX, MEM Failure and AHB Error interrupts */
478
479    DBG("B1553RT DMA_AREA: 0x%x\n", (unsigned int)rt->mem);
480
481    return RTEMS_SUCCESSFUL;
482}
483
484
485static rtems_device_driver rt_initialize(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
486{
487    return RTEMS_SUCCESSFUL;
488}
489
490static rtems_device_driver rt_open(rtems_device_major_number major, rtems_device_minor_number minor, void *arg) {
491    rt_priv *rt;
492    struct drvmgr_dev *dev;
493
494    FUNCDBG("rt_open\n");
495
496    if ( drvmgr_get_dev(&b1553rt_drv_info.general, minor, &dev) ) {
497        DBG("Wrong minor %d\n", minor);
498        return RTEMS_UNSATISFIED;
499    }
500    rt = (rt_priv *)dev->priv;
501
502    if (rtems_semaphore_obtain(rt->dev_sem, RTEMS_NO_WAIT, RTEMS_NO_TIMEOUT) != RTEMS_SUCCESSFUL) {
503        DBG("rt_open: resource in use\n");
504        return RTEMS_RESOURCE_IN_USE; /* EBUSY */
505    }
506
507    /* Set defaults */
508    rt->event_id = 0;
509
510    start_operation(rt);
511
512    /* Register interrupt routine */
513    if (drvmgr_interrupt_register(rt->dev, 0, "b1553rt", b1553rt_interrupt, rt)) {
514        rtems_semaphore_release(rt->dev_sem);
515        return -1;
516    }
517
518
519    return RTEMS_SUCCESSFUL;
520}
521 
522static rtems_device_driver rt_close(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
523{
524    rt_priv *rt;
525    struct drvmgr_dev *dev;
526
527    FUNCDBG("rt_close");
528
529    if ( drvmgr_get_dev(&b1553rt_drv_info.general, minor, &dev) ) {
530        return RTEMS_UNSATISFIED;
531    }
532    rt = (rt_priv *)dev->priv;
533
534    drvmgr_interrupt_unregister(rt->dev, 0, b1553rt_interrupt, rt);
535
536    stop_operation(rt);
537    rtems_semaphore_release(rt->dev_sem);
538
539    return RTEMS_SUCCESSFUL;
540}
541 
542static int get_messages(rt_priv *rt, void *buf, unsigned int msg_count)
543{
544
545    struct rt_msg *dest = (struct rt_msg *) buf;
546    int count = 0;
547
548    if (rt->head == rt->tail) {
549        return 0;
550    }
551
552    do {
553
554        DBG("rt read - head: %d, tail: %d\n", rt->head, rt->tail);
555        dest[count++] = rt->rt_event[INDEX(rt->tail++)];
556
557    } while (rt->head != rt->tail && count < msg_count);
558
559    return count;
560
561}
562static rtems_device_driver rt_read(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
563{
564    rtems_libio_rw_args_t *rw_args;
565    int count = 0;
566    rt_priv *rt;
567    struct drvmgr_dev *dev;
568
569    if ( drvmgr_get_dev(&b1553rt_drv_info.general, minor, &dev) ) {
570        return RTEMS_UNSATISFIED;
571    }
572    rt = (rt_priv *)dev->priv;
573
574    rw_args = (rtems_libio_rw_args_t *) arg;
575
576    FUNCDBG("rt_read [%i,%i]: buf: 0x%x, len: %i\n",major, minor, (unsigned int)rw_args->buffer, rw_args->count);
577
578    while ( (count = get_messages(rt,rw_args->buffer, rw_args->count)) == 0 ) {
579
580        if (rt->rx_blocking) {
581            rtems_semaphore_obtain(rt->rx_sem, RTEMS_WAIT, RTEMS_NO_TIMEOUT);
582        } else {
583            /* Translates to EBUSY */
584            return RTEMS_RESOURCE_IN_USE;
585        }
586    }
587
588    rw_args->bytes_moved = count;
589    return RTEMS_SUCCESSFUL;
590}
591
592static rtems_device_driver rt_write(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
593{
594    rtems_libio_rw_args_t *rw_args;
595    struct rt_msg *source;
596    rt_priv *rt;
597    struct drvmgr_dev *dev;
598    unsigned int descriptor, suba, wc;
599
600    if ( drvmgr_get_dev(&b1553rt_drv_info.general, minor, &dev) ) {
601        return RTEMS_UNSATISFIED;
602    }
603    rt = (rt_priv *)dev->priv;
604
605    rw_args = (rtems_libio_rw_args_t *) arg;
606
607    if ( rw_args->count != 1 ) {
608        return RTEMS_INVALID_NAME;
609    }
610
611    source = (struct rt_msg *) rw_args->buffer;
612
613    descriptor = source[0].desc & 0x7F;
614    suba       = descriptor-32;
615    wc         = source[0].miw >> 11;
616    wc         = wc ? wc : 32;
617
618    FUNCDBG("rt_write [%i,%i]: buf: 0x%x\n",major, minor, (unsigned int)rw_args->buffer);
619
620    memcpy((void *)&rt->mem[0x400 + suba*32], &source[0].data[0], wc*2);
621
622    rw_args->bytes_moved = 1; 
623
624    return RTEMS_SUCCESSFUL;
625
626}
627
628static rtems_device_driver rt_control(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
629{
630    rtems_libio_ioctl_args_t *ioarg = (rtems_libio_ioctl_args_t *) arg;
631    unsigned int *data = ioarg->buffer;
632
633    rt_priv *rt;
634    struct drvmgr_dev *dev;
635
636    FUNCDBG("rt_control[%d]: [%i,%i]\n", minor, major, minor);
637
638    if ( drvmgr_get_dev(&b1553rt_drv_info.general, minor, &dev) ) {
639        return RTEMS_UNSATISFIED;
640    }
641    rt = (rt_priv *)dev->priv;
642
643    if (!ioarg) {
644        DBG("rt_control: invalid argument\n");
645        return RTEMS_INVALID_NAME;
646    }
647
648    ioarg->ioctl_return = 0;
649    switch (ioarg->command) {
650
651    case RT_SET_ADDR:   
652        set_rtaddr(rt, data[0]);
653        break;
654
655    case RT_SET_BCE:
656        set_broadcast_en(rt, data[0]);
657        break;
658
659    case RT_SET_VECTORW:
660        set_vector_word(rt, data[0]);
661        break;
662
663    case RT_SET_EXTMDATA:
664        set_extmdata_en(rt, data[0]);
665        break;
666
667    case RT_RX_BLOCK: 
668        rt->rx_blocking     = data[0];   
669        break;
670
671    case RT_CLR_STATUS:
672        rt->status = 0;
673        break;
674
675    case RT_GET_STATUS: /* copy status */
676        if ( !ioarg->buffer )
677            return RTEMS_INVALID_NAME;
678           
679        *(unsigned int *)ioarg->buffer = rt->status;
680        break;
681
682    case RT_SET_EVENTID:
683        rt->event_id = (rtems_id)ioarg->buffer;
684        break;
685
686    default:
687        return RTEMS_NOT_IMPLEMENTED;
688    }
689
690    return RTEMS_SUCCESSFUL;
691}
692
693static void b1553rt_interrupt(void *arg)
694{
695    rt_priv *rt = arg;
696    unsigned short descriptor;
697    int signal_event=0, wake_rx_task=0;
698    unsigned int event_status=0;
699    unsigned int wc, irqv, cmd, tsw, suba, tx, miw, i;
700    unsigned int ipend;
701
702    #define SET_ERROR_DESCRIPTOR(descriptor) (event_status = (event_status & 0x0000ffff) | descriptor<<16)
703    ipend = rt->regs->ipm;
704
705    if (ipend == 0) {
706      /* IRQ mask has been cleared, we must have been reset */
707      /* Restore ctrl registers */
708      rt->regs->ctrl = rt->ctrl_copy;
709      rt->regs->addr = rt->memarea_base_remote;
710      rt->regs->ipm = 0x70000;
711      /* Send reset mode code event */
712      if (rt->head - rt->tail != EVENT_QUEUE_SIZE) {
713        miw = (8<<11);
714        descriptor = 64 + 32 + 8;
715        rt->rt_event[INDEX(rt->head)].miw = miw;
716        rt->rt_event[INDEX(rt->head)].time = 0;
717        rt->rt_event[INDEX(rt->head)].desc = descriptor;
718        rt->head++;
719      }
720    }
721
722    if ( ipend & 0x1 ) {
723      /* RT IRQ */
724      if (rt->head - rt->tail != EVENT_QUEUE_SIZE) {
725
726        irqv = rt->regs->irq;
727        cmd  = irqv >> 7;
728        wc   = cmd & 0x1F;       /* word count / mode code  */
729        suba = irqv & 0x1F;      /* sub address (0-31) */
730        tx   = (irqv >> 5) & 1;   
731 
732        /* read status word */
733        tsw = READ_DMA(&rt->mem[tx*0x3E0+suba]); 
734
735        /* Build Message Information Word (B1553BRM-style) */
736        miw = (wc<<11) | (tsw&RT_TSW_BUS)>>4 | !(tsw&RT_TSW_OK)>>7 | (tsw&RT_TSW_ILL)>>5 | 
737            (tsw&RT_TSW_PAR)>>5 | (tsw&RT_TSW_MAN)>>7;
738
739        descriptor = (tx << 5) | suba;
740
741        /* Mode codes */
742        if (suba == 0 || suba == 31) {
743            descriptor = 64 + (tx*32) + wc;
744        }
745         
746        /* Data received or transmitted */
747        if (descriptor < 64) {
748            wc = wc ? wc : 32;   /* wc = 0 means 32 words transmitted */
749        }
750        /* RX Mode code */
751        else if (descriptor < 96) {
752            wc = (wc>>4);
753        }
754        /* TX Mode code */
755        else if (descriptor < 128) {
756            wc = (wc>>4);
757        }
758
759        /* Copy to event queue */
760        rt->rt_event[INDEX(rt->head)].miw  = miw;
761        rt->rt_event[INDEX(rt->head)].time = 0;
762
763        for (i = 0; i < wc; i++) {
764            rt->rt_event[INDEX(rt->head)].data[i] = READ_DMA(&rt->mem[tx*0x400 + suba*32 + i]);
765        }
766        rt->rt_event[INDEX(rt->head)].desc = descriptor;
767        rt->head++;
768
769
770        /* Handle errors */
771        if ( tsw & RT_TSW_ILL){
772            FUNCDBG("RT: RT_ILLCMD\n\r");
773            rt->status |= RT_ILLCMD_IRQ;
774            event_status |= RT_ILLCMD_IRQ;
775            SET_ERROR_DESCRIPTOR(descriptor);
776            signal_event=1;
777        }
778           
779        if ( !(tsw & RT_TSW_OK) ) {
780            FUNCDBG("RT: RT_MERR_IRQ\n\r");
781            rt->status |= RT_MERR_IRQ;
782            event_status |= RT_MERR_IRQ;
783            SET_ERROR_DESCRIPTOR(descriptor);
784            signal_event=1;
785        }
786       
787      }
788      else {
789        /* Indicate overrun */
790        rt->rt_event[INDEX(rt->head)].desc |= 0x8000;
791      }
792    }
793
794    if ( ipend & 0x2 ) {
795      /* Memory failure IRQ */
796      FUNCDBG("B1553RT: Memory failure\n");
797                        event_status |= RT_DMAF_IRQ;
798                        signal_event=1;
799    }
800
801    if ( ipend & 0x4 ) {
802      /* AHB Error */
803      FUNCDBG("B1553RT: AHB ERROR\n");
804                        event_status |= RT_DMAF_IRQ;
805                        signal_event=1;
806    }
807
808#ifdef DEBUG           
809    rt->log[rt->log_i++ % EVENT_QUEUE_SIZE] = descriptor; 
810    rt->log[rt->log_i++ % EVENT_QUEUE_SIZE] = cmd;
811    rt->log[rt->log_i++ % EVENT_QUEUE_SIZE] = miw;
812    rt->log[rt->log_i++ % EVENT_QUEUE_SIZE] = tsw;
813#endif
814
815    wake_rx_task = 1;
816
817    /* Wake any blocked rx thread only on receive interrupts */
818    if ( wake_rx_task ) {
819        rtems_semaphore_release(rt->rx_sem);
820    }
821
822    /* Copy current mask to status mask */
823    if ( event_status ) {
824        if ( event_status & 0xffff0000 )
825            rt->status &= 0x0000ffff;
826        rt->status |= event_status;
827    }
828
829    /* signal event once */
830    if ( signal_event && (rt->event_id != 0) ) {
831        rtems_event_send(rt->event_id, event_status);
832    }
833
834}
835
836void b1553rt_print_dev(struct drvmgr_dev *dev, int options)
837{
838    rt_priv *pDev = dev->priv;
839
840    /* Print */
841    printf("--- B1553RT[%d] %s ---\n", pDev->minor, pDev->devName);
842    printf(" REGS:            0x%x\n", (unsigned int)pDev->regs);
843    printf(" IRQ:             %d\n", pDev->irqno);
844
845}
846
847void b1553rt_print(int options)
848{
849    struct amba_drv_info *drv = &b1553rt_drv_info;
850    struct drvmgr_dev *dev;
851
852    dev = drv->general.dev;
853    while(dev) {
854        b1553rt_print_dev(dev, options);
855        dev = dev->next_in_drv;
856    }
857}
Note: See TracBrowser for help on using the repository browser.