Changeset 3681925 in rtems


Ignore:
Timestamp:
Dec 20, 2011, 3:27:54 PM (8 years ago)
Author:
Daniel Hellstrom <daniel@…>
Branches:
4.11, master
Children:
fe1aa9c0
Parents:
e67b2b8d
git-author:
Daniel Hellstrom <daniel@…> (12/20/11 15:27:54)
git-committer:
Daniel Hellstrom <daniel@…> (04/16/15 23:10:17)
Message:

LEON: updated shared drivers to Driver Manger framework

Some bugfixes at the same time. After this patch the drivers
may be used on RASTA systems having a big-endian PCI layout.

Removed not up to date changelogs, rely on git log instead.

Location:
c/src/lib/libbsp/sparc/shared
Files:
10 edited

Legend:

Unmodified
Added
Removed
  • c/src/lib/libbsp/sparc/shared/1553/b1553brm.c

    re67b2b8d r3681925  
    33 *
    44 *  COPYRIGHT (c) 2006.
    5  *  Gaisler Research.
     5 *  Cobham Gaisler AB.
    66 *
    77 *  The license and distribution terms for this file may be
    88 *  found in the file LICENSE in this distribution or at
    99 *  http://www.rtems.org/license/LICENSE.
    10  *
    1110 */
    1211
    1312/********** Set defaults **********/
    1413
    15 /* basic bus/interface of device,
    16  * Default to direct accessed AMBA bus.
    17  */
    18 #ifndef B1553BRM_NO_AMBA
    19  #define B1553BRM_AMBA
    20  #undef B1553BRM_PCI
    21 #endif
    22 
    23 /* default name to /dev/brm */
    24 #if !defined(B1553BRM_DEVNAME) || !defined(B1553BRM_DEVNAME_NO)
    25  #undef B1553BRM_DEVNAME
    26  #undef B1553BRM_DEVNAME_NO
    27  #define B1553BRM_DEVNAME "/dev/brm0"
    28  #define B1553BRM_DEVNAME_NO(devstr,no) ((devstr)[8]='0'+(no))
    29 #endif
    30 
    31 #ifndef B1553BRM_PREFIX
    32  #define B1553BRM_PREFIX(name) b1553brm##name
    33 #else
    34  #define B1553BRM_REGISTER_STATIC
    35 #endif
    36 
    37 /* default to no translation */
    38 #ifndef B1553BRM_ADR_TO
    39  #define memarea_to_hw(x) ((unsigned int)(x))
    40 #endif
    41 
    42 #ifndef B1553BRM_REG_INT
    43  #define B1553BRM_REG_INT(handler,irqno,arg) set_vector(handler,(irqno)+0x10,1)
    44   #undef  B1553BRM_DEFINE_INTHANDLER
    45   #define B1553BRM_DEFINE_INTHANDLER
    46 #endif
    47 
    48 /* default to 128K memory layout */
    49 #if !defined(DMA_MEM_16K)
    50  #define DMA_MEM_128K
     14/* default to 16K memory layout */
     15#define DMA_MEM_128K
     16#if !defined(DMA_MEM_128K)
     17 #define DMA_MEM_16K
    5118#endif
    5219
     
    6027#include <rtems/bspIo.h>
    6128
     29#include <drvmgr/drvmgr.h>
    6230#include <b1553brm.h>
    6331#include <ambapp.h>
     32#include <drvmgr/ambapp_bus.h>
    6433
    6534/* Uncomment for debug output */
     
    7140/* EVENT_QUEUE_SIZE sets the size of the event queue
    7241 */
    73 #define EVENT_QUEUE_SIZE           1024
     42#define EVENT_QUEUE_SIZE           1024 
    7443
    7544
    7645#define INDEX(x) ( x&(EVENT_QUEUE_SIZE-1) )
    7746
    78 #ifdef DEBUG
     47#if 0
    7948#define DBG(x...) printk(x)
    8049#else
    81 #define DBG(x...)
     50#define DBG(x...) 
    8251#endif
    8352
     
    8554#define FUNCDBG(x...) printk(x)
    8655#else
    87 #define FUNCDBG(x...)
     56#define FUNCDBG(x...) 
    8857#endif
    8958
    90 #define READ_REG(address) _BRM_REG_READ16((unsigned int)address)
     59#define READ_REG(address) (*(volatile unsigned int *)address)
    9160#define READ_DMA(address) _BRM_REG_READ16((unsigned int)address)
    9261static __inline__ unsigned short _BRM_REG_READ16(unsigned int addr) {
     
    10877#define BRM_DRIVER_TABLE_ENTRY { brm_initialize, brm_open, brm_close, brm_read, brm_write, brm_control }
    10978
    110 static rtems_driver_address_table brm_driver = BRM_DRIVER_TABLE_ENTRY;
     79static rtems_driver_address_table b1553brm_driver = BRM_DRIVER_TABLE_ENTRY;
    11180
    11281struct msg {
     
    135104};
    136105
    137 typedef struct {
    138 
    139     unsigned int memarea_base;
    140     struct brm_reg *regs;
    141 
    142     /* BRM descriptors */
    143     struct desc_table {
    144 
    145         volatile unsigned short ctrl;
    146         volatile unsigned short top;
    147         volatile unsigned short cur;
    148         volatile unsigned short bot;
    149 
    150     } *desc;
    151 
    152     volatile unsigned short *mem;
    153                 /* bc mem struct */
     106typedef struct {
     107
     108        struct drvmgr_dev *dev; /* Driver manager device */
     109        char devName[32]; /* Device Name */
     110        struct brm_reg *regs;
     111
     112        unsigned int memarea_base;
     113        unsigned int memarea_base_remote;
     114        unsigned int cfg_clksel;
     115        unsigned int cfg_clkdiv;
     116        unsigned int cfg_freq;
     117
     118        /* BRM descriptors */
     119        struct desc_table {
     120                volatile unsigned short ctrl;       
     121                volatile unsigned short top;
     122                volatile unsigned short cur;
     123                volatile unsigned short bot;
     124        } *desc;
     125
     126        volatile unsigned short *mem;
     127        /* bc mem struct */
     128        struct {
     129                /* BC Descriptors */
    154130                struct {
    155                         /* BC Descriptors */
    156                         struct {
    157                                 unsigned short ctrl; /* control */
    158                                 unsigned short cw1;  /* Command word 1*/
    159                                 unsigned short cw2;  /* Command word 1*/
    160                                 unsigned short dptr; /* Data pointer in halfword offset from bcmem */
    161                                 unsigned short tsw[2]; /* status word 1 & 2 */
    162                                 unsigned short ba;     /* branch address */
    163                                 unsigned short timer;  /* timer value */
    164                         } descs[128]; /* 2k (1024 half words) */
    165 
    166                         /* message data */
    167                         struct {
    168                                 unsigned short data[32]; /* 1 message's data */
    169                         } msg_data[128]; /* 8k */
     131                        unsigned short ctrl; /* control */
     132                        unsigned short cw1;  /* Command word 1*/
     133                        unsigned short cw2;  /* Command word 1*/
     134                        unsigned short dptr; /* Data pointer in halfword offset from bcmem */
     135                        unsigned short tsw[2]; /* status word 1 & 2 */
     136                        unsigned short ba;     /* branch address */
     137                        unsigned short timer;  /* timer value */
     138                } descs[128]; /* 2k (1024 half words) */
     139
     140                /* message data */
     141                struct {
     142                        unsigned short data[32]; /* 1 message's data */
     143                } msg_data[128]; /* 8k */
    170144
    171145#if defined(DMA_MEM_128K)
    172                         /* offset to last 64bytes of 128k */
    173                         unsigned short unused[(64*1024-(128*8+128*32))-16*2];
     146                /* offset to last 64bytes of 128k */
     147                unsigned short unused[(64*1024-(128*8+128*32))-16*2];
    174148#elif defined(DMA_MEM_16K)
    175                         unsigned short unused[(8*1024-(128*8+128*32))-16*2];
     149                unsigned short unused[(8*1024-(128*8+128*32))-16*2];
    176150#endif
    177                         /* interrupt log at 64 bytes from end */
    178                         struct irq_log_list irq_logs[16];
    179                 } *bcmem;
     151                /* interrupt log at 64 bytes from end */
     152                struct irq_log_list irq_logs[16];
     153        } *bcmem;
    180154
    181155#if defined(DMA_MEM_128K)
    182                 /* Memory structure of a RT being inited, just used
    183                  * for RT initialization.
    184                  *
    185                  * *mesgs[32] fit each minimally 8 messages per sub address.
    186                  */
    187                 struct {
    188                         /* RX Sub Address descriptors */
    189                         struct desc_table rxsubs[32];
    190                         /* TX Sub Address descriptors */
    191                         struct desc_table txsubs[32];
    192                         /* RX mode code descriptors */
    193                         struct desc_table rxmodes[32];
    194                         /* TX mode code descriptors */
    195                         struct desc_table txmodes[32];
    196 
    197                         /* RX Sub Address messages */
    198                         struct circ_buf rxsuba_msgs[32];
    199                         /* TX Sub Address messages */
    200                         struct circ_buf txsuba_msgs[32];
    201                         /* RX Mode Code messages */
    202                         struct circ_buf rxmode_msgs[32];
    203                         /* RX Mode Code messages */
    204                         struct circ_buf txmode_msgs[32];
    205 
    206 
    207                         /* offset to last 64bytes of 128k: tot-used-needed */
    208                         unsigned short unused[(64*1024-(4*32*4+4*32*9*34))-16*2];
    209 
    210                         /* interrupt log at 64 bytes from end */
    211                         struct irq_log_list irq_logs[16];
    212                 } *rtmem;
     156        /* Memory structure of a RT being inited, just used
     157         * for RT initialization.
     158         *
     159         * *mesgs[32] fit each minimally 8 messages per sub address.
     160         */
     161        struct {
     162                /* RX Sub Address descriptors */
     163                struct desc_table rxsubs[32];
     164                /* TX Sub Address descriptors */
     165                struct desc_table txsubs[32];
     166                /* RX mode code descriptors */
     167                struct desc_table rxmodes[32];
     168                /* TX mode code descriptors */
     169                struct desc_table txmodes[32];
     170
     171                /* RX Sub Address messages */
     172                struct circ_buf rxsuba_msgs[32];
     173                /* TX Sub Address messages */
     174                struct circ_buf txsuba_msgs[32];
     175                /* RX Mode Code messages */
     176                struct circ_buf rxmode_msgs[32];
     177                /* RX Mode Code messages */
     178                struct circ_buf txmode_msgs[32];
     179
     180                /* offset to last 64bytes of 128k: tot-used-needed */
     181                unsigned short unused[(64*1024-(4*32*4+4*32*9*34))-16*2];
     182
     183                /* interrupt log at 64 bytes from end */
     184                struct irq_log_list irq_logs[16];
     185        } *rtmem;
    213186#elif defined(DMA_MEM_16K)
    214                 /* Memory structure of a RT being inited, just used
    215                  * for RT initialization.
    216                  *
    217                  * circ_buf_2 *mesgs[32] fit each minimally 2 messages per queue.
    218                  * circ_buf_1 *mesgs[32] fit each minimally 1 messages per queue.
    219                  */
    220                 struct {
    221                         /* RX Sub Address descriptors */
    222                         struct desc_table rxsubs[32];
    223                         /* TX Sub Address descriptors */
    224                         struct desc_table txsubs[32];
    225                         /* RX mode code descriptors */
    226                         struct desc_table rxmodes[32];
    227                         /* TX mode code descriptors */
    228                         struct desc_table txmodes[32];
    229 
    230                         /* RX Sub Address messages */
    231                         struct circ_buf_2 rxsuba_msgs[32];
    232                         /* TX Sub Address messages */
    233                         struct circ_buf_2 txsuba_msgs[32];
    234                         /* RX Mode Code messages */
    235                         struct circ_buf_2 rxmode_msgs[32];
    236                         /* RX Mode Code messages */
    237                         struct circ_buf_1 txmode_msgs[32];
    238 
    239 
    240                         /* offset to last 64bytes of 16k: tot-used-needed */
    241                         unsigned short unused[8*1024 -(4*32*4 +3*32*2*34 +1*32*1*34) -16*2];
    242 
    243                         /* interrupt log at 64 bytes from end */
    244                         struct irq_log_list irq_logs[16];
    245                 } *rtmem;
     187        /* Memory structure of a RT being inited, just used
     188         * for RT initialization.
     189         *
     190         * circ_buf_2 *mesgs[32] fit each minimally 2 messages per queue.
     191         * circ_buf_1 *mesgs[32] fit each minimally 1 messages per queue.
     192         */
     193        struct {
     194                /* RX Sub Address descriptors */
     195                struct desc_table rxsubs[32];
     196                /* TX Sub Address descriptors */
     197                struct desc_table txsubs[32];
     198                /* RX mode code descriptors */
     199                struct desc_table rxmodes[32];
     200                /* TX mode code descriptors */
     201                struct desc_table txmodes[32];
     202
     203                /* RX Sub Address messages */
     204                struct circ_buf_2 rxsuba_msgs[32];
     205                /* TX Sub Address messages */
     206                struct circ_buf_2 txsuba_msgs[32];
     207                /* RX Mode Code messages */
     208                struct circ_buf_2 rxmode_msgs[32];
     209                /* RX Mode Code messages */
     210                struct circ_buf_1 txmode_msgs[32];
     211
     212                /* offset to last 64bytes of 16k: tot-used-needed */
     213                unsigned short unused[8*1024 -(4*32*4 +3*32*2*34 +1*32*1*34) -16*2];
     214
     215                /* interrupt log at 64 bytes from end */
     216                struct irq_log_list irq_logs[16];
     217        } *rtmem;
    246218#else
    247219        #error You must define one DMA_MEM_???K
    248220#endif
    249221
    250     /* Interrupt log list */
    251     struct irq_log_list *irq_log;
    252     unsigned int irq;
    253 
    254     /* Received events waiting to be read */
    255     struct rt_msg *rt_event;
    256     struct bm_msg *bm_event;
    257 
    258     unsigned int head, tail;
    259 
    260     unsigned int last_read[128];
    261     unsigned int written[32];
    262 
    263     struct bc_msg *cur_list;
    264 
    265     int tx_blocking, rx_blocking;
    266 
    267     rtems_id rx_sem, tx_sem, dev_sem;
    268                 int minor;
    269                 int irqno;
    270                 unsigned int mode;
     222        /* Interrupt log list */
     223        struct irq_log_list *irq_log;
     224        unsigned int irq;
     225
     226        /* Received events waiting to be read */
     227        struct rt_msg *rt_event;
     228        struct bm_msg *bm_event;
     229
     230        unsigned int head, tail;
     231
     232        unsigned int last_read[128];
     233        unsigned int written[32];
     234
     235        struct bc_msg *cur_list;
     236
     237        int tx_blocking, rx_blocking;
     238
     239        rtems_id rx_sem, tx_sem, dev_sem;
     240        int minor;
     241        int irqno;
     242        unsigned int mode;
     243#ifdef DEBUG                   
     244        unsigned int log[EVENT_QUEUE_SIZE*4];
     245        unsigned int log_i;
     246#endif
     247
     248        rtems_id event_id; /* event that may be signalled upon errors, needs to be set through ioctl command BRM_SET_EVENTID */
     249        unsigned int status;
     250        int bc_list_fail;
     251} brm_priv;
     252
     253static void b1553brm_interrupt(void *arg);
     254static rtems_device_driver rt_init(brm_priv *brm);
     255
     256#define OFS(ofs) (((unsigned int)&ofs & 0x1ffff)>>1)
     257
     258static int b1553brm_driver_io_registered = 0;
     259static rtems_device_major_number b1553brm_driver_io_major = 0;
     260
     261/******************* Driver manager interface ***********************/
     262
     263/* Driver prototypes */
     264int b1553brm_register_io(rtems_device_major_number *m);
     265int b1553brm_device_init(brm_priv *pDev);
     266
     267int b1553brm_init2(struct drvmgr_dev *dev);
     268int b1553brm_init3(struct drvmgr_dev *dev);
     269int b1553brm_remove(struct drvmgr_dev *dev);
     270
     271struct drvmgr_drv_ops b1553brm_ops =
     272{
     273        .init = {NULL, b1553brm_init2, b1553brm_init3, NULL},
     274        .remove = b1553brm_remove,
     275        .info = NULL
     276};
     277
     278struct amba_dev_id b1553brm_ids[] =
     279{
     280        {VENDOR_GAISLER, GAISLER_B1553BRM},
     281        {0, 0}          /* Mark end of table */
     282};
     283
     284struct amba_drv_info b1553brm_drv_info =
     285{
     286        {
     287                DRVMGR_OBJ_DRV,                         /* Driver */
     288                NULL,                                   /* Next driver */
     289                NULL,                                   /* Device list */
     290                DRIVER_AMBAPP_GAISLER_B1553BRM_ID,      /* Driver ID */
     291                "B1553BRM_DRV",                         /* Driver Name */
     292                DRVMGR_BUS_TYPE_AMBAPP,                 /* Bus Type */
     293                &b1553brm_ops,
     294                NULL,                                   /* Funcs */
     295                0,                                      /* No devices yet */
     296                0,
     297        },
     298        &b1553brm_ids[0]
     299};
     300
     301void b1553brm_register_drv (void)
     302{
     303        DBG("Registering B1553BRM driver\n");
     304        drvmgr_drv_register(&b1553brm_drv_info.general);
     305}
     306
     307int b1553brm_init2(struct drvmgr_dev *dev)
     308{
     309        brm_priv *priv;
     310
     311        DBG("B1553BRM[%d] on bus %s\n", dev->minor_drv, dev->parent->dev->name);
     312        priv = dev->priv = malloc(sizeof(brm_priv));
     313        if ( !priv )
     314                return DRVMGR_NOMEM;
     315        memset(priv, 0, sizeof(*priv));
     316        priv->dev = dev;
     317
     318        /* This core will not find other cores, so we wait for init2() */
     319
     320        return DRVMGR_OK;
     321}
     322
     323int b1553brm_init3(struct drvmgr_dev *dev)
     324{
     325        brm_priv *priv;
     326        char prefix[32];
     327        rtems_status_code status;
     328
     329        priv = dev->priv;
     330
     331        /* Do initialization */
     332
     333        if ( b1553brm_driver_io_registered == 0) {
     334                /* Register the I/O driver only once for all cores */
     335                if ( b1553brm_register_io(&b1553brm_driver_io_major) ) {
     336                        /* Failed to register I/O driver */
     337                        dev->priv = NULL;
     338                        return DRVMGR_FAIL;
     339                }
     340
     341                b1553brm_driver_io_registered = 1;
     342        }
     343
     344        /* I/O system registered and initialized
     345         * Now we take care of device initialization.
     346         */
     347
     348        if ( b1553brm_device_init(priv) ) {
     349                return DRVMGR_FAIL;
     350        }
     351
     352        /* Get Filesystem name prefix */
     353        prefix[0] = '\0';
     354        if ( drvmgr_get_dev_prefix(dev, prefix) ) {
     355                /* Failed to get prefix, make sure of a unique FS name
     356                 * by using the driver minor.
     357                 */
     358                sprintf(priv->devName, "/dev/b1553brm%d", dev->minor_drv);
     359        } else {
     360                /* Got special prefix, this means we have a bus prefix
     361                 * And we should use our "bus minor"
     362                 */
     363                sprintf(priv->devName, "/dev/%sb1553brm%d", prefix, dev->minor_bus);
     364        }
     365
     366        /* Register Device */
     367        status = rtems_io_register_name(priv->devName, b1553brm_driver_io_major, dev->minor_drv);
     368        if (status != RTEMS_SUCCESSFUL) {
     369                return DRVMGR_FAIL;
     370        }
     371
     372        return DRVMGR_OK;
     373}
     374
     375int b1553brm_remove(struct drvmgr_dev *dev)
     376{
     377        /* Stop more tasks to open driver */
     378
     379        /* Throw out all tasks using this driver */
     380
     381        /* Unregister I/O node */
     382
     383        /* Unregister and disable Interrupt */
     384
     385        /* Free device memory */
     386
     387        /* Return sucessfully */
     388
     389        return DRVMGR_OK;
     390}
     391
     392/******************* Driver Implementation ***********************/
     393
     394int b1553brm_register_io(rtems_device_major_number *m)
     395{
     396        rtems_status_code r;
     397
     398        if ((r = rtems_io_register_driver(0, &b1553brm_driver, m)) == RTEMS_SUCCESSFUL) {
     399                DBG("B1553BRM driver successfully registered, major: %d\n", *m);
     400        } else {
     401                switch(r) {
     402                case RTEMS_TOO_MANY:
     403                        printk("B1553BRM rtems_io_register_driver failed: RTEMS_TOO_MANY\n");
     404                        return -1;
     405                case RTEMS_INVALID_NUMBER: 
     406                        printk("B1553BRM rtems_io_register_driver failed: RTEMS_INVALID_NUMBER\n");
     407                        return -1;
     408                case RTEMS_RESOURCE_IN_USE:
     409                        printk("B1553BRM rtems_io_register_driver failed: RTEMS_RESOURCE_IN_USE\n");
     410                        return -1;
     411                default:
     412                        printk("B1553BRM rtems_io_register_driver failed\n");
     413                        return -1;
     414                }
     415        }
     416        return 0;
     417}
     418
     419int b1553brm_device_init(brm_priv *pDev)
     420{
     421        struct amba_dev_info *ambadev;
     422        struct ambapp_core *pnpinfo;
     423        union drvmgr_key_value *value;
     424        unsigned int mem;
     425        int size;
     426
     427        /* Get device information from AMBA PnP information */
     428        ambadev = (struct amba_dev_info *)pDev->dev->businfo;
     429        if ( ambadev == NULL ) {
     430                return -1;
     431        }
     432        pnpinfo = &ambadev->info;
     433        pDev->irqno = pnpinfo->irq;
     434        /* Two versions of the BRM core. One where the registers are accessed using the AHB bus
     435         * and one where the APB bus is used
     436         */
     437        if ( pnpinfo->ahb_slv ) {
     438                /* Registers accessed over AHB */
     439                pDev->regs = (struct brm_reg *)pnpinfo->ahb_slv->start[0];
     440        } else {
     441                /* Registers accessed over APB */
     442                pDev->regs = (struct brm_reg *)pnpinfo->apb_slv->start;
     443        }
     444        pDev->minor = pDev->dev->minor_drv;
    271445#ifdef DEBUG
    272                 unsigned int log[EVENT_QUEUE_SIZE*4];
    273                 unsigned int log_i;
     446        pDev->log_i = 0;       
     447        memset(pDev->log,0,sizeof(pDev->log));
    274448#endif
    275449
    276                 rtems_id event_id; /* event that may be signalled upon errors, needs to be set through ioctl command BRM_SET_EVENTID */
    277                 unsigned int status;
    278                 int bc_list_fail;
    279 } brm_priv;
    280 
    281 static int brm_cores;
    282 static unsigned int allbrm_memarea;
    283 static brm_priv *brms;
    284 static struct ambapp_bus *amba_bus;
    285 static unsigned int     allbrm_cfg_clksel;
    286 static unsigned int allbrm_cfg_clkdiv;
    287 static unsigned int allbrm_cfg_freq;
    288 
    289 static void brm_interrupt(brm_priv *brm);
    290 #ifdef B1553BRM_DEFINE_INTHANDLER
    291 static void b1553brm_interrupt_handler(rtems_vector_number v);
     450#ifdef DMA_MEM_128K
     451        size = 128 * 1024;
     452#else
     453        size = 16 * 1024;
    292454#endif
    293455
    294 #define OFS(ofs) (((unsigned int)&ofs & 0x1ffff)>>1)
     456        /* Get memory configuration from bus resources */
     457        value = drvmgr_dev_key_get(pDev->dev, "dmaBaseAdr", KEY_TYPE_POINTER);
     458        if (value)
     459                mem = (unsigned int)value->ptr;
     460
     461        if (value && (mem & 1)) {
     462                /* Remote address, address as BRM looks at it. */
     463
     464                /* Translate the base address into an address that the the CPU can understand */
     465                pDev->memarea_base_remote = mem & ~1;
     466                drvmgr_translate_check(pDev->dev, DMAMEM_TO_CPU,
     467                                        (void *)pDev->memarea_base_remote,
     468                                        (void **)&pDev->memarea_base,
     469                                        size);
     470        } else {
     471                if (!value) {
     472                        /* Use dynamically allocated memory + 128k for
     473                         * alignment
     474                         */
     475                        mem = (unsigned int)malloc(size + 128 * 1024);
     476                        if (!mem){
     477                                printk("BRM: Failed to allocate HW memory\n\r");
     478                                return -1;
     479                        }
     480                        /* align memory to 128k boundary */
     481                        pDev->memarea_base = (mem + 0x1ffff) & ~0x1ffff;
     482                } else {
     483                        pDev->memarea_base = mem;
     484                }
     485
     486                /* Translate the base address into an address that the BRM core can understand */
     487                drvmgr_translate_check(pDev->dev, CPUMEM_TO_DMA,
     488                                        (void *)pDev->memarea_base,
     489                                        (void **)&pDev->memarea_base_remote,
     490                                        size);
     491        }
     492
     493        /* clear the used memory */
     494        memset((char *)pDev->memarea_base, 0, size);
     495
     496        /* Set base address of all descriptors */
     497        pDev->desc = (struct desc_table *) pDev->memarea_base;
     498        pDev->mem = (volatile unsigned short *) pDev->memarea_base;
     499        pDev->irq_log   = (struct irq_log_list *)(pDev->memarea_base + (0xFFE0<<1)); /* last 64byte */
     500
     501        pDev->bm_event = NULL;
     502        pDev->rt_event = NULL;
     503
     504        pDev->cfg_clksel = 0;
     505        pDev->cfg_clkdiv = 0;
     506        pDev->cfg_freq = BRM_FREQ_24MHZ;
     507
     508        value = drvmgr_dev_key_get(pDev->dev, "clkSel", KEY_TYPE_INT);
     509        if ( value ) {
     510                pDev->cfg_clksel = value->i & CLKSEL_MASK;
     511        }
     512
     513        value = drvmgr_dev_key_get(pDev->dev, "clkDiv", KEY_TYPE_INT);
     514        if ( value ) {
     515                pDev->cfg_clkdiv = value->i & CLKDIV_MASK;
     516        }
     517
     518        value = drvmgr_dev_key_get(pDev->dev, "coreFreq", KEY_TYPE_INT);
     519        if ( value ) {
     520                pDev->cfg_freq = value->i & BRM_FREQ_MASK;
     521        }
     522
     523        /* Sel clock so that we can write to BRM's registers */
     524        pDev->regs->w_ctrl = (pDev->cfg_clksel<<9) | (pDev->cfg_clkdiv<<5);
     525        /* Reset BRM core */
     526        pDev->regs->w_ctrl = 1<<10 | READ_REG(&pDev->regs->w_ctrl);
     527
     528        /* RX Semaphore created with count = 0 */
     529        if ( rtems_semaphore_create(rtems_build_name('B', 'M', 'R', '0' + pDev->minor),
     530                0,
     531                RTEMS_FIFO|RTEMS_SIMPLE_BINARY_SEMAPHORE|RTEMS_NO_INHERIT_PRIORITY|RTEMS_LOCAL|RTEMS_NO_PRIORITY_CEILING,
     532                0,
     533                &pDev->rx_sem) != RTEMS_SUCCESSFUL ) {
     534                printk("BRM: Failed to create rx semaphore\n");
     535                return RTEMS_INTERNAL_ERROR;
     536        }
     537
     538        /* TX Semaphore created with count = 1 */
     539        if ( rtems_semaphore_create(rtems_build_name('B', 'M', 'T', '0' + pDev->minor),
     540                1,
     541                RTEMS_FIFO|RTEMS_SIMPLE_BINARY_SEMAPHORE|RTEMS_NO_INHERIT_PRIORITY|RTEMS_LOCAL|RTEMS_NO_PRIORITY_CEILING,
     542                0,
     543                &pDev->tx_sem) != RTEMS_SUCCESSFUL ){
     544                printk("BRM: Failed to create tx semaphore\n");
     545                return RTEMS_INTERNAL_ERROR;
     546        }
     547
     548        /* Device Semaphore created with count = 1 */
     549        if ( rtems_semaphore_create(rtems_build_name('B', 'M', 'D', '0' + pDev->minor),
     550                1,
     551                RTEMS_FIFO|RTEMS_SIMPLE_BINARY_SEMAPHORE|RTEMS_NO_INHERIT_PRIORITY|RTEMS_LOCAL|RTEMS_NO_PRIORITY_CEILING,
     552                0,
     553                &pDev->dev_sem) != RTEMS_SUCCESSFUL ){
     554                printk("BRM: Failed to create device semaphore\n");
     555                return RTEMS_INTERNAL_ERROR;
     556        }
     557
     558        /* Default to RT-mode */
     559        rt_init(pDev);
     560
     561        return 0;
     562}
    295563
    296564static int odd_parity(unsigned int data) {
     
    301569        i++;
    302570        data &= (data - 1);
    303     }
     571    } 
    304572
    305573    return !(i&1);
    306574}
    307 
    308575
    309576static void start_operation(brm_priv *brm) {
     
    316583  brm->regs->ctrl = ctrl & ~0x8000;
    317584}
     585
    318586static int is_executing(brm_priv *brm) {
    319587        unsigned int ctrl = READ_REG(&brm->regs->ctrl);
    320588        return ((ctrl>>15) & 1);
    321 }
    322 
    323 #ifdef LEON3
    324 #ifndef DONT_DEF_RAMON
    325 int brm_register_leon3_ramon_fpga(void){
    326         /* Clock div & Clock sel is NOT available.
    327          * The BRM is always clocked with 24MHz.
    328          * 3 in BRM enhanced register will select 24MHz
    329          */
    330         return b1553brm_register(&ambapp_plb, 0, 0, 3);
    331 }
    332 
    333 int brm_register_leon3_ramon_asic(void){
    334         /* Clock div & Clock sel is available.
    335          * Clkdiv only matter when clksel is 1.
    336          * clksel=2, clkdiv=don't care, brm_frq=24MHz
    337          *
    338          * 3 in BRM enhanced register will select 24MHz
    339          */
    340         return b1553brm_register(&ambapp_plb, 2, 0, 3);
    341 }
    342 #endif
    343 #endif
    344 
    345 #ifdef B1553BRM_REGISTER_STATIC
    346 static
    347 #endif
    348 int B1553BRM_PREFIX(_register)(struct ambapp_bus *bus, unsigned int clksel, unsigned int clkdiv, unsigned int brm_freq)
    349 {
    350     rtems_status_code r;
    351     rtems_device_major_number m;
    352 
    353     FUNCDBG("brm_register:\n\r");
    354 
    355                 /* save amba bus pointer */
    356                 amba_bus = bus;
    357                 if ( !bus ){
    358                         printk("brm_register: bus is NULL\n\r");
    359                         return 1;
    360                 }
    361 
    362 #ifdef B1553BRM_LOCAL_MEM
    363                 allbrm_memarea = B1553BRM_LOCAL_MEM_ADR;
    364 #else
    365                 allbrm_memarea = 0;
    366 #endif
    367 
    368                 /* Save clksel, clkdiv and brm_freq for later use */
    369                 allbrm_cfg_clksel = clksel & CLKSEL_MASK;
    370                 allbrm_cfg_clkdiv = clkdiv & CLKDIV_MASK;
    371                 allbrm_cfg_freq = brm_freq & BRM_FREQ_MASK;
    372 
    373     if ((r = rtems_io_register_driver(0, &brm_driver, &m)) == RTEMS_SUCCESSFUL) {
    374         DBG("BRM: driver successfully registered, major: %d\n",m);
    375 
    376     } else {
    377         switch(r) {
    378         case RTEMS_TOO_MANY:
    379             printk("BRM rtems_io_register_driver failed: RTEMS_TOO_MANY\n"); break;
    380         case RTEMS_INVALID_NUMBER:
    381             printk("BRM rtems_io_register_driver failed: RTEMS_INVALID_NUMBER\n"); break;
    382         case RTEMS_RESOURCE_IN_USE:
    383             printk("BRM rtems_io_register_driver failed: RTEMS_RESOURCE_IN_USE\n"); break;
    384         default:
    385            printk("BRM rtems_io_register_driver failed\n");
    386         }
    387                                 return 1;
    388     }
    389                 return 0;
    390589}
    391590
     
    404603        brm->rx_blocking = brm->tx_blocking = 1;
    405604
    406   if ( brm->bm_event )
     605        if ( brm->bm_event )
    407606                free(brm->bm_event);
    408607        brm->bm_event = NULL;
    409608
    410   if ( brm->rt_event )
     609        if ( brm->rt_event )
    411610                free(brm->rt_event);
    412 
     611       
    413612        brm->bcmem = NULL;
    414613        brm->rtmem = (void *)brm->mem;
    415614
    416615        brm->rt_event = (struct rt_msg *) malloc(EVENT_QUEUE_SIZE*sizeof(struct rt_msg));
    417 
     616 
    418617        if (brm->rt_event == NULL) {
    419618                DBG("BRM driver failed to allocated memory.");
     
    428627        brm->regs->dpoint       = 0;
    429628        brm->regs->ipoint       = OFS(brm->rtmem->irq_logs[0]);
    430         brm->regs->enhanced  = 0x0000 | allbrm_cfg_freq;  /* BRM clocked with freq = 12,16,20 or 24MHz */
    431         brm->regs->w_ctrl       = (allbrm_cfg_clksel<<9) | (allbrm_cfg_clkdiv<<5) | 1;
     629        brm->regs->enhanced  = 0x0000 | brm->cfg_freq;  /* BRM clocked with freq = 12,16,20 or 24MHz */
     630        brm->regs->w_ctrl       = (brm->cfg_clksel<<9) | (brm->cfg_clkdiv<<5) | 1;
    432631        brm->regs->w_irqctrl = 6;
    433         brm->regs->w_ahbaddr = (unsigned int) memarea_to_hw(brm->memarea_base);
    434 
     632        brm->regs->w_ahbaddr = brm->memarea_base_remote;
     633               
    435634        clr_int_logs(brm->irq_log);
    436635
     
    440639        }
    441640
    442         /* Init descriptor table
    443          *
     641        /* Init descriptor table 
     642         * 
    444643         * Each circular buffer has room for 8 messages with up to 34 (32 data + miw + time) words (16b) in each.
    445644         * The buffers must separated by 34 words.
    446645         */
    447646
    448 
     647 
    449648        /* RX Sub-address 0 - 31 */
    450649        for (i = 0; i < 32; i++) {
     
    452651                brm->rtmem->rxsubs[i].top  = OFS(brm->rtmem->rxsuba_msgs[i]);                /* Top address */
    453652                brm->rtmem->rxsubs[i].cur  = OFS(brm->rtmem->rxsuba_msgs[i]);                /* Current address */
    454                 brm->rtmem->rxsubs[i].bot  = OFS(brm->rtmem->rxsuba_msgs[i+1]) - sizeof(struct msg)/2; /* Bottom address */
     653                brm->rtmem->rxsubs[i].bot  = OFS(brm->rtmem->rxsuba_msgs[i+1]) - sizeof(struct msg)/2; /* Bottom address */     
    455654                brm->last_read[i] = OFS(brm->rtmem->rxsuba_msgs[i]);
    456655        }
     
    471670                brm->rtmem->rxmodes[i].bot  = OFS(brm->rtmem->rxmode_msgs[i+1]) - sizeof(struct msg)/2; /* Bottom address */
    472671                brm->last_read[i+64] = OFS(brm->rtmem->rxmode_msgs[i]);
    473         }
     672        }   
    474673        /* TX mode code 0 - 31 */
    475674        for (i = 0; i < 32; i++) {
     
    481680        }
    482681
     682#ifdef DEBUG
     683        printk("b1553BRM DMA_AREA: 0x%x\n", (unsigned int)brm->rtmem);
     684        printk("LOG: 0x%x\n", &brm->log[0]);
     685        printk("LOG_I: 0x%x\n", &brm->log_i);
     686#endif
     687
    483688        brm->mode = BRM_MODE_RT;
    484689
     
    488693static rtems_device_driver bc_init(brm_priv *brm){
    489694
    490   if ( brm->bm_event )
     695        if ( brm->bm_event )
    491696                free(brm->bm_event);
    492697        brm->bm_event = NULL;
    493698
    494   if ( brm->rt_event )
     699        if ( brm->rt_event )
    495700                free(brm->rt_event);
    496701        brm->rt_event = NULL;
    497 
     702       
    498703        brm->bcmem = (void *)brm->mem;
    499704        brm->rtmem = NULL;
    500705        brm->irq_log = (struct irq_log_list *)&brm->bcmem->irq_logs[0];
    501 
     706       
    502707        brm->head = brm->tail = 0;
    503708        brm->rx_blocking = brm->tx_blocking = 1;
    504 
     709       
    505710        brm->regs->ctrl   = 0x0006;  /* ping pong enable and enable interrupt log */
    506711        brm->regs->oper   = 0x0800;  /* configure as BC */
    507712        brm->regs->imask         = BRM_EOL_IRQ|BRM_BC_ILLCMD_IRQ|BRM_ILLOP_IRQ|BRM_DMAF_IRQ|BRM_WRAPF_IRQ|BRM_MERR_IRQ;
    508713        brm->regs->dpoint       = 0;
    509         printk("Set BC interrupt log: 0x%lx, 0x%lx, 0x%lx\n",OFS(brm->bcmem->irq_logs[0]),&brm->bcmem->irq_logs[0],brm->bcmem);
    510714        brm->regs->ipoint       = OFS(brm->bcmem->irq_logs[0]);
    511         brm->regs->enhanced  = 0x0000 | (allbrm_cfg_freq&0x3);  /* freq = 24 */
    512         brm->regs->w_ctrl       = (allbrm_cfg_clksel<<9) | (allbrm_cfg_clkdiv<<5) | 1;
     715        brm->regs->enhanced  = 0x0000 | (brm->cfg_freq&BRM_FREQ_MASK);  /* freq = 24 */
     716        brm->regs->w_ctrl       = (brm->cfg_clksel<<9) | (brm->cfg_clkdiv<<5) | 1;
    513717        brm->regs->w_irqctrl = 6;
    514         brm->regs->w_ahbaddr = (unsigned int) memarea_to_hw(brm->memarea_base);
    515 
     718        brm->regs->w_ahbaddr = brm->memarea_base_remote;
     719       
    516720        clr_int_logs(brm->irq_log);
    517 
     721       
    518722        brm->mode = BRM_MODE_BC;
    519 
     723       
    520724        return RTEMS_SUCCESSFUL;
    521725}
     
    527731        brm->rx_blocking = brm->tx_blocking = 1;
    528732
    529   if ( brm->rt_event )
     733        if ( brm->rt_event )
    530734                free(brm->rt_event);
    531735        brm->rt_event = NULL;
    532736
    533   if ( brm->bm_event )
     737        if ( brm->bm_event )
    534738                free(brm->bm_event);
    535 
     739       
    536740        brm->bcmem = NULL;
    537741        brm->rtmem = NULL;
    538 
     742       
    539743        brm->bm_event    = (struct bm_msg *) malloc(EVENT_QUEUE_SIZE*sizeof(struct bm_msg));
    540 
     744 
    541745        if (brm->bm_event == NULL) {
    542746                DBG("BRM driver failed to allocated memory.");
     
    549753        brm->regs->ctrl   = 0x0006;  /* ping pong enable and enable interrupt log */
    550754        brm->regs->oper   = 0x0A00;  /* configure as BM */
    551         brm->regs->imask        = BRM_MBC_IRQ|BRM_MERR_IRQ|BRM_DMAF_IRQ|BRM_MERR_IRQ;
     755        brm->regs->imask        = BRM_MBC_IRQ|BRM_MERR_IRQ|BRM_DMAF_IRQ;
    552756        brm->regs->dpoint       = 0;
    553757        brm->regs->ipoint       = OFS(brm->mem[8*1024-16*2]);
     
    555759        brm->regs->mdpoint   = 0x100;   /* Data pointer */
    556760        brm->regs->mbc     = 1;    /* Block count */
    557         brm->regs->enhanced  = 0x0000 | (allbrm_cfg_freq&0x3);  /* freq = 24 */
    558         brm->regs->w_ctrl       = (allbrm_cfg_clksel<<9) | (allbrm_cfg_clkdiv<<5) | 1;
     761        brm->regs->enhanced  = 0x0000 | (brm->cfg_freq&BRM_FREQ_MASK);  /* freq = 24 */
     762        brm->regs->w_ctrl       = (brm->cfg_clksel<<9) | (brm->cfg_clkdiv<<5) | 1;
    559763        brm->regs->w_irqctrl = 6;
    560         brm->regs->w_ahbaddr = (unsigned int) memarea_to_hw(brm->memarea_base);
    561 
     764        brm->regs->w_ahbaddr = brm->memarea_base_remote;
     765       
    562766        clr_int_logs(brm->irq_log);
    563 
     767       
    564768        brm->mode = BRM_MODE_BM;
    565 
     769       
    566770        return RTEMS_SUCCESSFUL;
    567771}
     
    570774static rtems_device_driver brm_initialize(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
    571775{
    572         rtems_status_code status;
    573         int dev_cnt;
    574         char fs_name[20];
     776        return RTEMS_SUCCESSFUL;
     777}
     778
     779static rtems_device_driver brm_open(rtems_device_major_number major, rtems_device_minor_number minor, void *arg) {
    575780        brm_priv *brm;
    576         struct ambapp_ahb_info ambadev;
    577         char *mem;
    578 
    579         FUNCDBG("brm_initialize\n");
    580 
    581         brm_cores = 0;
    582         strcpy(fs_name,B1553BRM_DEVNAME);
    583 
    584         /* Find all BRM devices */
    585         dev_cnt = ambapp_get_number_ahbslv_devices(amba_bus, VENDOR_GAISLER, GAISLER_B1553BRM);
    586         if ( dev_cnt < 1 ){
    587                 /* Failed to find any CAN cores! */
    588                 printk("BRM: Failed to find any BRM cores\n\r");
    589                 return -1;
    590         }
    591 
    592         /* allocate & zero memory for the brm devices */
    593         brms = (brm_priv *)malloc(sizeof(*brms)*dev_cnt);
    594         if ( !brms ){
    595                 printk("BRM: Failed to allocate SW memory\n\r");
    596                 return -1;
    597         }
    598         memset(brms,0,sizeof(*brms)*dev_cnt);
    599 
    600         /* Allocate memory for all device's descriptors,
    601          * they must be aligned to a XXX byte boundary.
    602          */
    603         #define BRM_DESCS_PER_CTRL 128
    604         if ( allbrm_memarea ){
    605                 mem = (char *)allbrm_memarea;
    606         }else{
    607                 /* sizeof(struct desc_table) * BRM_DESCS_PER_CTRL * dev_cnt */
    608                 mem = (char *)malloc( (128*1024) * (dev_cnt+1)); /* 128k per core + 128k for alignment */
    609                 if ( !mem ){
    610                         free(brms);
    611                         printk("BRM: Failed to allocate HW memory\n\r");
    612                         return -1;
    613                 }
    614 
    615                 /* align memory to 128k boundary */
    616                 mem = (char *)(((unsigned int)mem+0x1ffff) & ~0x1ffff);
    617         }
    618 
    619         /* clear the used memory */
    620         memset(mem,0,(128*1024) * dev_cnt);
    621 
    622         /* initialize each brm device, one at a time */
    623         for(minor=0; minor<dev_cnt; minor++){
    624                 brm = &brms[minor];
    625 
    626                 /* Get AMBA AHB device info from Plug&Play */
    627                 ambapp_find_ahbslv_next(amba_bus, VENDOR_GAISLER,
    628                                         GAISLER_B1553BRM, &ambadev, minor);
    629 
    630                 /* Copy Basic HW info */
    631                 brm->regs = (void *)ambadev.start[0];
    632                 brm->irqno = ambadev.irq;
    633                 brm->minor = minor;
    634                 brm->irq = 0;
    635 #ifdef DEBUG
    636                 brm->log_i = 0;
    637                 memset(brm->log,0,sizeof(brm->log));
    638 #endif
    639 
    640                 /* Set unique name */
    641                 B1553BRM_DEVNAME_NO(fs_name,minor);
    642 
    643     DBG("Registering BRM core at [0x%x] irq %d, minor %d as %s\n",brm->regs,brm->irqno,minor,fs_name);
    644 
    645                 /* Bind filesystem name to device number (minor) */
    646                 status = rtems_io_register_name(fs_name, major, minor);
    647     if (status != RTEMS_SUCCESSFUL)
    648                         rtems_fatal_error_occurred(status);
    649 
    650                 /* RX Semaphore created with count = 0 */
    651                 if ( rtems_semaphore_create(rtems_build_name('B', 'M', 'R', '0'+minor),
    652                          0,
    653                          RTEMS_FIFO|RTEMS_SIMPLE_BINARY_SEMAPHORE|RTEMS_NO_INHERIT_PRIORITY|RTEMS_LOCAL|RTEMS_NO_PRIORITY_CEILING,
    654                          0,
    655                          &brm->rx_sem) != RTEMS_SUCCESSFUL ){
    656       printk("BRM: Failed to create rx semaphore\n");
    657                 return RTEMS_INTERNAL_ERROR;
    658     }
    659 
    660                         /* TX Semaphore created with count = 1 */
    661                 if ( rtems_semaphore_create(rtems_build_name('B', 'M', 'T', '0'+minor),
    662                          1,
    663                          RTEMS_FIFO|RTEMS_SIMPLE_BINARY_SEMAPHORE|RTEMS_NO_INHERIT_PRIORITY|RTEMS_LOCAL|RTEMS_NO_PRIORITY_CEILING,
    664                          0,
    665                          &brm->tx_sem) != RTEMS_SUCCESSFUL ){
    666       printk("BRM: Failed to create tx semaphore\n");
    667                 return RTEMS_INTERNAL_ERROR;
    668     }
    669 
    670                 /* Device Semaphore created with count = 1 */
    671                 if ( rtems_semaphore_create(rtems_build_name('B', 'M', 'D', '0'+minor),
    672                          1,
    673                          RTEMS_FIFO|RTEMS_SIMPLE_BINARY_SEMAPHORE|RTEMS_NO_INHERIT_PRIORITY|RTEMS_LOCAL|RTEMS_NO_PRIORITY_CEILING,
    674                          0,
    675                          &brm->dev_sem) != RTEMS_SUCCESSFUL ){
    676       printk("BRM: Failed to create device semaphore\n");
    677                 return RTEMS_INTERNAL_ERROR;
    678     }
    679 
    680 
    681                 /* Set base address of all descriptors */
    682                 brm->memarea_base = (unsigned int)&mem[(128*1024) * minor];
    683                 brm->desc = (struct desc_table *) brm->memarea_base;
    684                 brm->mem = (volatile unsigned short *) brm->memarea_base;
    685           brm->irq_log  = (struct irq_log_list *)(brm->memarea_base + (0xFFE0<<1)); /* last 64byte */
    686 
    687                 brm->bm_event = NULL;
    688                 brm->rt_event = NULL;
    689 
    690                 /* Sel clock so that we can write to BRM's registers */
    691                 brm->regs->w_ctrl = (allbrm_cfg_clksel<<9) | (allbrm_cfg_clkdiv<<5);
    692                 /* Reset BRM core */
    693                 brm->regs->w_ctrl = 1<<10 | READ_REG(&brm->regs->w_ctrl);
    694 
    695                 /* Register interrupt handler */
    696                 B1553BRM_REG_INT(B1553BRM_PREFIX(_interrupt_handler), brm->irqno, brm);
    697 
    698                 rt_init(brm);
    699 
    700                 DBG("BRM: LOG: 0x%lx, 0x%lx\n\r",brm->log,brm);
    701         }
    702 
    703         /* save number of BRM cores found */
    704         brm_cores = dev_cnt;
    705 
    706         DBG("BRM initialisation done.\n");
     781        struct drvmgr_dev *dev;
     782
     783        FUNCDBG("brm_open\n");
     784
     785        if ( drvmgr_get_dev(&b1553brm_drv_info.general, minor, &dev) ) {
     786                DBG("Wrong minor %d\n", minor);
     787                return RTEMS_UNSATISFIED;
     788        }
     789        brm = (brm_priv *)dev->priv;
     790
     791        if (rtems_semaphore_obtain(brm->dev_sem, RTEMS_NO_WAIT, RTEMS_NO_TIMEOUT) != RTEMS_SUCCESSFUL) {
     792                DBG("brm_open: resource in use\n");
     793                return RTEMS_RESOURCE_IN_USE; /* EBUSY */
     794        }
     795
     796        /* Set defaults */
     797        brm->event_id = 0;
     798
     799        start_operation(brm);
     800
     801        /* Register interrupt routine */
     802        if ( drvmgr_interrupt_register(brm->dev, 0, "b1553brm", b1553brm_interrupt, brm) ) {
     803                rtems_semaphore_release(brm->dev_sem);
     804                return RTEMS_UNSATISFIED;
     805        }
    707806
    708807        return RTEMS_SUCCESSFUL;
    709808}
    710 
    711 static rtems_device_driver brm_open(rtems_device_major_number major, rtems_device_minor_number minor, void *arg) {
    712                 brm_priv *brm;
    713 
    714     FUNCDBG("brm_open\n");
    715 
    716     if (minor >= brm_cores) {
    717         DBG("Wrong minor %d\n", minor);
    718         return RTEMS_UNSATISFIED; /* ENODEV */
    719     }
    720 
    721                 brm = &brms[minor];
    722 
    723     if (rtems_semaphore_obtain(brm->dev_sem, RTEMS_NO_WAIT, RTEMS_NO_TIMEOUT) != RTEMS_SUCCESSFUL) {
    724         DBG("brm_open: resource in use\n");
    725         return RTEMS_RESOURCE_IN_USE; /* EBUSY */
    726     }
    727 
    728                 /* Set defaults */
    729                 brm->event_id = 0;
    730 
    731     start_operation(brm);
    732 
    733     return RTEMS_SUCCESSFUL;
    734 }
    735 
     809 
    736810static rtems_device_driver brm_close(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
    737811{
    738         brm_priv *brm = &brms[minor];
     812        brm_priv *brm;
     813        struct drvmgr_dev *dev;
     814
    739815        FUNCDBG("brm_close");
     816       
     817        if ( drvmgr_get_dev(&b1553brm_drv_info.general, minor, &dev) ) {
     818                return RTEMS_UNSATISFIED;
     819        }
     820        brm = (brm_priv *)dev->priv;
     821
     822        drvmgr_interrupt_unregister(brm->dev, 0, b1553brm_interrupt, brm);
    740823
    741824        stop_operation(brm);
     
    744827        return RTEMS_SUCCESSFUL;
    745828}
    746 
    747 static int get_rt_messages(brm_priv *brm, void *buf, unsigned int msg_count) {
    748 
    749     struct rt_msg *dest = (struct rt_msg *) buf;
    750     int count = 0;
    751 
    752     if (brm->head == brm->tail) {
    753         return 0;
    754     }
    755 
    756     do {
    757 
    758         DBG("rt read - head: %d, tail: %d\n", brm->head, brm->tail);
    759         dest[count++] = brm->rt_event[INDEX(brm->tail++)];
    760 
    761     } while (brm->head != brm->tail && count < msg_count);
    762 
    763     return count;
    764 
    765 }
    766 
    767 static int get_bm_messages(brm_priv *brm, void *buf, unsigned int msg_count) {
    768 
    769     struct bm_msg *dest = (struct bm_msg *) buf;
    770     int count = 0;
    771 
    772     if (brm->head == brm->tail) {
    773         return 0;
    774     }
    775 
    776     do {
    777 
    778         DBG("bm read - head: %d, tail: %d\n", brm->head, brm->tail);
    779         dest[count++] = brm->bm_event[INDEX(brm->tail++)];
    780 
    781     } while (brm->head != brm->tail && count < msg_count);
    782 
    783     return count;
    784 
     829 
     830static int get_rt_messages(brm_priv *brm, void *buf, unsigned int msg_count)
     831{
     832        struct rt_msg *dest = (struct rt_msg *) buf;
     833        int count = 0;
     834
     835        if (brm->head == brm->tail) {
     836                return 0;
     837        }
     838
     839        do {
     840
     841                DBG("rt read - head: %d, tail: %d\n", brm->head, brm->tail);
     842                dest[count++] = brm->rt_event[INDEX(brm->tail++)];
     843        } while (brm->head != brm->tail && count < msg_count);
     844
     845        return count;
     846}
     847
     848static int get_bm_messages(brm_priv *brm, void *buf, unsigned int msg_count)
     849{
     850        struct bm_msg *dest = (struct bm_msg *) buf;
     851        int count = 0;
     852
     853        if (brm->head == brm->tail) {
     854                return 0;
     855        }
     856
     857        do {
     858
     859                DBG("bm read - head: %d, tail: %d\n", brm->head, brm->tail);
     860                dest[count++] = brm->bm_event[INDEX(brm->tail++)];
     861
     862        } while (brm->head != brm->tail && count < msg_count);
     863
     864        return count;
    785865}
    786866
    787867static rtems_device_driver brm_read(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
    788868{
    789     rtems_libio_rw_args_t *rw_args;
    790     int count = 0;
    791                 brm_priv *brm = &brms[minor];
    792     int (*get_messages)(brm_priv *brm, void *buf, unsigned int count);
    793 
    794                 if ( ! (brm->mode & (BRM_MODE_RT | BRM_MODE_BM)) ){
     869        rtems_libio_rw_args_t *rw_args;
     870        int count = 0;
     871        brm_priv *brm;
     872        struct drvmgr_dev *dev;
     873        int (*get_messages)(brm_priv *brm, void *buf, unsigned int count);
     874
     875        if ( drvmgr_get_dev(&b1553brm_drv_info.general, minor, &dev) ) {
     876                return RTEMS_UNSATISFIED;
     877        }
     878        brm = (brm_priv *)dev->priv;
     879
     880        if ( ! (brm->mode & (BRM_MODE_RT | BRM_MODE_BM)) ){
     881                return RTEMS_INVALID_NAME;
     882        }
     883
     884        rw_args = (rtems_libio_rw_args_t *) arg;
     885
     886        if ( ((READ_REG(&brm->regs->oper)>>8) & 3) == 1 ) { /* RT */
     887                get_messages = get_rt_messages;
     888        } else { /* BM */
     889                get_messages = get_bm_messages;
     890        }
     891
     892        FUNCDBG("brm_read [%i,%i]: buf: 0x%x len: %i\n",major, minor, (unsigned int)rw_args->buffer,rw_args->count);
     893
     894        while ( (count=get_messages(brm,rw_args->buffer, rw_args->count)) == 0 ) {
     895                if (brm->rx_blocking) {
     896                        rtems_semaphore_obtain(brm->rx_sem, RTEMS_WAIT, RTEMS_NO_TIMEOUT);
     897                } else {
     898                        /* Translates to EBUSY */
     899                        return RTEMS_RESOURCE_IN_USE;
     900                }
     901        }
     902
     903        rw_args->bytes_moved = count;
     904        return RTEMS_SUCCESSFUL;
     905}
     906
     907static rtems_device_driver brm_write(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
     908{
     909        rtems_libio_rw_args_t *rw_args;
     910        struct rt_msg *source;
     911        unsigned int count=0, current, next, descriptor, wc, suba;
     912        brm_priv *brm;
     913        struct drvmgr_dev *dev;
     914 
     915        if ( drvmgr_get_dev(&b1553brm_drv_info.general, minor, &dev) ) {
     916                return RTEMS_UNSATISFIED;
     917        }
     918        brm = (brm_priv *)dev->priv;
     919       
     920        if ( ! (brm->mode & BRM_MODE_RT) ){
     921                return RTEMS_INVALID_NAME;
     922        }
     923       
     924        rw_args = (rtems_libio_rw_args_t *) arg;
     925        source = (struct rt_msg *) rw_args->buffer;
     926
     927        FUNCDBG("brm_write [%i,%i]: buf: 0x%x len: %i\n",major, minor, (unsigned int)rw_args->buffer,rw_args->count);
     928
     929        do {
     930
     931                descriptor = source[count].desc & 0x7F;
     932                suba       = descriptor-32;
     933                wc         = source[count].miw >> 11;
     934                wc = wc ? wc : 32;
     935
     936                /* Only subaddress transmission is allowed with write */
     937                if (descriptor < 32 || descriptor >= 64)
    795938                        return RTEMS_INVALID_NAME;
    796                 }
    797 
    798     rw_args = (rtems_libio_rw_args_t *) arg;
    799 
    800     if ( ((READ_REG(&brm->regs->oper)>>8) & 3) == 1 ) { /* RT */
    801         get_messages = get_rt_messages;
    802     }
    803     else { /* BM */
    804         get_messages = get_bm_messages;
    805     }
    806 
    807 
    808     FUNCDBG("brm_read [%i,%i]: buf: 0x%x len: %i\n",major, minor, (unsigned int)rw_args->buffer,rw_args->count);
    809 
    810     while ( (count=get_messages(brm,rw_args->buffer, rw_args->count)) == 0 ) {
    811 
    812         if (brm->rx_blocking) {
    813             rtems_semaphore_obtain(brm->rx_sem, RTEMS_WAIT, RTEMS_NO_TIMEOUT);
    814         }
    815         else {
    816             /* Translates to EBUSY */
    817             return RTEMS_RESOURCE_IN_USE;
    818         }
    819 
    820     }
    821 
    822     rw_args->bytes_moved = count;
    823     return RTEMS_SUCCESSFUL;
    824 
    825 }
    826 
    827 
    828 static rtems_device_driver brm_write(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
    829 {
    830     rtems_libio_rw_args_t *rw_args;
    831     struct rt_msg *source;
    832     unsigned int count=0, current, next, descriptor, wc, suba;
    833                 brm_priv *brm = &brms[minor];
    834 
    835                 if ( ! (brm->mode & BRM_MODE_RT) ){
     939
     940                current = brm->desc[descriptor].cur;
     941                next = brm->written[suba] + 2 + wc;
     942
     943                if (brm->written[suba] < current) {
     944
     945                        if (next > current) {
     946
     947                                /* No room in transmission buffer */
     948                                if (brm->tx_blocking && count == 0) {
     949                                        rtems_semaphore_obtain(brm->tx_sem, RTEMS_WAIT, RTEMS_NO_TIMEOUT);
     950                                } else if ( count > 0 ) {
     951                                        /* return the number of messages sent so far */
     952                                        break;
     953                                } else {       
     954                                        /* Translates to posix EBUSY */
     955                                        return RTEMS_RESOURCE_IN_USE;
     956                                }
     957                        }
     958                }
     959
     960                memcpy((void *)&brm->mem[brm->written[suba]], &source[count], (2+wc)*2);
     961
     962                count++;
     963
     964                if (next >= brm->desc[descriptor].bot) {
     965                        next = brm->desc[descriptor].top;
     966                }
     967                brm->written[suba] = next;
     968
     969        }  while (count < rw_args->count);
     970
     971        rw_args->bytes_moved = count;
     972
     973        if (count >= 0) {
     974                return RTEMS_SUCCESSFUL;
     975        }
     976        return RTEMS_UNSATISFIED;
     977}
     978
     979static rtems_device_driver brm_control(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
     980{
     981   
     982        unsigned int i=0;
     983        unsigned short ctrl, oper, cw1, cw2;
     984        rtems_libio_ioctl_args_t *ioarg = (rtems_libio_ioctl_args_t *) arg;
     985        unsigned int *data = ioarg->buffer;
     986        struct bc_msg *cmd_list = (struct bc_msg *) ioarg->buffer;
     987        brm_priv *brm;
     988        struct drvmgr_dev *dev;
     989        rtems_device_driver ret;
     990        int len, msglen;
     991
     992        FUNCDBG("brm_control[%d]: [%i,%i]\n", minor, major, minor);
     993
     994        if ( drvmgr_get_dev(&b1553brm_drv_info.general, minor, &dev) ) {
     995                return RTEMS_UNSATISFIED;
     996        }
     997        brm = (brm_priv *)dev->priv;
     998
     999        if (!ioarg) {
     1000                DBG("brm_control: invalid argument\n");
     1001                return RTEMS_INVALID_NAME;
     1002        }
     1003
     1004        ioarg->ioctl_return = 0;
     1005        switch (ioarg->command) {
     1006
     1007                case BRM_SET_MODE:
     1008                if ( data[0] > 2 )
    8361009                        return RTEMS_INVALID_NAME;
    837                 }
    838 
    839     rw_args = (rtems_libio_rw_args_t *) arg;
    840     source = (struct rt_msg *) rw_args->buffer;
    841 
    842     FUNCDBG("brm_write [%i,%i]: buf: 0x%x len: %i\n",major, minor, (unsigned int)rw_args->buffer,rw_args->count);
    843 
    844     do {
    845 
    846         descriptor = source[count].desc & 0x7F;
    847         suba       = descriptor-32;
    848         wc         = source[count].miw >> 11;
    849         wc = wc ? wc : 32;
    850 
    851         /* Only subaddress transmission is allowed with write */
    852         if (descriptor < 32 || descriptor >= 64)
    853             return RTEMS_INVALID_NAME;
    854 
    855         current = brm->desc[descriptor].cur;
    856         next = brm->written[suba] + 2 + wc;
    857 
    858         if (brm->written[suba] < current) {
    859 
    860             if (next > current) {
    861 
    862                 /* No room in transmission buffer */
    863 
    864                 if (brm->tx_blocking && count == 0) {
    865                     rtems_semaphore_obtain(brm->tx_sem, RTEMS_WAIT, RTEMS_NO_TIMEOUT);
    866                 }
    867                                                                 else if ( brm->tx_blocking && (count != 0) ){
    868                                                                         /* return the number of messages sent so far */
    869                                                                         break;
    870                                                                 }
    871                 else {
    872                     /* Translates to posix EBUSY */
    873                     return RTEMS_RESOURCE_IN_USE;
    874                 }
    875             }
    876         }
    877 
    878         memcpy((void *)&brm->mem[brm->written[suba]], &source[count], (2+wc)*2);
    879 
    880         count++;
    881 
    882         if (next >= brm->desc[descriptor].bot) {
    883             next = brm->desc[descriptor].top;
    884         }
    885         brm->written[suba] = next;
    886 
    887     }  while (count < rw_args->count);
    888 
    889     rw_args->bytes_moved = count;
    890 
    891     if (count >= 0) {
    892         return RTEMS_SUCCESSFUL;
    893     }
    894     return RTEMS_UNSATISFIED;
    895 }
    896 
    897 static rtems_device_driver brm_control(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
    898 {
    899 
    900     unsigned int i=0;
    901     unsigned short ctrl, oper, cw1, cw2;
    902     rtems_libio_ioctl_args_t *ioarg = (rtems_libio_ioctl_args_t *) arg;
    903     unsigned int *data = ioarg->buffer;
    904     struct bc_msg *cmd_list = (struct bc_msg *) ioarg->buffer;
    905                 brm_priv *brm = &brms[minor];
    906                 rtems_device_driver ret;
    907                 int len;
    908 
    909     FUNCDBG("brm_control[%d]: [%i,%i]\n",minor,major, minor);
    910 
    911     if (!ioarg) {
    912         DBG("brm_control: invalid argument\n");
    913         return RTEMS_INVALID_NAME;
    914     }
    915 
    916     ioarg->ioctl_return = 0;
    917     switch(ioarg->command) {
    918 
    919     case BRM_SET_MODE:
    920                         if ( data[0] > 2 )
    921                                 return RTEMS_INVALID_NAME;
    922       stop_operation(brm);
    923         if (data[0] == 0) {
    924             ret = bc_init(brm);
    925         }
    926         else if (data[0] == 1) {
    927             ret = rt_init(brm);
    928         }
    929         else if (data[0] == 2) {
    930             ret = bm_init(brm);
    931         }else
    932                                         ret = RTEMS_INVALID_NAME;
    933 
    934                                 if ( ret != RTEMS_SUCCESSFUL)
    935                                                         return ret;
    936 
    937         if ( brm->mode & (BRM_MODE_RT | BRM_MODE_BM ) )
    938                                         start_operation(brm);
    939         break;
    940 
    941     case BRM_SET_BUS:
    942         stop_operation(brm);
    943                                 ctrl = READ_REG(&brm->regs->ctrl);
    944         ctrl &= 0xE7FF;                               /* Clear bit 12-11 ...      */
    945         ctrl |= (data[0]&0x3)<<11;                    /* ... OR in new bus status */
    946                                 brm->regs->ctrl = ctrl;
    947         start_operation(brm);
    948         break;
    949 
    950     case BRM_SET_MSGTO:
    951         stop_operation(brm);
    952         ctrl = READ_REG(&brm->regs->ctrl);
    953                                 ctrl &= 0xFDFF;                               /* Clear bit 9 ...          */
    954         ctrl |= (data[0]&1)<<9;                       /* ... OR in new MSGTO      */
    955                                 brm->regs->ctrl = ctrl;
    956         start_operation(brm);
    957         break;
    958 
    959     case BRM_SET_RT_ADDR:
    960         stop_operation(brm);
    961                                 oper = READ_REG(&brm->regs->oper);
    962         oper &= 0x03FF;                               /* Clear bit 15-10 ...      */
    963         oper |= (data[0]&0x1f)<<11;                   /* ... OR in new address    */
    964         oper |= odd_parity(data[0]&0x1f)<<10;         /* ... OR in parity         */
    965                                 brm->regs->oper = oper;
    966         start_operation(brm);
    967         break;
    968 
    969     case BRM_SET_STD:
    970         stop_operation(brm);
    971                                 ctrl = READ_REG(&brm->regs->ctrl);
    972         ctrl &= 0xFF7F;                               /* Clear bit 7 ...           */
    973         ctrl |= (data[0]&1)<<7;                       /* ... OR in new ABSTD (1=A) */
    974                                 brm->regs->ctrl = ctrl;
    975         start_operation(brm);
    976         break;
    977 
    978     case BRM_SET_BCE:
    979         stop_operation(brm);
    980                                 ctrl = READ_REG(&brm->regs->ctrl);
    981         ctrl &= 0xFFEF;                               /* Clear bit 4 ...           */
    982         ctrl |= (data[0]&1)<<4;                       /* ... OR in new BCE         */
    983                                 brm->regs->ctrl = ctrl;
    984         start_operation(brm);
    985      break;
    986 
    987     case BRM_TX_BLOCK:
    988         brm->tx_blocking     = data[0];
    989         break;
    990 
    991     case BRM_RX_BLOCK:
    992         brm->rx_blocking     = data[0];
    993         break;
    994 
    995     case BRM_DO_LIST:
    996 
    997         if ( brm->mode != BRM_MODE_BC ){
    998           return RTEMS_INVALID_NAME;
     1010                stop_operation(brm);
     1011                if (data[0] == 0) {
     1012                        ret = bc_init(brm);
     1013                } else if (data[0] == 1) {
     1014                        ret = rt_init(brm);                             
     1015                } else if (data[0] == 2) {
     1016                        ret = bm_init(brm);                                             
     1017                } else {
     1018                        ret = RTEMS_INVALID_NAME;
     1019                }
     1020                if ( ret != RTEMS_SUCCESSFUL)
     1021                        return ret;
     1022
     1023                if ( brm->mode & (BRM_MODE_RT | BRM_MODE_BM ) )
     1024                        start_operation(brm);
     1025                break;
     1026
     1027                case BRM_SET_BUS:
     1028                stop_operation(brm);
     1029                ctrl = READ_REG(&brm->regs->ctrl);
     1030                ctrl &= 0xE7FF;                               /* Clear bit 12-11 ...      */
     1031                ctrl |= (data[0]&0x3)<<11;                    /* ... OR in new bus status */
     1032                brm->regs->ctrl = ctrl;
     1033                start_operation(brm);
     1034                break;
     1035
     1036                case BRM_SET_MSGTO:
     1037                stop_operation(brm);
     1038                ctrl = READ_REG(&brm->regs->ctrl);
     1039                ctrl &= 0xFDFF;                               /* Clear bit 9 ...          */
     1040                ctrl |= (data[0]&1)<<9;                       /* ... OR in new MSGTO      */
     1041                brm->regs->ctrl = ctrl;
     1042                start_operation(brm);
     1043                break;
     1044
     1045                case BRM_SET_RT_ADDR:   
     1046                stop_operation(brm);
     1047                oper = READ_REG(&brm->regs->oper);
     1048                oper &= 0x03FF;                               /* Clear bit 15-10 ...      */
     1049                oper |= (data[0]&0x1f)<<11;                   /* ... OR in new address    */
     1050                oper |= odd_parity(data[0]&0x1f)<<10;         /* ... OR in parity         */
     1051                brm->regs->oper = oper;
     1052                start_operation(brm);
     1053                break;
     1054
     1055                case BRM_SET_STD:   
     1056                stop_operation(brm);
     1057                ctrl = READ_REG(&brm->regs->ctrl);
     1058                ctrl &= 0xFF7F;                               /* Clear bit 7 ...           */
     1059                ctrl |= (data[0]&1)<<7;                       /* ... OR in new ABSTD (1=A) */
     1060                brm->regs->ctrl = ctrl;
     1061                start_operation(brm);
     1062                break;
     1063
     1064                case BRM_SET_BCE:
     1065                stop_operation(brm);
     1066                ctrl = READ_REG(&brm->regs->ctrl);
     1067                ctrl &= 0xFFEF;                               /* Clear bit 4 ...           */
     1068                ctrl |= (data[0]&1)<<4;                       /* ... OR in new BCE         */
     1069                brm->regs->ctrl = ctrl;
     1070                start_operation(brm);
     1071                break;
     1072
     1073                case BRM_TX_BLOCK:
     1074                brm->tx_blocking     = data[0];     
     1075                break;
     1076
     1077                case BRM_RX_BLOCK:
     1078                brm->rx_blocking     = data[0];   
     1079                break;
     1080
     1081                case BRM_DO_LIST:
     1082                if ( brm->mode != BRM_MODE_BC ){
     1083                        return RTEMS_INVALID_NAME;
     1084                }
     1085
     1086                /* Check if we are bus controller */
     1087                if ( ((READ_REG(&brm->regs->oper)>>8) & 3) != 0 ) {
     1088                        return RTEMS_INVALID_NAME;
     1089                }
     1090
     1091                /* Already processing list? */
     1092                if (is_executing(brm)) {
     1093                        return RTEMS_RESOURCE_IN_USE;
     1094                }
     1095
     1096                /* clear any earlier releases */
     1097                rtems_semaphore_obtain(brm->tx_sem, RTEMS_NO_WAIT, RTEMS_NO_TIMEOUT);
     1098
     1099                brm->bc_list_fail = 0;
     1100                brm->cur_list = cmd_list;
     1101                brm->regs->dpoint = 0;
     1102
     1103                i = 0;
     1104                while ( (cmd_list[i].ctrl & BC_EOL) == 0) {
     1105
     1106                        ctrl = (4<<12) | (((cmd_list[i].ctrl&BC_BUSA)==BC_BUSA)<<9) | (((cmd_list[i].ctrl&BC_RTRT)==BC_RTRT)<<8);
     1107
     1108                        if (cmd_list[i].ctrl&BC_RTRT) {
     1109                                cw1 = (cmd_list[i].rtaddr[0]<<11) | (0<<10) | (cmd_list[i].subaddr[0]<<5) | (cmd_list[i].wc & 0x1f); /* receive cw */
     1110                                cw2 = (cmd_list[i].rtaddr[1]<<11) | (1<<10) | (cmd_list[i].subaddr[1]<<5) | (cmd_list[i].wc & 0x1f); /* transmit cw */
     1111                        } else {
     1112                                cw1 = (cmd_list[i].rtaddr[0]<<11) | (((cmd_list[i].ctrl&BC_TR)==BC_TR)<<10) | (cmd_list[i].subaddr[0]<<5) | (cmd_list[i].wc&0x1f);
     1113                                cw2 = 0;
     1114                        }
     1115
     1116                        /* Set up command block */
     1117                        brm->bcmem->descs[i].ctrl = ctrl;
     1118                        brm->bcmem->descs[i].cw1 = cw1;
     1119                        brm->bcmem->descs[i].cw2 = cw2;
     1120                        /* data pointer:
     1121                         * (&brm->bcmem->msg_data[i].data[0] & 0x1ffff) / 2
     1122                         */
     1123                        brm->bcmem->descs[i].dptr = 1024+i*32;  /* data pointer */
     1124                        brm->bcmem->descs[i].tsw[0] = 0;
     1125                        brm->bcmem->descs[i].tsw[1] = 0;
     1126                        brm->bcmem->descs[i].ba = 0;
     1127                        brm->bcmem->descs[i].timer = 0;
     1128
     1129                        msglen = cmd_list[i].wc;
     1130                        if ( msglen == 0 )
     1131                                msglen = 32;
     1132                        memcpy((void *)&brm->bcmem->msg_data[i].data[0], &cmd_list[i].data[0], msglen*2);
     1133
     1134                        i++;
     1135                }
     1136
     1137                brm->bcmem->descs[i].ctrl = 0; /* end of list */
     1138
     1139                start_operation(brm);       
     1140                break; 
     1141
     1142                case BRM_LIST_DONE:
     1143
     1144                if ( brm->mode != BRM_MODE_BC ){
     1145                        return RTEMS_INVALID_NAME;
     1146                }
     1147                               
     1148                /* Check if we are bus controller */
     1149                if ( ((READ_REG(&brm->regs->oper)>>8) & 3) != 0 ) {
     1150                        return RTEMS_INVALID_NAME;
     1151                }
     1152
     1153                if (is_executing(brm)) {
     1154
     1155                        data[0] = 0;
     1156                        if (brm->tx_blocking) {
     1157                                rtems_semaphore_obtain(brm->tx_sem, RTEMS_WAIT, RTEMS_NO_TIMEOUT);
     1158                                data[0] = 1;
     1159                                if ( brm->bc_list_fail ){
     1160                                        return RTEMS_INVALID_NAME;
    9991161                                }
    1000 
    1001         /* Check if we are bus controller */
    1002         if ( ((READ_REG(&brm->regs->oper)>>8) & 3) != 0 ) {
    1003             return RTEMS_INVALID_NAME;
    1004         }
    1005 
    1006         /* Already processing list? */
    1007         if (is_executing(brm)) {
    1008             return RTEMS_RESOURCE_IN_USE;
    1009         }
    1010 
    1011                                 /* clear any earlier releases */
    1012         rtems_semaphore_obtain(brm->tx_sem, RTEMS_NO_WAIT, RTEMS_NO_TIMEOUT);
    1013 
    1014                                 brm->bc_list_fail = 0;
    1015         brm->cur_list = cmd_list;
    1016                                 brm->regs->dpoint = 0;
    1017 
    1018         i = 0;
    1019         while ( (cmd_list[i].ctrl & BC_EOL) == 0) {
    1020 
    1021             ctrl = (4<<12) | (((cmd_list[i].ctrl&BC_BUSA)==BC_BUSA)<<9) | (((cmd_list[i].ctrl&BC_RTRT)==BC_RTRT)<<8);
    1022 
    1023             if (cmd_list[i].ctrl&BC_RTRT) {
    1024                 cw1 = (cmd_list[i].rtaddr[0]<<11) | (0<<10) | (cmd_list[i].subaddr[0]<<5) | (cmd_list[i].wc & 0x1f); /* receive cw */
    1025                 cw2 = (cmd_list[i].rtaddr[1]<<11) | (1<<10) | (cmd_list[i].subaddr[1]<<5) | (cmd_list[i].wc & 0x1f); /* transmit cw */
    1026             }
    1027             else {
    1028                 cw1 = (cmd_list[i].rtaddr[0]<<11) | (((cmd_list[i].ctrl&BC_TR)==BC_TR)<<10) | (cmd_list[i].subaddr[0]<<5) | (cmd_list[i].wc&0x1f);
    1029                                                                 cw2 = 0;
    1030             }
    1031 
    1032 
    1033             /* Set up command block */
    1034             brm->bcmem->descs[i].ctrl = ctrl;
    1035             brm->bcmem->descs[i].cw1 = cw1;
    1036             brm->bcmem->descs[i].cw2 = cw2;
    1037                                                 /* data pointer:
    1038                                                  * (&brm->bcmem->msg_data[i].data[0] & 0x1ffff) / 2
    1039                                                  */
    1040             brm->bcmem->descs[i].dptr = 1024+i*32;  /* data pointer */
    1041                                                 brm->bcmem->descs[i].tsw[0] = 0;
    1042                                                 brm->bcmem->descs[i].tsw[1] = 0;
    1043                                                 brm->bcmem->descs[i].ba = 0;
    1044                                                 brm->bcmem->descs[i].timer = 0;
    1045 
    1046             memcpy((void *)&brm->bcmem->msg_data[i].data[0], &cmd_list[i].data[0], cmd_list[i].wc*2);
    1047 
    1048             i++;
    1049         }
    1050 
    1051         brm->bcmem->descs[i].ctrl = 0; /* end of list */
    1052 
    1053         start_operation(brm);
    1054 
    1055         break;
    1056 
    1057     case BRM_LIST_DONE:
    1058 
    1059         if ( brm->mode != BRM_MODE_BC ){
    1060           return RTEMS_INVALID_NAME;
     1162                        } else {
     1163                                return RTEMS_RESOURCE_IN_USE;
     1164                        }
     1165                } else {
     1166                        data[0] = 1; /* done */
     1167                }
     1168
     1169                /* copy finished list results back into bc_msg array */
     1170                i = 0;
     1171                while ( (brm->cur_list[i].ctrl & BC_EOL) == 0) {
     1172                        if (READ_DMA(&brm->bcmem->descs[i].ctrl) & 1) {
     1173                                brm->cur_list[i].ctrl |= 0x8000; /* Set BAME */
     1174                        }
     1175                        if (brm->cur_list[i].ctrl & BC_TR) {
     1176                                /* RT Transmit command, copy received data */
     1177                                len = brm->cur_list[i].wc;
     1178                                if ( len == 0 )
     1179                                        len = 32;
     1180                                while ( len-- > 0) {
     1181                                        brm->cur_list[i].data[len] = READ_DMA(&brm->bcmem->msg_data[i].data[len]);
    10611182                                }
    1062 
    1063                                 /* Check if we are bus controller */
    1064         if ( ((READ_REG(&brm->regs->oper)>>8) & 3) != 0 ) {
    1065             return RTEMS_INVALID_NAME;
    1066         }
    1067 
    1068         if (is_executing(brm)) {
    1069 
    1070             data[0] = 0;
    1071             if (brm->tx_blocking) {
    1072                 rtems_semaphore_obtain(brm->tx_sem, RTEMS_WAIT, RTEMS_NO_TIMEOUT);
    1073                 data[0] = 1;
    1074                                                                 if ( brm->bc_list_fail ){
    1075                                                                         return RTEMS_INVALID_NAME;
    1076                                                                 }
    1077             }else{
    1078                                                         return RTEMS_RESOURCE_IN_USE;
    1079                                                 }
    1080 
    1081 
    1082         }
    1083         else {
    1084             data[0] = 1; /* done */
    1085         }
    1086 
    1087         /* copy finished list results back into bc_msg array */
    1088         i = 0;
    1089         while ( (brm->cur_list[i].ctrl & BC_EOL) == 0) {
    1090 
    1091             if (READ_DMA(&brm->bcmem->descs[i].ctrl) & 1) {
    1092                 brm->cur_list[i].ctrl |= 0x8000; /* Set BAME */
    1093             }
    1094             if (brm->cur_list[i].ctrl & BC_TR) {
    1095                 /* RT Transmit command, copy received data */
    1096                                                                 len = brm->cur_list[i].wc;
    1097                                                                 while( len-- > 0){
    1098                                                                         brm->cur_list[i].data[len] = READ_DMA(&brm->bcmem->msg_data[i].data[len]);
    1099                                                                 }
    1100             }
    1101             brm->cur_list[i].tsw[0] = READ_DMA(&brm->bcmem->descs[i].tsw[0]);
    1102             brm->cur_list[i].tsw[1] = READ_DMA(&brm->bcmem->descs[i].tsw[1]);
    1103 
    1104             i++;
    1105         }
    1106         break;
    1107 
    1108 
    1109     case BRM_CLR_STATUS:
    1110                         brm->status = 0;
    1111                         break;
    1112 
    1113     case BRM_GET_STATUS: /* copy status */
    1114 
    1115                         if ( !ioarg->buffer )
    1116                                 return RTEMS_INVALID_NAME;
    1117 
    1118                         *(unsigned int *)ioarg->buffer = brm->status;
    1119                         break;
    1120 
     1183                        }
     1184                        brm->cur_list[i].tsw[0] = READ_DMA(&brm->bcmem->descs[i].tsw[0]);
     1185                        brm->cur_list[i].tsw[1] = READ_DMA(&brm->bcmem->descs[i].tsw[1]);
     1186
     1187                        i++;
     1188                }
     1189                break;
     1190
     1191                case BRM_CLR_STATUS:
     1192                brm->status = 0;
     1193                break;
     1194
     1195                case BRM_GET_STATUS: /* copy status */
     1196                if ( !ioarg->buffer )
     1197                        return RTEMS_INVALID_NAME;
     1198
     1199                *(unsigned int *)ioarg->buffer = brm->status;
     1200                break;
     1201   
    11211202                case BRM_SET_EVENTID:
    1122                         brm->event_id = (rtems_id)ioarg->buffer;
    1123                         break;
    1124 
    1125     default:
    1126         return RTEMS_NOT_DEFINED;
    1127     }
    1128     return RTEMS_SUCCESSFUL;
    1129 }
    1130 
    1131 #ifdef B1553BRM_DEFINE_INTHANDLER
    1132 static void b1553brm_interrupt_handler(rtems_vector_number v){
    1133         int i;
    1134         /* find minor */
    1135         for(i=0; i<brm_cores; i++){
    1136                 if ( (brms[i].irqno+0x10) == v ){
    1137                         brm_interrupt(&brms[i]);
    1138                         return;
    1139                 }
    1140         }
    1141 }
    1142 #endif
    1143 
    1144 static void brm_interrupt(brm_priv *brm) {
    1145   unsigned short descriptor, current, pending, miw, wc, tmp;
     1203                brm->event_id = (rtems_id)ioarg->buffer;
     1204                break;
     1205
     1206                default:
     1207                return RTEMS_NOT_DEFINED;
     1208        }
     1209        return RTEMS_SUCCESSFUL;
     1210}
     1211
     1212static void b1553brm_interrupt(void *arg)
     1213{
     1214        brm_priv *brm = arg;
     1215        unsigned short descriptor, current, pending, miw, wc, tmp, ctrl;
    11461216        unsigned short msgadr, iaw, iiw;
    11471217        int len;
    1148         int signal_event=0;
     1218        int signal_event=0, wake_rx_task=0, wake_tx_task=0;
    11491219        unsigned int event_status=0;
     1220        int accessed;
    11501221        #define SET_ERROR_DESCRIPTOR(descriptor) (event_status = (event_status & 0x0000ffff) | descriptor<<16)
    1151 
    1152         while( (iiw=READ_REG(&brm->irq_log[brm->irq].iiw)) != 0xffff ){
    1153                 iaw=READ_REG(&brm->irq_log[brm->irq].iaw);
    1154 
     1222               
     1223        while( (iiw=READ_DMA(&brm->irq_log[brm->irq].iiw)) != 0xffff ){
     1224                iaw=READ_DMA(&brm->irq_log[brm->irq].iaw);
     1225               
    11551226                /* indicate that the interrupt log entry has been processed */
    11561227                brm->irq_log[brm->irq].iiw = 0xffff;
    11571228
    11581229                /* Interpret interrupt log entry  */
    1159     descriptor = iaw >> 2;
    1160     pending    = iiw;
    1161     brm->irq = (brm->irq + 1) % 16;
    1162 
     1230                descriptor = iaw >> 2;
     1231                pending    = iiw;
     1232                brm->irq = (brm->irq + 1) % 16;
     1233               
    11631234                /* Clear the log so that we */
    11641235
    11651236
    1166     /* Subaddress accessed irq (RT only)
    1167      *
    1168      * Can be either a receive or transmit command
    1169      * as well as a mode code.
    1170      */
    1171     if (pending & BRM_SUBAD_IRQ) {
    1172 
    1173         /* Pointer to next free message in circular buffer */
    1174         current = READ_DMA(&brm->desc[descriptor].cur);
    1175 
    1176         while  ( (msgadr=brm->last_read[descriptor]) != current) {
    1177 
    1178             /* Get word count */
    1179             miw = READ_DMA(&brm->mem[msgadr]);
    1180             wc  = miw >> 11;
    1181 
    1182             /* Data received */
    1183             if (descriptor < 32) {
    1184                 wc = wc ? wc : 32;
    1185             }
    1186             /* Data transmitted */
    1187             else if (descriptor < 64) {
    1188                 wc = wc ? wc : 32;
    1189                 rtems_semaphore_release(brm->tx_sem);
    1190             }
    1191             /* RX Mode code */
    1192             else if (descriptor < 96) {
    1193                 wc = (wc>>4);
    1194             }
    1195             /* TX Mode code */
    1196             else if (descriptor < 128) {
    1197                 wc = (wc>>4);
    1198             }
    1199 
     1237                /* Subaddress accessed irq (RT only)
     1238                 *
     1239                 * Can be either a receive or transmit command
     1240                 * as well as a mode code.
     1241                */
     1242                if (pending & BRM_SUBAD_IRQ) {
     1243
     1244                        /* Pointer to next free message in circular buffer */
     1245                        current = READ_DMA(&brm->desc[descriptor].cur);
     1246                        ctrl = READ_DMA(&brm->desc[descriptor].ctrl);
    12001247#ifdef DEBUG
    1201             brm->log[brm->log_i++ % EVENT_QUEUE_SIZE] = (descriptor << 16) | wc;
    1202             brm->log[brm->log_i++ % EVENT_QUEUE_SIZE] = current;
    1203             brm->log[brm->log_i++ % EVENT_QUEUE_SIZE] = msgadr;
     1248                        brm->log[brm->log_i++ % EVENT_QUEUE_SIZE] = (0xff<<16);
     1249                        brm->log[brm->log_i++ % EVENT_QUEUE_SIZE] = current;
     1250                        brm->log[brm->log_i++ % EVENT_QUEUE_SIZE] = ctrl;
     1251                        brm->log[brm->log_i++ % EVENT_QUEUE_SIZE] = 0;
    12041252#endif
    1205 
    1206             /* If there is room in the event queue, copy the event there */
    1207             if (brm->head - brm->tail != EVENT_QUEUE_SIZE) {
    1208 
    1209                 /* Copy to event queue */
    1210                                                                 brm->rt_event[INDEX(brm->head)].miw = READ_DMA(&brm->mem[msgadr]);
    1211                                                                 brm->rt_event[INDEX(brm->head)].time = READ_DMA(&brm->mem[msgadr+1]);
    1212                                                                 len = wc;
    1213                                                                 while( len-- > 0){
    1214                                                                         brm->rt_event[INDEX(brm->head)].data[len] = READ_DMA(&brm->mem[msgadr+2+len]);
    1215                                                                 }
    1216                 brm->rt_event[INDEX(brm->head)].desc = descriptor;
    1217                 brm->head++;
    1218 
    1219             }
    1220             else {
    1221                 /* Indicate overrun */
    1222                 brm->rt_event[INDEX(brm->head)].desc |= 0x8000;
    1223             }
    1224 
    1225             msgadr += (2+wc);
    1226 
    1227             if (msgadr >= brm->desc[descriptor].bot) {
    1228                 msgadr = brm->desc[descriptor].top;
    1229             }
    1230                                                 brm->last_read[descriptor] = msgadr;
     1253                        accessed = ctrl & 0x10;
     1254                        /* Note that current may be equal to bot and top when
     1255                         * circular buffer one can handle one message.
     1256                         */
     1257                        if ( accessed )
     1258                          do {
     1259                                msgadr = brm->last_read[descriptor];
     1260
     1261                                /* Get word count */
     1262                                miw = READ_DMA(&brm->mem[msgadr]);
     1263                                wc  = miw >> 11;
     1264
     1265                                /* Data received */
     1266                                if (descriptor < 32) {
     1267                                        wc = wc ? wc : 32;
     1268                                }
     1269                                /* Data transmitted */
     1270                                else if (descriptor < 64) {
     1271                                        wc = wc ? wc : 32; 
     1272                                        wake_tx_task=1;
     1273                                }
     1274                                /* RX Mode code */
     1275                                else if (descriptor < 96) {
     1276                                        wc = (wc>>4);
     1277                                }
     1278                                /* TX Mode code */
     1279                                else if (descriptor < 128) {
     1280                                        wc = (wc>>4);
     1281                                }
     1282
     1283#ifdef DEBUG           
     1284                                brm->log[brm->log_i++ % EVENT_QUEUE_SIZE] = (descriptor << 16) | wc;
     1285                                brm->log[brm->log_i++ % EVENT_QUEUE_SIZE] = current;
     1286                                brm->log[brm->log_i++ % EVENT_QUEUE_SIZE] = msgadr;
     1287#endif
     1288
     1289                                /* If there is room in the event queue, copy the event there */
     1290                                if (brm->head - brm->tail != EVENT_QUEUE_SIZE) {
     1291
     1292                                        /* Copy to event queue */
     1293                                        brm->rt_event[INDEX(brm->head)].miw = READ_DMA(&brm->mem[msgadr]);
     1294                                        brm->rt_event[INDEX(brm->head)].time = READ_DMA(&brm->mem[msgadr+1]);
     1295                                        len = wc;
     1296                                        while( len-- > 0){
     1297                                                brm->rt_event[INDEX(brm->head)].data[len] = READ_DMA(&brm->mem[msgadr+2+len]);
     1298                                        }
     1299                                        brm->rt_event[INDEX(brm->head)].desc = descriptor;
     1300                                        brm->head++;
     1301                                }
     1302                                else {
     1303                                        /* Indicate overrun */
     1304                                        brm->rt_event[INDEX(brm->head)].desc |= 0x8000;
     1305                                }
     1306
     1307                                msgadr += (2+wc);
     1308
     1309                                if (msgadr >= READ_DMA(&brm->desc[descriptor].bot)) {
     1310                                        msgadr = READ_DMA(&brm->desc[descriptor].top);
     1311                                }
     1312                                brm->last_read[descriptor] = msgadr;
    12311313
    12321314#ifdef DEBUG
    1233             brm->log[brm->log_i++ % EVENT_QUEUE_SIZE] = msgadr;
     1315                                brm->log[brm->log_i++ % EVENT_QUEUE_SIZE] = msgadr;
    12341316#endif
    1235 
    1236             /* Wake any blocked rx thread */
    1237             rtems_semaphore_release(brm->rx_sem);
    1238 
    1239         }
    1240 
    1241     }
    1242 
    1243     if (pending & BRM_EOL_IRQ) {
    1244         rtems_semaphore_release(brm->tx_sem);
    1245     }
    1246 
    1247     if (pending & BRM_BC_ILLCMD_IRQ) {
    1248                                 brm->bc_list_fail = 1;
    1249         rtems_semaphore_release(brm->tx_sem);
    1250                                 SET_ERROR_DESCRIPTOR(descriptor);
    1251                                 FUNCDBG("BRM: ILLCMD IRQ\n\r");
    1252     }
    1253 
    1254     /* Monitor irq */
    1255     if (pending & BRM_MBC_IRQ) {
    1256 
    1257         stop_operation(brm);
    1258         brm->regs->mbc = 1;
    1259         start_operation(brm);
    1260 
    1261         /* If there is room in the event queue, copy the event there */
    1262         if (brm->head - brm->tail != EVENT_QUEUE_SIZE) {
    1263 
    1264             /* Copy to event queue */
    1265 
    1266             brm->bm_event[INDEX(brm->head)].miw  =  READ_DMA(&brm->mem[0]);
    1267             brm->bm_event[INDEX(brm->head)].cw1  =  READ_DMA(&brm->mem[1]);
    1268             brm->bm_event[INDEX(brm->head)].cw2  =  READ_DMA(&brm->mem[2]);
    1269             brm->bm_event[INDEX(brm->head)].sw1  =  READ_DMA(&brm->mem[4]);
    1270             brm->bm_event[INDEX(brm->head)].sw2  =  READ_DMA(&brm->mem[5]);
    1271             brm->bm_event[INDEX(brm->head)].time =  READ_DMA(&brm->mem[6]);
    1272 
    1273             len = 32;
    1274             while ( len-- ){
    1275               brm->bm_event[INDEX(brm->head)].data[len] =  READ_DMA(&brm->mem[0x100+len]);
    1276               len--;
    1277               brm->bm_event[INDEX(brm->head)].data[len] =  READ_DMA(&brm->mem[0x100+len]);
    1278               len--;
    1279               brm->bm_event[INDEX(brm->head)].data[len] =  READ_DMA(&brm->mem[0x100+len]);
    1280               len--;
    1281               brm->bm_event[INDEX(brm->head)].data[len] =  READ_DMA(&brm->mem[0x100+len]);
    1282             }
    1283 /*            memcpy((void *)brm->bm_event[INDEX(brm->head)].data, &brm->mem[0x100], 32);*/
     1317                                wake_rx_task = 1;
     1318                          } while ( (msgadr=brm->last_read[descriptor]) != current );
     1319                }
     1320
     1321                if (pending & BRM_EOL_IRQ) { 
     1322                        wake_tx_task = 1;
     1323                }
     1324
     1325                if (pending & BRM_BC_ILLCMD_IRQ) {
     1326                        brm->bc_list_fail = 1;
     1327                        wake_tx_task = 1;
     1328                        SET_ERROR_DESCRIPTOR(descriptor);
     1329                        FUNCDBG("BRM: ILLCMD IRQ\n\r");
     1330                }
     1331
     1332                /* Monitor irq */
     1333                if (pending & BRM_MBC_IRQ) {
     1334
     1335                        stop_operation(brm);
     1336                        brm->regs->mbc = 1;
     1337                        start_operation(brm);
     1338
     1339                        /* If there is room in the event queue, copy the event there */
     1340                        if (brm->head - brm->tail != EVENT_QUEUE_SIZE) {
     1341
     1342                                /* Copy to event queue */
     1343
     1344                                brm->bm_event[INDEX(brm->head)].miw  =  READ_DMA(&brm->mem[0]);
     1345                                brm->bm_event[INDEX(brm->head)].cw1  =  READ_DMA(&brm->mem[1]);
     1346                                brm->bm_event[INDEX(brm->head)].cw2  =  READ_DMA(&brm->mem[2]);
     1347                                brm->bm_event[INDEX(brm->head)].sw1  =  READ_DMA(&brm->mem[4]);
     1348                                brm->bm_event[INDEX(brm->head)].sw2  =  READ_DMA(&brm->mem[5]);
     1349                                brm->bm_event[INDEX(brm->head)].time =  READ_DMA(&brm->mem[6]);
     1350
     1351                                len = 32;
     1352                                while ( len-- ){
     1353                                        brm->bm_event[INDEX(brm->head)].data[len] =  READ_DMA(&brm->mem[0x100+len]);
     1354                                        len--;
     1355                                        brm->bm_event[INDEX(brm->head)].data[len] =  READ_DMA(&brm->mem[0x100+len]);
     1356                                        len--;
     1357                                        brm->bm_event[INDEX(brm->head)].data[len] =  READ_DMA(&brm->mem[0x100+len]);
     1358                                        len--;
     1359                                        brm->bm_event[INDEX(brm->head)].data[len] =  READ_DMA(&brm->mem[0x100+len]);
     1360                                }
     1361/*                              memcpy((void *)brm->bm_event[INDEX(brm->head)].data, &brm->mem[0x100], 32);*/
    12841362
    12851363#ifdef DEBUG
    1286             brm->log[brm->log_i++ % EVENT_QUEUE_SIZE] = READ_REG(&brm->regs->mbc);
    1287             brm->log[brm->log_i++ % EVENT_QUEUE_SIZE] = READ_DMA(&brm->mem[0]);
    1288             brm->log[brm->log_i++ % EVENT_QUEUE_SIZE] = READ_DMA(&brm->mem[1]);
    1289             brm->log[brm->log_i++ % EVENT_QUEUE_SIZE] = READ_DMA(&brm->mem[4]);
     1364                                brm->log[brm->log_i++ % EVENT_QUEUE_SIZE] = READ_REG(&brm->regs->mbc) & 0xffff;
     1365                                brm->log[brm->log_i++ % EVENT_QUEUE_SIZE] = READ_DMA(&brm->mem[0]);
     1366                                brm->log[brm->log_i++ % EVENT_QUEUE_SIZE] = READ_DMA(&brm->mem[1]);
     1367                                brm->log[brm->log_i++ % EVENT_QUEUE_SIZE] = READ_DMA(&brm->mem[4]);
    12901368#endif
    12911369
    1292             brm->head++;
    1293 
    1294         }
    1295         else {
    1296             /* Indicate overrun */
    1297             brm->rt_event[INDEX(brm->head)].miw |= 0x8000;
    1298         }
    1299 
    1300         /* Wake any blocking thread */
    1301         rtems_semaphore_release(brm->rx_sem);
    1302 
    1303     }
    1304 
    1305                 /* The reset of the interrupts
     1370                                brm->head++;
     1371
     1372                        }
     1373                        else {
     1374                                /* Indicate overrun */
     1375                                brm->bm_event[INDEX(brm->head)].miw |= 0x8000;
     1376                        }
     1377
     1378                        /* Wake any blocking thread */
     1379                        wake_rx_task = 1;
     1380                }
     1381
     1382                /* The reset of the interrupts
    13061383                 * cause a event to be signalled
    13071384                 * so that user can handle error.
     
    13181395                        FUNCDBG("BRM: BRM_ILLOP_IRQ\n\r");
    13191396                        brm->bc_list_fail = 1;
    1320                         rtems_semaphore_release(brm->tx_sem);
     1397                        wake_tx_task = 1;
    13211398                        event_status |= BRM_ILLOP_IRQ;
    1322                         SET_ERROR_DESCRIPTOR(descriptor);
     1399                        SET_ERROR_DESCRIPTOR(descriptor);                       
    13231400                        signal_event=1;
    13241401                }
     
    13301407                        signal_event=1;
    13311408                }
    1332             /* Clear Block Accessed Bit */
    1333     tmp = READ_REG(&brm->desc[descriptor].ctrl);
    1334     brm->desc[descriptor].ctrl = tmp & ~0x10;
    1335 
     1409                /* Clear Block Accessed Bit */
     1410                tmp = READ_DMA(&brm->desc[descriptor].ctrl);
     1411                brm->desc[descriptor].ctrl = tmp & ~0x10;
     1412#ifdef DEBUG
     1413                brm->log[brm->log_i++ % EVENT_QUEUE_SIZE] = (0xfe<<16);
     1414                brm->log[brm->log_i++ % EVENT_QUEUE_SIZE] = 0;
     1415                brm->log[brm->log_i++ % EVENT_QUEUE_SIZE] = tmp & ~0x10;
     1416                brm->log[brm->log_i++ % EVENT_QUEUE_SIZE] = tmp;
     1417#endif
    13361418        } /* While */
    13371419
     
    13641446        }
    13651447
     1448        /* Wake any blocked rx thread only on receive interrupts */
     1449        if ( wake_rx_task ) {
     1450                rtems_semaphore_release(brm->rx_sem);
     1451        }
     1452
     1453        /* Wake any blocked tx thread only on transmit interrupts */
     1454        if ( wake_tx_task ) {
     1455                rtems_semaphore_release(brm->tx_sem);
     1456        }       
     1457
    13661458        /* signal event once */
    13671459        if ( signal_event && (brm->event_id!=0) ){
     
    13701462
    13711463}
     1464
     1465void b1553brm_print_dev(struct drvmgr_dev *dev, int options)
     1466{
     1467        brm_priv *pDev = dev->priv;
     1468        struct amba_dev_info *devinfo;
     1469        struct brm_reg *regs = pDev->regs;
     1470
     1471        devinfo = (struct amba_dev_info *)pDev->dev->businfo;
     1472
     1473        /* Print */
     1474        printf("--- B1553BRM[%d] %s ---\n", pDev->minor, pDev->devName);
     1475        printf(" REGS:            0x%x\n", (unsigned int)pDev->regs);
     1476        printf(" IRQ:             %d\n", pDev->irqno);
     1477        switch (pDev->mode) {
     1478                case BRM_MODE_BC:
     1479                        printf(" MODE:            BC\n");
     1480                        printf(" DESCS:           0x%x\n", (unsigned int)&pDev->bcmem->descs[0]);
     1481                        printf(" DATA:            0x%x\n", (unsigned int)&pDev->bcmem->msg_data[0].data[0]);
     1482                        printf(" IRQLOG:          0x%x\n", (unsigned int)&pDev->bcmem->irq_logs[0]);
     1483                        break;
     1484                case BRM_MODE_BM:
     1485                        printf(" MODE:            BM\n");
     1486                        break;
     1487                case BRM_MODE_RT:
     1488                        printf(" MODE:            RT\n");
     1489                        printf(" RXSUBS:          0x%x\n", (unsigned int)&pDev->rtmem->rxsubs[0]);
     1490                        printf(" TXSUBS:          0x%x\n", (unsigned int)&pDev->rtmem->txsubs[0]);
     1491                        printf(" RXMODES:         0x%x\n", (unsigned int)&pDev->rtmem->rxmodes[0]);
     1492                        printf(" TXOMODES:        0x%x\n", (unsigned int)&pDev->rtmem->txmodes[0]);
     1493                        printf(" RXSUBS MSGS:     0x%x\n", (unsigned int)&pDev->rtmem->rxsuba_msgs[0]);
     1494                        printf(" TXSUBS MSGS:     0x%x\n", (unsigned int)&pDev->rtmem->txsuba_msgs[0]);
     1495                        printf(" RXMODES MSGS:    0x%x\n", (unsigned int)&pDev->rtmem->rxmode_msgs[0]);
     1496                        printf(" TXMODES MSGS:    0x%x\n", (unsigned int)&pDev->rtmem->txmode_msgs[0]);
     1497                        printf(" IRQLOG:          0x%x\n", (unsigned int)&pDev->rtmem->irq_logs[0]);
     1498                        break;
     1499        }
     1500        printf(" CTRL:            0x%x\n", regs->ctrl);
     1501        printf(" OPER:            0x%x\n", regs->oper);
     1502        printf(" CUR_CMD:         0x%x\n", regs->cur_cmd);
     1503        printf(" IMASK:           0x%x\n", regs->imask);
     1504        printf(" IPEND:           0x%x\n", regs->ipend);
     1505        printf(" IPOINT:          0x%x\n", regs->ipoint);
     1506        printf(" BIT_REG:         0x%x\n", regs->bit_reg);
     1507        printf(" TTAG:            0x%x\n", regs->ttag);
     1508        printf(" DPOINT:          0x%x\n", regs->dpoint);
     1509        printf(" SW:              0x%x\n", regs->sw);
     1510        printf(" INITCOUNT:       0x%x\n", regs->initcount);
     1511        printf(" MCPOINT:         0x%x\n", regs->mcpoint);
     1512        printf(" MDPOINT:         0x%x\n", regs->mdpoint);
     1513        printf(" MBC:             0x%x\n", regs->mbc);
     1514        printf(" MFILTA:          0x%x\n", regs->mfilta);
     1515        printf(" MFILTB:          0x%x\n", regs->mfiltb);
     1516        printf(" ENHANCED:        0x%x\n", regs->enhanced);
     1517        printf(" W_CTRL:          0x%x\n", regs->w_ctrl);
     1518        printf(" W_IRQCTRL:       0x%x\n", regs->w_irqctrl);
     1519        printf(" W_AHBADDR:       0x%x\n", regs->w_ahbaddr);
     1520}
     1521
     1522void b1553brm_print(int options)
     1523{
     1524        struct amba_drv_info *drv = &b1553brm_drv_info;
     1525        struct drvmgr_dev *dev;
     1526
     1527        dev = drv->general.dev;
     1528        while(dev) {
     1529                b1553brm_print_dev(dev, options);
     1530                dev = dev->next_in_drv;
     1531        }
     1532}
  • c/src/lib/libbsp/sparc/shared/can/grcan.c

    re67b2b8d r3681925  
    11/*
    22 *  GRCAN driver
    3  */
    4 
    5 /*
     3 *
    64 *  COPYRIGHT (c) 2007.
    7  *  Gaisler Research.
     5 *  Cobham Gaisler AB.
    86 *
    97 *  The license and distribution terms for this file may be
    108 *  found in the file LICENSE in this distribution or at
    119 *  http://www.rtems.org/license/LICENSE.
    12  *
    13  *
    14  *  2007-06-13, Daniel Hellstrom <daniel@gaisler.com>
    15  *    New driver in sparc shared directory. Parts taken
    16  *    from rasta grhcan driver.
    1710 */
    1811
     
    2720
    2821#include <grcan.h>
     22#include <drvmgr/drvmgr.h>
     23#include <drvmgr/ambapp_bus.h>
    2924#include <ambapp.h>
    30 #include <grlib.h>
    3125
    3226#define WRAP_AROUND_TX_MSGS 1
     
    3529#define BLOCK_SIZE (16*4)
    3630
     31/* grcan needs to have it buffers aligned to 1k boundaries */
     32#define BUFFER_ALIGNMENT_NEEDS 1024
     33
    3734/* Default Maximium buffer size for statically allocated buffers */
    3835#ifndef TX_BUF_SIZE
    39 #define TX_BUF_SIZE (BLOCK_SIZE*16)
     36 #define TX_BUF_SIZE (BLOCK_SIZE*16)
    4037#endif
    4138
    4239/* Make receiver buffers bigger than transmitt */
    4340#ifndef RX_BUF_SIZE
    44 #define RX_BUF_SIZE ((3*BLOCK_SIZE)*16)
     41 #define RX_BUF_SIZE ((3*BLOCK_SIZE)*16)
    4542#endif
    4643
     
    6966#endif
    7067
    71 #ifndef GRCAN_PREFIX
    72  #define GRCAN_PREFIX(name) grcan##name
     68#ifndef GRCAN_DEFAULT_BAUD
     69 /* default to 500kbits/s */
     70 #define GRCAN_DEFAULT_BAUD 500000
    7371#endif
    7472
    75 #ifndef MEMAREA_TO_HW
    76   #define MEMAREA_TO_HW(x) (x)
    77 #endif
    78 
    79 /* default name to /dev/grcan0 */
    80 #if !defined(GRCAN_DEVNAME) || !defined(GRCAN_DEVNAME_NO)
    81  #undef GRCAN_DEVNAME
    82  #undef GRCAN_DEVNAME_NO
    83  #define GRCAN_DEVNAME "/dev/grcan0"
    84  #define GRCAN_DEVNAME_NO(devstr,no) ((devstr)[10]='0'+(no))
    85 #endif
    86 
    87 #ifndef GRCAN_REG_INT
    88  #define GRCAN_REG_INT(handler,irqno,arg) set_vector(handler,irqno+0x10,1)
    89  #undef  GRCAN_DEFINE_INTHANDLER
    90  #define GRCAN_DEFINE_INTHANDLER
    91 #endif
    92 
    93 #ifndef GRCAN_DEFAULT_BAUD
    94   /* default to 500kbits/s */
    95   #define GRCAN_DEFAULT_BAUD 500000
    96 #endif
    97 
    9873#ifndef GRCAN_SAMPLING_POINT
    99 #define GRCAN_SAMPLING_POINT 80
     74 #define GRCAN_SAMPLING_POINT 80
    10075#endif
    10176
     
    11590/*********************************************************/
    11691
    117 /* grcan needs to have it buffers aligned to 1k boundaries */
    118 #define BUFFER_ALIGNMENT_NEEDS 1024
    119 
    120 #ifdef STATICALLY_ALLOCATED_TX_BUFFER
    121 static unsigned int tx_circbuf[GRCAN_MAX_CORES][TX_BUF_SIZE]
    122         __attribute__ ((aligned(BUFFER_ALIGNMENT_NEEDS)));
    123 #define STATIC_TX_BUF_SIZE TX_BUF_SIZE
    124 #define STATIC_TX_BUF_ADDR(core) (&tx_circbuf[(core)][0])
    125 #endif
    126 
    127 #ifdef STATICALLY_ALLOCATED_RX_BUFFER
    128 static unsigned int rx_circbuf[GRCAN_MAX_CORES][RX_BUF_SIZE]
    129         __attribute__ ((aligned(BUFFER_ALIGNMENT_NEEDS)));
    130 #define STATIC_RX_BUF_SIZE RX_BUF_SIZE
    131 #define STATIC_RX_BUF_ADDR(core) (&rx_circbuf[(core)][0])
    132 #endif
    133 
    134 /*
    135  * If USE_AT697_RAM is defined the RAM on the AT697 board will be used for DMA buffers (but rx message queue is always in AT697 ram).
    136  * USE_AT697_DMA specifies whether the messages will be fetched using DMA or PIO.
    137  *
    138  * RASTA_PCI_BASE is the base address of the GRPCI AHB slave
    139  *
    140  * GRCAN_BUF_SIZE must be set to the size (in bytes) of the GRCAN DMA buffers.
    141  *
    142  * RX_QUEUE_SIZE defines the number of messages that fits in the  RX message queue. On RX interrupts the messages in the DMA buffer
    143  * are copied into the message queue (using dma if the rx buf is not in the AT697 ram).
    144  */
    145 
    146 /*#define USE_AT697_RAM              1      */
    147 #define USE_AT697_DMA              1
    148 #define RASTA_PCI_BASE             0xe0000000
    149 #define GRCAN_BUF_SIZE            4096
    150 #define RX_QUEUE_SIZE              1024
    151 
    152 #define INDEX(x) ( x&(RX_QUEUE_SIZE-1) )
    153 
    154 /* pa(x)
    155  *
    156  * x: address in AT697 address space
    157  *
    158  * returns the address in the RASTA address space that can be used to access x with dma.
    159  *
    160 */
    161 #ifdef USE_AT697_RAM
    162 static inline unsigned int pa(unsigned int addr) {
    163     return ((addr & 0x0fffffff) | RASTA_PCI_BASE);
    164 }
    165 #else
    166 static inline unsigned int pa(unsigned int addr) {
    167     return ((addr & 0x0fffffff) | 0x40000000);
    168 }
    169 #endif
    170 
    17192struct grcan_msg {
    172     unsigned int head[2];
    173     unsigned char data[8];
     93        unsigned int head[2];
     94        unsigned char data[8];
    17495};
    17596
     
    182103
    183104struct grcan_priv {
    184   unsigned int baseaddr, ram_base;
    185   struct grcan_regs *regs;
    186   int irq;
    187   int minor;
    188   int open;
    189   int started;
    190   unsigned int channel;
    191   int flushing;
    192   unsigned int corefreq_hz;
    193 
    194   /* Circular DMA buffers */
    195   void *_rx;
    196   void *_tx;
    197   struct grcan_msg *rx;
    198   struct grcan_msg *tx;
    199   unsigned int rxbuf_size;    /* requested RX buf size in bytes */
    200   unsigned int txbuf_size;    /* requested TX buf size in bytes */
    201 
    202   int txblock, rxblock;
    203   int txcomplete, rxcomplete;
    204   int txerror, rxerror;
    205 
    206   struct grcan_filter sfilter;
    207   struct grcan_filter afilter;
    208   int config_changed; /* 0=no changes, 1=changes ==> a Core reset is needed */
    209   struct grcan_config config;
    210   struct grcan_stats stats;
    211 
    212   rtems_id rx_sem, tx_sem, txempty_sem, dev_sem;
     105        struct drvmgr_dev *dev; /* Driver manager device */
     106        char devName[32];       /* Device Name */
     107        unsigned int baseaddr, ram_base;
     108        struct grcan_regs *regs;
     109        int irq;
     110        int minor;
     111        int open;
     112        int started;
     113        unsigned int channel;
     114        int flushing;
     115        unsigned int corefreq_hz;
     116
     117        /* Circular DMA buffers */
     118        void *_rx, *_rx_hw;
     119        void *_tx, *_tx_hw;
     120        void *txbuf_adr;
     121        void *rxbuf_adr;
     122        struct grcan_msg *rx;
     123        struct grcan_msg *tx;
     124        unsigned int rxbuf_size;    /* requested RX buf size in bytes */
     125        unsigned int txbuf_size;    /* requested TX buf size in bytes */
     126
     127        int txblock, rxblock;
     128        int txcomplete, rxcomplete;
     129        int txerror, rxerror;
     130
     131        struct grcan_filter sfilter;
     132        struct grcan_filter afilter;
     133        int config_changed; /* 0=no changes, 1=changes ==> a Core reset is needed */
     134        struct grcan_config config;
     135        struct grcan_stats stats;
     136
     137        rtems_id rx_sem, tx_sem, txempty_sem, dev_sem;
    213138};
    214 
    215 static int grcan_core_cnt;
    216 struct grcan_priv *grcans;
    217 static struct ambapp_bus *amba_bus;
    218 struct grcan_device_info *grcan_cores;
    219 static int grcan_core_cnt;
    220139
    221140static rtems_device_driver grcan_initialize(rtems_device_major_number  major, rtems_device_minor_number  minor, void *arg);
     
    228147#define GRCAN_DRIVER_TABLE_ENTRY { grcan_initialize, grcan_open, grcan_close, grcan_read, grcan_write, grcan_ioctl }
    229148
     149static void __inline__ grcan_hw_reset(struct grcan_regs *regs);
     150
    230151static unsigned int grcan_hw_read_try(
    231152  struct grcan_priv *pDev,
     
    252173  struct grcan_filter *sfilter);
    253174
    254 #ifndef GRCAN_DONT_DECLARE_IRQ_HANDLER
    255 static rtems_isr grcan_interrupt_handler(rtems_vector_number v);
    256 #endif
    257 
    258 static void grcan_interrupt(struct grcan_priv *pDev);
     175static void grcan_interrupt(void *arg);
    259176
    260177#ifdef GRCAN_REG_BYPASS_CACHE
     
    269186static unsigned char __inline__ _grcan_read_nocache_byte(unsigned int address)
    270187{
    271   unsigned char tmp;
    272   __asm__ (" lduba [%1]1, %0 "
    273     : "=r"(tmp)
    274     : "r"(address)
    275   );
    276   return tmp;
     188        unsigned char tmp;
     189        __asm__ (" lduba [%1]1, %0 "
     190            : "=r"(tmp)
     191            : "r"(address)
     192        );
     193        return tmp;
    277194}
    278195#else
     
    284201static unsigned int __inline__ _grcan_read_nocache(unsigned int address)
    285202{
    286   unsigned int tmp;
    287   __asm__ (" lda [%1]1, %0 "
    288     : "=r"(tmp)
    289     : "r"(address)
    290   );
    291   return tmp;
     203        unsigned int tmp;
     204        __asm__ (" lda [%1]1, %0 "
     205                : "=r"(tmp)
     206                : "r"(address)
     207        );
     208        return tmp;
    292209}
    293210#endif
     
    295212
    296213static rtems_driver_address_table grcan_driver = GRCAN_DRIVER_TABLE_ENTRY;
     214static int grcan_driver_io_registered = 0;
     215static rtems_device_major_number grcan_driver_io_major = 0;
     216
     217/******************* Driver manager interface ***********************/
     218
     219/* Driver prototypes */
     220int grcan_register_io(rtems_device_major_number *m);
     221int grcan_device_init(struct grcan_priv *pDev);
     222
     223int grcan_init2(struct drvmgr_dev *dev);
     224int grcan_init3(struct drvmgr_dev *dev);
     225
     226struct drvmgr_drv_ops grcan_ops =
     227{
     228        .init = {NULL, grcan_init2, grcan_init3, NULL},
     229        .remove = NULL,
     230        .info = NULL
     231};
     232
     233struct amba_dev_id grcan_ids[] =
     234{
     235        {VENDOR_GAISLER, GAISLER_GRCAN},
     236        {VENDOR_GAISLER, GAISLER_GRHCAN},
     237        {0, 0}          /* Mark end of table */
     238};
     239
     240struct amba_drv_info grcan_drv_info =
     241{
     242        {
     243                DRVMGR_OBJ_DRV,                 /* Driver */
     244                NULL,                           /* Next driver */
     245                NULL,                           /* Device list */
     246                DRIVER_AMBAPP_GAISLER_GRCAN_ID, /* Driver ID */
     247                "GRCAN_DRV",                    /* Driver Name */
     248                DRVMGR_BUS_TYPE_AMBAPP,         /* Bus Type */
     249                &grcan_ops,
     250                NULL,                           /* Funcs */
     251                0,                              /* No devices yet */
     252                0,
     253        },
     254        &grcan_ids[0]
     255};
     256
     257void grcan_register_drv (void)
     258{
     259        DBG("Registering GRCAN driver\n");
     260        drvmgr_drv_register(&grcan_drv_info.general);
     261}
     262
     263int grcan_init2(struct drvmgr_dev *dev)
     264{
     265        struct grcan_priv *priv;
     266
     267        DBG("GRCAN[%d] on bus %s\n", dev->minor_drv, dev->parent->dev->name);
     268        priv = dev->priv = malloc(sizeof(struct grcan_priv));
     269        if ( !priv )
     270                return DRVMGR_NOMEM;
     271        memset(priv, 0, sizeof(*priv));
     272        priv->dev = dev;
     273
     274        /* This core will not find other cores, so we wait for init2() */
     275
     276        return DRVMGR_OK;
     277}
     278
     279int grcan_init3(struct drvmgr_dev *dev)
     280{
     281        struct grcan_priv *priv;
     282        char prefix[32];
     283        rtems_status_code status;
     284
     285        priv = dev->priv;
     286
     287        /* Do initialization */
     288
     289        if ( grcan_driver_io_registered == 0) {
     290                /* Register the I/O driver only once for all cores */
     291                if ( grcan_register_io(&grcan_driver_io_major) ) {
     292                        /* Failed to register I/O driver */
     293                        dev->priv = NULL;
     294                        return DRVMGR_FAIL;
     295                }
     296
     297                grcan_driver_io_registered = 1;
     298        }
     299
     300        /* I/O system registered and initialized
     301         * Now we take care of device initialization.
     302         */
     303
     304        if ( grcan_device_init(priv) ) {
     305                return DRVMGR_FAIL;
     306        }
     307
     308        /* Get Filesystem name prefix */
     309        prefix[0] = '\0';
     310        if ( drvmgr_get_dev_prefix(dev, prefix) ) {
     311                /* Failed to get prefix, make sure of a unique FS name
     312                 * by using the driver minor.
     313                 */
     314                sprintf(priv->devName, "/dev/grcan%d", dev->minor_drv);
     315        } else {
     316                /* Got special prefix, this means we have a bus prefix
     317                 * And we should use our "bus minor"
     318                 */
     319                sprintf(priv->devName, "/dev/%sgrcan%d", prefix, dev->minor_bus);
     320        }
     321
     322        /* Register Device */
     323        status = rtems_io_register_name(priv->devName, grcan_driver_io_major, dev->minor_drv);
     324        if (status != RTEMS_SUCCESSFUL) {
     325                return DRVMGR_FAIL;
     326        }
     327
     328        return DRVMGR_OK;
     329}
     330
     331/******************* Driver Implementation ***********************/
     332
     333int grcan_register_io(rtems_device_major_number *m)
     334{
     335        rtems_status_code r;
     336
     337        if ((r = rtems_io_register_driver(0, &grcan_driver, m)) == RTEMS_SUCCESSFUL) {
     338                DBG("GRCAN driver successfully registered, major: %d\n", *m);
     339        } else {
     340                switch(r) {
     341                case RTEMS_TOO_MANY:
     342                        printk("GRCAN rtems_io_register_driver failed: RTEMS_TOO_MANY\n");
     343                        return -1;
     344                case RTEMS_INVALID_NUMBER: 
     345                        printk("GRCAN rtems_io_register_driver failed: RTEMS_INVALID_NUMBER\n");
     346                        return -1;
     347                case RTEMS_RESOURCE_IN_USE:
     348                        printk("GRCAN rtems_io_register_driver failed: RTEMS_RESOURCE_IN_USE\n");
     349                        return -1;
     350                default:
     351                        printk("GRCAN rtems_io_register_driver failed\n");
     352                        return -1;
     353                }
     354        }
     355        return 0;
     356}
     357
     358int grcan_device_init(struct grcan_priv *pDev)
     359{
     360        struct amba_dev_info *ambadev;
     361        struct ambapp_core *pnpinfo;
     362
     363        /* Get device information from AMBA PnP information */
     364        ambadev = (struct amba_dev_info *)pDev->dev->businfo;
     365        if ( ambadev == NULL ) {
     366                return -1;
     367        }
     368        pnpinfo = &ambadev->info;
     369        pDev->irq = pnpinfo->irq;
     370        pDev->regs = (struct grcan_regs *)pnpinfo->apb_slv->start;
     371        pDev->minor = pDev->dev->minor_drv;
     372
     373        /* Get frequency in Hz */
     374        if ( drvmgr_freq_get(pDev->dev, DEV_APB_SLV, &pDev->corefreq_hz) ) {
     375                return -1;
     376        }
     377
     378        DBG("GRCAN frequency: %d Hz\n", pDev->corefreq_hz);
     379
     380        /* Reset Hardware before attaching IRQ handler */
     381        grcan_hw_reset(pDev->regs);
     382
     383        /* RX Semaphore created with count = 0 */
     384        if ( rtems_semaphore_create(rtems_build_name('G', 'C', 'R', '0' + pDev->minor),
     385                0,
     386                RTEMS_FIFO|RTEMS_SIMPLE_BINARY_SEMAPHORE|RTEMS_NO_INHERIT_PRIORITY|\
     387                RTEMS_LOCAL|RTEMS_NO_PRIORITY_CEILING,
     388                0,
     389                &pDev->rx_sem) != RTEMS_SUCCESSFUL ) {
     390                return RTEMS_INTERNAL_ERROR;
     391        }
     392
     393        /* TX Semaphore created with count = 0 */
     394        if ( rtems_semaphore_create(rtems_build_name('G', 'C', 'T', '0' + pDev->minor),
     395                0,
     396                RTEMS_FIFO|RTEMS_SIMPLE_BINARY_SEMAPHORE|RTEMS_NO_INHERIT_PRIORITY|\
     397                RTEMS_LOCAL|RTEMS_NO_PRIORITY_CEILING,
     398                0,
     399                &pDev->tx_sem) != RTEMS_SUCCESSFUL ) {
     400                return RTEMS_INTERNAL_ERROR;
     401        }
     402
     403        /* TX Empty Semaphore created with count = 0 */
     404        if ( rtems_semaphore_create(rtems_build_name('G', 'C', 'E', '0' + pDev->minor),
     405                0,
     406                RTEMS_FIFO|RTEMS_SIMPLE_BINARY_SEMAPHORE|RTEMS_NO_INHERIT_PRIORITY|\
     407                RTEMS_LOCAL|RTEMS_NO_PRIORITY_CEILING,
     408                0,
     409                &pDev->txempty_sem) != RTEMS_SUCCESSFUL ) {
     410                return RTEMS_INTERNAL_ERROR;
     411        }
     412
     413        /* Device Semaphore created with count = 1 */
     414        if ( rtems_semaphore_create(rtems_build_name('G', 'C', 'A', '0' + pDev->minor),
     415                1,
     416                RTEMS_FIFO|RTEMS_SIMPLE_BINARY_SEMAPHORE|RTEMS_NO_INHERIT_PRIORITY|\
     417                RTEMS_LOCAL|RTEMS_NO_PRIORITY_CEILING,
     418                0,
     419                &pDev->dev_sem) != RTEMS_SUCCESSFUL ) {
     420                return RTEMS_INTERNAL_ERROR;
     421        }
     422
     423        return 0;
     424}
    297425
    298426static void __inline__ grcan_hw_reset(struct grcan_regs *regs)
     
    326454
    327455  /* Setup receiver */
    328   pDev->regs->rx0addr = MEMAREA_TO_HW((unsigned int)pDev->rx);
     456  pDev->regs->rx0addr = (unsigned int)pDev->_rx_hw;
    329457  pDev->regs->rx0size = pDev->rxbuf_size;
    330458
    331459  /* Setup Transmitter */
    332   pDev->regs->tx0addr = MEMAREA_TO_HW((unsigned int)pDev->tx);
     460  pDev->regs->tx0addr = (unsigned int)pDev->_tx_hw;
    333461  pDev->regs->tx0size = pDev->txbuf_size;
    334462
     
    8761004  IRQ_GLOBAL_DISABLE(oldLevel);
    8771005
    878   /*pDev->regs->tx0ctrl = GRCAN_TXCTRL_ENABLE;*/
     1006  pDev->regs->tx0ctrl = GRCAN_TXCTRL_ENABLE;
    8791007
    8801008  size = READ_REG(&pDev->regs->tx0size);
     
    9731101static int grcan_alloc_buffers(struct grcan_priv *pDev, int rx, int tx)
    9741102{
     1103        unsigned int adr;
     1104        FUNCDBG();
     1105
     1106        if ( tx ) {
     1107                adr = (unsigned int)pDev->txbuf_adr;
     1108                if (adr & 0x1) {
     1109                        /* User defined "remote" address. Translate it into
     1110                         * a CPU accessible address
     1111                         */
     1112                        pDev->_tx_hw = (void *)(adr & ~0x1);
     1113                        drvmgr_translate_check(
     1114                                pDev->dev,
     1115                                DMAMEM_TO_CPU,
     1116                                (void *)pDev->_tx_hw,
     1117                                (void **)&pDev->_tx,
     1118                                pDev->txbuf_size);
     1119                        pDev->tx = (struct grcan_msg *)pDev->_tx;
     1120                } else {
     1121                        if (adr == 0) {
     1122                                pDev->_tx = malloc(pDev->txbuf_size +
     1123                                                   BUFFER_ALIGNMENT_NEEDS);
     1124                                if (!pDev->_tx)
     1125                                        return -1;
     1126                        } else {
     1127                                /* User defined "cou-local" address. Translate
     1128                                 * it into a CPU accessible address
     1129                                 */
     1130                                pDev->_tx = (void *)adr;
     1131                        }
     1132                        /* Align TX buffer */
     1133                        pDev->tx = (struct grcan_msg *)
     1134                                   (((unsigned int)pDev->_tx +
     1135                                   (BUFFER_ALIGNMENT_NEEDS-1)) &
     1136                                   ~(BUFFER_ALIGNMENT_NEEDS-1));
     1137
     1138                        /* Translate address into an hardware accessible
     1139                         * address
     1140                         */
     1141                        drvmgr_translate_check(
     1142                                pDev->dev,
     1143                                CPUMEM_TO_DMA,
     1144                                (void *)pDev->tx,
     1145                                (void **)&pDev->_tx_hw,
     1146                                pDev->txbuf_size);
     1147                }
     1148        }
     1149
     1150        if ( rx ) {
     1151                adr = (unsigned int)pDev->rxbuf_adr;
     1152                if (adr & 0x1) {
     1153                        /* User defined "remote" address. Translate it into
     1154                         * a CPU accessible address
     1155                         */
     1156                        pDev->_rx_hw = (void *)(adr & ~0x1);
     1157                        drvmgr_translate_check(
     1158                                pDev->dev,
     1159                                DMAMEM_TO_CPU,
     1160                                (void *)pDev->_rx_hw,
     1161                                (void **)&pDev->_rx,
     1162                                pDev->rxbuf_size);
     1163                        pDev->rx = (struct grcan_msg *)pDev->_rx;
     1164                } else {
     1165                        if (adr == 0) {
     1166                                pDev->_rx = malloc(pDev->rxbuf_size +
     1167                                                   BUFFER_ALIGNMENT_NEEDS);
     1168                                if (!pDev->_rx)
     1169                                        return -1;
     1170                        } else {
     1171                                /* User defined "cou-local" address. Translate
     1172                                 * it into a CPU accessible address
     1173                                 */
     1174                                pDev->_rx = (void *)adr;
     1175                        }
     1176                        /* Align RX buffer */
     1177                        pDev->rx = (struct grcan_msg *)
     1178                                   (((unsigned int)pDev->_rx +
     1179                                   (BUFFER_ALIGNMENT_NEEDS-1)) &
     1180                                   ~(BUFFER_ALIGNMENT_NEEDS-1));
     1181
     1182                        /* Translate address into an hardware accessible
     1183                         * address
     1184                         */
     1185                        drvmgr_translate_check(
     1186                                pDev->dev,
     1187                                CPUMEM_TO_DMA,
     1188                                (void *)pDev->rx,
     1189                                (void **)&pDev->_rx_hw,
     1190                                pDev->rxbuf_size);
     1191                }
     1192        }
     1193        return 0;
     1194}
     1195
     1196static void grcan_free_buffers(struct grcan_priv *pDev, int rx, int tx)
     1197{
    9751198  FUNCDBG();
    9761199
    977   if ( tx ) {
    978 #ifdef STATIC_TX_BUF_ADDR
    979     pDev->_tx = STATIC_TX_BUF_ADDR(pDev->minor);
    980     if ( pDev->txbuf_size > STATIC_TX_BUF_SIZE ){
    981       pDev->txbuf_size = STATIC_TX_BUF_SIZE;
    982       return -1;
    983     }
    984     /* Assume aligned buffer */
    985     pDev->tx = (struct grcan_msg *)pDev->_tx;
    986 #else
    987     pDev->_tx = malloc(pDev->txbuf_size + BUFFER_ALIGNMENT_NEEDS);
    988     if ( !pDev->_tx )
    989       return -1;
    990 
    991     /* Align TX buffer */
    992     pDev->tx = (struct grcan_msg *)
    993                (((unsigned int)pDev->_tx + (BUFFER_ALIGNMENT_NEEDS-1)) &
    994                ~(BUFFER_ALIGNMENT_NEEDS-1));
    995 #endif
    996   }
    997 
    998   if ( rx ) {
    999 #ifdef STATIC_RX_BUF_ADDR
    1000     pDev->_rx = STATIC_RX_BUF_ADDR(pDev->minor);
    1001     if ( pDev->rxbuf_size > STATIC_RX_BUF_SIZE ){
    1002       pDev->rxbuf_size = STATIC_RX_BUF_SIZE;
    1003       return -1;
    1004     }
    1005     /* Assume aligned buffer */
    1006     pDev->rx = (struct grcan_msg *)pDev->_rx;
    1007 #else
    1008     pDev->_rx = malloc(pDev->rxbuf_size + BUFFER_ALIGNMENT_NEEDS);
    1009     if ( !pDev->_rx )
    1010       return -1;
    1011 
    1012     /* Align TX buffer */
    1013     pDev->rx = (struct grcan_msg *)
    1014                (((unsigned int)pDev->_rx + (BUFFER_ALIGNMENT_NEEDS-1)) &
    1015                ~(BUFFER_ALIGNMENT_NEEDS-1));
    1016 #endif
    1017   }
    1018   return 0;
    1019 }
    1020 
    1021 static void grcan_free_buffers(struct grcan_priv *pDev, int rx, int tx)
    1022 {
    1023   FUNCDBG();
    1024 
    1025 #ifndef STATIC_TX_BUF_ADDR
    10261200  if ( tx && pDev->_tx ){
    10271201    free(pDev->_tx);
     
    10291203    pDev->tx = NULL;
    10301204  }
    1031 #endif
    1032 #ifndef STATIC_RX_BUF_ADDR
     1205
    10331206  if ( rx && pDev->_rx ){
    10341207    free(pDev->_rx);
     
    10361209    pDev->rx = NULL;
    10371210  }
    1038 #endif
    1039 }
    1040 
    1041 #if 0
    1042 static char *almalloc(int sz)
    1043 {
    1044   char *tmp;
    1045   tmp = calloc(1,2*sz);
    1046   tmp = (char *) (((int)tmp+sz) & ~(sz -1));
    1047   return(tmp);
    1048 }
    1049 #endif
     1211}
    10501212
    10511213static rtems_device_driver grcan_initialize(
     
    10551217  )
    10561218{
    1057   int minor;
    1058   struct grcan_priv *pDev;
    1059   struct ambapp_apb_info dev;
    1060   rtems_status_code status;
    1061   char fs_name[20];
    1062   unsigned int sys_freq_hz;
    1063   unsigned int deviceid = GAISLER_GRHCAN;
    1064 
    1065   printk("grcan_initialize()\n\r");
    1066 
    1067   FUNCDBG();
    1068 
    1069   /* find GRCAN cores */
    1070   if ( !grcan_cores ) {
    1071     grcan_core_cnt = ambapp_get_number_apbslv_devices(amba_bus, VENDOR_GAISLER,
    1072                                                       deviceid);
    1073     if ( grcan_core_cnt < 1 ){
    1074       deviceid = GAISLER_GRCAN;
    1075       grcan_core_cnt = ambapp_get_number_apbslv_devices(amba_bus, VENDOR_GAISLER,
    1076                                                         deviceid);
    1077       if ( grcan_core_cnt < 1 ) {
    1078         DBG("GRCAN: Using AMBA Plug&Play, found %d cores\n",grcan_core_cnt);
    1079         return RTEMS_UNSATISFIED;
    1080       }
    1081     }
    1082     DBG("GRCAN: Using AMBA Plug&Play, found %d cores\n",grcan_core_cnt);
    1083   }
    1084 
    1085 #ifdef GRCAN_MAX_CORENR
    1086   /* limit number of cores */
    1087   if ( grcan_core_cnt > GRCAN_MAX_CORENR )
    1088     grcan_core_cnt = GRCAN_MAX_CORENR;
    1089 #endif
    1090 
    1091   /* Allocate memory for cores */
    1092   grcans = malloc(grcan_core_cnt * sizeof(struct grcan_priv));
    1093   if ( !grcans )
    1094     return RTEMS_NO_MEMORY;
    1095   memset(grcans,0,grcan_core_cnt * sizeof(struct grcan_priv));
    1096 
    1097   /* make a local copy of device name */
    1098   strcpy(fs_name,GRCAN_DEVNAME);
    1099 
    1100   /* Detect System Frequency from initialized timer */
    1101 #ifndef SYS_FREQ_HZ
    1102 #if defined(LEON3)
    1103   /* LEON3: find timer address via AMBA Plug&Play info */
    1104   {
    1105     struct ambapp_apb_info gptimer;
    1106     struct gptimer_regs *tregs;
    1107 
    1108     if (ambapp_find_apbslv (&ambapp_plb, VENDOR_GAISLER, GAISLER_GPTIMER, &gptimer)
    1109         == 1) {
    1110       tregs = (struct gptimer_regs *) gptimer.start;
    1111       sys_freq_hz = (tregs->scaler_reload + 1) * 1000 * 1000;
    1112       DBG("GRCAN: detected %dHZ system frequency\n\r", sys_freq_hz);
    1113     } else {
    1114       sys_freq_hz = 40000000;   /* Default to 40MHz */
    1115       printk("GRCAN: Failed to detect system frequency\n\r");
    1116     }
    1117   }
    1118 #elif defined(LEON2)
    1119   /* LEON2: use hardcoded address to get to timer */
    1120   {
    1121     LEON_Register_Map *regs = (LEON_Register_Map *) 0x80000000;
    1122 
    1123     sys_freq_hz = (regs->Scaler_Reload + 1) * 1000 * 1000;
    1124   }
    1125 #else
    1126 #error CPU not supported by driver
    1127 #endif
    1128 #else
    1129   /* Use hardcoded frequency */
    1130   sys_freq_hz = SYS_FREQ_HZ;
    1131 #endif
    1132 
    1133   for(minor=0; minor<grcan_core_cnt; minor++){
    1134 
    1135     pDev = &grcans[minor];
    1136     pDev->minor = minor;
    1137     pDev->open = 0;
    1138     pDev->corefreq_hz = sys_freq_hz;
    1139     GRCAN_DEVNAME_NO(fs_name,minor);
    1140 
    1141     /* Find core address & IRQ */
    1142     if ( !grcan_cores ) {
    1143       ambapp_find_apbslv_next(amba_bus, VENDOR_GAISLER, deviceid, &dev, minor);
    1144       pDev->irq = dev.irq;
    1145       pDev->regs = (struct grcan_regs *)dev.start;
    1146     }else{
    1147       pDev->irq = grcan_cores[minor].irq;
    1148       pDev->regs = (struct grcan_regs *)grcan_cores[minor].base_address;
    1149     }
    1150 
    1151     printk("Registering GRCAN core at [0x%x] irq %d, minor %d as %s\n\r",pDev->regs,pDev->irq,minor,fs_name);
    1152 
    1153     status = rtems_io_register_name(fs_name, major, 0);
    1154     if (status != RTEMS_SUCCESSFUL)
    1155       rtems_fatal_error_occurred(status);
    1156 
    1157                 /* Reset Hardware before attaching IRQ handler */
    1158     grcan_hw_reset(pDev->regs);
    1159 
    1160     /* Register interrupt handler */
    1161     GRCAN_REG_INT(GRCAN_PREFIX(_interrupt_handler), pDev->irq+GRCAN_IRQ_IRQ, pDev);
    1162     /*
    1163     GRCAN_REG_INT(grcan_interrupt_handler, pDev->irq+GRCAN_IRQ_TXSYNC, pDev);
    1164     GRCAN_REG_INT(grcan_interrupt_handler, pDev->irq+GRCAN_IRQ_RXSYNC, pDev);
    1165     */
    1166 
    1167     /* RX Semaphore created with count = 0 */
    1168     if ( rtems_semaphore_create(rtems_build_name('G', 'C', 'R', '0'+minor),
    1169         0,
    1170         RTEMS_FIFO|RTEMS_SIMPLE_BINARY_SEMAPHORE|RTEMS_NO_INHERIT_PRIORITY|\
    1171         RTEMS_LOCAL|RTEMS_NO_PRIORITY_CEILING,
    1172         0,
    1173         &pDev->rx_sem) != RTEMS_SUCCESSFUL )
    1174       return RTEMS_INTERNAL_ERROR;
    1175 
    1176     /* TX Semaphore created with count = 0 */
    1177     if ( rtems_semaphore_create(rtems_build_name('G', 'C', 'T', '0'+minor),
    1178         0,
    1179         RTEMS_FIFO|RTEMS_SIMPLE_BINARY_SEMAPHORE|RTEMS_NO_INHERIT_PRIORITY|\
    1180         RTEMS_LOCAL|RTEMS_NO_PRIORITY_CEILING,
    1181         0,
    1182         &pDev->tx_sem) != RTEMS_SUCCESSFUL )
    1183       return RTEMS_INTERNAL_ERROR;
    1184 
    1185     /* TX Empty Semaphore created with count = 0 */
    1186     if ( rtems_semaphore_create(rtems_build_name('G', 'C', 'E', '0'+minor),
    1187         0,
    1188         RTEMS_FIFO|RTEMS_SIMPLE_BINARY_SEMAPHORE|RTEMS_NO_INHERIT_PRIORITY|\
    1189         RTEMS_LOCAL|RTEMS_NO_PRIORITY_CEILING,
    1190         0,
    1191         &pDev->txempty_sem) != RTEMS_SUCCESSFUL )
    1192       return RTEMS_INTERNAL_ERROR;
    1193 
    1194     /* Device Semaphore created with count = 1 */
    1195     if ( rtems_semaphore_create(rtems_build_name('G', 'C', 'A', '0'+minor),
    1196         1,
    1197         RTEMS_FIFO|RTEMS_SIMPLE_BINARY_SEMAPHORE|RTEMS_NO_INHERIT_PRIORITY|\
    1198         RTEMS_LOCAL|RTEMS_NO_PRIORITY_CEILING,
    1199         0,
    1200         &pDev->dev_sem) != RTEMS_SUCCESSFUL )
    1201       return RTEMS_INTERNAL_ERROR;
    1202   }
    1203 
    1204   return RTEMS_SUCCESSFUL;
    1205 }
    1206 
    1207 static rtems_device_driver grcan_open(rtems_device_major_number major, rtems_device_minor_number minor, void *arg) {
    1208   struct grcan_priv *pDev;
    1209   rtems_device_driver ret;
    1210 
    1211   FUNCDBG();
    1212 
    1213   if ( (minor < 0) || (minor>=grcan_core_cnt) ) {
    1214     DBG("Wrong minor %d\n", minor);
    1215     return RTEMS_INVALID_NUMBER;
    1216   }
    1217 
    1218   pDev = &grcans[minor];
    1219 
    1220   /* Wait until we get semaphore */
    1221   if ( rtems_semaphore_obtain(pDev->dev_sem, RTEMS_WAIT, RTEMS_NO_TIMEOUT) !=
    1222        RTEMS_SUCCESSFUL ){
    1223     return RTEMS_INTERNAL_ERROR;
    1224   }
    1225 
    1226   /* is device busy/taken? */
    1227   if  ( pDev->open ) {
    1228     ret=RTEMS_RESOURCE_IN_USE;
    1229     goto out;
    1230   }
    1231 
    1232   /* Mark device taken */
    1233   pDev->open = 1;
    1234 
    1235   pDev->txblock = pDev->rxblock = 1;
    1236   pDev->txcomplete = pDev->rxcomplete = 0;
    1237   pDev->started = 0;
    1238   pDev->config_changed = 1;
    1239   pDev->config.silent = 0;
    1240   pDev->config.abort = 0;
    1241   pDev->config.selection.selection = 0;
    1242   pDev->config.selection.enable0 = 0;
    1243   pDev->config.selection.enable1 = 1;
    1244   pDev->flushing = 0;
    1245   pDev->rx = pDev->_rx = NULL;
    1246   pDev->tx = pDev->_tx = NULL;
    1247   pDev->txbuf_size = TX_BUF_SIZE;
    1248   pDev->rxbuf_size = RX_BUF_SIZE;
    1249   printk("Defaulting to rxbufsize: %d, txbufsize: %d\n",RX_BUF_SIZE,TX_BUF_SIZE);
    1250 
    1251   /* Default to accept all messages */
    1252   pDev->afilter.mask = 0x00000000;
    1253   pDev->afilter.code = 0x00000000;
    1254 
    1255   /* Default to disable sync messages (only trigger when id is set to all ones) */
    1256   pDev->sfilter.mask = 0xffffffff;
    1257   pDev->sfilter.code = 0x00000000;
    1258 
    1259   /* Calculate default timing register values */
    1260   grcan_calc_timing(GRCAN_DEFAULT_BAUD,pDev->corefreq_hz,GRCAN_SAMPLING_POINT,&pDev->config.timing);
    1261 
    1262   if ( grcan_alloc_buffers(pDev,1,1) ) {
    1263     ret=RTEMS_NO_MEMORY;
    1264     goto out;
    1265   }
    1266 
    1267   /* Clear statistics */
    1268   memset(&pDev->stats,0,sizeof(struct grcan_stats));
    1269 
    1270   ret = RTEMS_SUCCESSFUL;
     1219        return RTEMS_SUCCESSFUL;
     1220}
     1221
     1222static rtems_device_driver grcan_open(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
     1223{
     1224        struct grcan_priv *pDev;
     1225        rtems_device_driver ret;
     1226        struct drvmgr_dev *dev;
     1227        union drvmgr_key_value *value;
     1228
     1229        FUNCDBG();
     1230
     1231        if ( drvmgr_get_dev(&grcan_drv_info.general, minor, &dev) ) {
     1232                DBG("Wrong minor %d\n", minor);
     1233                return RTEMS_INVALID_NAME;
     1234        }
     1235        pDev = (struct grcan_priv *)dev->priv;
     1236
     1237        /* Wait until we get semaphore */
     1238        if (rtems_semaphore_obtain(pDev->dev_sem, RTEMS_WAIT, RTEMS_NO_TIMEOUT)
     1239            != RTEMS_SUCCESSFUL) {
     1240                return RTEMS_INTERNAL_ERROR;
     1241        }
     1242
     1243        /* is device busy/taken? */
     1244        if ( pDev->open ) {
     1245                ret=RTEMS_RESOURCE_IN_USE;
     1246                goto out;
     1247        }
     1248
     1249        /* Mark device taken */
     1250        pDev->open = 1;
     1251
     1252        pDev->txblock = pDev->rxblock = 1;
     1253        pDev->txcomplete = pDev->rxcomplete = 0;
     1254        pDev->started = 0;
     1255        pDev->config_changed = 1;
     1256        pDev->config.silent = 0;
     1257        pDev->config.abort = 0;
     1258        pDev->config.selection.selection = 0;
     1259        pDev->config.selection.enable0 = 0;
     1260        pDev->config.selection.enable1 = 1;
     1261        pDev->flushing = 0;
     1262        pDev->rx = pDev->_rx = NULL;
     1263        pDev->tx = pDev->_tx = NULL;
     1264        pDev->txbuf_adr = 0;
     1265        pDev->rxbuf_adr = 0;
     1266        pDev->txbuf_size = TX_BUF_SIZE;
     1267        pDev->rxbuf_size = RX_BUF_SIZE;
     1268
     1269        /* Override default buffer sizes if available from bus resource */
     1270        value = drvmgr_dev_key_get(pDev->dev, "txBufSize", KEY_TYPE_INT);
     1271        if ( value )
     1272                pDev->txbuf_size = value->i;
     1273
     1274        value = drvmgr_dev_key_get(pDev->dev, "rxBufSize", KEY_TYPE_INT);
     1275        if ( value )
     1276                pDev->rxbuf_size = value->i;
     1277
     1278        value = drvmgr_dev_key_get(pDev->dev, "txBufAdr", KEY_TYPE_POINTER);
     1279        if ( value )
     1280                pDev->txbuf_adr = value->ptr;
     1281
     1282        value = drvmgr_dev_key_get(pDev->dev, "rxBufAdr", KEY_TYPE_POINTER);
     1283        if ( value )
     1284                pDev->rxbuf_adr = value->ptr;
     1285
     1286        DBG("Defaulting to rxbufsize: %d, txbufsize: %d\n",RX_BUF_SIZE,TX_BUF_SIZE);
     1287
     1288        /* Default to accept all messages */
     1289        pDev->afilter.mask = 0x00000000;
     1290        pDev->afilter.code = 0x00000000;
     1291
     1292        /* Default to disable sync messages (only trigger when id is set to all ones) */
     1293        pDev->sfilter.mask = 0xffffffff;
     1294        pDev->sfilter.code = 0x00000000;
     1295
     1296        /* Calculate default timing register values */
     1297        grcan_calc_timing(GRCAN_DEFAULT_BAUD,pDev->corefreq_hz,GRCAN_SAMPLING_POINT,&pDev->config.timing);
     1298
     1299        if ( grcan_alloc_buffers(pDev,1,1) ) {
     1300                ret=RTEMS_NO_MEMORY;
     1301                goto out;
     1302        }
     1303
     1304        /* Clear statistics */
     1305        memset(&pDev->stats,0,sizeof(struct grcan_stats));
     1306
     1307        ret = RTEMS_SUCCESSFUL;
    12711308out:
    1272   rtems_semaphore_release(pDev->dev_sem);
    1273   return ret;
     1309        rtems_semaphore_release(pDev->dev_sem);
     1310        return ret;
    12741311}
    12751312
    12761313static rtems_device_driver grcan_close(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
    12771314{
    1278   struct grcan_priv *pDev = &grcans[minor];
    1279 
    1280   FUNCDBG();
    1281 
    1282   if ( pDev->started )
    1283     grcan_stop(pDev);
    1284 
    1285   grcan_hw_reset(pDev->regs);
    1286 
    1287   grcan_free_buffers(pDev,1,1);
    1288 
    1289   /* Mark Device as closed */
    1290   pDev->open = 0;
    1291 
    1292   return RTEMS_SUCCESSFUL;
     1315        struct grcan_priv *pDev;
     1316        struct drvmgr_dev *dev;
     1317
     1318        FUNCDBG();
     1319
     1320        if ( drvmgr_get_dev(&grcan_drv_info.general, minor, &dev) ) {
     1321                return RTEMS_INVALID_NAME;
     1322        }
     1323        pDev = (struct grcan_priv *)dev->priv;
     1324
     1325        if ( pDev->started )
     1326                grcan_stop(pDev);
     1327
     1328        grcan_hw_reset(pDev->regs);
     1329
     1330        grcan_free_buffers(pDev,1,1);
     1331
     1332        /* Mark Device as closed */
     1333        pDev->open = 0;
     1334
     1335        return RTEMS_SUCCESSFUL;
    12931336}
    12941337
    12951338static rtems_device_driver grcan_read(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
    12961339{
    1297   struct grcan_priv *pDev = &grcans[minor];
    1298   rtems_libio_rw_args_t *rw_args;
    1299   CANMsg *dest;
    1300   unsigned int count, left;
    1301   int req_cnt;
    1302 
    1303   rw_args = (rtems_libio_rw_args_t *) arg;
    1304   dest = (CANMsg *) rw_args->buffer;
    1305   req_cnt = rw_args->count / sizeof(CANMsg);
    1306 
    1307   FUNCDBG();
    1308 
    1309   if ( (!dest) || (req_cnt<1) )
    1310     return RTEMS_INVALID_NAME;
    1311 
    1312   if ( !pDev->started )
    1313     return RTEMS_RESOURCE_IN_USE;
    1314 
    1315 /*  FUNCDBG("grcan_read [%i,%i]: buf: 0x%x len: %i\n",major, minor, (unsigned int)rw_args->buffer,rw_args->count);*/
    1316 
    1317   count = grcan_hw_read_try(pDev,pDev->regs,dest,req_cnt);
    1318   if ( !( pDev->rxblock && pDev->rxcomplete && (count!=req_cnt) ) ){
    1319     if ( count > 0 ) {
    1320       /* Successfully received messages (at least one) */
    1321       rw_args->bytes_moved = count * sizeof(CANMsg);
    1322       return RTEMS_SUCCESSFUL;
    1323     }
    1324 
    1325     /* nothing read, shall we block? */
    1326     if ( !pDev->rxblock ) {
    1327       /* non-blocking mode */
    1328       rw_args->bytes_moved = 0;
    1329       return RTEMS_TIMEOUT;
    1330     }
    1331   }
    1332 
    1333   while(count == 0 || (pDev->rxcomplete && (count!=req_cnt)) ){
    1334 
    1335     if ( !pDev->rxcomplete ){
    1336       left = 1; /* return as soon as there is one message available */
    1337     }else{
    1338       left = req_cnt - count;     /* return as soon as all data are available */
    1339 
    1340       /* never wait for more than the half the maximum size of the receive buffer
    1341        * Why? We need some time to copy buffer before to catch up with hw, otherwise
    1342        * we would have to copy everything when the data has been received.
    1343        */
    1344       if ( left > ((pDev->rxbuf_size/GRCAN_MSG_SIZE)/2) ){
    1345         left = (pDev->rxbuf_size/GRCAN_MSG_SIZE)/2;
    1346       }
    1347     }
    1348 
    1349     if ( grcan_wait_rxdata(pDev,left) ) {
    1350       /* The wait has been aborted, probably due to
    1351        * the device driver has been closed by another
    1352        * thread.
    1353        */
    1354       rw_args->bytes_moved = count * sizeof(CANMsg);
    1355       return RTEMS_UNSATISFIED;
    1356     }
    1357 
    1358     /* Try read bytes from circular buffer */
    1359     count += grcan_hw_read_try(
    1360       pDev,
    1361       pDev->regs,
    1362       dest+count,
    1363       req_cnt-count);
    1364   }
    1365   /* no need to unmask IRQ as IRQ Handler do that for us. */
    1366   rw_args->bytes_moved = count * sizeof(CANMsg);
    1367   return RTEMS_SUCCESSFUL;
     1340        struct grcan_priv *pDev;
     1341        struct drvmgr_dev *dev;
     1342        rtems_libio_rw_args_t *rw_args; 
     1343        CANMsg *dest;
     1344        unsigned int count, left;
     1345        int req_cnt;
     1346
     1347        FUNCDBG();
     1348
     1349        if ( drvmgr_get_dev(&grcan_drv_info.general, minor, &dev) ) {
     1350                return RTEMS_INVALID_NAME;
     1351        }
     1352        pDev = (struct grcan_priv *)dev->priv;
     1353
     1354        rw_args = (rtems_libio_rw_args_t *) arg;
     1355        dest = (CANMsg *) rw_args->buffer;
     1356        req_cnt = rw_args->count / sizeof(CANMsg);
     1357
     1358        if ( (!dest) || (req_cnt<1) )
     1359                return RTEMS_INVALID_NAME;
     1360
     1361        if ( !pDev->started )
     1362                return RTEMS_RESOURCE_IN_USE;
     1363
     1364        /*FUNCDBG("grcan_read [%i,%i]: buf: 0x%x len: %i\n",major, minor, (unsigned int)rw_args->buffer,rw_args->count);*/
     1365
     1366        count = grcan_hw_read_try(pDev,pDev->regs,dest,req_cnt);
     1367        if ( !( pDev->rxblock && pDev->rxcomplete && (count!=req_cnt) ) ){
     1368                if ( count > 0 ) {
     1369                        /* Successfully received messages (at least one) */
     1370                        rw_args->bytes_moved = count * sizeof(CANMsg);
     1371                        return RTEMS_SUCCESSFUL;
     1372                }
     1373
     1374                /* nothing read, shall we block? */
     1375                if ( !pDev->rxblock ) {
     1376                        /* non-blocking mode */
     1377                        rw_args->bytes_moved = 0;
     1378                        return RTEMS_TIMEOUT;
     1379                }
     1380        }
     1381
     1382        while(count == 0 || (pDev->rxcomplete && (count!=req_cnt)) ){
     1383
     1384        if ( !pDev->rxcomplete ){
     1385                left = 1; /* return as soon as there is one message available */
     1386        }else{
     1387                left = req_cnt - count;     /* return as soon as all data are available */
     1388
     1389                /* never wait for more than the half the maximum size of the receive buffer
     1390                 * Why? We need some time to copy buffer before to catch up with hw,
     1391                 * otherwise we would have to copy everything when the data has been
     1392                 * received.
     1393                 */
     1394                if ( left > ((pDev->rxbuf_size/GRCAN_MSG_SIZE)/2) ){
     1395                        left = (pDev->rxbuf_size/GRCAN_MSG_SIZE)/2;
     1396                }
     1397        }
     1398
     1399        if ( grcan_wait_rxdata(pDev,left) ) {
     1400                /* The wait has been aborted, probably due to
     1401                 * the device driver has been closed by another
     1402                 * thread.
     1403                 */
     1404                rw_args->bytes_moved = count * sizeof(CANMsg);
     1405                return RTEMS_UNSATISFIED;
     1406        }
     1407
     1408        /* Try read bytes from circular buffer */
     1409        count += grcan_hw_read_try(
     1410                        pDev,
     1411                        pDev->regs,
     1412                        dest+count,
     1413                        req_cnt-count);
     1414        }
     1415        /* no need to unmask IRQ as IRQ Handler do that for us. */
     1416        rw_args->bytes_moved = count * sizeof(CANMsg);
     1417        return RTEMS_SUCCESSFUL;
    13681418}
    13691419
    13701420static rtems_device_driver grcan_write(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
    13711421{
    1372   struct grcan_priv *pDev = &grcans[minor];
    1373   rtems_libio_rw_args_t *rw_args;
    1374   CANMsg *source;
    1375   unsigned int count, left;
    1376   int req_cnt;
    1377 
    1378   DBGC(DBG_TX,"\n");
    1379   /*FUNCDBG();*/
    1380 
    1381   if ( !pDev->started || pDev->config.silent || pDev->flushing )
    1382     return RTEMS_RESOURCE_IN_USE;
    1383 
    1384   rw_args = (rtems_libio_rw_args_t *) arg;
    1385   req_cnt = rw_args->count / sizeof(CANMsg);
    1386   source = (CANMsg *) rw_args->buffer;
    1387 
    1388   /* check proper length and buffer pointer */
    1389   if (( req_cnt < 1) || (source == NULL) ){
    1390     return RTEMS_INVALID_NAME;
    1391   }
    1392 
    1393   count = grcan_hw_write_try(pDev,pDev->regs,source,req_cnt);
    1394   if ( !(pDev->txblock && pDev->txcomplete && (count!=req_cnt)) ) {
    1395     if ( count > 0 ) {
    1396       /* Successfully transmitted chars (at least one char) */
    1397       rw_args->bytes_moved = count * sizeof(CANMsg);
    1398       return RTEMS_SUCCESSFUL;
    1399     }
    1400 
    1401     /* nothing written, shall we block? */
    1402     if ( !pDev->txblock ) {
    1403       /* non-blocking mode */
    1404       rw_args->bytes_moved = 0;
    1405       return RTEMS_TIMEOUT;
    1406     }
    1407   }
    1408 
    1409   /* if in txcomplete mode we need to transmit all chars */
    1410   while((count == 0) || (pDev->txcomplete && (count!=req_cnt)) ){
    1411     /*** block until room to fit all or as much of transmit buffer as possible IRQ comes
    1412      * Set up a valid IRQ point so that an IRQ is received
    1413      * when we can put a chunk of data into transmit fifo
    1414      */
    1415     if ( !pDev->txcomplete ){
    1416       left = 1; /* wait for anything to fit buffer */
    1417     }else{
    1418       left = req_cnt - count; /* wait for all data to fit in buffer */
    1419 
    1420       /* never wait for more than the half the maximum size of the transmitt buffer
    1421        * Why? We need some time to fill buffer before hw catches up.
    1422        */
    1423       if ( left > ((pDev->txbuf_size/GRCAN_MSG_SIZE)/2) ){
    1424         left = (pDev->txbuf_size/GRCAN_MSG_SIZE)/2;
    1425       }
    1426     }
    1427 
    1428     /* Wait until more room in transmit buffer */
    1429     if ( grcan_wait_txspace(pDev,left) ){
    1430       /* The wait has been aborted, probably due to
    1431        * the device driver has been closed by another
    1432        * thread. To avoid deadlock we return directly
    1433        * with error status.
    1434        */
    1435       rw_args->bytes_moved = count * sizeof(CANMsg);
    1436       return RTEMS_UNSATISFIED;
    1437     }
    1438 
    1439     if ( pDev->txerror ){
    1440       /* Return number of bytes sent, compare write pointers */
    1441       pDev->txerror = 0;
    1442 #if 0
     1422        struct grcan_priv *pDev;
     1423        struct drvmgr_dev *dev;
     1424        rtems_libio_rw_args_t *rw_args;
     1425        CANMsg *source;
     1426        unsigned int count, left;
     1427        int req_cnt;
     1428
     1429        DBGC(DBG_TX,"\n");
     1430
     1431        if ( drvmgr_get_dev(&grcan_drv_info.general, minor, &dev) ) {
     1432                return RTEMS_INVALID_NAME;
     1433        }
     1434        pDev = (struct grcan_priv *)dev->priv;
     1435
     1436        if ( !pDev->started || pDev->config.silent || pDev->flushing )
     1437                return RTEMS_RESOURCE_IN_USE;
     1438
     1439        rw_args = (rtems_libio_rw_args_t *) arg;
     1440        req_cnt = rw_args->count / sizeof(CANMsg);
     1441        source = (CANMsg *) rw_args->buffer;
     1442
     1443        /* check proper length and buffer pointer */
     1444        if (( req_cnt < 1) || (source == NULL) ){
     1445                return RTEMS_INVALID_NAME;
     1446        }
     1447
     1448        count = grcan_hw_write_try(pDev,pDev->regs,source,req_cnt);
     1449        if ( !(pDev->txblock && pDev->txcomplete && (count!=req_cnt)) ) {
     1450                if ( count > 0 ) {
     1451                        /* Successfully transmitted chars (at least one char) */
     1452                        rw_args->bytes_moved = count * sizeof(CANMsg);
     1453                        return RTEMS_SUCCESSFUL;
     1454                }
     1455
     1456                /* nothing written, shall we block? */
     1457                if ( !pDev->txblock ) {
     1458                        /* non-blocking mode */
     1459                        rw_args->bytes_moved = 0;
     1460                        return RTEMS_TIMEOUT;
     1461                }
     1462        }
     1463
     1464        /* if in txcomplete mode we need to transmit all chars */
     1465        while((count == 0) || (pDev->txcomplete && (count!=req_cnt)) ){
     1466                /*** block until room to fit all or as much of transmit buffer as possible
     1467                 * IRQ comes. Set up a valid IRQ point so that an IRQ is received
     1468                 * when we can put a chunk of data into transmit fifo
     1469                 */
     1470                if ( !pDev->txcomplete ){
     1471                        left = 1; /* wait for anything to fit buffer */
     1472                }else{
     1473                        left = req_cnt - count; /* wait for all data to fit in buffer */
     1474
     1475                        /* never wait for more than the half the maximum size of the transmit
     1476                         * buffer
     1477                         * Why? We need some time to fill buffer before hw catches up.
     1478                         */
     1479                        if ( left > ((pDev->txbuf_size/GRCAN_MSG_SIZE)/2) ){
     1480                                left = (pDev->txbuf_size/GRCAN_MSG_SIZE)/2;
     1481                        }
     1482                }
     1483
     1484                /* Wait until more room in transmit buffer */
     1485                if ( grcan_wait_txspace(pDev,left) ){
     1486                        /* The wait has been aborted, probably due to
     1487                         * the device driver has been closed by another
     1488                         * thread. To avoid deadlock we return directly
     1489                         * with error status.
     1490                         */
     1491                        rw_args->bytes_moved = count * sizeof(CANMsg);
     1492                        return RTEMS_UNSATISFIED;
     1493                }
     1494
     1495                if ( pDev->txerror ){
     1496                        /* Return number of bytes sent, compare write pointers */
     1497                        pDev->txerror = 0;
     1498#if 0
    14431499#error HANDLE AMBA error
    14441500#endif
    1445     }
    1446 
    1447     /* Try read bytes from circular buffer */
    1448     count += grcan_hw_write_try(
    1449       pDev,
    1450       pDev->regs,
    1451       source+count,
    1452       req_cnt-count);
    1453   }
    1454   /* no need to unmask IRQ as IRQ Handler do that for us. */
    1455 
    1456   rw_args->bytes_moved = count * sizeof(CANMsg);
    1457   return RTEMS_SUCCESSFUL;
     1501                }
     1502
     1503                /* Try read bytes from circular buffer */
     1504                count += grcan_hw_write_try(
     1505                        pDev,
     1506                        pDev->regs,
     1507                        source+count,
     1508                        req_cnt-count);
     1509        }
     1510        /* no need to unmask IRQ as IRQ Handler do that for us. */
     1511
     1512        rw_args->bytes_moved = count * sizeof(CANMsg);
     1513        return RTEMS_SUCCESSFUL;
    14581514}
    14591515
    14601516static rtems_device_driver grcan_ioctl(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
    14611517{
    1462   struct grcan_priv *pDev = &grcans[minor];
    1463   rtems_libio_ioctl_args_t *ioarg = (rtems_libio_ioctl_args_t *)arg;
    1464   unsigned int *data = ioarg->buffer;
    1465   struct grcan_timing timing;
    1466   unsigned int speed;
    1467   struct grcan_selection *selection;
    1468   int tmp,ret;
    1469   rtems_device_driver status;
    1470   struct grcan_stats *stats;
    1471   struct grcan_filter *filter;
    1472   IRQ_GLOBAL_PREPARE(oldLevel);
    1473 
    1474   FUNCDBG();
    1475 
    1476   if (!ioarg)
    1477     return RTEMS_INVALID_NAME;
    1478 
    1479   ioarg->ioctl_return = 0;
    1480   switch(ioarg->command) {
    1481     case GRCAN_IOC_START:
    1482       if ( pDev->started )
    1483         return RTEMS_RESOURCE_IN_USE; /* EBUSY */
    1484 
    1485       if ( (status=grcan_start(pDev)) != RTEMS_SUCCESSFUL ){
    1486         return status;
    1487       }
    1488       /* Read and write are now open... */
    1489       pDev->started = 1;
    1490       break;
    1491 
    1492     case GRCAN_IOC_STOP:
    1493       if ( !pDev->started )
    1494         return RTEMS_RESOURCE_IN_USE;
    1495 
    1496       grcan_stop(pDev);
    1497       pDev->started = 0;
    1498       break;
    1499 
    1500     case GRCAN_IOC_ISSTARTED:
    1501       if ( !pDev->started )
    1502         return RTEMS_RESOURCE_IN_USE;
    1503       break;
    1504 
    1505     case GRCAN_IOC_FLUSH:
    1506       if ( !pDev->started || pDev->flushing || pDev->config.silent )
    1507         return RTEMS_RESOURCE_IN_USE;
    1508 
    1509       pDev->flushing = 1;
    1510       tmp = grcan_tx_flush(pDev);
    1511       pDev->flushing = 0;
    1512       if ( tmp ) {
    1513         /* The wait has been aborted, probably due to
    1514          * the device driver has been closed by another
    1515          * thread.
    1516          */
    1517          return RTEMS_UNSATISFIED;
    1518       }
    1519       break;
     1518        struct grcan_priv *pDev;
     1519        struct drvmgr_dev *dev;
     1520        rtems_libio_ioctl_args_t *ioarg = (rtems_libio_ioctl_args_t *)arg;
     1521        unsigned int *data = ioarg->buffer;
     1522        struct grcan_timing timing;
     1523        unsigned int speed;
     1524        struct grcan_selection *selection;
     1525        int tmp,ret;
     1526        rtems_device_driver status;
     1527        struct grcan_stats *stats;
     1528        struct grcan_filter *filter;
     1529        IRQ_GLOBAL_PREPARE(oldLevel);
     1530
     1531        FUNCDBG();
     1532
     1533        if ( drvmgr_get_dev(&grcan_drv_info.general, minor, &dev) ) {
     1534                return RTEMS_INVALID_NAME;
     1535        }
     1536        pDev = (struct grcan_priv *)dev->priv;
     1537
     1538        if (!ioarg)
     1539                return RTEMS_INVALID_NAME;
     1540
     1541        ioarg->ioctl_return = 0;
     1542        switch(ioarg->command) {
     1543                case GRCAN_IOC_START:
     1544                if ( pDev->started )
     1545                        return RTEMS_RESOURCE_IN_USE; /* EBUSY */
     1546
     1547                if ( (status=grcan_start(pDev)) != RTEMS_SUCCESSFUL ){
     1548                        return status;
     1549                }
     1550                /* Read and write are now open... */
     1551                pDev->started = 1;
     1552
     1553                /* Register interrupt routine and enable IRQ at IRQ ctrl */
     1554                drvmgr_interrupt_register(dev, 0, "grcan", grcan_interrupt, pDev);
     1555
     1556                break;
     1557
     1558                case GRCAN_IOC_STOP:
     1559                if ( !pDev->started )
     1560                        return RTEMS_RESOURCE_IN_USE;
     1561
     1562                /* Disable interrupts */
     1563                drvmgr_interrupt_unregister(dev, 0, grcan_interrupt, pDev);
     1564
     1565                grcan_stop(pDev);
     1566                pDev->started = 0;
     1567                break;
     1568
     1569                case GRCAN_IOC_ISSTARTED:
     1570                if ( !pDev->started )
     1571                        return RTEMS_RESOURCE_IN_USE;
     1572                break;
     1573
     1574                case GRCAN_IOC_FLUSH:
     1575                if ( !pDev->started || pDev->flushing || pDev->config.silent )
     1576                        return RTEMS_RESOURCE_IN_USE;
     1577
     1578                pDev->flushing = 1;
     1579                tmp = grcan_tx_flush(pDev);
     1580                pDev->flushing = 0;
     1581                if ( tmp ) {
     1582                        /* The wait has been aborted, probably due to
     1583                         * the device driver has been closed by another
     1584                         * thread.
     1585                         */
     1586                        return RTEMS_UNSATISFIED;
     1587                }
     1588                break;
    15201589
    15211590#if 0
    1522     /* Set physical link */
     1591                /* Set physical link */
    15231592                case GRCAN_IOC_SET_LINK:
    15241593#ifdef REDUNDANT_CHANNELS
    1525                         if ( pDev->started )
    1526                                 return RTEMS_RESOURCE_IN_USE; /* EBUSY */
    1527 
    1528                         /* switch HW channel */
    1529                         pDev->channel = (unsigned int)ioargs->buffer;
     1594                if ( pDev->started )
     1595                        return RTEMS_RESOURCE_IN_USE; /* EBUSY */
     1596
     1597                /* switch HW channel */
     1598                pDev->channel = (unsigned int)ioargs->buffer;
    15301599#else
    1531                         return RTEMS_NOT_IMPLEMENTED;
     1600                return RTEMS_NOT_IMPLEMENTED;
    15321601#endif
    1533                         break;
     1602                break;
    15341603#endif
    15351604
    1536     case GRCAN_IOC_SET_SILENT:
    1537       if ( pDev->started )
    1538         return RTEMS_RESOURCE_IN_USE;
    1539       pDev->config.silent = (int)ioarg->buffer;
    1540       pDev->config_changed = 1;
    1541       break;
    1542 
    1543     case GRCAN_IOC_SET_ABORT:
    1544       if ( pDev->started )
    1545         return RTEMS_RESOURCE_IN_USE;
    1546       pDev->config.abort = (int)ioarg->buffer;
    1547       /* This Configuration parameter doesn't need HurriCANe reset
    1548       * ==> no pDev->config_changed = 1;
    1549       */
    1550       break;
    1551 
    1552     case GRCAN_IOC_SET_SELECTION:
    1553       if ( pDev->started )
    1554         return RTEMS_RESOURCE_IN_USE;
    1555 
    1556       selection = (struct grcan_selection *)ioarg->buffer;
    1557       if ( !selection )
    1558         return RTEMS_INVALID_NAME;
    1559 
    1560       pDev->config.selection = *selection;
    1561       pDev->config_changed = 1;
    1562       break;
    1563 
    1564     case GRCAN_IOC_SET_RXBLOCK:
    1565       pDev->rxblock = (int)ioarg->buffer;
    1566       break;
    1567 
    1568     case GRCAN_IOC_SET_TXBLOCK:
    1569       pDev->txblock = (int)ioarg->buffer;
    1570       break;
    1571 
    1572     case GRCAN_IOC_SET_TXCOMPLETE:
    1573       pDev->txcomplete = (int)ioarg->buffer;
    1574       break;
    1575 
    1576     case GRCAN_IOC_SET_RXCOMPLETE:
    1577       pDev->rxcomplete = (int)ioarg->buffer;
    1578       break;
    1579 
    1580     case GRCAN_IOC_GET_STATS:
    1581       stats = (struct grcan_stats *)ioarg->buffer;
    1582       if ( !stats )
    1583         return RTEMS_INVALID_NAME;
    1584       *stats = pDev->stats;
    1585       break;
    1586 
    1587     case GRCAN_IOC_CLR_STATS:
    1588       IRQ_GLOBAL_DISABLE(oldLevel);
    1589       memset(&pDev->stats,0,sizeof(struct grcan_stats));
    1590       IRQ_GLOBAL_ENABLE(oldLevel);
    1591       break;
     1605                case GRCAN_IOC_SET_SILENT:
     1606                if ( pDev->started )
     1607                        return RTEMS_RESOURCE_IN_USE;
     1608                pDev->config.silent = (int)ioarg->buffer;
     1609                pDev->config_changed = 1;
     1610                break;
     1611
     1612                case GRCAN_IOC_SET_ABORT:
     1613                if ( pDev->started )
     1614                        return RTEMS_RESOURCE_IN_USE;
     1615                pDev->config.abort = (int)ioarg->buffer;
     1616                /* This Configuration parameter doesn't need HurriCANe reset
     1617                * ==> no pDev->config_changed = 1;
     1618                */
     1619                break;
     1620
     1621                case GRCAN_IOC_SET_SELECTION:
     1622                if ( pDev->started )
     1623                        return RTEMS_RESOURCE_IN_USE;
     1624
     1625                selection = (struct grcan_selection *)ioarg->buffer;
     1626                if ( !selection )
     1627                        return RTEMS_INVALID_NAME;
     1628
     1629                pDev->config.selection = *selection;
     1630                pDev->config_changed = 1;
     1631                break;
     1632
     1633                case GRCAN_IOC_SET_RXBLOCK:
     1634                pDev->rxblock = (int)ioarg->buffer;
     1635                break;
     1636
     1637                case GRCAN_IOC_SET_TXBLOCK:
     1638                pDev->txblock = (int)ioarg->buffer;
     1639                break;
     1640
     1641                case GRCAN_IOC_SET_TXCOMPLETE:
     1642                pDev->txcomplete = (int)ioarg->buffer;
     1643                break;
     1644
     1645                case GRCAN_IOC_SET_RXCOMPLETE:
     1646                pDev->rxcomplete = (int)ioarg->buffer;
     1647                break;
     1648
     1649                case GRCAN_IOC_GET_STATS:
     1650                stats = (struct grcan_stats *)ioarg->buffer;
     1651                if ( !stats )
     1652                        return RTEMS_INVALID_NAME;
     1653                *stats = pDev->stats;
     1654                break;
     1655
     1656                case GRCAN_IOC_CLR_STATS:
     1657                IRQ_GLOBAL_DISABLE(oldLevel);
     1658                memset(&pDev->stats,0,sizeof(struct grcan_stats));
     1659                IRQ_GLOBAL_ENABLE(oldLevel);
     1660                break;
    15921661
    15931662                case GRCAN_IOC_SET_SPEED:
    1594 
    1595                         /* cannot change speed during run mode */
    1596                         if ( pDev->started )
    1597                                 return RTEMS_RESOURCE_IN_USE; /* EBUSY */
    1598 
    1599                         /* get speed rate from argument */
    1600                         speed = (unsigned int)ioarg->buffer;
    1601                         ret = grcan_calc_timing(speed,pDev->corefreq_hz,GRCAN_SAMPLING_POINT,&timing);
    1602                         if ( ret )
    1603                                 return  RTEMS_INVALID_NAME; /* EINVAL */
    1604 
    1605                         /* save timing/speed */
    1606                         pDev->config.timing = timing;
    1607       pDev->config_changed = 1;
    1608                         break;
    1609 
    1610     case GRCAN_IOC_SET_BTRS:
    1611                         /* Set BTR registers manually
    1612                          * Read GRCAN/HurriCANe Manual.
    1613                          */
    1614                         if ( pDev->started )
    1615                                 return RTEMS_RESOURCE_IN_USE; /* EBUSY */
    1616 
    1617                         if ( !ioarg->buffer )
    1618         return RTEMS_INVALID_NAME;
    1619 
    1620                         pDev->config.timing = *(struct grcan_timing *)ioarg->buffer;
    1621       pDev->config_changed = 1;
    1622                         break;
    1623 
    1624     case GRCAN_IOC_SET_AFILTER:
    1625       filter = (struct grcan_filter *)ioarg->buffer;
    1626       if ( !filter ){
    1627         /* Disable filtering - let all messages pass */
    1628         pDev->afilter.mask = 0x0;
    1629         pDev->afilter.code = 0x0;
    1630       }else{
    1631         /* Save filter */
    1632         pDev->afilter = *filter;
    1633       }
    1634       /* Set hardware acceptance filter */
    1635       grcan_hw_accept(pDev->regs,&pDev->afilter);
    1636       break;
    1637 
    1638     case GRCAN_IOC_SET_SFILTER:
    1639       filter = (struct grcan_filter *)ioarg->buffer;
    1640       if ( !filter ){
    1641         /* disable TX/RX SYNC filtering */
    1642         pDev->sfilter.mask = 0xffffffff;
    1643         pDev->sfilter.mask = 0;
    1644 
    1645         /* disable Sync interrupt */
    1646         pDev->regs->imr = READ_REG(&pDev->regs->imr) & ~(GRCAN_RXSYNC_IRQ|GRCAN_TXSYNC_IRQ);
    1647       }else{
    1648         /* Save filter */
    1649         pDev->sfilter = *filter;
    1650 
    1651         /* Enable Sync interrupt */
    1652         pDev->regs->imr = READ_REG(&pDev->regs->imr) | (GRCAN_RXSYNC_IRQ|GRCAN_TXSYNC_IRQ);
    1653       }
    1654       /* Set Sync RX/TX filter */
    1655       grcan_hw_sync(pDev->regs,&pDev->sfilter);
    1656       break;
    1657 
    1658     case GRCAN_IOC_GET_STATUS:
    1659       if ( !data )
    1660         return RTEMS_INVALID_NAME;
    1661       /* Read out the statsu register from the GRCAN core */
    1662       data[0] = READ_REG(&pDev->regs->stat);
    1663       break;
    1664 
    1665     default:
    1666       return RTEMS_NOT_DEFINED;
    1667   }
    1668   return RTEMS_SUCCESSFUL;
    1669 }
    1670 
    1671 #ifndef GRCAN_DONT_DECLARE_IRQ_HANDLER
    1672 /* Find what device caused the IRQ */
    1673 static rtems_isr grcan_interrupt_handler(rtems_vector_number v)
    1674 {
    1675   int minor=0;
    1676   while ( minor < grcan_core_cnt ){
    1677     if ( (grcans[minor].irq+0x10) == v ){
    1678       grcan_interrupt(&grcans[minor]);
    1679       break;
    1680     }
    1681   }
    1682 }
    1683 #endif
     1663                /* cannot change speed during run mode */
     1664                if ( pDev->started )
     1665                        return RTEMS_RESOURCE_IN_USE; /* EBUSY */
     1666
     1667                /* get speed rate from argument */
     1668                speed = (unsigned int)ioarg->buffer;
     1669                ret = grcan_calc_timing(speed, pDev->corefreq_hz, GRCAN_SAMPLING_POINT, &timing);
     1670                if ( ret )
     1671                        return  RTEMS_INVALID_NAME; /* EINVAL */
     1672
     1673                /* save timing/speed */
     1674                pDev->config.timing = timing;
     1675                pDev->config_changed = 1;
     1676                break;
     1677
     1678                case GRCAN_IOC_SET_BTRS:
     1679                /* Set BTR registers manually
     1680                 * Read GRCAN/HurriCANe Manual.
     1681                 */
     1682                if ( pDev->started )
     1683                        return RTEMS_RESOURCE_IN_USE; /* EBUSY */
     1684                if ( !ioarg->buffer )
     1685                        return RTEMS_INVALID_NAME;
     1686
     1687                pDev->config.timing = *(struct grcan_timing *)ioarg->buffer;
     1688                pDev->config_changed = 1;
     1689                break;
     1690
     1691                case GRCAN_IOC_SET_AFILTER:
     1692                filter = (struct grcan_filter *)ioarg->buffer;
     1693                if ( !filter ){
     1694                        /* Disable filtering - let all messages pass */
     1695                        pDev->afilter.mask = 0x0;
     1696                        pDev->afilter.code = 0x0;
     1697                }else{
     1698                        /* Save filter */
     1699                        pDev->afilter = *filter;
     1700                }
     1701                /* Set hardware acceptance filter */     
     1702                grcan_hw_accept(pDev->regs,&pDev->afilter);
     1703                break;
     1704
     1705                case GRCAN_IOC_SET_SFILTER:
     1706                filter = (struct grcan_filter *)ioarg->buffer;
     1707                if ( !filter ){
     1708                        /* disable TX/RX SYNC filtering */
     1709                        pDev->sfilter.mask = 0xffffffff;
     1710                        pDev->sfilter.mask = 0;
     1711
     1712                         /* disable Sync interrupt */
     1713                        pDev->regs->imr = READ_REG(&pDev->regs->imr) & ~(GRCAN_RXSYNC_IRQ|GRCAN_TXSYNC_IRQ);
     1714                }else{
     1715                        /* Save filter */
     1716                        pDev->sfilter = *filter;
     1717
     1718                        /* Enable Sync interrupt */
     1719                        pDev->regs->imr = READ_REG(&pDev->regs->imr) | (GRCAN_RXSYNC_IRQ|GRCAN_TXSYNC_IRQ);
     1720                }
     1721                /* Set Sync RX/TX filter */
     1722                grcan_hw_sync(pDev->regs,&pDev->sfilter);
     1723                break;
     1724
     1725                case GRCAN_IOC_GET_STATUS:
     1726                if ( !data )
     1727                        return RTEMS_INVALID_NAME;
     1728                /* Read out the statsu register from the GRCAN core */
     1729                data[0] = READ_REG(&pDev->regs->stat);
     1730                break;
     1731
     1732        default:
     1733                return RTEMS_NOT_DEFINED;
     1734        }
     1735        return RTEMS_SUCCESSFUL;
     1736}
    16841737
    16851738/* Handle the IRQ */
    1686 static void grcan_interrupt(struct grcan_priv *pDev)
    1687 {
    1688   unsigned int status = READ_REG(&pDev->regs->pimsr);
    1689   unsigned int canstat = READ_REG(&pDev->regs->stat);
    1690 
    1691   /* Spurious IRQ call? */
    1692   if ( !status && !canstat )
    1693     return;
    1694 
    1695   FUNCDBG();
    1696 
    1697   /* Increment number of interrupts counter */
    1698   pDev->stats.ints++;
    1699 
    1700   if ( (status & GRCAN_ERR_IRQ) || (canstat & GRCAN_STAT_PASS) ){
    1701     /* Error-Passive interrupt */
    1702     pDev->stats.passive_cnt++;
    1703   }
    1704 
    1705   if ( (status & GRCAN_OFF_IRQ) || (canstat & GRCAN_STAT_OFF) ){
    1706     /* Bus-off condition interrupt
    1707      * The link is brought down by hardware, we wake all threads
    1708      * that is blocked in read/write calls and stop futher calls
    1709      * to read/write until user has called ioctl(fd,START,0).
    1710      */
    1711      pDev->started = 0;
    1712      grcan_stop(pDev); /* this mask all IRQ sources */
    1713      status=0x1ffff; /* clear all interrupts */
    1714      goto out;
    1715   }
    1716 
    1717   if ( (status & GRCAN_OR_IRQ) || (canstat & GRCAN_STAT_OR) ){
    1718     /* Over-run during reception interrupt */
    1719     pDev->stats.overrun_cnt++;
    1720   }
    1721 
    1722   if ( (status & GRCAN_RXAHBERR_IRQ) ||
    1723        (status & GRCAN_TXAHBERR_IRQ) ||
    1724        (canstat & GRCAN_STAT_AHBERR) ){
    1725     /* RX or Tx AHB Error interrupt */
    1726     printk("AHBERROR: status: 0x%x, canstat: 0x%x\n",status,canstat);
    1727     pDev->stats.ahberr_cnt++;
    1728   }
    1729 
    1730   if ( status & GRCAN_TXLOSS_IRQ ) {
    1731     pDev->stats.txloss_cnt++;
    1732   }
    1733 
    1734   if ( status & GRCAN_RXIRQ_IRQ ){
    1735     /* RX IRQ pointer interrupt */
    1736     /*printk("RxIrq 0x%x\n",status);*/
    1737     pDev->regs->imr = READ_REG(&pDev->regs->imr) & ~GRCAN_RXIRQ_IRQ;
    1738     rtems_semaphore_release(pDev->rx_sem);
    1739   }
    1740 
    1741   if ( status & GRCAN_TXIRQ_IRQ ){
    1742     /* TX IRQ pointer interrupt */
    1743     pDev->regs->imr = READ_REG(&pDev->regs->imr) & ~GRCAN_TXIRQ_IRQ;
    1744     rtems_semaphore_release(pDev->tx_sem);
    1745   }
    1746 
    1747   if ( status & GRCAN_TXSYNC_IRQ ){
    1748     /* TxSync message transmitted interrupt */
    1749     pDev->stats.txsync_cnt++;
    1750   }
    1751 
    1752   if ( status & GRCAN_RXSYNC_IRQ ){
    1753     /* RxSync message received interrupt */
    1754     pDev->stats.rxsync_cnt++;
    1755   }
    1756 
    1757   if ( status & GRCAN_TXEMPTY_IRQ ){
    1758     pDev->regs->imr = READ_REG(&pDev->regs->imr) & ~GRCAN_TXEMPTY_IRQ;
    1759     rtems_semaphore_release(pDev->txempty_sem);
    1760   }
     1739static void grcan_interrupt(void *arg)
     1740{
     1741        struct grcan_priv *pDev = arg;
     1742        unsigned int status = READ_REG(&pDev->regs->pimsr);
     1743        unsigned int canstat = READ_REG(&pDev->regs->stat);
     1744
     1745        /* Spurious IRQ call? */
     1746        if ( !status && !canstat )
     1747                return;
     1748
     1749        FUNCDBG();
     1750
     1751        /* Increment number of interrupts counter */
     1752        pDev->stats.ints++;
     1753
     1754        if ( (status & GRCAN_ERR_IRQ) || (canstat & GRCAN_STAT_PASS) ){
     1755                /* Error-Passive interrupt */
     1756                pDev->stats.passive_cnt++;
     1757        }
     1758
     1759        if ( (status & GRCAN_OFF_IRQ) || (canstat & GRCAN_STAT_OFF) ){
     1760                /* Bus-off condition interrupt
     1761                 * The link is brought down by hardware, we wake all threads
     1762                 * that is blocked in read/write calls and stop futher calls
     1763                 * to read/write until user has called ioctl(fd,START,0).
     1764                 */
     1765                pDev->started = 0;
     1766                grcan_stop(pDev); /* this mask all IRQ sources */
     1767                status=0x1ffff; /* clear all interrupts */
     1768                goto out;
     1769        }
     1770
     1771        if ( (status & GRCAN_OR_IRQ) || (canstat & GRCAN_STAT_OR) ){
     1772                /* Over-run during reception interrupt */
     1773                pDev->stats.overrun_cnt++;
     1774        }
     1775
     1776        if ( (status & GRCAN_RXAHBERR_IRQ) ||
     1777             (status & GRCAN_TXAHBERR_IRQ) ||
     1778             (canstat & GRCAN_STAT_AHBERR) ){
     1779                /* RX or Tx AHB Error interrupt */
     1780                printk("AHBERROR: status: 0x%x, canstat: 0x%x\n",status,canstat);
     1781                pDev->stats.ahberr_cnt++;
     1782        }
     1783
     1784        if ( status & GRCAN_TXLOSS_IRQ ) {
     1785                pDev->stats.txloss_cnt++;
     1786        }
     1787
     1788        if ( status & GRCAN_RXIRQ_IRQ ){
     1789                /* RX IRQ pointer interrupt */
     1790                /*printk("RxIrq 0x%x\n",status);*/
     1791                pDev->regs->imr = READ_REG(&pDev->regs->imr) & ~GRCAN_RXIRQ_IRQ;
     1792                rtems_semaphore_release(pDev->rx_sem);
     1793        }
     1794
     1795        if ( status & GRCAN_TXIRQ_IRQ ){
     1796                /* TX IRQ pointer interrupt */
     1797                pDev->regs->imr = READ_REG(&pDev->regs->imr) & ~GRCAN_TXIRQ_IRQ;
     1798                rtems_semaphore_release(pDev->tx_sem);
     1799        }
     1800
     1801        if ( status & GRCAN_TXSYNC_IRQ ){
     1802                /* TxSync message transmitted interrupt */
     1803                pDev->stats.txsync_cnt++;
     1804        }
     1805
     1806        if ( status & GRCAN_RXSYNC_IRQ ){
     1807                /* RxSync message received interrupt */
     1808                pDev->stats.rxsync_cnt++;
     1809        }
     1810
     1811        if ( status & GRCAN_TXEMPTY_IRQ ){
     1812                pDev->regs->imr = READ_REG(&pDev->regs->imr) & ~GRCAN_TXEMPTY_IRQ;
     1813                rtems_semaphore_release(pDev->txempty_sem);
     1814        }
    17611815
    17621816out:
    1763   /* Clear IRQs */
    1764   pDev->regs->picr = status;
    1765 }
    1766 
    1767 static int grcan_register_internal(void)
    1768 {
    1769   rtems_status_code r;
    1770   rtems_device_major_number m;
    1771 
    1772   if ((r = rtems_io_register_driver(0, &grcan_driver, &m)) !=
    1773        RTEMS_SUCCESSFUL) {
    1774     switch(r) {
    1775       case RTEMS_TOO_MANY:
    1776         DBG2("failed RTEMS_TOO_MANY\n");
    1777         break;
    1778       case RTEMS_INVALID_NUMBER:
    1779         DBG2("failed RTEMS_INVALID_NUMBER\n");
    1780         break;
    1781       case RTEMS_RESOURCE_IN_USE:
    1782         DBG2("failed RTEMS_RESOURCE_IN_USE\n");
    1783         break;
    1784       default:
    1785         DBG("failed %i\n",r);
    1786         break;
    1787     }
    1788     return 1;
    1789   }
    1790   DBG("Registered GRCAN on major %d\n",m);
    1791   return 0;
    1792 }
    1793 
    1794 
    1795 int grcan_register_abs(struct grcan_device_info *devices, int dev_cnt);
    1796 
    1797 /* Use custom addresses and IRQs to find hardware */
    1798 int GRCAN_PREFIX(_register_abs)(struct grcan_device_info *devices, int dev_cnt)
    1799 {
    1800   FUNCDBG();
    1801 
    1802   if ( !devices || (dev_cnt<0) )
    1803     return 1;
    1804   grcan_cores = devices;
    1805   grcan_core_cnt = dev_cnt;
    1806 
    1807   amba_bus = NULL;
    1808         return grcan_register_internal();
    1809 }
    1810 
    1811 /* Use prescanned AMBA Plug&Play information to find all GRCAN cores */
    1812 int GRCAN_PREFIX(_register)(struct ambapp_bus *abus)
    1813 {
    1814   FUNCDBG();
    1815 
    1816   if ( !abus )
    1817     return 1;
    1818   amba_bus = abus;
    1819   grcan_cores = NULL;
    1820   grcan_core_cnt = 0;
    1821   return grcan_register_internal();
    1822 }
     1817        /* Clear IRQs */
     1818        pDev->regs->picr = status;
     1819}
  • c/src/lib/libbsp/sparc/shared/can/occan.c

    re67b2b8d r3681925  
    22 *
    33 *  COPYRIGHT (c) 2007.
    4  *  Gaisler Research.
     4 *  Cobham Gaisler AB.
    55 *
    66 *  The license and distribution terms for this file may be
    77 *  found in the file LICENSE in this distribution or at
    88 *  http://www.rtems.org/license/LICENSE.
    9  *
    10  *  Author: Daniel Hellström, Gaisler Research AB, www.gaisler.com
    119 */
    1210
     
    1816#include <rtems/bspIo.h> /* printk */
    1917
    20 #include <leon.h>
    21 #include <ambapp.h>
    22 #include <grlib.h>
     18#include <drvmgr/drvmgr.h>
     19#include <drvmgr/ambapp_bus.h>
    2320#include <occan.h>
    2421
     
    5451#endif
    5552
    56 #ifndef OCCAN_PREFIX
    57  #define OCCAN_PREFIX(name) occan##name
    58 #endif
    59 
    60 #if !defined(OCCAN_DEVNAME) || !defined(OCCAN_DEVNAME_NO)
    61  #undef OCCAN_DEVNAME
    62  #undef OCCAN_DEVNAME_NO
    63  #define OCCAN_DEVNAME "/dev/occan0"
    64  #define OCCAN_DEVNAME_NO(devstr,no) ((devstr)[10]='0'+(no))
    65 #endif
    66 
    67 #ifndef OCCAN_REG_INT
    68         #define OCCAN_REG_INT(handler,irq,arg) set_vector(handler,irq+0x10,1)
    69   #undef  OCCAN_DEFINE_INTHANDLER
    70   #define OCCAN_DEFINE_INTHANDLER
    71 #endif
    72 
    73 /* Default to 40MHz system clock */
    74 /*#ifndef SYS_FREQ_HZ
    75  #define SYS_FREQ_HZ 40000000
    76 #endif*/
     53/* Enable Fixup code older OCCAN with a TX IRQ-FLAG bug */
     54#define OCCAN_TX_IRQ_FLAG_FIXUP 1
    7755
    7856#define OCCAN_WORD_REG_OFS 0x80
     
    10482
    10583/* PELICAN */
    106 #ifdef OCCAN_BYTE_REGS
     84
    10785typedef struct {
    10886        unsigned char
     
    150128                unsigned char unused1;
    151129                unsigned char clkdiv;
    152 } pelican_regs;
    153 #else
     130} pelican8_regs;
     131
    154132typedef struct {
    155133        unsigned char
     
    197175                unsigned char unused17[4];
    198176                unsigned char clkdiv,unused18[3];
    199 } pelican_regs;
     177} pelican32_regs;
     178
     179#ifdef OCCAN_BYTE_REGS
     180#define pelican_regs pelican8_regs
     181#else
     182#define pelican_regs pelican32_regs
    200183#endif
     184
    201185
    202186#define MAX_TSEG2 7
     
    218202
    219203typedef struct {
     204        struct drvmgr_dev *dev;
     205        char devName[32];
     206
    220207        /* hardware shortcuts */
    221208        pelican_regs *regs;
     209        int byte_regs;
    222210        int irq;
    223211        occan_speed_regs timing;
    224212        int channel; /* 0=default, 1=second bus */
    225213        int single_mode;
     214        unsigned int sys_freq_hz;
    226215
    227216        /* driver state */
     
    233222        int rxblk;
    234223        int txblk;
     224        int sending;
    235225        unsigned int status;
    236226        occan_stats stats;
     
    267257static int pelican_send(occan_priv *can, CANMsg *msg);
    268258static void pelican_set_accept(occan_priv *priv, unsigned char *acode, unsigned char *amask);
    269 static void occan_interrupt(occan_priv *can);
     259void occan_interrupt(void *arg);
    270260#ifdef DEBUG_PRINT_REGMAP
    271261static void pelican_regadr_print(pelican_regs *regs);
     
    279269static rtems_device_driver occan_open(rtems_device_major_number major, rtems_device_minor_number minor, void *arg);
    280270static rtems_device_driver occan_initialize(rtems_device_major_number major, rtems_device_minor_number unused, void *arg);
    281 #ifdef OCCAN_DEFINE_INTHANDLER
    282 static void occan_interrupt_handler(rtems_vector_number v);
    283 #endif
    284 static int can_cores;
    285 static occan_priv *cans;
    286 static struct ambapp_bus *amba_bus;
    287 static unsigned int sys_freq_hz;
     271
     272#define OCCAN_DRIVER_TABLE_ENTRY { occan_initialize, occan_open, occan_close, occan_read, occan_write, occan_ioctl }
     273static rtems_driver_address_table occan_driver = OCCAN_DRIVER_TABLE_ENTRY;
    288274
    289275
    290276/* Read byte bypassing */
    291277
    292 #ifdef OCCAN_DONT_BYPASS_CACHE
    293  #define READ_REG(address) (*(volatile unsigned char *)(address))
    294 #else
    295  /* Bypass cache */
    296  #define READ_REG(address) _OCCAN_REG_READ((unsigned int)(address))
    297  static __inline__ unsigned char _OCCAN_REG_READ(unsigned int addr) {
    298         unsigned char tmp;
    299         __asm__ (" lduba [%1]1, %0 "
    300             : "=r"(tmp)
    301             : "r"(addr)
    302            );
    303         return tmp;
    304         }
    305 #endif
    306 
    307 #define WRITE_REG(address,data) (*(volatile unsigned char *)(address) = (data))
     278
     279/* Bypass cache */
     280#define READ_REG(priv, address) occan_reg_read(priv, (unsigned int)address)
     281#define WRITE_REG(priv, address, data) occan_reg_write(priv, (unsigned int)address, data)
     282
     283unsigned int occan_reg_read(occan_priv *priv, unsigned int address)
     284{
     285        unsigned int adr;
     286        if ( priv->byte_regs ) {
     287                adr = address;
     288        } else {
     289                /* Word accessed registers */
     290                adr = (address & (~0x7f)) | ((address & 0x7f)<<2);
     291        }
     292        return *(volatile unsigned char *)adr;
     293}
     294
     295void occan_reg_write(occan_priv *priv, unsigned int address, unsigned char value)
     296{
     297        unsigned int adr;
     298        if ( priv->byte_regs ) {
     299                adr = address;
     300        } else {
     301                /* Word accessed registers */
     302                adr = (address & (~0x7f)) | ((address & 0x7f)<<2);
     303        }
     304        *(volatile unsigned char *)adr = value;;
     305}
    308306
    309307/* Mode register bit definitions */
     
    392390*/
    393391
    394 static void pelican_init(occan_priv *priv){
    395         /* Reset core */
    396         priv->regs->mode = PELICAN_MOD_RESET;
    397 
    398         /* wait for core to reset complete */
    399         /*usleep(1);*/
    400 }
    401 
    402 static void pelican_open(occan_priv *priv){
    403         /* unsigned char tmp; */
    404         int ret;
    405 
    406         /* Set defaults */
    407         priv->speed = OCCAN_SPEED_250K;
    408 
    409         /* set acceptance filters to accept all messages */
    410         priv->acode[0] = 0;
    411         priv->acode[1] = 0;
    412         priv->acode[2] = 0;
    413         priv->acode[3] = 0;
    414         priv->amask[0] = 0xff;
    415         priv->amask[1] = 0xff;
    416         priv->amask[2] = 0xff;
    417         priv->amask[3] = 0xff;
    418 
    419         /* Set clock divider to extended mode, clkdiv not connected
     392static int occan_driver_io_registered = 0;
     393static rtems_device_major_number occan_driver_io_major = 0;
     394
     395/******************* Driver manager interface ***********************/
     396
     397/* Driver prototypes */
     398int occan_register_io(rtems_device_major_number *m);
     399int occan_device_init(occan_priv *pDev);
     400
     401int occan_init2(struct drvmgr_dev *dev);
     402int occan_init3(struct drvmgr_dev *dev);
     403
     404struct drvmgr_drv_ops occan_ops =
     405{
     406        .init = {NULL, occan_init2, occan_init3, NULL},
     407        .remove = NULL,
     408        .info = NULL
     409};
     410
     411struct amba_dev_id occan_ids[] =
     412{
     413        {VENDOR_GAISLER, GAISLER_CANAHB},
     414        {0, 0}          /* Mark end of table */
     415};
     416
     417struct amba_drv_info occan_drv_info =
     418{
     419        {
     420                DRVMGR_OBJ_DRV,                 /* Driver */
     421                NULL,                           /* Next driver */
     422                NULL,                           /* Device list */
     423                DRIVER_AMBAPP_GAISLER_OCCAN_ID, /* Driver ID */
     424                "OCCAN_DRV",                    /* Driver Name */
     425                DRVMGR_BUS_TYPE_AMBAPP,         /* Bus Type */
     426                &occan_ops,
     427                NULL,                           /* Funcs */
     428                0,                              /* No devices yet */
     429                0,
     430        },
     431        &occan_ids[0]
     432};
     433
     434void occan_register_drv (void)
     435{
     436        DBG("Registering OCCAN driver\n");
     437        drvmgr_drv_register(&occan_drv_info.general);
     438}
     439
     440int occan_init2(struct drvmgr_dev *dev)
     441{
     442        occan_priv *priv;
     443
     444        DBG("OCCAN[%d] on bus %s\n", dev->minor_drv, dev->parent->dev->name);
     445        priv = dev->priv = malloc(sizeof(occan_priv));
     446        if ( !priv )
     447                return DRVMGR_NOMEM;
     448        memset(priv, 0, sizeof(*priv));
     449        priv->dev = dev;
     450
     451        return DRVMGR_OK;
     452}
     453
     454int occan_init3(struct drvmgr_dev *dev)
     455{
     456        occan_priv *priv;
     457        char prefix[32];
     458        rtems_status_code status;
     459
     460        priv = dev->priv;
     461
     462        /* Do initialization */
     463
     464        if ( occan_driver_io_registered == 0) {
     465                /* Register the I/O driver only once for all cores */
     466                if ( occan_register_io(&occan_driver_io_major) ) {
     467                        /* Failed to register I/O driver */
     468                        dev->priv = NULL;
     469                        return DRVMGR_FAIL;
     470                }
     471
     472                occan_driver_io_registered = 1;
     473        }
     474
     475        /* I/O system registered and initialized
     476         * Now we take care of device initialization.
    420477         */
    421         priv->regs->clkdiv = (1<<PELICAN_CDR_MODE_BITS) | (DEFAULT_CLKDIV & PELICAN_CDR_DIV);
    422 
    423         ret = occan_calc_speedregs(sys_freq_hz,priv->speed,&priv->timing);
    424         if ( ret ){
    425                 /* failed to set speed for this system freq, try with 50K instead */
    426                 priv->speed = OCCAN_SPEED_50K;
    427                 occan_calc_speedregs(sys_freq_hz,priv->speed,&priv->timing);
    428         }
    429 
    430         /* disable all interrupts */
    431         priv->regs->inten = 0;
    432 
    433         /* clear pending interrupts by reading */
    434         /* tmp = */ READ_REG(&priv->regs->intflags);
    435 }
    436 
    437 static int pelican_start(occan_priv *priv){
    438         /* unsigned char tmp; */
    439         /* Start HW communication */
    440 
    441         if ( !priv->rxfifo || !priv->txfifo )
     478
     479        if ( occan_device_init(priv) ) {
     480                return DRVMGR_FAIL;
     481        }
     482
     483        /* Get Filesystem name prefix */
     484        prefix[0] = '\0';
     485        if ( drvmgr_get_dev_prefix(dev, prefix) ) {
     486                /* Failed to get prefix, make sure of a unique FS name
     487                 * by using the driver minor.
     488                 */
     489                sprintf(priv->devName, "/dev/occan%d", dev->minor_drv);
     490        } else {
     491                /* Got special prefix, this means we have a bus prefix
     492                 * And we should use our "bus minor"
     493                 */
     494                sprintf(priv->devName, "/dev/%soccan%d", prefix, dev->minor_bus);
     495        }
     496
     497        /* Register Device */
     498        DBG("OCCAN[%d]: Registering %s\n", dev->minor_drv, priv->devName);
     499        status = rtems_io_register_name(priv->devName, occan_driver_io_major, dev->minor_drv);
     500        if (status != RTEMS_SUCCESSFUL) {
     501                return DRVMGR_FAIL;
     502        }
     503
     504        return DRVMGR_OK;
     505}
     506
     507/******************* Driver Implementation ***********************/
     508
     509int occan_register_io(rtems_device_major_number *m)
     510{
     511        rtems_status_code r;
     512
     513        if ((r = rtems_io_register_driver(0, &occan_driver, m)) == RTEMS_SUCCESSFUL) {
     514                DBG("OCCAN driver successfully registered, major: %d\n", *m);
     515        } else {
     516                switch(r) {
     517                case RTEMS_TOO_MANY:
     518                        printk("OCCAN rtems_io_register_driver failed: RTEMS_TOO_MANY\n");
     519                        return -1;
     520                case RTEMS_INVALID_NUMBER: 
     521                        printk("OCCAN rtems_io_register_driver failed: RTEMS_INVALID_NUMBER\n");
     522                        return -1;
     523                case RTEMS_RESOURCE_IN_USE:
     524                        printk("OCCAN rtems_io_register_driver failed: RTEMS_RESOURCE_IN_USE\n");
     525                        return -1;
     526                default:
     527                        printk("OCCAN rtems_io_register_driver failed\n");
     528                        return -1;
     529                }
     530        }
     531        return 0;
     532}
     533
     534int occan_device_init(occan_priv *pDev)
     535{
     536        struct amba_dev_info *ambadev;
     537        struct ambapp_core *pnpinfo;
     538        rtems_status_code status;
     539        int minor;
     540
     541        /* Get device information from AMBA PnP information */
     542        ambadev = (struct amba_dev_info *)pDev->dev->businfo;
     543        if ( ambadev == NULL ) {
    442544                return -1;
    443 
    444   /* In case we were started before and stopped we
    445    * should empty the TX fifo or try to resend those
    446    * messages. We make it simple...
    447    */
    448   occan_fifo_clr(priv->txfifo);
    449 
    450         /* Clear status bits */
    451         priv->status = 0;
    452 
    453         /* clear pending interrupts */
    454         /* tmp = */ READ_REG(&priv->regs->intflags);
    455 
    456         /* clear error counters */
    457         priv->regs->rx_err_cnt = 0;
    458         priv->regs->tx_err_cnt = 0;
    459 
    460 #ifdef REDUNDANT_CHANNELS
    461         if ( (priv->channel == 0) || (priv->channel >= REDUNDANT_CHANNELS) ){
    462                 /* Select the first (default) channel */
    463                 OCCAN_SET_CHANNEL(priv,0);
     545        }
     546        pnpinfo = &ambadev->info;
     547        pDev->irq = pnpinfo->irq;
     548        pDev->regs = (pelican_regs *)(pnpinfo->ahb_slv->start[0] + OCCAN_NCORE_OFS*pnpinfo->index);
     549        pDev->byte_regs = 1;
     550        minor = pDev->dev->minor_drv;
     551
     552        /* Get frequency in Hz */
     553        if ( drvmgr_freq_get(pDev->dev, DEV_AHB_SLV, &pDev->sys_freq_hz) ) {
     554                return -1;
     555        }
     556
     557        DBG("OCCAN frequency: %d Hz\n", pDev->sys_freq_hz);
     558
     559        /* initialize software */
     560        pDev->open = 0;
     561        pDev->started = 0; /* Needed for spurious interrupts */
     562        pDev->rxfifo = NULL;
     563        pDev->txfifo = NULL;
     564        status = rtems_semaphore_create(
     565                        rtems_build_name('C', 'd', 'v', '0'+minor),
     566                        1,
     567                        RTEMS_FIFO | RTEMS_SIMPLE_BINARY_SEMAPHORE | RTEMS_NO_INHERIT_PRIORITY | \
     568                        RTEMS_NO_PRIORITY_CEILING,
     569                        0,
     570                        &pDev->devsem);
     571        if ( status != RTEMS_SUCCESSFUL ){
     572                printk("OCCAN[%d]: Failed to create dev semaphore, (%d)\n\r",minor, status);
     573                return RTEMS_UNSATISFIED;
     574        }
     575        status = rtems_semaphore_create(
     576                        rtems_build_name('C', 't', 'x', '0'+minor),
     577                        0,
     578                        RTEMS_FIFO | RTEMS_SIMPLE_BINARY_SEMAPHORE | RTEMS_NO_INHERIT_PRIORITY | \
     579                        RTEMS_NO_PRIORITY_CEILING,
     580                        0,
     581                        &pDev->txsem);
     582        if ( status != RTEMS_SUCCESSFUL ){
     583                printk("OCCAN[%d]: Failed to create tx semaphore, (%d)\n\r",minor, status);
     584                return RTEMS_UNSATISFIED;
     585        }
     586        status = rtems_semaphore_create(
     587                        rtems_build_name('C', 'r', 'x', '0'+minor),
     588                        0,
     589                        RTEMS_FIFO | RTEMS_SIMPLE_BINARY_SEMAPHORE | RTEMS_NO_INHERIT_PRIORITY | \
     590                        RTEMS_NO_PRIORITY_CEILING,
     591                        0,
     592                        &pDev->rxsem);
     593        if ( status != RTEMS_SUCCESSFUL ){
     594                printk("OCCAN[%d]: Failed to create rx semaphore, (%d)\n\r",minor, status);
     595                return RTEMS_UNSATISFIED;
     596        }
     597
     598        /* hardware init/reset */
     599        pelican_init(pDev);
     600
     601#ifdef DEBUG_PRINT_REGMAP
     602        pelican_regadr_print(pDev->regs);
     603#endif
     604
     605        return 0;
     606}
     607
     608
     609#ifdef DEBUG
     610static void pelican_regs_print(occan_priv *pDev){
     611        pelican_regs *regs = pDev->regs;
     612        printk("--- PELICAN 0x%lx ---\n\r",(unsigned int)regs);
     613        printk(" MODE: 0x%02x\n\r",READ_REG(pDev, &regs->mode));
     614        printk(" CMD: 0x%02x\n\r",READ_REG(pDev, &regs->cmd));
     615        printk(" STATUS: 0x%02x\n\r",READ_REG(pDev, &regs->status));
     616        /*printk(" INTFLG: 0x%02x\n\r",READ_REG(pDev, &regs->intflags));*/
     617        printk(" INTEN: 0x%02x\n\r",READ_REG(pDev, &regs->inten));
     618        printk(" BTR0: 0x%02x\n\r",READ_REG(pDev, &regs->bustim0));
     619        printk(" BTR1: 0x%02x\n\r",READ_REG(pDev, &regs->bustim1));
     620        printk(" ARBCODE: 0x%02x\n\r",READ_REG(pDev, &regs->arbcode));
     621        printk(" ERRCODE: 0x%02x\n\r",READ_REG(pDev, &regs->errcode));
     622        printk(" ERRWARN: 0x%02x\n\r",READ_REG(pDev, &regs->errwarn));
     623        printk(" RX_ERR_CNT: 0x%02x\n\r",READ_REG(pDev, &regs->rx_err_cnt));
     624        printk(" TX_ERR_CNT: 0x%02x\n\r",READ_REG(pDev, &regs->tx_err_cnt));
     625        if ( READ_REG(pDev, &regs->mode) & PELICAN_MOD_RESET ){
     626                /* in reset mode it is possible to read acceptance filters */
     627                printk(" ACR0: 0x%02x (0x%lx)\n\r",READ_REG(pDev, &regs->rx_fi_xff),&regs->rx_fi_xff);
     628                printk(" ACR1: 0x%02x (0x%lx)\n\r",READ_REG(pDev, &regs->msg.rst_accept.code[0]),(unsigned int)&regs->msg.rst_accept.code[0]);
     629                printk(" ACR1: 0x%02x (0x%lx)\n\r",READ_REG(pDev, &regs->msg.rst_accept.code[1]),(unsigned int)&regs->msg.rst_accept.code[1]);
     630                printk(" ACR1: 0x%02x (0x%lx)\n\r",READ_REG(pDev, &regs->msg.rst_accept.code[2]),(unsigned int)&regs->msg.rst_accept.code[2]);
     631                printk(" AMR0: 0x%02x (0x%lx)\n\r",READ_REG(pDev, &regs->msg.rst_accept.mask[0]),(unsigned int)&regs->msg.rst_accept.mask[0]);
     632                printk(" AMR1: 0x%02x (0x%lx)\n\r",READ_REG(pDev, &regs->msg.rst_accept.mask[1]),(unsigned int)&regs->msg.rst_accept.mask[1]);
     633                printk(" AMR2: 0x%02x (0x%lx)\n\r",READ_REG(pDev, &regs->msg.rst_accept.mask[2]),(unsigned int)&regs->msg.rst_accept.mask[2]);
     634                printk(" AMR3: 0x%02x (0x%lx)\n\r",READ_REG(pDev, &regs->msg.rst_accept.mask[3]),(unsigned int)&regs->msg.rst_accept.mask[3]);
     635               
    464636        }else{
    465                 /* set gpio bit, or something */
    466                 OCCAN_SET_CHANNEL(priv,priv->channel);
    467         }
    468 #endif
    469         /* set the speed regs of the CAN core */
    470         occan_set_speedregs(priv,&priv->timing);
    471 
    472         DBG("OCCAN: start: set timing regs btr0: 0x%x, btr1: 0x%x\n\r",READ_REG(&priv->regs->bustim0),READ_REG(&priv->regs->bustim1));
    473 
    474         /* Set default acceptance filter */
    475         pelican_set_accept(priv,priv->acode,priv->amask);
    476 
    477         /* turn on interrupts */
    478         priv->regs->inten = PELICAN_IE_RX | PELICAN_IE_TX | PELICAN_IE_ERRW |
    479                             PELICAN_IE_ERRP | PELICAN_IE_BUS;
    480 
    481 #ifdef DEBUG
    482         /* print setup before starting */
    483         pelican_regs_print(priv->regs);
    484         occan_stat_print(&priv->stats);
    485 #endif
    486 
    487         /* core already in reset mode,
    488          *  € Exit reset mode
    489          *  € Enter Single/Dual mode filtering.
    490          */
    491         priv->regs->mode =  (priv->single_mode << 3);
    492 
    493         return 0;
    494 }
    495 
    496 static void pelican_stop(occan_priv *priv){
    497         /* stop HW */
    498 
    499 #ifdef DEBUG
    500         /* print setup before stopping */
    501         pelican_regs_print(priv->regs);
    502         occan_stat_print(&priv->stats);
    503 #endif
    504 
    505         /* put core in reset mode */
    506         priv->regs->mode = PELICAN_MOD_RESET;
    507 
    508         /* turn off interrupts */
    509         priv->regs->inten = 0;
    510 
    511         priv->status |= OCCAN_STATUS_RESET;
    512 }
    513 
    514 
    515 /* Try to send message "msg", if hardware txfifo is
    516  * full, then -1 is returned.
    517  *
    518  * Be sure to have disabled CAN interrupts when
    519  * entering this function.
    520  */
    521 static int pelican_send(occan_priv *can, CANMsg *msg){
    522         unsigned char tmp,status;
    523         pelican_regs *regs = can->regs;
    524 
    525         /* is there room in send buffer? */
    526         status = READ_REG(&regs->status);
    527         if ( !(status & PELICAN_STAT_TXBUF) ){
    528                 /* tx fifo taken, we have to wait */
    529                 return -1;
    530         }
    531 
    532         tmp = msg->len & 0xf;
    533         if ( msg->rtr )
    534                 tmp |= 0x40;
    535 
    536         if ( msg->extended ){
    537                 /* Extended Frame */
    538                 regs->rx_fi_xff = 0x80 | tmp;
    539                 WRITE_REG(&regs->msg.tx_eff.id[0],(msg->id >> (5+8+8)) & 0xff);
    540                 WRITE_REG(&regs->msg.tx_eff.id[1],(msg->id >> (5+8)) & 0xff);
    541                 WRITE_REG(&regs->msg.tx_eff.id[2],(msg->id >> (5)) & 0xff);
    542                 WRITE_REG(&regs->msg.tx_eff.id[3],(msg->id << 3) & 0xf8);
    543                 tmp = msg->len;
    544                 while(tmp--){
    545                         WRITE_REG(&regs->msg.tx_eff.data[tmp],msg->data[tmp]);
    546                 }
    547         }else{
    548                 /* Standard Frame */
    549                 regs->rx_fi_xff = tmp;
    550                 WRITE_REG(&regs->msg.tx_sff.id[0],(msg->id >> 3) & 0xff);
    551                 WRITE_REG(&regs->msg.tx_sff.id[1],(msg->id << 5) & 0xe0);
    552                 tmp = msg->len;
    553                 while(tmp--){
    554                         WRITE_REG(&regs->msg.tx_sff.data[tmp],msg->data[tmp]);
    555                 }
    556         }
    557 
    558         /* let HW know of new message */
    559         if ( msg->sshot ){
    560                 regs->cmd = PELICAN_CMD_TXREQ | PELICAN_CMD_ABORT;
    561         }else{
    562                 /* normal case -- try resend until sent */
    563                 regs->cmd = PELICAN_CMD_TXREQ;
    564         }
    565 
    566         return 0;
    567 }
    568 
    569 
    570 static void pelican_set_accept(occan_priv *priv, unsigned char *acode, unsigned char *amask){
    571         unsigned char *acode0, *acode1, *acode2, *acode3;
    572         unsigned char *amask0, *amask1, *amask2, *amask3;
    573 
    574         acode0 = &priv->regs->rx_fi_xff;
    575         acode1 = (unsigned char *)&priv->regs->msg.rst_accept.code[0];
    576         acode2 = (unsigned char *)&priv->regs->msg.rst_accept.code[1];
    577         acode3 = (unsigned char *)&priv->regs->msg.rst_accept.code[2];
    578 
    579         amask0 = (unsigned char *)&priv->regs->msg.rst_accept.mask[0];
    580         amask1 = (unsigned char *)&priv->regs->msg.rst_accept.mask[1];
    581         amask2 = (unsigned char *)&priv->regs->msg.rst_accept.mask[2];
    582         amask3 = (unsigned char *)&priv->regs->msg.rst_accept.mask[3];
    583 
    584         /* Set new mask & code */
    585         *acode0 = acode[0];
    586         *acode1 = acode[1];
    587         *acode2 = acode[2];
    588         *acode3 = acode[3];
    589 
    590         *amask0 = amask[0];
    591         *amask1 = amask[1];
    592         *amask2 = amask[2];
    593         *amask3 = amask[3];
    594 }
    595 
    596 #ifdef DEBUG
    597 static void pelican_regs_print(pelican_regs *regs){
    598         printk("--- PELICAN 0x%lx ---\n\r",(unsigned int)regs);
    599         printk(" MODE: 0x%02x\n\r",READ_REG(&regs->mode));
    600         printk(" CMD: 0x%02x\n\r",READ_REG(&regs->cmd));
    601         printk(" STATUS: 0x%02x\n\r",READ_REG(&regs->status));
    602         /*printk(" INTFLG: 0x%02x\n\r",READ_REG(&regs->intflags));*/
    603         printk(" INTEN: 0x%02x\n\r",READ_REG(&regs->inten));
    604         printk(" BTR0: 0x%02x\n\r",READ_REG(&regs->bustim0));
    605         printk(" BTR1: 0x%02x\n\r",READ_REG(&regs->bustim1));
    606         printk(" ARBCODE: 0x%02x\n\r",READ_REG(&regs->arbcode));
    607         printk(" ERRCODE: 0x%02x\n\r",READ_REG(&regs->errcode));
    608         printk(" ERRWARN: 0x%02x\n\r",READ_REG(&regs->errwarn));
    609         printk(" RX_ERR_CNT: 0x%02x\n\r",READ_REG(&regs->rx_err_cnt));
    610         printk(" TX_ERR_CNT: 0x%02x\n\r",READ_REG(&regs->tx_err_cnt));
    611         if ( READ_REG(&regs->mode) & PELICAN_MOD_RESET ){
    612                 /* in reset mode it is possible to read acceptance filters */
    613                 printk(" ACR0: 0x%02x (0x%lx)\n\r",READ_REG(&regs->rx_fi_xff),&regs->rx_fi_xff);
    614                 printk(" ACR1: 0x%02x (0x%lx)\n\r",READ_REG(&regs->msg.rst_accept.code[0]),(unsigned int)&regs->msg.rst_accept.code[0]);
    615                 printk(" ACR1: 0x%02x (0x%lx)\n\r",READ_REG(&regs->msg.rst_accept.code[1]),(unsigned int)&regs->msg.rst_accept.code[1]);
    616                 printk(" ACR1: 0x%02x (0x%lx)\n\r",READ_REG(&regs->msg.rst_accept.code[2]),(unsigned int)&regs->msg.rst_accept.code[2]);
    617                 printk(" AMR0: 0x%02x (0x%lx)\n\r",READ_REG(&regs->msg.rst_accept.mask[0]),(unsigned int)&regs->msg.rst_accept.mask[0]);
    618                 printk(" AMR1: 0x%02x (0x%lx)\n\r",READ_REG(&regs->msg.rst_accept.mask[1]),(unsigned int)&regs->msg.rst_accept.mask[1]);
    619                 printk(" AMR2: 0x%02x (0x%lx)\n\r",READ_REG(&regs->msg.rst_accept.mask[2]),(unsigned int)&regs->msg.rst_accept.mask[2]);
    620                 printk(" AMR3: 0x%02x (0x%lx)\n\r",READ_REG(&regs->msg.rst_accept.mask[3]),(unsigned int)&regs->msg.rst_accept.mask[3]);
    621 
    622         }else{
    623                 printk(" RXFI_XFF: 0x%02x\n\r",READ_REG(&regs->rx_fi_xff));
    624         }
    625         printk(" RX_MSG_CNT: 0x%02x\n\r",READ_REG(&regs->rx_msg_cnt));
    626         printk(" CLKDIV: 0x%02x\n\r",READ_REG(&regs->clkdiv));
     637                printk(" RXFI_XFF: 0x%02x\n\r",READ_REG(pDev, &regs->rx_fi_xff));
     638        }
     639        printk(" RX_MSG_CNT: 0x%02x\n\r",READ_REG(pDev, &regs->rx_msg_cnt));
     640        printk(" CLKDIV: 0x%02x\n\r",READ_REG(pDev, &regs->clkdiv));
    627641        printk("-------------------\n\r");
    628642}
     
    647661        /* in reset mode it is possible to read acceptance filters */
    648662        printk(" RXFI_XFF: 0x%lx\n\r",(unsigned int)&regs->rx_fi_xff);
    649 
     663       
    650664        /* reset registers */
    651665        printk(" ACR0: 0x%lx\n\r",(unsigned int)&regs->rx_fi_xff);
     
    657671        printk(" AMR2: 0x%lx\n\r",(unsigned int)&regs->msg.rst_accept.mask[2]);
    658672        printk(" AMR3: 0x%lx\n\r",(unsigned int)&regs->msg.rst_accept.mask[3]);
    659 
     673       
    660674        /* TX Extended */
    661675        printk(" EFFTX_ID[0]: 0x%lx\n\r",(unsigned int)&regs->msg.tx_eff.id[0]);
     
    666680        printk(" EFFTX_DATA[0]: 0x%lx\n\r",(unsigned int)&regs->msg.tx_eff.data[0]);
    667681        printk(" EFFTX_DATA[1]: 0x%lx\n\r",(unsigned int)&regs->msg.tx_eff.data[1]);
    668         printk(" EFFTX_DATA[2]: 0x%lx\n\r",(unsigned int)&regs->msg.tx_eff.data[2]);
    669         printk(" EFFTX_DATA[3]: 0x%lx\n\r",(unsigned int)&regs->msg.tx_eff.data[3]);
    670         printk(" EFFTX_DATA[4]: 0x%lx\n\r",(unsigned int)&regs->msg.tx_eff.data[4]);
    671         printk(" EFFTX_DATA[5]: 0x%lx\n\r",(unsigned int)&regs->msg.tx_eff.data[5]);
    672         printk(" EFFTX_DATA[6]: 0x%lx\n\r",(unsigned int)&regs->msg.tx_eff.data[6]);
    673         printk(" EFFTX_DATA[7]: 0x%lx\n\r",(unsigned int)&regs->msg.tx_eff.data[7]);
     682        printk(" EFFTX_DATA[2]: 0x%lx\n\r",(unsigned int)&regs->msg.tx_eff.data[2]);   
     683        printk(" EFFTX_DATA[3]: 0x%lx\n\r",(unsigned int)&regs->msg.tx_eff.data[3]);   
     684        printk(" EFFTX_DATA[4]: 0x%lx\n\r",(unsigned int)&regs->msg.tx_eff.data[4]);   
     685        printk(" EFFTX_DATA[5]: 0x%lx\n\r",(unsigned int)&regs->msg.tx_eff.data[5]);   
     686        printk(" EFFTX_DATA[6]: 0x%lx\n\r",(unsigned int)&regs->msg.tx_eff.data[6]);   
     687        printk(" EFFTX_DATA[7]: 0x%lx\n\r",(unsigned int)&regs->msg.tx_eff.data[7]);   
    674688
    675689        /* RX Extended */
     
    681695        printk(" EFFRX_DATA[0]: 0x%lx\n\r",(unsigned int)&regs->msg.rx_eff.data[0]);
    682696        printk(" EFFRX_DATA[1]: 0x%lx\n\r",(unsigned int)&regs->msg.rx_eff.data[1]);
    683         printk(" EFFRX_DATA[2]: 0x%lx\n\r",(unsigned int)&regs->msg.rx_eff.data[2]);
    684         printk(" EFFRX_DATA[3]: 0x%lx\n\r",(unsigned int)&regs->msg.rx_eff.data[3]);
    685         printk(" EFFRX_DATA[4]: 0x%lx\n\r",(unsigned int)&regs->msg.rx_eff.data[4]);
    686         printk(" EFFRX_DATA[5]: 0x%lx\n\r",(unsigned int)&regs->msg.rx_eff.data[5]);
    687         printk(" EFFRX_DATA[6]: 0x%lx\n\r",(unsigned int)&regs->msg.rx_eff.data[6]);
    688         printk(" EFFRX_DATA[7]: 0x%lx\n\r",(unsigned int)&regs->msg.rx_eff.data[7]);
     697        printk(" EFFRX_DATA[2]: 0x%lx\n\r",(unsigned int)&regs->msg.rx_eff.data[2]);   
     698        printk(" EFFRX_DATA[3]: 0x%lx\n\r",(unsigned int)&regs->msg.rx_eff.data[3]);   
     699        printk(" EFFRX_DATA[4]: 0x%lx\n\r",(unsigned int)&regs->msg.rx_eff.data[4]);   
     700        printk(" EFFRX_DATA[5]: 0x%lx\n\r",(unsigned int)&regs->msg.rx_eff.data[5]);   
     701        printk(" EFFRX_DATA[6]: 0x%lx\n\r",(unsigned int)&regs->msg.rx_eff.data[6]);   
     702        printk(" EFFRX_DATA[7]: 0x%lx\n\r",(unsigned int)&regs->msg.rx_eff.data[7]);   
    689703
    690704
     
    695709        printk(" SFFRX_DATA[0]: 0x%lx\n\r",(unsigned int)&regs->msg.rx_sff.data[0]);
    696710        printk(" SFFRX_DATA[1]: 0x%lx\n\r",(unsigned int)&regs->msg.rx_sff.data[1]);
    697         printk(" SFFRX_DATA[2]: 0x%lx\n\r",(unsigned int)&regs->msg.rx_sff.data[2]);
    698         printk(" SFFRX_DATA[3]: 0x%lx\n\r",(unsigned int)&regs->msg.rx_sff.data[3]);
    699         printk(" SFFRX_DATA[4]: 0x%lx\n\r",(unsigned int)&regs->msg.rx_sff.data[4]);
    700         printk(" SFFRX_DATA[5]: 0x%lx\n\r",(unsigned int)&regs->msg.rx_sff.data[5]);
    701         printk(" SFFRX_DATA[6]: 0x%lx\n\r",(unsigned int)&regs->msg.rx_sff.data[6]);
    702         printk(" SFFRX_DATA[7]: 0x%lx\n\r",(unsigned int)&regs->msg.rx_sff.data[7]);
     711        printk(" SFFRX_DATA[2]: 0x%lx\n\r",(unsigned int)&regs->msg.rx_sff.data[2]);   
     712        printk(" SFFRX_DATA[3]: 0x%lx\n\r",(unsigned int)&regs->msg.rx_sff.data[3]);   
     713        printk(" SFFRX_DATA[4]: 0x%lx\n\r",(unsigned int)&regs->msg.rx_sff.data[4]);   
     714        printk(" SFFRX_DATA[5]: 0x%lx\n\r",(unsigned int)&regs->msg.rx_sff.data[5]);   
     715        printk(" SFFRX_DATA[6]: 0x%lx\n\r",(unsigned int)&regs->msg.rx_sff.data[6]);   
     716        printk(" SFFRX_DATA[7]: 0x%lx\n\r",(unsigned int)&regs->msg.rx_sff.data[7]);   
    703717
    704718        /* TX Extended */
     
    737751#endif
    738752
     753static void pelican_init(occan_priv *priv){
     754        /* Reset core */
     755        WRITE_REG(priv, &priv->regs->mode, PELICAN_MOD_RESET);
     756
     757        /* wait for core to reset complete */
     758        /*usleep(1);*/
     759}
     760
     761static void pelican_open(occan_priv *priv){
     762        int ret;
     763
     764        /* Set defaults */
     765        priv->speed = OCCAN_SPEED_250K;
     766
     767        /* set acceptance filters to accept all messages */
     768        priv->acode[0] = 0;
     769        priv->acode[1] = 0;
     770        priv->acode[2] = 0;
     771        priv->acode[3] = 0;
     772        priv->amask[0] = 0xff;
     773        priv->amask[1] = 0xff;
     774        priv->amask[2] = 0xff;
     775        priv->amask[3] = 0xff;
     776
     777        /* Set clock divider to extended mode, clkdiv not connected
     778         */
     779        WRITE_REG(priv, &priv->regs->clkdiv, (1<<PELICAN_CDR_MODE_BITS) | (DEFAULT_CLKDIV & PELICAN_CDR_DIV));
     780
     781        ret = occan_calc_speedregs(priv->sys_freq_hz,priv->speed,&priv->timing);
     782        if ( ret ){
     783                /* failed to set speed for this system freq, try with 50K instead */
     784                priv->speed = OCCAN_SPEED_50K;
     785                occan_calc_speedregs(priv->sys_freq_hz, priv->speed,
     786                        &priv->timing);
     787        }
     788
     789        /* disable all interrupts */
     790        WRITE_REG(priv, &priv->regs->inten, 0);
     791
     792        /* clear pending interrupts by reading */
     793        READ_REG(priv, &priv->regs->intflags);
     794}
     795
     796static int pelican_start(occan_priv *priv){
     797        /* Start HW communication */
     798
     799        if ( !priv->rxfifo || !priv->txfifo )
     800                return -1;
     801
     802        /* In case we were started before and stopped we
     803         * should empty the TX fifo or try to resend those
     804         * messages. We make it simple...
     805         */
     806        occan_fifo_clr(priv->txfifo);
     807
     808        /* Clear status bits */
     809        priv->status = 0;
     810        priv->sending = 0;
     811
     812        /* clear pending interrupts */
     813        READ_REG(priv, &priv->regs->intflags);
     814
     815        /* clear error counters */
     816        WRITE_REG(priv, &priv->regs->rx_err_cnt, 0);
     817        WRITE_REG(priv, &priv->regs->tx_err_cnt, 0);
     818
     819#ifdef REDUNDANT_CHANNELS
     820        if ( (priv->channel == 0) || (priv->channel >= REDUNDANT_CHANNELS) ){
     821                /* Select the first (default) channel */
     822                OCCAN_SET_CHANNEL(priv,0);
     823        }else{
     824                /* set gpio bit, or something */
     825                OCCAN_SET_CHANNEL(priv,priv->channel);
     826        }
     827#endif
     828        /* set the speed regs of the CAN core */
     829        occan_set_speedregs(priv,&priv->timing);
     830
     831        DBG("OCCAN: start: set timing regs btr0: 0x%x, btr1: 0x%x\n\r",
     832                READ_REG(priv, &priv->regs->bustim0),
     833                READ_REG(priv, &priv->regs->bustim1));
     834
     835        /* Set default acceptance filter */
     836        pelican_set_accept(priv,priv->acode,priv->amask);
     837
     838        /* Nothing can fail from here, this must be set before interrupts are
     839         * enabled */
     840        priv->started = 1;
     841
     842        /* turn on interrupts */
     843        WRITE_REG(priv, &priv->regs->inten,
     844                PELICAN_IE_RX | PELICAN_IE_TX | PELICAN_IE_ERRW |
     845                PELICAN_IE_ERRP | PELICAN_IE_BUS);
     846#ifdef DEBUG
     847        /* print setup before starting */
     848        pelican_regs_print(priv->regs);
     849        occan_stat_print(&priv->stats);
     850#endif
     851
     852        /* core already in reset mode,
     853         *  € Exit reset mode
     854         *  € Enter Single/Dual mode filtering.
     855         */
     856        WRITE_REG(priv, &priv->regs->mode, (priv->single_mode << 3));
     857
     858        /* Register interrupt routine and unmask IRQ at IRQ controller */
     859        drvmgr_interrupt_register(priv->dev, 0, "occan", occan_interrupt, priv);
     860
     861        return 0;
     862}
     863
     864static void pelican_stop(occan_priv *priv)
     865{
     866        /* stop HW */
     867
     868        drvmgr_interrupt_unregister(priv->dev, 0, occan_interrupt, priv);
     869
     870#ifdef DEBUG
     871        /* print setup before stopping */
     872        pelican_regs_print(priv->regs);
     873        occan_stat_print(&priv->stats);
     874#endif
     875
     876        /* put core in reset mode */
     877        WRITE_REG(priv, &priv->regs->mode, PELICAN_MOD_RESET);
     878
     879        /* turn off interrupts */
     880        WRITE_REG(priv, &priv->regs->inten, 0);
     881
     882        priv->status |= OCCAN_STATUS_RESET;
     883}
     884
     885static inline int pelican_tx_ready(occan_priv *can)
     886{
     887        unsigned char status;
     888        pelican_regs *regs = can->regs;
     889
     890        /* is there room in send buffer? */
     891        status = READ_REG(can, &regs->status);
     892        if ( !(status & PELICAN_STAT_TXBUF) ) {
     893                /* tx fifo taken, we have to wait */
     894                return 0;
     895        }
     896
     897        return 1;
     898}
     899
     900/* Try to send message "msg", if hardware txfifo is
     901 * full, then -1 is returned.
     902 *
     903 * Be sure to have disabled CAN interrupts when
     904 * entering this function.
     905 */
     906static int pelican_send(occan_priv *can, CANMsg *msg){
     907        unsigned char tmp;
     908        pelican_regs *regs = can->regs;
     909
     910        /* is there room in send buffer? */
     911        if ( !pelican_tx_ready(can) ) {
     912                /* tx fifo taken, we have to wait */
     913                return -1;
     914        }
     915
     916        tmp = msg->len & 0xf;
     917        if ( msg->rtr )
     918                tmp |= 0x40;
     919
     920        if ( msg->extended ){
     921                /* Extended Frame */
     922                WRITE_REG(can, &regs->rx_fi_xff, 0x80 | tmp);
     923                WRITE_REG(can, &regs->msg.tx_eff.id[0],(msg->id >> (5+8+8)) & 0xff);
     924                WRITE_REG(can, &regs->msg.tx_eff.id[1],(msg->id >> (5+8)) & 0xff);
     925                WRITE_REG(can, &regs->msg.tx_eff.id[2],(msg->id >> (5)) & 0xff);
     926                WRITE_REG(can, &regs->msg.tx_eff.id[3],(msg->id << 3) & 0xf8);
     927                tmp = msg->len;
     928                while(tmp--){
     929                        WRITE_REG(can, &regs->msg.tx_eff.data[tmp], msg->data[tmp]);
     930                }
     931        }else{
     932                /* Standard Frame */
     933                WRITE_REG(can, &regs->rx_fi_xff, tmp);
     934                WRITE_REG(can, &regs->msg.tx_sff.id[0],(msg->id >> 3) & 0xff);
     935                WRITE_REG(can, &regs->msg.tx_sff.id[1],(msg->id << 5) & 0xe0);
     936                tmp = msg->len;
     937                while(tmp--){
     938                        WRITE_REG(can, &regs->msg.tx_sff.data[tmp],msg->data[tmp]);
     939                }
     940        }
     941
     942        /* let HW know of new message */
     943        if ( msg->sshot ){
     944                WRITE_REG(can, &regs->cmd, PELICAN_CMD_TXREQ | PELICAN_CMD_ABORT);
     945        }else{
     946                /* normal case -- try resend until sent */
     947                WRITE_REG(can, &regs->cmd, PELICAN_CMD_TXREQ);
     948        }
     949
     950        return 0;
     951}
     952
     953
     954static void pelican_set_accept(occan_priv *priv, unsigned char *acode, unsigned char *amask)
     955{
     956        unsigned char *acode0, *acode1, *acode2, *acode3;
     957        unsigned char *amask0, *amask1, *amask2, *amask3;
     958
     959        acode0 = &priv->regs->rx_fi_xff;
     960        acode1 = (unsigned char *)&priv->regs->msg.rst_accept.code[0];
     961        acode2 = (unsigned char *)&priv->regs->msg.rst_accept.code[1];
     962        acode3 = (unsigned char *)&priv->regs->msg.rst_accept.code[2];
     963
     964        amask0 = (unsigned char *)&priv->regs->msg.rst_accept.mask[0];
     965        amask1 = (unsigned char *)&priv->regs->msg.rst_accept.mask[1];
     966        amask2 = (unsigned char *)&priv->regs->msg.rst_accept.mask[2];
     967        amask3 = (unsigned char *)&priv->regs->msg.rst_accept.mask[3];
     968
     969        /* Set new mask & code */
     970        WRITE_REG(priv, acode0, acode[0]);
     971        WRITE_REG(priv, acode1, acode[1]);
     972        WRITE_REG(priv, acode2, acode[2]);
     973        WRITE_REG(priv, acode3, acode[3]);
     974
     975        WRITE_REG(priv, amask0, amask[0]);
     976        WRITE_REG(priv, amask1, amask[1]);
     977        WRITE_REG(priv, amask2, amask[2]);
     978        WRITE_REG(priv, amask3, amask[3]);
     979}
     980
     981
    739982/* This function calculates BTR0 and BTR1 values for a given bitrate.
    740983 *
     
    745988 * \return zero if successful to calculate a baud rate.
    746989 */
    747 static int occan_calc_speedregs(unsigned int clock_hz, unsigned int rate, occan_speed_regs *result){
     990static int occan_calc_speedregs(unsigned int clock_hz, unsigned int rate, occan_speed_regs *result)
     991{
    748992        int best_error = 1000000000;
    749993        int error;
     
    8281072}
    8291073
    830 static int occan_set_speedregs(occan_priv *priv, occan_speed_regs *timing){
     1074static int occan_set_speedregs(occan_priv *priv, occan_speed_regs *timing)
     1075{
    8311076        if ( !timing || !priv || !priv->regs)
    8321077                return -1;
    8331078
    834         priv->regs->bustim0 = timing->btr0;
    835         priv->regs->bustim1 = timing->btr1;
     1079        WRITE_REG(priv, &priv->regs->bustim0, timing->btr0);
     1080        WRITE_REG(priv, &priv->regs->bustim1, timing->btr1);
     1081
    8361082        return 0;
    8371083}
     
    8621108
    8631109                /* Reset core */
    864                 priv->regs->mode = PELICAN_MOD_RESET;
     1110                WRITE_REG(priv, &priv->regs->mode, PELICAN_MOD_RESET);
    8651111
    8661112                /* tell int handler about the auto speed detection test */
     
    8741120
    8751121                /* calc timing params for this */
    876                 if ( occan_calc_speedregs(sys_freq_hz,speed,&timing) ){
     1122                if ( occan_calc_speedregs(priv->sys_freq_hz,speed,&timing) ){
    8771123                        /* failed to get good timings for this frequency
    8781124                         * test with next
     
    8881134                /* Empty previous messages in hardware RX fifo */
    8891135                /*
    890                 while( READ_REG(&priv->regs->) ){
     1136                while( READ_REG(priv, &priv->regs->) ){
    8911137
    8921138                }
     
    8941140
    8951141                /* Clear pending interrupts */
    896                 tmp = READ_REG(&priv->regs->intflags);
     1142                tmp = READ_REG(priv, &priv->regs->intflags);
    8971143
    8981144                /* enable RX & ERR interrupt */
     
    9121158}
    9131159
    914 
    915 static rtems_device_driver occan_initialize(rtems_device_major_number major, rtems_device_minor_number unused, void *arg){
    916         int dev_cnt,minor,subcore_cnt,devi,subi,subcores;
    917         struct ambapp_ahb_info ambadev;
    918         occan_priv *can;
    919         char fs_name[20];
    920         rtems_status_code status;
    921 
    922         strcpy(fs_name,OCCAN_DEVNAME);
    923 
    924         /* find device on amba bus */
    925         dev_cnt = ambapp_get_number_ahbslv_devices(amba_bus, VENDOR_GAISLER,
    926                                                    GAISLER_CANAHB);
    927         if ( dev_cnt < 1 ){
    928                 /* Failed to find any CAN cores! */
    929                 printk("OCCAN: Failed to find any CAN cores\n\r");
    930                 return -1;
    931         }
    932 
    933         /* Detect System Frequency from initialized timer */
    934 #ifndef SYS_FREQ_HZ
    935 #if defined(LEON3)
    936         /* LEON3: find timer address via AMBA Plug&Play info */
    937         {
    938                 struct ambapp_apb_info gptimer;
    939                 struct gptimer_regs *tregs;
    940 
    941                 if ( ambapp_find_apbslv(&ambapp_plb, VENDOR_GAISLER,
    942                                         GAISLER_GPTIMER, &gptimer) == 1 ){
    943                         tregs = (struct gptimer_regs *)gptimer.start;
    944                         sys_freq_hz = (tregs->scaler_reload+1)*1000*1000;
    945                         DBG("OCCAN: detected %dHZ system frequency\n\r",sys_freq_hz);
    946                 }else{
    947                         sys_freq_hz = 40000000; /* Default to 40MHz */
    948                         printk("OCCAN: Failed to detect system frequency\n\r");
    949                 }
    950 
    951         }
    952 #elif defined(LEON2)
    953         /* LEON2: use hardcoded address to get to timer */
    954         {
    955                 LEON_Register_Map *regs = (LEON_Register_Map *)0x80000000;
    956                 sys_freq_hz = (regs->Scaler_Reload+1)*1000*1000;
    957         }
    958 #else
    959   #error CPU not supported for OC_CAN driver
    960 #endif
    961 #else
    962         /* Use hardcoded frequency */
    963         sys_freq_hz = SYS_FREQ_HZ;
    964 #endif
    965 
    966         DBG("OCCAN: Detected %dHz system frequency\n\r",sys_freq_hz);
    967 
    968         /* OCCAN speciality:
    969          *  Mulitple cores are supported through the same amba AHB interface.
    970          *  The number of "sub cores" can be detected by decoding the AMBA
    971          *  Plug&Play version information. verion = ncores. A maximum of 8
    972          *  sub cores are supported, each separeated with 0x100 inbetween.
    973          *
    974          *  Now, lets detect sub cores.
    975          */
    976 
    977         for(subcore_cnt=devi=0; devi<dev_cnt; devi++){
    978                 ambapp_find_ahbslv_next(amba_bus, VENDOR_GAISLER,
    979                                         GAISLER_CANAHB, &ambadev, devi);
    980                 subcore_cnt += (ambadev.ver & 0x7)+1;
    981         }
    982 
    983         printk("OCCAN: Found %d devs, totally %d sub cores\n\r",dev_cnt,subcore_cnt);
    984 
    985         /* allocate memory for cores */
    986         can_cores = subcore_cnt;
    987         cans = calloc(subcore_cnt*sizeof(occan_priv),1);
    988 
    989         minor=0;
    990         for(devi=0; devi<dev_cnt; devi++){
    991 
    992                 /* Get AHB device info */
    993                 ambapp_find_ahbslv_next(amba_bus, VENDOR_GAISLER,
    994                                         GAISLER_CANAHB, &ambadev, devi);
    995                 subcores = (ambadev.ver & 0x7)+1;
    996                 DBG("OCCAN: on dev %d found %d sub cores\n\r",devi,subcores);
    997 
    998                 /* loop all subcores, at least 1 */
    999                 for(subi=0; subi<subcores; subi++){
    1000                         can = &cans[minor];
    1001 
    1002 #ifdef OCCAN_BYTE_REGS
    1003                         /* regs is byte regs */
    1004                         can->regs = (void *)(ambadev.start[0] + OCCAN_NCORE_OFS*subi);
    1005 #else
    1006                         /* regs is word regs, accessed 0x100 from base address */
    1007                         can->regs = (void *)(ambadev.start[0] + OCCAN_NCORE_OFS*subi+ OCCAN_WORD_REG_OFS);
    1008 #endif
    1009 
    1010                         /* remember IRQ number */
    1011                         can->irq = ambadev.irq+subi;
    1012 
    1013                         /* bind filesystem name to device */
    1014                         OCCAN_DEVNAME_NO(fs_name,minor);
    1015                         printk("OCCAN: Registering %s to [%d %d] @ 0x%lx irq %d\n\r",fs_name,major,minor,(unsigned int)can->regs,can->irq);
    1016                         status = rtems_io_register_name(fs_name, major, minor);
    1017                         if (RTEMS_SUCCESSFUL != status )
    1018                                 rtems_fatal_error_occurred(status);
    1019 
    1020                         /* initialize software */
    1021                         can->open = 0;
    1022                         can->rxfifo = NULL;
    1023                         can->txfifo = NULL;
    1024                         status = rtems_semaphore_create(
    1025                         rtems_build_name('C', 'd', 'v', '0'+minor),
    1026                         1,
    1027                         RTEMS_FIFO | RTEMS_SIMPLE_BINARY_SEMAPHORE | RTEMS_NO_INHERIT_PRIORITY | \
    1028                         RTEMS_NO_PRIORITY_CEILING,
    1029                         0,
    1030                         &can->devsem);
    1031                         if ( status != RTEMS_SUCCESSFUL ){
    1032                                 printk("OCCAN: Failed to create dev semaphore for minor %d, (%d)\n\r",minor,status);
    1033                                 return RTEMS_UNSATISFIED;
    1034                         }
    1035                         status = rtems_semaphore_create(
    1036                         rtems_build_name('C', 't', 'x', '0'+minor),
    1037                         0,
    1038                         RTEMS_FIFO | RTEMS_SIMPLE_BINARY_SEMAPHORE | RTEMS_NO_INHERIT_PRIORITY | \
    1039                         RTEMS_NO_PRIORITY_CEILING,
    1040                         0,
    1041                         &can->txsem);
    1042                         if ( status != RTEMS_SUCCESSFUL ){
    1043                                 printk("OCCAN: Failed to create tx semaphore for minor %d, (%d)\n\r",minor,status);
    1044                                 return RTEMS_UNSATISFIED;
    1045                         }
    1046                         status = rtems_semaphore_create(
    1047                         rtems_build_name('C', 'r', 'x', '0'+minor),
    1048                         0,
    1049                         RTEMS_FIFO | RTEMS_SIMPLE_BINARY_SEMAPHORE | RTEMS_NO_INHERIT_PRIORITY | \
    1050                         RTEMS_NO_PRIORITY_CEILING,
    1051                         0,
    1052                         &can->rxsem);
    1053                         if ( status != RTEMS_SUCCESSFUL ){
    1054                                 printk("OCCAN: Failed to create rx semaphore for minor %d, (%d)\n\r",minor,status);
    1055                                 return RTEMS_UNSATISFIED;
    1056                         }
    1057 
    1058                         /* hardware init/reset */
    1059                         pelican_init(can);
    1060 
    1061                         /* Setup interrupt handler for each channel */
    1062         OCCAN_REG_INT(OCCAN_PREFIX(_interrupt_handler), can->irq, can);
    1063 
    1064                         minor++;
    1065 #ifdef DEBUG_PRINT_REGMAP
    1066                         pelican_regadr_print(can->regs);
    1067 #endif
    1068                 }
    1069         }
    1070 
     1160static rtems_device_driver occan_initialize(rtems_device_major_number major, rtems_device_minor_number unused, void *arg)
     1161{
    10711162        return RTEMS_SUCCESSFUL;
    10721163}
     
    10741165static rtems_device_driver occan_open(rtems_device_major_number major, rtems_device_minor_number minor, void *arg){
    10751166        occan_priv *can;
     1167        struct drvmgr_dev *dev;
    10761168
    10771169        DBG("OCCAN: Opening %d\n\r",minor);
    10781170
    1079         if ( minor >= can_cores )
     1171        /* get can device */   
     1172        if ( drvmgr_get_dev(&occan_drv_info.general, minor, &dev) ) {
     1173                DBG("Wrong minor %d\n", minor);
    10801174                return RTEMS_UNSATISFIED; /* NODEV */
    1081 
    1082         /* get can device */
    1083         can = &cans[minor];
     1175        }
     1176        can = (occan_priv *)dev->priv;
    10841177
    10851178        /* already opened? */
     
    11271220}
    11281221
    1129 static rtems_device_driver occan_close(rtems_device_major_number major, rtems_device_minor_number minor, void *arg){
    1130         occan_priv *can = &cans[minor];
     1222static rtems_device_driver occan_close(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
     1223{
     1224        occan_priv *can;
     1225        struct drvmgr_dev *dev;
    11311226
    11321227        DBG("OCCAN: Closing %d\n\r",minor);
     1228
     1229        if ( drvmgr_get_dev(&occan_drv_info.general, minor, &dev) ) {
     1230                return RTEMS_INVALID_NAME;
     1231        }
     1232        can = (occan_priv *)dev->priv;
    11331233
    11341234        /* stop if running */
     
    11371237
    11381238        /* Enter Reset Mode */
    1139         can->regs->mode = PELICAN_MOD_RESET;
     1239        WRITE_REG(can, &can->regs->mode, PELICAN_MOD_RESET);
    11401240
    11411241        /* free fifo memory */
     
    11461246        can->txfifo = NULL;
    11471247
     1248        can->open = 0;
     1249
    11481250        return RTEMS_SUCCESSFUL;
    11491251}
    11501252
    11511253static rtems_device_driver occan_read(rtems_device_major_number major, rtems_device_minor_number minor, void *arg){
    1152         occan_priv *can = &cans[minor];
     1254        occan_priv *can;
     1255        struct drvmgr_dev *dev;
    11531256        rtems_libio_rw_args_t *rw_args=(rtems_libio_rw_args_t *) arg;
    11541257        CANMsg *dstmsg, *srcmsg;
    11551258        rtems_interrupt_level oldLevel;
    11561259        int left;
     1260
     1261        if ( drvmgr_get_dev(&occan_drv_info.general, minor, &dev) ) {
     1262                return RTEMS_INVALID_NAME;
     1263        }
     1264        can = (occan_priv *)dev->priv;
    11571265
    11581266        if ( !can->started ){
     
    12061314                        DBG("OCCAN: Waiting for RX int\n\r");
    12071315
    1208                         /* wait for incomming messages */
    1209                         rtems_semaphore_obtain(can->rxsem,RTEMS_WAIT,RTEMS_NO_TIMEOUT);
     1316                        /* wait for incoming messages */
     1317                        rtems_semaphore_obtain(can->rxsem, RTEMS_WAIT,
     1318                                RTEMS_NO_TIMEOUT);
    12101319
    12111320                        /* did we get woken up by a BUS OFF error? */
     
    12451354
    12461355static rtems_device_driver occan_write(rtems_device_major_number major, rtems_device_minor_number minor, void *arg){
    1247         occan_priv *can = &cans[minor];
     1356        occan_priv *can;
     1357        struct drvmgr_dev *dev;
    12481358        rtems_libio_rw_args_t *rw_args=(rtems_libio_rw_args_t *) arg;
    12491359        CANMsg *msg,*fifo_msg;
     
    12521362
    12531363        DBG("OCCAN: Writing %d bytes from 0x%lx (%d)\n\r",rw_args->count,rw_args->buffer,sizeof(CANMsg));
     1364
     1365        if ( drvmgr_get_dev(&occan_drv_info.general, minor, &dev) ) {
     1366                return RTEMS_INVALID_NAME;
     1367        }
     1368        can = (occan_priv *)dev->priv;
    12541369
    12551370        if ( !can->started )
     
    12941409                        msg++;
    12951410
     1411#ifdef OCCAN_TX_IRQ_FLAG_FIXUP
     1412                        /* Mark that we have put at least one msg in TX FIFO */
     1413                        can->sending = 1;
     1414#endif
     1415
    12961416                        /* bump stat counters */
    12971417                        can->stats.tx_msgs++;