grlib/spw/grspw.c

0001 /* SPDX-License-Identifier: BSD-2-Clause */
0002
0003 /*
0004  *  This file contains the GRSPW SpaceWire Driver for LEON2 and LEON3.
0005  *
0006  *  COPYRIGHT (c) 2006
0007  *  Cobham Gaisler AB.
0008  *
0009  * Redistribution and use in source and binary forms, with or without
0010  * modification, are permitted provided that the following conditions
0011  * are met:
0012  * 1. Redistributions of source code must retain the above copyright
0013  *    notice, this list of conditions and the following disclaimer.
0014  * 2. Redistributions in binary form must reproduce the above copyright
0015  *    notice, this list of conditions and the following disclaimer in the
0016  *    documentation and/or other materials provided with the distribution.
0017  *
0018  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
0019  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
0020  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
0021  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
0022  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
0023  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
0024  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
0025  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
0026  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
0027  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
0028  * POSSIBILITY OF SUCH DAMAGE.
0029  */
0030
0031 #include <bsp.h>
0032 #include <rtems/libio.h>
0033 #include <stdlib.h>
0034 #include <stdio.h>
0035 #include <string.h>
0036 #include <assert.h>
0037 #include <ctype.h>
0038 #include <rtems/bspIo.h>
0039 #include <grlib/ambapp.h>
0040
0041 #include <drvmgr/drvmgr.h>
0042 #include <grlib/ambapp_bus.h>
0043 #include <grlib/grspw.h>
0044
0045 #include <grlib/grlib_impl.h>
0046
0047 #define DBGSPW_IOCALLS 1
0048 #define DBGSPW_TX 2
0049 #define DBGSPW_RX 4
0050 #define DBGSPW_IOCTRL 1
0051 #define DBGSPW_DUMP 16
0052 #define DEBUG_SPACEWIRE_FLAGS (DBGSPW_IOCALLS | DBGSPW_TX | DBGSPW_RX )
0053
0054 /* #define DEBUG_SPACEWIRE_ONOFF */
0055
0056 #ifdef DEBUG_SPACEWIRE_ONOFF
0057 #define SPACEWIRE_DBG(fmt, args...)    do { { printk(" : %03d @ %18s()]:" fmt , __LINE__,__FUNCTION__,## args); }} while(0)
0058 #define SPACEWIRE_DBG2(fmt)            do { { printk(" : %03d @ %18s()]:" fmt , __LINE__,__FUNCTION__); }} while(0)
0059 #define SPACEWIRE_DBGC(c,fmt, args...) do { if (DEBUG_SPACEWIRE_FLAGS & c) { printk(" : %03d @ %18s()]:" fmt , __LINE__,__FUNCTION__,## args);  }} while(0)
0060 #else
0061 #define SPACEWIRE_DBG(fmt, args...)
0062 #define SPACEWIRE_DBG2(fmt, args...)
0063 #define SPACEWIRE_DBGC(c, fmt, args...)
0064 #endif
0065
0066 typedef struct {
0067    volatile unsigned int ctrl;
0068    volatile unsigned int status;
0069    volatile unsigned int nodeaddr;
0070    volatile unsigned int clkdiv;
0071    volatile unsigned int destkey;
0072    volatile unsigned int time;
0073    volatile unsigned int timer;
0074    volatile unsigned int pad;
0075
0076    volatile unsigned int dma0ctrl;
0077    volatile unsigned int dma0rxmax;
0078    volatile unsigned int dma0txdesc;
0079    volatile unsigned int dma0rxdesc;
0080
0081    /* For GRSPW core 2 and onwards */
0082    volatile unsigned int dma0addr;
0083
0084 } LEON3_SPACEWIRE_Regs_Map;
0085
0086 typedef struct {
0087    volatile unsigned int ctrl;
0088    volatile unsigned int addr;
0089 } SPACEWIRE_RXBD;
0090
0091 typedef struct {
0092    volatile unsigned int ctrl;
0093    volatile unsigned int addr_header;
0094    volatile unsigned int len;
0095    volatile unsigned int addr_data;
0096 } SPACEWIRE_TXBD;
0097
0098 #define SPACEWIRE_INIT_TIMEOUT 10
0099 #define SPACEWIRE_BDTABLE_SIZE 0x400
0100 #define SPACEWIRE_TXD_SIZE 1024
0101 #define SPACEWIRE_TXH_SIZE 64
0102 #define SPACEWIRE_RXPCK_SIZE 1024
0103 #define SPACEWIRE_TXBUFS_NR 64
0104 #define SPACEWIRE_RXBUFS_NR 128
0105
0106 #define BUFMEM_PER_LINK (SPACEWIRE_TXBUFS_NR*(SPACEWIRE_TXD_SIZE+SPACEWIRE_TXH_SIZE) + SPACEWIRE_RXBUFS_NR*SPACEWIRE_RXPCK_SIZE)
0107
0108 typedef struct {
0109    /* configuration parameters */
0110    struct drvmgr_dev *dev; /* Driver manager device */
0111    char devName[49]; /* Device Name */
0112    LEON3_SPACEWIRE_Regs_Map *regs;
0113    spw_config config;
0114
0115    unsigned int tx_all_in_use;
0116    unsigned int tx_sent;
0117    unsigned int tx_cur;
0118    unsigned int rxcur;
0119    unsigned int rxbufcur;
0120    unsigned int txdbufsize;
0121    unsigned int txhbufsize;
0122    unsigned int rxbufsize;
0123    unsigned int txbufcnt;
0124    unsigned int rxbufcnt;
0125
0126    /* DMA Area set by user */
0127    unsigned int rx_dma_area;
0128    unsigned int tx_data_dma_area;
0129    unsigned int tx_hdr_dma_area;
0130    unsigned int bd_dma_area;
0131
0132    /* statistics */
0133    spw_stats stat;
0134
0135    unsigned int _ptr_rxbuf0;
0136    char *ptr_rxbuf0;
0137    char *ptr_txdbuf0;
0138    char *ptr_txhbuf0;
0139    char *_ptr_bd0, *ptr_bd0;
0140
0141    char *ptr_rxbuf0_remote;
0142    char *ptr_txdbuf0_remote;
0143    char *ptr_txhbuf0_remote;
0144    char *ptr_bd0_remote;
0145
0146    unsigned int irq;
0147    int minor;
0148    int core_ver;
0149    int open;
0150    int running;
0151    unsigned int core_freq_khz;
0152    unsigned int rtimeout;
0153
0154    /* semaphores*/
0155    rtems_id txsp;
0156    rtems_id rxsp;
0157
0158    SPACEWIRE_RXBD *rx;
0159    SPACEWIRE_TXBD *tx;
0160
0161    unsigned int rx_remote;
0162    unsigned int tx_remote;
0163 } GRSPW_DEV;
0164
0165 /* Function pointer called upon timecode receive */
0166 void (*grspw_timecode_callback)
0167     (void *pDev, void *regs, int minor, unsigned int tc) = NULL;
0168
0169 #ifdef GRSPW_DONT_BYPASS_CACHE
0170 #define _SPW_READ(address) (*(volatile unsigned int *)(address))
0171 #define _MEM_READ8(address) (*(volatile unsigned char *)(address))
0172 #define _MEM_READ32(address) (*(volatile unsigned int *)(address))
0173 #else
0174 #define _SPW_READ(address) grlib_read_uncached32((unsigned int) address)
0175 #define _MEM_READ8(address) grlib_read_uncached8((unsigned int) address)
0176 #define _MEM_READ32(address) grlib_read_uncached32((unsigned int) address)
0177 #endif
0178
0179 #define MEM_READ8(addr) _MEM_READ8((volatile void *)(addr))
0180 #define MEM_READ32(addr) _MEM_READ32((volatile void *)(addr))
0181 #define SPW_READ(addr) _SPW_READ((volatile void *)(addr))
0182 #define SPW_WRITE(addr,v) (*(volatile unsigned int *)addr)=v
0183
0184 #define SPW_REG(c,r) (c->regs->r)
0185 #define SPW_REG_CTRL(c) SPW_REG(c,ctrl)
0186 #define SPW_REG_STATUS(c) SPW_REG(c,status)
0187 #define SPW_REG_NODEADDR(c) SPW_REG(c,nodeaddr)
0188
0189 #define SPW_CTRL_READ(c)      SPW_READ(&SPW_REG_CTRL(c))
0190 #define SPW_CTRL_WRITE(c,v)   SPW_WRITE(&SPW_REG_CTRL(c),v)
0191 #define SPW_STATUS_READ(c)    SPW_READ(&SPW_REG_STATUS(c))
0192 #define SPW_STATUS_WRITE(c,v) SPW_WRITE(&SPW_REG_STATUS(c),v)
0193
0194 #define SPW_LINKSTATE(c) (((c) >> 21) & 0x7)
0195
0196 #define SPACEWIRE_RXNR(c) ((c&~(SPACEWIRE_BDTABLE_SIZE-1))>>3)
0197 #define SPACEWIRE_TXNR(c) ((c&~(SPACEWIRE_BDTABLE_SIZE-1))>>4)
0198
0199 #define SPW_RXBD_LENGTH 0x1ffffff
0200 #define SPW_RXBD_EN (1 << 25)
0201 #define SPW_RXBD_WR (1 << 26)
0202 #define SPW_RXBD_IE (1 << 27)
0203
0204 #define SPW_RXBD_EEP (1 << 28)
0205 #define SPW_RXBD_EHC (1 << 29)
0206 #define SPW_RXBD_EDC (1 << 30)
0207 #define SPW_RXBD_ETR (1 << 31)
0208
0209 #define SPW_RXBD_ERROR (SPW_RXBD_EEP | \
0210                         SPW_RXBD_ETR)
0211
0212 #define SPW_RXBD_RMAPERROR (SPW_RXBD_EHC | SPW_RXBD_EDC)
0213
0214 #define SPW_TXBD_LENGTH 0xffffff
0215
0216 #define SPW_TXBD_EN (1 << 12)
0217 #define SPW_TXBD_WR (1 << 13)
0218 #define SPW_TXBD_IE (1 << 14)
0219 #define SPW_TXBD_LE (1 << 15)
0220 #define SPW_TXBD_HC (1 << 16)
0221 #define SPW_TXBD_DC (1 << 17)
0222
0223 #define SPW_TXBD_ERROR (SPW_TXBD_LE)
0224
0225 #define SPW_CTRL_LINKDISABLED (1 << 0)
0226 #define SPW_CTRL_LINKSTART    (1 << 1)
0227 #define SPW_CTRL_AUTOSTART    (1 << 2)
0228 #define SPW_CTRL_IE           (1 << 3)
0229 #define SPW_CTRL_TI           (1 << 4)
0230 #define SPW_CTRL_PM           (1 << 5)
0231 #define SPW_CTRL_RESET        (1 << 6)
0232 #define SPW_CTRL_TQ           (1 << 8)
0233 #define SPW_CTRL_LI           (1 << 9)
0234 #define SPW_CTRL_TT           (1 << 10)
0235 #define SPW_CTRL_TR           (1 << 11)
0236 #define SPW_CTRL_RE           (1 << 16)
0237 #define SPW_CTRL_RD           (1 << 17)
0238
0239 #define SPW_CTRL_RC           (1 << 29)
0240 #define SPW_CTRL_RX           (1 << 30)
0241 #define SPW_CTRL_RA           (1 << 31)
0242
0243 #define SPW_STATUS_TO (1 << 0)
0244 #define SPW_STATUS_CE (1 << 1)
0245 #define SPW_STATUS_ER (1 << 2)
0246 #define SPW_STATUS_DE (1 << 3)
0247 #define SPW_STATUS_PE (1 << 4)
0248 #define SPW_STATUS_WE (1 << 6)
0249 #define SPW_STATUS_IA (1 << 7)
0250 #define SPW_STATUS_EE (1 << 8)
0251
0252 #define SPW_DMACTRL_TXEN (1 << 0)
0253 #define SPW_DMACTRL_RXEN (1 << 1)
0254 #define SPW_DMACTRL_TXIE (1 << 2)
0255 #define SPW_DMACTRL_RXIE (1 << 3)
0256 #define SPW_DMACTRL_AI   (1 << 4)
0257 #define SPW_DMACTRL_PS   (1 << 5)
0258 #define SPW_DMACTRL_PR   (1 << 6)
0259 #define SPW_DMACTRL_TA   (1 << 7)
0260 #define SPW_DMACTRL_RA   (1 << 8)
0261 #define SPW_DMACTRL_AT   (1 << 9)
0262 #define SPW_DMACTRL_RX   (1 << 10)
0263 #define SPW_DMACTRL_RD   (1 << 11)
0264 #define SPW_DMACTRL_NS   (1 << 12)
0265
0266 #define SPW_PREPAREMASK_TX (SPW_DMACTRL_RXEN | SPW_DMACTRL_RXIE | SPW_DMACTRL_PS | SPW_DMACTRL_TA | SPW_DMACTRL_RD | SPW_DMACTRL_NS)
0267 #define SPW_PREPAREMASK_RX (SPW_DMACTRL_TXEN | SPW_DMACTRL_TXIE | SPW_DMACTRL_AI | SPW_DMACTRL_PR | SPW_DMACTRL_RA)
0268
0269 static int grspw_hw_init(GRSPW_DEV *pDev);
0270 static int grspw_hw_send(GRSPW_DEV *pDev, unsigned int hlen, char *hdr, unsigned int dlen, char *data, unsigned int options);
0271 static int grspw_hw_receive(GRSPW_DEV *pDev,char *b,int c);
0272 static int grspw_hw_startup (GRSPW_DEV *pDev, int timeout);
0273 static int grspw_hw_stop (GRSPW_DEV *pDev, int rx, int tx);
0274 static void grspw_hw_wait_rx_inactive(GRSPW_DEV *pDev);
0275 static int grspw_hw_waitlink (GRSPW_DEV *pDev, int timeout);
0276 static void grspw_hw_reset(GRSPW_DEV *pDev);
0277 static void grspw_hw_read_config(GRSPW_DEV *pDev);
0278
0279 static void check_rx_errors(GRSPW_DEV *pDev, int ctrl);
0280 static void grspw_rxnext(GRSPW_DEV *pDev);
0281 static void grspw_interrupt(void *arg);
0282 static int grspw_buffer_alloc(GRSPW_DEV *pDev);
0283 static int grspw_dmatables_alloc(GRSPW_DEV *pDev);
0284
0285 static rtems_device_driver grspw_initialize(
0286         rtems_device_major_number  major,
0287         rtems_device_minor_number  minor,
0288         void                    * arg
0289         );
0290
0291 static rtems_device_driver grspw_open(
0292         rtems_device_major_number major,
0293         rtems_device_minor_number minor,
0294         void                    * arg
0295         );
0296
0297 static rtems_device_driver grspw_close(
0298         rtems_device_major_number major,
0299         rtems_device_minor_number minor,
0300         void                    * arg
0301         );
0302
0303 static rtems_device_driver grspw_read(
0304         rtems_device_major_number major,
0305         rtems_device_minor_number minor,
0306         void                    * arg
0307         );
0308
0309 static rtems_device_driver grspw_write(
0310         rtems_device_major_number major,
0311         rtems_device_minor_number minor,
0312         void                    * arg
0313         );
0314
0315 static rtems_device_driver grspw_control(
0316         rtems_device_major_number major,
0317         rtems_device_minor_number minor,
0318         void                    * arg
0319         );
0320
0321 #define GRSPW_DRIVER_TABLE_ENTRY \
0322   { grspw_initialize, \
0323     grspw_open, \
0324     grspw_close, \
0325     grspw_read, \
0326     grspw_write, \
0327     grspw_control }
0328
0329 static rtems_driver_address_table grspw_driver = GRSPW_DRIVER_TABLE_ENTRY;
0330 static int grspw_driver_io_registered = 0;
0331 static rtems_device_major_number grspw_driver_io_major = 0;
0332
0333 /******************* Driver manager interface ***********************/
0334
0335 /* Driver prototypes */
0336 int grspw_register_io(rtems_device_major_number *m);
0337 int grspw_device_init(GRSPW_DEV *pDev);
0338
0339 int grspw_init2(struct drvmgr_dev *dev);
0340 int grspw_init3(struct drvmgr_dev *dev);
0341
0342 struct drvmgr_drv_ops grspw_ops =
0343 {
0344     .init = {NULL,  grspw_init2, grspw_init3, NULL},
0345     .remove = NULL,
0346     .info = NULL
0347 };
0348
0349 struct amba_dev_id grspw_ids[] =
0350 {
0351     {VENDOR_GAISLER, GAISLER_SPW},
0352     {VENDOR_GAISLER, GAISLER_SPW2},
0353     {VENDOR_GAISLER, GAISLER_SPW2_DMA},
0354     {0, 0}      /* Mark end of table */
0355 };
0356
0357 struct amba_drv_info grspw_drv_info =
0358 {
0359     {
0360         DRVMGR_OBJ_DRV,         /* Driver */
0361         NULL,               /* Next driver */
0362         NULL,               /* Device list */
0363         DRIVER_AMBAPP_GAISLER_GRSPW_ID, /* Driver ID */
0364         "GRSPW_DRV",            /* Driver Name */
0365         DRVMGR_BUS_TYPE_AMBAPP,     /* Bus Type */
0366         &grspw_ops,
0367         NULL,               /* Funcs */
0368         0,              /* No devices yet */
0369         0,
0370     },
0371     &grspw_ids[0]
0372 };
0373
0374 void grspw_register_drv (void)
0375 {
0376     SPACEWIRE_DBG("Registering GRSPW driver\n");
0377     drvmgr_drv_register(&grspw_drv_info.general);
0378 }
0379
0380 int grspw_init2(struct drvmgr_dev *dev)
0381 {
0382     GRSPW_DEV *priv;
0383
0384     SPACEWIRE_DBG("GRSPW[%d] on bus %s\n", dev->minor_drv,
0385         dev->parent->dev->name);
0386     priv = dev->priv = grlib_calloc(1, sizeof(*priv));
0387     if ( !priv )
0388         return DRVMGR_NOMEM;
0389     priv->dev = dev;
0390
0391     /* This core will not find other cores, so we wait for init2() */
0392
0393     return DRVMGR_OK;
0394 }
0395
0396 int grspw_init3(struct drvmgr_dev *dev)
0397 {
0398     GRSPW_DEV *priv;
0399     char prefix[32];
0400     rtems_status_code status;
0401
0402     priv = dev->priv;
0403
0404     /* Do initialization */
0405
0406     if ( grspw_driver_io_registered == 0) {
0407         /* Register the I/O driver only once for all cores */
0408         if ( grspw_register_io(&grspw_driver_io_major) ) {
0409             /* Failed to register I/O driver */
0410             free(dev->priv);
0411             dev->priv = NULL;
0412             return DRVMGR_FAIL;
0413         }
0414
0415         grspw_driver_io_registered = 1;
0416     }
0417
0418     /* I/O system registered and initialized
0419      * Now we take care of device initialization.
0420      */
0421
0422     /* Get frequency in Hz */
0423     if ( drvmgr_freq_get(dev, DEV_APB_SLV, &priv->core_freq_khz) ) {
0424         return DRVMGR_FAIL;
0425     }
0426     /* Convert from Hz -> kHz */
0427     priv->core_freq_khz = priv->core_freq_khz / 1000;
0428
0429     if ( grspw_device_init(priv) ) {
0430         return DRVMGR_FAIL;
0431     }
0432
0433     /* Get Filesystem name prefix */
0434     prefix[0] = '\0';
0435     if ( drvmgr_get_dev_prefix(dev, prefix) ) {
0436         /* Failed to get prefix, make sure of a unique FS name
0437          * by using the driver minor.
0438          */
0439         sprintf(priv->devName, "/dev/grspw%d", dev->minor_drv);
0440     } else {
0441         /* Got special prefix, this means we have a bus prefix
0442          * And we should use our "bus minor"
0443          */
0444         sprintf(priv->devName, "/dev/%sgrspw%d", prefix, dev->minor_bus);
0445     }
0446
0447     /* Register Device */
0448     status = rtems_io_register_name(priv->devName, grspw_driver_io_major, dev->minor_drv);
0449     if (status != RTEMS_SUCCESSFUL) {
0450         return DRVMGR_FAIL;
0451     }
0452
0453     return DRVMGR_OK;
0454 }
0455
0456 /******************* Driver Implementation ***********************/
0457
0458 int grspw_register_io(rtems_device_major_number *m)
0459 {
0460     rtems_status_code r;
0461
0462     if ((r = rtems_io_register_driver(0, &grspw_driver, m)) == RTEMS_SUCCESSFUL) {
0463         SPACEWIRE_DBG("GRSPW driver successfully registered, major: %d\n", *m);
0464     } else {
0465         switch(r) {
0466         case RTEMS_TOO_MANY:
0467             printk("GRSPW rtems_io_register_driver failed: RTEMS_TOO_MANY\n");
0468             return -1;
0469         case RTEMS_INVALID_NUMBER:
0470             printk("GRSPW rtems_io_register_driver failed: RTEMS_INVALID_NUMBER\n");
0471             return -1;
0472         case RTEMS_RESOURCE_IN_USE:
0473             printk("GRSPW rtems_io_register_driver failed: RTEMS_RESOURCE_IN_USE\n");
0474             return -1;
0475         default:
0476             printk("GRSPW rtems_io_register_driver failed\n");
0477             return -1;
0478         }
0479     }
0480     return 0;
0481 }
0482
0483 int grspw_device_init(GRSPW_DEV *pDev)
0484 {
0485     struct amba_dev_info *ambadev;
0486     struct ambapp_core *pnpinfo;
0487     union drvmgr_key_value *value;
0488         rtems_status_code status;
0489
0490     /* Get device information from AMBA PnP information */
0491     ambadev = (struct amba_dev_info *)pDev->dev->businfo;
0492     if ( ambadev == NULL ) {
0493         return -1;
0494     }
0495     pnpinfo = &ambadev->info;
0496     pDev->irq = pnpinfo->irq;
0497     pDev->regs = (LEON3_SPACEWIRE_Regs_Map *)pnpinfo->apb_slv->start;
0498     pDev->minor = pDev->dev->minor_drv;
0499
0500     /* Get SpaceWire core version */
0501     switch( pnpinfo->device ) {
0502         case GAISLER_SPW:
0503             pDev->core_ver = 1;
0504             break;
0505         case GAISLER_SPW2:
0506             pDev->core_ver = 2;
0507             break;
0508         case GAISLER_SPW2_DMA:
0509             pDev->core_ver = 3;
0510             break;
0511         default:
0512             return -1;
0513     }
0514
0515     /* initialize the code with some resonable values,
0516      * actual initialization is done later using ioctl(fd)
0517      * on the opened device */
0518     pDev->config.rxmaxlen = SPACEWIRE_RXPCK_SIZE;
0519     pDev->txdbufsize = SPACEWIRE_TXD_SIZE;
0520     pDev->txhbufsize = SPACEWIRE_TXH_SIZE;
0521     pDev->rxbufsize = SPACEWIRE_RXPCK_SIZE;
0522     pDev->txbufcnt = SPACEWIRE_TXBUFS_NR;
0523     pDev->rxbufcnt = SPACEWIRE_RXBUFS_NR;
0524
0525     pDev->_ptr_rxbuf0 = 0;
0526     pDev->ptr_rxbuf0 = 0;
0527     pDev->ptr_txdbuf0 = 0;
0528     pDev->ptr_txhbuf0 = 0;
0529     pDev->ptr_bd0 = 0;
0530     pDev->rx_dma_area = 0;
0531     pDev->tx_data_dma_area = 0;
0532     pDev->tx_hdr_dma_area = 0;
0533     pDev->bd_dma_area = 0;
0534
0535     /* Get Configuration from Bus resources (Let user override defaults) */
0536
0537     value = drvmgr_dev_key_get(pDev->dev, "txBdCnt", DRVMGR_KT_INT);
0538     if ( value )
0539         pDev->txbufcnt = value->i;
0540
0541     value = drvmgr_dev_key_get(pDev->dev, "rxBdCnt", DRVMGR_KT_INT);
0542     if ( value )
0543         pDev->rxbufcnt = value->i;
0544
0545     value = drvmgr_dev_key_get(pDev->dev, "txDataSize", DRVMGR_KT_INT);
0546     if ( value )
0547         pDev->txdbufsize = value->i;
0548
0549     value = drvmgr_dev_key_get(pDev->dev, "txHdrSize", DRVMGR_KT_INT);
0550     if ( value )
0551         pDev->txhbufsize = value->i;
0552
0553     value = drvmgr_dev_key_get(pDev->dev, "rxPktSize", DRVMGR_KT_INT);
0554     if ( value ) {
0555         pDev->rxbufsize = value->i;
0556         pDev->config.rxmaxlen = pDev->rxbufsize;
0557     }
0558
0559     value = drvmgr_dev_key_get(pDev->dev, "rxDmaArea", DRVMGR_KT_INT);
0560     if ( value )
0561         pDev->rx_dma_area = value->i;
0562
0563     value = drvmgr_dev_key_get(pDev->dev, "txDataDmaArea", DRVMGR_KT_INT);
0564     if ( value )
0565         pDev->tx_data_dma_area = value->i;
0566
0567     value = drvmgr_dev_key_get(pDev->dev, "txHdrDmaArea", DRVMGR_KT_INT);
0568     if ( value )
0569         pDev->tx_hdr_dma_area = value->i;
0570
0571     value = drvmgr_dev_key_get(pDev->dev, "bdDmaArea", DRVMGR_KT_INT);
0572     if ( value )
0573         pDev->bd_dma_area = value->i;
0574
0575     if (grspw_buffer_alloc(pDev))
0576         return RTEMS_NO_MEMORY;
0577     if (grspw_dmatables_alloc(pDev))
0578         return RTEMS_NO_MEMORY;
0579
0580     /* Create semaphores */
0581     status = rtems_semaphore_create(
0582         rtems_build_name('T', 'x', 'S', '0' + pDev->minor),
0583         0,
0584         RTEMS_FIFO | RTEMS_SIMPLE_BINARY_SEMAPHORE | RTEMS_NO_INHERIT_PRIORITY | \
0585         RTEMS_NO_PRIORITY_CEILING,
0586         0,
0587         &(pDev->txsp));
0588         _Assert_Unused_variable_equals(status, RTEMS_SUCCESSFUL);
0589
0590     status = rtems_semaphore_create(
0591         rtems_build_name('R', 'x', 'S', '0' + pDev->minor),
0592         0,
0593         RTEMS_FIFO | RTEMS_SIMPLE_BINARY_SEMAPHORE | RTEMS_NO_INHERIT_PRIORITY | \
0594         RTEMS_NO_PRIORITY_CEILING,
0595         0,
0596         &(pDev->rxsp));
0597         _Assert_Unused_variable_equals(status, RTEMS_SUCCESSFUL);
0598
0599     grspw_hw_init(pDev);
0600
0601     return 0;
0602 }
0603
0604 /* Get a value at least 6.4us in number of clock cycles */
0605 static unsigned int grspw_calc_timer64(int freq_khz){
0606     unsigned int timer64 = (freq_khz*64+9999)/10000;
0607     return timer64 & 0xfff;
0608 }
0609
0610 /* Get a value at least 850ns in number of clock cycles - 3 */
0611 static unsigned int grspw_calc_disconnect(int freq_khz){
0612     unsigned int disconnect = ((freq_khz*85+99999)/100000) - 3;
0613     return disconnect & 0x3ff;
0614 }
0615
0616 static int grspw_buffer_alloc(GRSPW_DEV *pDev)
0617 {
0618     /* RX DMA AREA */
0619     if (pDev->rx_dma_area & 1) {
0620         /* Address given in remote address */
0621         pDev->ptr_rxbuf0_remote = (char *)(pDev->rx_dma_area & ~1);
0622         drvmgr_translate_check(
0623             pDev->dev,
0624             DMAMEM_TO_CPU,
0625             (void *)pDev->ptr_rxbuf0_remote,
0626             (void **)&pDev->ptr_rxbuf0,
0627             pDev->rxbufsize * pDev->rxbufcnt);
0628
0629     } else {
0630         if (pDev->rx_dma_area == 0) {
0631             if (pDev->_ptr_rxbuf0)
0632                 free((void *)pDev->_ptr_rxbuf0);
0633             pDev->_ptr_rxbuf0 = (unsigned int) grlib_malloc(
0634                 pDev->rxbufsize * pDev->rxbufcnt+4);
0635             pDev->ptr_rxbuf0 = (char *)((pDev->_ptr_rxbuf0+7)&~7);
0636             if ( !pDev->ptr_rxbuf0 )
0637                 return 1;
0638         } else {
0639             pDev->ptr_rxbuf0 = (char *)pDev->rx_dma_area;
0640         }
0641         drvmgr_translate_check(
0642             pDev->dev,
0643             CPUMEM_TO_DMA,
0644             (void *)pDev->ptr_rxbuf0,
0645             (void **)&pDev->ptr_rxbuf0_remote,
0646             pDev->rxbufsize * pDev->rxbufcnt);
0647     }
0648
0649     /* TX-DATA DMA AREA */
0650     if (pDev->tx_data_dma_area & 1) {
0651         /* Address given in remote address */
0652         pDev->ptr_txdbuf0_remote = (char*)(pDev->tx_data_dma_area & ~1);
0653         drvmgr_translate_check(
0654             pDev->dev,
0655             DMAMEM_TO_CPU,
0656             (void *)pDev->ptr_txdbuf0_remote,
0657             (void **)&pDev->ptr_txdbuf0,
0658             pDev->txdbufsize * pDev->txbufcnt);
0659     } else {
0660         if (pDev->tx_data_dma_area == 0) {
0661             if (pDev->ptr_txdbuf0)
0662                 free(pDev->ptr_txdbuf0);
0663             pDev->ptr_txdbuf0 = (char *) grlib_malloc(
0664                 pDev->txdbufsize * pDev->txbufcnt);
0665             if (!pDev->ptr_txdbuf0)
0666                 return 1;
0667         } else {
0668             pDev->ptr_txdbuf0 = (char *)pDev->tx_data_dma_area;
0669         }
0670         drvmgr_translate_check(
0671             pDev->dev,
0672             CPUMEM_TO_DMA,
0673             (void *)pDev->ptr_txdbuf0,
0674             (void **)&pDev->ptr_txdbuf0_remote,
0675             pDev->txdbufsize * pDev->txbufcnt);
0676     }
0677
0678     /* TX-HEADER DMA AREA */
0679     if (pDev->tx_hdr_dma_area & 1) {
0680         /* Address given in remote address */
0681         pDev->ptr_txhbuf0_remote = (char *)(pDev->tx_hdr_dma_area & ~1);
0682         drvmgr_translate_check(
0683             pDev->dev,
0684             DMAMEM_TO_CPU,
0685             (void *)pDev->ptr_txhbuf0_remote,
0686             (void **)&pDev->ptr_txhbuf0,
0687             pDev->txhbufsize * pDev->txbufcnt);
0688     } else {
0689         if (pDev->tx_hdr_dma_area == 0) {
0690             if (pDev->ptr_txhbuf0)
0691                 free(pDev->ptr_txhbuf0);
0692             pDev->ptr_txhbuf0 = (char *) grlib_malloc(
0693                 pDev->txhbufsize * pDev->txbufcnt);
0694             if (!pDev->ptr_txhbuf0)
0695                 return 1;
0696         } else {
0697             pDev->ptr_txhbuf0 = (char *)pDev->tx_hdr_dma_area;
0698         }
0699         drvmgr_translate_check(
0700             pDev->dev,
0701             CPUMEM_TO_DMA,
0702             (void *)pDev->ptr_txhbuf0,
0703             (void **)&pDev->ptr_txhbuf0_remote,
0704             pDev->txhbufsize * pDev->txbufcnt);
0705     }
0706     return 0;
0707 }
0708
0709 static int grspw_dmatables_alloc(GRSPW_DEV *pDev)
0710 {
0711     /* DMA DESCRIPTOR TABLES */
0712     if (pDev->bd_dma_area & 1) {
0713         /* Address given in remote address */
0714         pDev->ptr_bd0_remote = (char *)(pDev->bd_dma_area & ~1);
0715         drvmgr_translate_check(
0716             pDev->dev,
0717             DMAMEM_TO_CPU,
0718             (void *)pDev->ptr_bd0_remote,
0719             (void **)&pDev->ptr_bd0,
0720             2 * SPACEWIRE_BDTABLE_SIZE);
0721     } else {
0722         if (pDev->bd_dma_area == 0) {
0723             if (pDev->_ptr_bd0)
0724                 free(pDev->_ptr_bd0);
0725             pDev->_ptr_bd0 =
0726                 rtems_heap_allocate_aligned_with_boundary(
0727                     SPACEWIRE_BDTABLE_SIZE*2, 1024, 0);
0728             if (!pDev->_ptr_bd0)
0729                 return 1;
0730             pDev->ptr_bd0 = (char *)pDev->_ptr_bd0;
0731         } else {
0732             pDev->ptr_bd0 = (char *)pDev->bd_dma_area;
0733         }
0734         drvmgr_translate_check(
0735             pDev->dev,
0736             CPUMEM_TO_DMA,
0737             (void *)pDev->ptr_bd0,
0738             (void **)&pDev->ptr_bd0_remote,
0739             2 * SPACEWIRE_BDTABLE_SIZE);
0740     }
0741
0742     return 0;
0743 }
0744
0745 static void grspw_interrupt(void *arg)
0746 {
0747     GRSPW_DEV *pDev = (GRSPW_DEV *)arg;
0748     int dmactrl;
0749     int status;
0750     int ctrl;
0751     unsigned int timecode;
0752
0753     status = SPW_STATUS_READ(pDev);
0754     /*SPW_STATUS_WRITE(pDev, SPW_STATUS_CE | SPW_STATUS_ER | SPW_STATUS_DE | SPW_STATUS_PE | SPW_STATUS_WE | SPW_STATUS_IA | SPW_STATUS_EE | SPW_STATUS_TO);*/
0755     SPW_STATUS_WRITE(pDev, status & (SPW_STATUS_CE | SPW_STATUS_ER | SPW_STATUS_DE | SPW_STATUS_PE | SPW_STATUS_WE | SPW_STATUS_IA | SPW_STATUS_EE));
0756
0757     /* Make sure to put the timecode handling first in order to get the smallest
0758      * possible interrupt latency
0759      */
0760     if ( (status & SPW_STATUS_TO) && (grspw_timecode_callback != NULL) ) {
0761         /* Timecode received. Let custom function handle this */
0762         SPW_STATUS_WRITE(pDev, SPW_STATUS_TO);
0763         timecode = SPW_READ(&pDev->regs->time);
0764         (grspw_timecode_callback)(pDev,pDev->regs,pDev->minor,timecode);
0765     }
0766
0767     /* Clear SPW_DMACTRL_PR if set */
0768     dmactrl = SPW_READ(&pDev->regs->dma0ctrl);
0769     /*SPW_WRITE(&pDev->regs->dma0ctrl, dmactrl | SPW_DMACTRL_PR);*/
0770     SPW_WRITE(&pDev->regs->dma0ctrl, dmactrl);
0771
0772     /* If linkinterrupts are enabled check if it was a linkerror irq and then send an event to the
0773        process set in the config */
0774     if (pDev->config.link_err_irq) {
0775         if (status & (SPW_STATUS_CE | SPW_STATUS_ER | SPW_STATUS_DE | SPW_STATUS_PE | SPW_STATUS_WE)) {
0776             rtems_event_send(pDev->config.event_id, SPW_LINKERR_EVENT);
0777             if (pDev->config.disable_err) {
0778                 /* disable link*/
0779                 SPW_CTRL_WRITE(pDev, (SPW_CTRL_READ(pDev) & 0xFFFFFFFC) | SPW_CTRL_LINKDISABLED);
0780                 pDev->config.linkdisabled = 1;
0781                 pDev->config.linkstart = 0;
0782                 pDev->running = 0;
0783             }
0784         }
0785     }
0786     if (status & SPW_STATUS_CE) {
0787         pDev->stat.credit_err++;
0788     }
0789     if (status & SPW_STATUS_ER) {
0790         pDev->stat.escape_err++;
0791     }
0792     if (status & SPW_STATUS_DE) {
0793         pDev->stat.disconnect_err++;
0794     }
0795     if (status & SPW_STATUS_PE) {
0796         pDev->stat.parity_err++;
0797     }
0798     if (status & SPW_STATUS_WE) {
0799         pDev->stat.write_sync_err++;
0800     }
0801     if (status & SPW_STATUS_IA) {
0802         pDev->stat.invalid_address++;
0803     }
0804     if (status & SPW_STATUS_EE) {
0805         pDev->stat.early_ep++;
0806     }
0807
0808     /* Check for tx interrupts */
0809     while( (pDev->tx_sent != pDev->tx_cur) || pDev->tx_all_in_use) {
0810         /* Has this descriptor been sent? */
0811         ctrl = SPW_READ((volatile void *)&pDev->tx[pDev->tx_sent].ctrl);
0812         if ( ctrl & SPW_TXBD_EN ) {
0813             break;
0814         }
0815         /* Yes, increment status counters & tx_sent so we can use this descriptor to send more packets with */
0816         pDev->stat.packets_sent++;
0817
0818         rtems_semaphore_release(pDev->txsp);
0819
0820         if ( ctrl & SPW_TXBD_LE ) {
0821             pDev->stat.tx_link_err++;
0822         }
0823
0824         /* step to next descriptor */
0825         pDev->tx_sent = (pDev->tx_sent + 1) % pDev->txbufcnt;
0826         pDev->tx_all_in_use = 0; /* not all of the descriptors can be in use since we just freed one. */
0827     }
0828
0829     /* Check for rx interrupts */
0830     if (dmactrl & SPW_DMACTRL_PR) {
0831         rtems_semaphore_release(pDev->rxsp);
0832     }
0833 }
0834
0835 static rtems_device_driver grspw_initialize(
0836     rtems_device_major_number major,
0837     rtems_device_minor_number minor,
0838     void *arg
0839 )
0840 {
0841     /* Initialize device-common data structures here */
0842     return RTEMS_SUCCESSFUL;
0843 }
0844
0845 static rtems_device_driver grspw_open(
0846         rtems_device_major_number major,
0847         rtems_device_minor_number minor,
0848         void * arg
0849         )
0850 {
0851     GRSPW_DEV *pDev;
0852     struct drvmgr_dev *dev;
0853     SPACEWIRE_DBGC(DBGSPW_IOCALLS, "open [%i,%i]\n", major, minor);
0854
0855     if ( drvmgr_get_dev(&grspw_drv_info.general, minor, &dev) ) {
0856         SPACEWIRE_DBG("Wrong minor %d\n", minor);
0857         return RTEMS_INVALID_NAME;
0858     }
0859     pDev = (GRSPW_DEV *)dev->priv;
0860
0861     if ( pDev->open )
0862         return RTEMS_RESOURCE_IN_USE;
0863
0864     /* Mark device open */
0865     pDev->open = 1;
0866
0867     pDev->stat.tx_link_err = 0;
0868     pDev->stat.rx_rmap_header_crc_err = 0;
0869     pDev->stat.rx_rmap_data_crc_err = 0;
0870     pDev->stat.rx_eep_err = 0;
0871     pDev->stat.rx_truncated = 0;
0872     pDev->stat.parity_err = 0;
0873     pDev->stat.escape_err = 0;
0874     pDev->stat.credit_err = 0;
0875     pDev->stat.write_sync_err = 0;
0876     pDev->stat.disconnect_err = 0;
0877     pDev->stat.early_ep = 0;
0878     pDev->stat.invalid_address = 0;
0879     pDev->stat.packets_sent = 0;
0880     pDev->stat.packets_received = 0;
0881
0882     pDev->config.rm_prot_id = 0;
0883     pDev->config.keep_source = 0;
0884     pDev->config.check_rmap_err = 0;
0885     pDev->config.tx_blocking = 0;
0886     pDev->config.tx_block_on_full = 0;
0887     pDev->config.rx_blocking = 0;
0888     pDev->config.disable_err = 0;
0889     pDev->config.link_err_irq = 0;
0890     pDev->config.event_id = 0;
0891     pDev->config.rtimeout = 0;
0892
0893     pDev->running = 0;
0894     pDev->core_freq_khz = 0;
0895
0896     /* Reset Core */
0897     grspw_hw_reset(pDev);
0898
0899     /* Read default configuration */
0900     grspw_hw_read_config(pDev);
0901
0902     return RTEMS_SUCCESSFUL;
0903 }
0904
0905 static rtems_device_driver grspw_close(
0906     rtems_device_major_number major,
0907     rtems_device_minor_number minor,
0908     void * arg
0909     )
0910 {
0911     GRSPW_DEV *pDev;
0912     struct drvmgr_dev *dev;
0913
0914     if ( drvmgr_get_dev(&grspw_drv_info.general, minor, &dev) ) {
0915         return RTEMS_INVALID_NAME;
0916     }
0917     pDev = (GRSPW_DEV *)dev->priv;
0918
0919     SPACEWIRE_DBGC(DBGSPW_IOCALLS, "close [%i,%i]\n", major, minor);
0920     rtems_semaphore_delete(pDev->txsp);
0921     rtems_semaphore_delete(pDev->rxsp);
0922
0923     grspw_hw_stop(pDev,1,1);
0924
0925     grspw_hw_reset(pDev);
0926
0927     /* Mark device closed - not open */
0928     pDev->open = 0;
0929
0930     return RTEMS_SUCCESSFUL;
0931 }
0932
0933 static rtems_device_driver grspw_read(
0934     rtems_device_major_number major,
0935     rtems_device_minor_number minor,
0936     void            * arg
0937     )
0938 {
0939     rtems_libio_rw_args_t *rw_args;
0940     unsigned int count = 0;
0941     GRSPW_DEV *pDev;
0942     struct drvmgr_dev *dev;
0943     int status;
0944
0945     if ( drvmgr_get_dev(&grspw_drv_info.general, minor, &dev) ) {
0946         return RTEMS_INVALID_NAME;
0947     }
0948     pDev = (GRSPW_DEV *)dev->priv;
0949
0950     rw_args = (rtems_libio_rw_args_t *) arg;
0951
0952     /* is link up? */
0953     if ( !pDev->running ) {
0954         return RTEMS_INVALID_NAME;
0955     }
0956
0957     if ((rw_args->count < 1) || (rw_args->buffer == NULL)) {
0958         return RTEMS_INVALID_NAME;
0959     }
0960
0961     SPACEWIRE_DBGC(DBGSPW_IOCALLS, "read  [%i,%i]: buf:0x%x len:%i \n", major, minor, (unsigned int)rw_args->buffer, rw_args->count);
0962
0963     while ( (count = grspw_hw_receive(pDev, rw_args->buffer, rw_args->count)) == 0) {
0964         /* wait a moment for any descriptors to get available
0965          *
0966          * Semaphore is signaled by interrupt handler
0967          */
0968         if (pDev->config.rx_blocking) {
0969             SPACEWIRE_DBG2("Rx blocking\n");
0970             if ( pDev->config.rtimeout ) {
0971                 status = rtems_semaphore_obtain(pDev->rxsp, RTEMS_WAIT, pDev->config.rtimeout);
0972                 if ( status == RTEMS_TIMEOUT )
0973                     return RTEMS_TIMEOUT;
0974             } else {
0975                 rtems_semaphore_obtain(pDev->rxsp, RTEMS_WAIT, RTEMS_NO_TIMEOUT);
0976             }
0977         } else {
0978             SPACEWIRE_DBG2("Rx non blocking\n");
0979             return RTEMS_RESOURCE_IN_USE;
0980         }
0981     }
0982
0983 #ifdef DEBUG_SPACEWIRE_ONOFF
0984     if (DEBUG_SPACEWIRE_FLAGS & DBGSPW_DUMP) {
0985         int k;
0986         for (k = 0; k < count; k++){
0987             if (k % 16 == 0) {
0988                 printf ("\n");
0989             }
0990             printf ("%.2x(%c) ", rw_args->buffer[k] & 0xff, isprint(rw_args->buffer[k] & 0xff) ? rw_args->buffer[k] & 0xff : ' ');
0991         }
0992         printf ("\n");
0993     }
0994 #endif
0995
0996     rw_args->bytes_moved = count;
0997     return RTEMS_SUCCESSFUL;
0998 }
0999
1000 static rtems_device_driver grspw_write(
1001     rtems_device_major_number major,
1002     rtems_device_minor_number minor,
1003     void            * arg
1004 )
1005 {
1006     rtems_libio_rw_args_t *rw_args;
1007     GRSPW_DEV *pDev;
1008     struct drvmgr_dev *dev;
1009
1010     if ( drvmgr_get_dev(&grspw_drv_info.general, minor, &dev) ) {
1011         return RTEMS_INVALID_NAME;
1012     }
1013     pDev = (GRSPW_DEV *)dev->priv;
1014
1015     rw_args = (rtems_libio_rw_args_t *) arg;
1016     SPACEWIRE_DBGC(DBGSPW_IOCALLS, "write [%i,%i]: buf:0x%x len:%i\n", major, minor, (unsigned int)rw_args->buffer, rw_args->count);
1017
1018     /* is link up? */
1019     if ( !pDev->running ) {
1020         return RTEMS_INVALID_NAME;
1021     }
1022
1023     if ((rw_args->count > pDev->txdbufsize) || (rw_args->count < 1) || (rw_args->buffer == NULL)) {
1024         return RTEMS_INVALID_NAME;
1025     }
1026
1027     while ((rw_args->bytes_moved = grspw_hw_send(pDev, 0, NULL, rw_args->count, rw_args->buffer, 0)) == 0) {
1028         if (pDev->config.tx_block_on_full == 1) {
1029             SPACEWIRE_DBG2("Tx Block on full \n");
1030             rtems_semaphore_obtain(pDev->txsp, RTEMS_WAIT, RTEMS_NO_TIMEOUT);
1031         } else {
1032             SPACEWIRE_DBG2("Tx non blocking return when full \n");
1033             return RTEMS_RESOURCE_IN_USE;
1034         }
1035     }
1036     return RTEMS_SUCCESSFUL;
1037 }
1038
1039 static rtems_device_driver grspw_control(
1040     rtems_device_major_number major,
1041     rtems_device_minor_number minor,
1042     void            * arg
1043     )
1044 {
1045     spw_ioctl_pkt_send *args;
1046     spw_ioctl_packetsize *ps;
1047     int status;
1048     unsigned int tmp,mask,nodeaddr,nodemask;
1049     int timeout;
1050     rtems_device_driver ret;
1051     rtems_libio_ioctl_args_t *ioarg = (rtems_libio_ioctl_args_t *) arg;
1052     GRSPW_DEV *pDev;
1053     struct drvmgr_dev *dev;
1054
1055     SPACEWIRE_DBGC(DBGSPW_IOCALLS, "ctrl [%i,%i]\n", major, minor);
1056
1057     if ( drvmgr_get_dev(&grspw_drv_info.general, minor, &dev) ) {
1058         return RTEMS_INVALID_NAME;
1059     }
1060     pDev = (GRSPW_DEV *)dev->priv;
1061
1062     if (!ioarg)
1063         return RTEMS_INVALID_NAME;
1064
1065     ioarg->ioctl_return = 0;
1066     switch(ioarg->command) {
1067         case SPACEWIRE_IOCTRL_SET_NODEADDR:
1068             /*set node address*/
1069             SPACEWIRE_DBGC(DBGSPW_IOCTRL, "SPACEWIRE_IOCTRL_SET_NODEADDR %i\n",(unsigned int)ioarg->buffer);
1070             if ((unsigned int)ioarg->buffer > 255) {
1071                 return RTEMS_INVALID_NAME;
1072             }
1073             nodeaddr = ((unsigned int)ioarg->buffer) & 0xff;
1074             tmp = SPW_READ(&pDev->regs->nodeaddr);
1075             tmp &= 0xffffff00; /* Remove old address */
1076             tmp |= nodeaddr;
1077             SPW_WRITE(&pDev->regs->nodeaddr, tmp);
1078             if ((SPW_READ(&pDev->regs->nodeaddr)&0xff) != nodeaddr) {
1079                 return RTEMS_IO_ERROR;
1080             }
1081             pDev->config.nodeaddr = nodeaddr;
1082             break;
1083         case SPACEWIRE_IOCTRL_SET_NODEMASK:
1084             /*set node address*/
1085             SPACEWIRE_DBGC(DBGSPW_IOCTRL, "SPACEWIRE_IOCTRL_SET_NODEMASK %i\n",(unsigned int)ioarg->buffer);
1086             if ( pDev->core_ver > 1 ){
1087                 if ((unsigned int)ioarg->buffer > 255) {
1088                     return RTEMS_INVALID_NAME;
1089                 }
1090                 nodemask = ((unsigned int)ioarg->buffer) & 0xff;
1091                 tmp = SPW_READ(&pDev->regs->nodeaddr);
1092                 tmp &= 0xffff00ff; /* Remove old mask */
1093                 tmp |= nodemask<<8;
1094                 SPW_WRITE(&pDev->regs->nodeaddr, tmp);
1095                 if (((SPW_READ(&pDev->regs->nodeaddr)>>8)&0xff) != nodemask) {
1096                     return RTEMS_IO_ERROR;
1097                 }
1098                 pDev->config.nodemask = nodemask;
1099             }else{
1100                 SPACEWIRE_DBG("SPACEWIRE_IOCTRL_SET_NODEMASK: not implemented in GRSPW1 HW\n");
1101             }
1102             break;
1103         case SPACEWIRE_IOCTRL_SET_RXBLOCK:
1104             SPACEWIRE_DBGC(DBGSPW_IOCTRL, "SPACEWIRE_IOCTRL_SET_RXBLOCK %i \n", (unsigned int)ioarg->buffer);
1105             if ((unsigned int)ioarg->buffer > 1) {
1106                 return RTEMS_INVALID_NAME;
1107             }
1108             pDev->config.rx_blocking = (unsigned int)ioarg->buffer;
1109             break;
1110         case SPACEWIRE_IOCTRL_SET_DESTKEY:
1111             SPACEWIRE_DBGC(DBGSPW_IOCTRL,"SPACEWIRE_IOCTRL_SET_DESTKEY %i\n", (unsigned int)ioarg->buffer);
1112             if (!pDev->config.is_rmap) {
1113                 return RTEMS_NOT_IMPLEMENTED;
1114             }
1115             if ((unsigned int)ioarg->buffer > 255) {
1116                 return RTEMS_INVALID_NAME;
1117             }
1118             SPW_WRITE(&pDev->regs->destkey, (unsigned int)ioarg->buffer);
1119             if (SPW_READ(&pDev->regs->destkey) != (unsigned int)ioarg->buffer) {
1120                 return RTEMS_IO_ERROR;
1121             }
1122             pDev->config.destkey = (unsigned int)ioarg->buffer;
1123             break;
1124         case SPACEWIRE_IOCTRL_SET_CLKDIV:
1125             SPACEWIRE_DBGC(DBGSPW_IOCTRL,"SPACEWIRE_IOCTRL_SET_CLKDIV %i\n", (unsigned int)ioarg->buffer);
1126             if ((unsigned int)ioarg->buffer > 255) {
1127                 return RTEMS_INVALID_NAME;
1128             }
1129             if ( pDev->core_ver == 3 )
1130                 break;
1131             tmp = SPW_READ(&pDev->regs->clkdiv);
1132             tmp &= ~0xff; /* Remove old Clockdiv Setting */
1133             tmp |= ((unsigned int)ioarg->buffer) & 0xff; /* add new clockdiv setting */
1134             SPW_WRITE(&pDev->regs->clkdiv, tmp);
1135             if (SPW_READ(&pDev->regs->clkdiv) != tmp) {
1136                 return RTEMS_IO_ERROR;
1137             }
1138             pDev->config.clkdiv = tmp;
1139             break;
1140         case SPACEWIRE_IOCTRL_SET_CLKDIVSTART:
1141             SPACEWIRE_DBGC(DBGSPW_IOCTRL,"SPACEWIRE_IOCTRL_SET_CLKDIVSTART %i\n", (unsigned int)ioarg->buffer);
1142             if ((unsigned int)ioarg->buffer > 255) {
1143                 return RTEMS_INVALID_NAME;
1144             }
1145             if ( pDev->core_ver == 3 )
1146                 break;
1147             tmp = SPW_READ(&pDev->regs->clkdiv);
1148             tmp &= ~0xff00; /* Remove old Clockdiv Start Setting */
1149             tmp |= (((unsigned int)ioarg->buffer) & 0xff)<<8; /* add new clockdiv start setting */
1150             SPW_WRITE(&pDev->regs->clkdiv, tmp);
1151             if (SPW_READ(&pDev->regs->clkdiv) != tmp) {
1152                 return RTEMS_IO_ERROR;
1153             }
1154             pDev->config.clkdiv = tmp;
1155             break;
1156         case SPACEWIRE_IOCTRL_SET_TIMER:
1157             SPACEWIRE_DBGC(DBGSPW_IOCTRL,"SPACEWIRE_IOCTRL_SET_TIMER %i\n", (unsigned int)ioarg->buffer);
1158             if ( pDev->core_ver <= 1 ) {
1159                 if ((unsigned int)ioarg->buffer > 4095) {
1160                     return RTEMS_INVALID_NAME;
1161                 }
1162                 SPW_WRITE(&pDev->regs->timer, (SPW_READ(&pDev->regs->timer) & 0xFFFFF000) | ((unsigned int)ioarg->buffer & 0xFFF));
1163                 if ((SPW_READ(&pDev->regs->timer) & 0xFFF) != (unsigned int)ioarg->buffer) {
1164                     return RTEMS_IO_ERROR;
1165                 }
1166                 pDev->config.timer = (unsigned int)ioarg->buffer;
1167             }else{
1168                 SPACEWIRE_DBG("SPACEWIRE_IOCTRL_SET_TIMER: removed in GRSPW2 HW\n");
1169             }
1170             break;
1171         case SPACEWIRE_IOCTRL_SET_DISCONNECT:
1172             SPACEWIRE_DBGC(DBGSPW_IOCTRL,"SPACEWIRE_IOCTRL_SET_DISCONNECT %i\n", (unsigned int)ioarg->buffer);
1173             if ( pDev->core_ver <= 1 ) {
1174                 if ((unsigned int)ioarg->buffer > 1023) {
1175                     return RTEMS_INVALID_NAME;
1176                 }
1177                 SPW_WRITE(&pDev->regs->timer, (SPW_READ(&pDev->regs->timer) & 0xFFC00FFF) | (((unsigned int)ioarg->buffer & 0x3FF) << 12));
1178                 if (((SPW_READ(&pDev->regs->timer) >> 12) & 0x3FF) != (unsigned int)ioarg->buffer) {
1179                     return RTEMS_IO_ERROR;
1180                 }
1181                 pDev->config.disconnect = (unsigned int)ioarg->buffer;
1182             }else{
1183                 SPACEWIRE_DBG("SPACEWIRE_IOCTRL_SET_DISCONNECT: not implemented for GRSPW2\n");
1184             }
1185             break;
1186         case SPACEWIRE_IOCTRL_SET_PROMISCUOUS:
1187             SPACEWIRE_DBGC(DBGSPW_IOCTRL,"SPACEWIRE_IOCTRL_SET_PROMISCUOUS %i \n", (unsigned int)ioarg->buffer);
1188             if ((unsigned int)ioarg->buffer > 1) {
1189                 return RTEMS_INVALID_NAME;
1190             }
1191             SPW_CTRL_WRITE(pDev, SPW_CTRL_READ(pDev) | ((unsigned int)ioarg->buffer << 5));
1192             if (((SPW_CTRL_READ(pDev) >> 5) & 1) != (unsigned int)ioarg->buffer) {
1193                 return RTEMS_IO_ERROR;
1194             }
1195             pDev->config.promiscuous = (unsigned int)ioarg->buffer;
1196             break;
1197         case SPACEWIRE_IOCTRL_SET_RMAPEN:
1198             SPACEWIRE_DBGC(DBGSPW_IOCTRL,"SPACEWIRE_IOCTRL_SET_RMAPEN %i \n", (unsigned int)ioarg->buffer);
1199             if ((unsigned int)ioarg->buffer > 1) {
1200                 return RTEMS_INVALID_NAME;
1201             }
1202             SPW_CTRL_WRITE(pDev, (SPW_CTRL_READ(pDev) & 0xFFFEFFFF) | ((unsigned int)ioarg->buffer << 16));
1203             if (((SPW_CTRL_READ(pDev) >> 16) & 1) != (unsigned int)ioarg->buffer) {
1204                 return RTEMS_IO_ERROR;
1205             }
1206             pDev->config.rmapen = (unsigned int)ioarg->buffer;
1207             break;
1208         case SPACEWIRE_IOCTRL_SET_RMAPBUFDIS:
1209             SPACEWIRE_DBGC(DBGSPW_IOCTRL,"SPACEWIRE_IOCTRL_SET_RMAPBUFDIS %i \n", (unsigned int)ioarg->buffer);
1210             if ((unsigned int)ioarg->buffer > 1) {
1211                 return RTEMS_INVALID_NAME;
1212             }
1213             SPW_CTRL_WRITE(pDev, (SPW_CTRL_READ(pDev) & 0xFFFDFFFF) | ((unsigned int)ioarg->buffer << 17));
1214             if (((SPW_CTRL_READ(pDev) >> 17) & 1) != (unsigned int)ioarg->buffer) {
1215                 return RTEMS_IO_ERROR;
1216             }
1217             pDev->config.rmapbufdis = (unsigned int)ioarg->buffer;
1218             break;
1219         case SPACEWIRE_IOCTRL_SET_CHECK_RMAP:
1220             SPACEWIRE_DBGC(DBGSPW_IOCTRL,"SPACEWIRE_IOCTRL_SET_CHECK_RMAP %i \n", (unsigned int)ioarg->buffer);
1221             if ((unsigned int)ioarg->buffer > 1) {
1222                 return RTEMS_INVALID_NAME;
1223             }
1224             pDev->config.check_rmap_err = (unsigned int)ioarg->buffer;
1225             break;
1226         case SPACEWIRE_IOCTRL_SET_RM_PROT_ID:
1227             SPACEWIRE_DBGC(DBGSPW_IOCTRL, "SPACEWIRE_IOCTRL_SET_RM_PROT_ID %i \n", (unsigned int)ioarg->buffer);
1228             if ((unsigned int)ioarg->buffer > 1) {
1229                 return RTEMS_INVALID_NAME;
1230             }
1231             pDev->config.rm_prot_id = (unsigned int)ioarg->buffer;
1232             break;
1233         case SPACEWIRE_IOCTRL_SET_KEEP_SOURCE:
1234             SPACEWIRE_DBGC(DBGSPW_IOCTRL, "SPACEWIRE_IOCTRL_SET_KEEP_SOURCE %i \n", (unsigned int)ioarg->buffer);
1235             if ((unsigned int)ioarg->buffer > 1) {
1236                 return RTEMS_INVALID_NAME;
1237             }
1238             pDev->config.keep_source = (unsigned int)ioarg->buffer;
1239             break;
1240         case SPACEWIRE_IOCTRL_SET_TXBLOCK:
1241             SPACEWIRE_DBGC(DBGSPW_IOCTRL, "SPACEWIRE_IOCTRL_SET_TXBLOCK %i \n", (unsigned int)ioarg->buffer);
1242             if ((unsigned int)ioarg->buffer > 1) {
1243                 return RTEMS_INVALID_NAME;
1244             }
1245             pDev->config.tx_blocking = (unsigned int)ioarg->buffer;
1246             break;
1247         case SPACEWIRE_IOCTRL_SET_TXBLOCK_ON_FULL:
1248             SPACEWIRE_DBGC(DBGSPW_IOCTRL, "SPACEWIRE_IOCTRL_SET_TXBLOCK_ON_FULL %i \n", (unsigned int)ioarg->buffer);
1249             if ((unsigned int)ioarg->buffer > 1) {
1250                 return RTEMS_INVALID_NAME;
1251             }
1252             pDev->config.tx_block_on_full = (unsigned int)ioarg->buffer;
1253             break;
1254         case SPACEWIRE_IOCTRL_SET_DISABLE_ERR:
1255             SPACEWIRE_DBGC(DBGSPW_IOCTRL, "SPACEWIRE_IOCTRL_SET_DISABLE_ERR %i \n", (unsigned int)ioarg->buffer);
1256             if ((unsigned int)ioarg->buffer > 1) {
1257                 return RTEMS_INVALID_NAME;
1258             }
1259             pDev->config.disable_err = (unsigned int)ioarg->buffer;
1260             break;
1261         case SPACEWIRE_IOCTRL_SET_LINK_ERR_IRQ:
1262             SPACEWIRE_DBGC(DBGSPW_IOCTRL, "SPACEWIRE_IOCTRL_SET_LINK_ERR_IRQ %i \n", (unsigned int)ioarg->buffer);
1263             SPACEWIRE_DBGC(DBGSPW_IOCTRL, "CTRL REG: %x\n", SPW_CTRL_READ(pDev));
1264             if ((unsigned int)ioarg->buffer > 1) {
1265                 return RTEMS_INVALID_NAME;
1266             }
1267             tmp = (SPW_CTRL_READ(pDev) & 0xFFFFFDF7) | ((unsigned int)ioarg->buffer << 9);
1268             if (tmp & (SPW_CTRL_LI|SPW_CTRL_TQ))
1269                 tmp |= SPW_CTRL_IE;
1270             SPW_CTRL_WRITE(pDev, tmp);
1271             SPACEWIRE_DBGC(DBGSPW_IOCTRL, "CTRL REG: %x\n", SPW_CTRL_READ(pDev));
1272             if (((SPW_CTRL_READ(pDev) >> 9) & 1) != (unsigned int)ioarg->buffer) {
1273                 return RTEMS_IO_ERROR;
1274             }
1275             pDev->config.link_err_irq = (unsigned int)ioarg->buffer;
1276             break;
1277         case SPACEWIRE_IOCTRL_SET_EVENT_ID:
1278             SPACEWIRE_DBGC(DBGSPW_IOCTRL, "SPACEWIRE_IOCTRL_SET_EVENT_ID %i \n", (unsigned int)ioarg->buffer);
1279             pDev->config.event_id = (rtems_id)ioarg->buffer;
1280             SPACEWIRE_DBGC(DBGSPW_IOCTRL, "Event id: %i\n", pDev->config.event_id);
1281             break;
1282
1283         /* Change MAX Packet size by:
1284          *  - stop RX/TX (if on)
1285          *  - wait for hw to complete RX DMA (if on)
1286          *  - reallocate buffers with new size
1287          *  - tell hw about new size & start RX/TX again (if previously on)
1288          */
1289         case SPACEWIRE_IOCTRL_SET_PACKETSIZE:
1290             if (ioarg->buffer == NULL)
1291                 return RTEMS_INVALID_NAME;
1292             ps = (spw_ioctl_packetsize*) ioarg->buffer;
1293             SPACEWIRE_DBGC(DBGSPW_IOCTRL,"SPACEWIRE_IOCTRL_SET_RXPACKETSIZE %i \n", (unsigned int)ioarg->buffer);
1294
1295             tmp = pDev->running;
1296
1297             if ( pDev->running ){
1298                 /* Stop RX */
1299                 grspw_hw_stop(pDev,1,1);
1300
1301                 /* If packetsize fails it is good to know if in running mode */
1302                 pDev->running = 0;
1303
1304                 /* Wait for Receiver to finnish pending DMA transfers if any */
1305                 grspw_hw_wait_rx_inactive(pDev);
1306             }
1307
1308             /* Save new buffer sizes */
1309             pDev->rxbufsize = ((ps->rxsize+7)&~7);
1310             pDev->txdbufsize = ps->txdsize;
1311             pDev->txhbufsize = ps->txhsize;
1312             pDev->config.rxmaxlen = pDev->rxbufsize;
1313
1314             /* Free previous buffers & allocate buffers with new size */
1315             if (grspw_buffer_alloc(pDev))
1316                 return RTEMS_NO_MEMORY;
1317
1318             /* if RX was actived before, we reactive it again */
1319             if ( tmp ) {
1320                 if ( (status = grspw_hw_startup(pDev,-1)) != RTEMS_SUCCESSFUL ) {
1321                     return status;
1322                 }
1323                 pDev->running = 1;
1324             }
1325 #if 0
1326             /* Rewrite previous config which was wasted due to reset in hw_startup */
1327             SPW_WRITE(&pDev->regs->nodeaddr, pDev->config.nodeaddr);
1328             SPW_WRITE(&pDev->regs->destkey, pDev->config.destkey);
1329             SPW_WRITE(&pDev->regs->clkdiv, pDev->config.clkdiv);
1330             SPW_WRITE(&pDev->regs->timer, pDev->config.timer | ( (pDev->config.disconnect & 0x3FF) << 12) );
1331             SPW_CTRL_WRITE(pDev, (SPW_CTRL_READ(pDev) & !(SPW_CTRL_LINKSTART | SPW_CTRL_PM | SPW_CTRL_RE | SPW_CTRL_RD | SPW_CTRL_TT | SPW_CTRL_TR)) | \
1332                           (pDev->config.promiscuous << 5) | (pDev->config.rmapen << 16) | (pDev->config.rmapbufdis << 17) | \
1333                           (pDev->config.linkdisabled) | (pDev->config.linkstart << 1));
1334 #endif
1335             break;
1336         case SPACEWIRE_IOCTRL_GET_CONFIG:
1337             if (ioarg->buffer == NULL)
1338                 return RTEMS_INVALID_NAME;
1339             SPACEWIRE_DBG2("SPACEWIRE_IOCTRL_GET_CONFIG \n");
1340             (*(spw_config *)ioarg->buffer).nodeaddr = pDev->config.nodeaddr;
1341             (*(spw_config *)ioarg->buffer).nodemask = pDev->config.nodemask;
1342             (*(spw_config *)ioarg->buffer).destkey = pDev->config.destkey;
1343             (*(spw_config *)ioarg->buffer).clkdiv = pDev->config.clkdiv;
1344             (*(spw_config *)ioarg->buffer).rxmaxlen = pDev->config.rxmaxlen;
1345             (*(spw_config *)ioarg->buffer).timer = pDev->config.timer;
1346             (*(spw_config *)ioarg->buffer).disconnect = pDev->config.disconnect;
1347             (*(spw_config *)ioarg->buffer).promiscuous = pDev->config.promiscuous;
1348             (*(spw_config *)ioarg->buffer).rmapen = pDev->config.rmapen;
1349             (*(spw_config *)ioarg->buffer).rmapbufdis = pDev->config.rmapbufdis;
1350             (*(spw_config *)ioarg->buffer).check_rmap_err = pDev->config.check_rmap_err;
1351             (*(spw_config *)ioarg->buffer).rm_prot_id = pDev->config.rm_prot_id;
1352             (*(spw_config *)ioarg->buffer).tx_blocking = pDev->config.tx_blocking;
1353             (*(spw_config *)ioarg->buffer).disable_err = pDev->config.disable_err;
1354             (*(spw_config *)ioarg->buffer).link_err_irq = pDev->config.link_err_irq;
1355             (*(spw_config *)ioarg->buffer).event_id = pDev->config.event_id;
1356             (*(spw_config *)ioarg->buffer).is_rmap = pDev->config.is_rmap;
1357             (*(spw_config *)ioarg->buffer).is_rmapcrc = pDev->config.is_rmapcrc;
1358             (*(spw_config *)ioarg->buffer).is_rxunaligned = pDev->config.is_rxunaligned;
1359             (*(spw_config *)ioarg->buffer).linkdisabled = pDev->config.linkdisabled;
1360             (*(spw_config *)ioarg->buffer).linkstart = pDev->config.linkstart;
1361             (*(spw_config *)ioarg->buffer).rx_blocking = pDev->config.rx_blocking;
1362             (*(spw_config *)ioarg->buffer).tx_block_on_full = pDev->config.tx_block_on_full;
1363             (*(spw_config *)ioarg->buffer).keep_source = pDev->config.keep_source;
1364             (*(spw_config *)ioarg->buffer).rtimeout = pDev->config.rtimeout;
1365             break;
1366         case SPACEWIRE_IOCTRL_GET_LINK_STATUS:
1367             SPACEWIRE_DBGC(DBGSPW_IOCTRL,"SPACEWIRE_IOCTRL_GET_STATUS=%i \n", (unsigned int)((SPW_STATUS_READ(pDev) >> 21) & 0x7));
1368             *(unsigned int *)ioarg->buffer = (unsigned int )((SPW_STATUS_READ(pDev) >> 21) & 0x7);
1369             break;
1370         case SPACEWIRE_IOCTRL_GET_STATISTICS:
1371             if (ioarg->buffer == NULL)
1372                 return RTEMS_INVALID_NAME;
1373             SPACEWIRE_DBG2("SPACEWIRE_IOCTRL_GET_STATISTICS \n");
1374             (*(spw_stats *)ioarg->buffer).tx_link_err = pDev->stat.tx_link_err;
1375             (*(spw_stats *)ioarg->buffer).rx_rmap_header_crc_err = pDev->stat.rx_rmap_header_crc_err;
1376             (*(spw_stats *)ioarg->buffer).rx_rmap_data_crc_err = pDev->stat.rx_rmap_data_crc_err;
1377             (*(spw_stats *)ioarg->buffer).rx_eep_err =  pDev->stat.rx_eep_err;
1378             (*(spw_stats *)ioarg->buffer).rx_truncated = pDev->stat.rx_truncated;
1379             (*(spw_stats *)ioarg->buffer).parity_err = pDev->stat.parity_err;
1380             (*(spw_stats *)ioarg->buffer).escape_err = pDev->stat.escape_err;
1381             (*(spw_stats *)ioarg->buffer).credit_err = pDev->stat.credit_err;
1382             (*(spw_stats *)ioarg->buffer).write_sync_err = pDev->stat.write_sync_err;
1383             (*(spw_stats *)ioarg->buffer).disconnect_err = pDev->stat.disconnect_err;
1384             (*(spw_stats *)ioarg->buffer).early_ep = pDev->stat.early_ep;
1385             (*(spw_stats *)ioarg->buffer).invalid_address = pDev->stat.invalid_address;
1386             (*(spw_stats *)ioarg->buffer).packets_sent = pDev->stat.packets_sent;
1387             (*(spw_stats *)ioarg->buffer).packets_received = pDev->stat.packets_received;
1388             break;
1389         case SPACEWIRE_IOCTRL_CLR_STATISTICS:
1390             SPACEWIRE_DBG2("SPACEWIRE_IOCTRL_CLR_STATISTICS \n");
1391             pDev->stat.tx_link_err = 0;
1392             pDev->stat.rx_rmap_header_crc_err = 0;
1393             pDev->stat.rx_rmap_data_crc_err = 0;
1394             pDev->stat.rx_eep_err = 0;
1395             pDev->stat.rx_truncated = 0;
1396             pDev->stat.parity_err = 0;
1397             pDev->stat.escape_err = 0;
1398             pDev->stat.credit_err = 0;
1399             pDev->stat.write_sync_err = 0;
1400             pDev->stat.disconnect_err = 0;
1401             pDev->stat.early_ep = 0;
1402             pDev->stat.invalid_address = 0;
1403             pDev->stat.packets_sent = 0;
1404             pDev->stat.packets_received = 0;
1405             break;
1406         case SPACEWIRE_IOCTRL_SEND:
1407             if (ioarg->buffer == NULL)
1408                 return RTEMS_INVALID_NAME;
1409             args = (spw_ioctl_pkt_send *)ioarg->buffer;
1410             args->sent = 0;
1411
1412             /* is link up? */
1413             if ( !pDev->running ) {
1414                 return RTEMS_INVALID_NAME;
1415             }
1416
1417             SPACEWIRE_DBGC(DBGSPW_IOCALLS, "write [%i,%i]: hlen: %i hbuf:0x%x dlen:%i dbuf:0x%x\n", major, minor,
1418                        (unsigned int)args->hlen, (int)args->hdr,(unsigned int)args->dlen, (int)args->data);
1419
1420             if ((args->hlen > pDev->txhbufsize) || (args->dlen > pDev->txdbufsize) ||
1421                 ((args->hlen+args->dlen) < 1) ||
1422                 ((args->hdr == NULL) && (args->hlen != 0)) || ((args->data == NULL) && (args->dlen != 0))) {
1423                 return RTEMS_INVALID_NAME;
1424             }
1425             while ((args->sent = grspw_hw_send(pDev, args->hlen, args->hdr, args->dlen, args->data, args->options)) == 0) {
1426                 if (pDev->config.tx_block_on_full == 1) {
1427                     SPACEWIRE_DBG2("Tx Block on full \n");
1428                     rtems_semaphore_obtain(pDev->txsp, RTEMS_WAIT, RTEMS_NO_TIMEOUT);
1429                 } else {
1430                     SPACEWIRE_DBG2("Tx non blocking return when full \n");
1431                     return RTEMS_RESOURCE_IN_USE;
1432                 }
1433             }
1434             SPACEWIRE_DBGC(DBGSPW_IOCALLS, "Tx ioctl return: %i  \n", args->sent);
1435             break;
1436
1437         case SPACEWIRE_IOCTRL_LINKDISABLE:
1438             pDev->config.linkdisabled = 1;
1439             pDev->config.linkstart = 0;
1440             if ( pDev->core_ver != 3 ) {
1441                 SPW_CTRL_WRITE(pDev, (SPW_CTRL_READ(pDev) & 0xFFFFFFFC) | 1);
1442                 if ((SPW_CTRL_READ(pDev) & 3) != 1) {
1443                     return RTEMS_IO_ERROR;
1444                 }
1445             }
1446             break;
1447
1448         case SPACEWIRE_IOCTRL_LINKSTART:
1449             pDev->config.linkdisabled = 0;
1450             pDev->config.linkstart = 1;
1451             if ( pDev->core_ver != 3 ) {
1452                 SPW_CTRL_WRITE(pDev, (SPW_CTRL_READ(pDev) & 0xFFFFFFFC) | 2);
1453                 if ((SPW_CTRL_READ(pDev) & 3) != 2) {
1454                     return RTEMS_IO_ERROR;
1455                 }
1456             }
1457             break;
1458
1459         /* Calculate timer register from GRSPW Core frequency
1460          * Also possible to set disconnect and timer64 from
1461          *  - SPACEWIRE_IOCTRL_SET_DISCONNECT
1462          *  - SPACEWIRE_IOCTRL_SET_TIMER
1463          */
1464         case SPACEWIRE_IOCTRL_SET_COREFREQ:
1465             pDev->core_freq_khz = (unsigned int)ioarg->buffer;
1466             if ( pDev->core_freq_khz == 0 ){
1467                 /* Get GRSPW clock frequency from system clock.
1468                  * System clock has been read from timer inited
1469                  * by RTEMS loader (mkprom)
1470                  */
1471                 drvmgr_freq_get(pDev->dev, DEV_APB_SLV,
1472                     &pDev->core_freq_khz);
1473                 /* Convert from Hz -> kHz */
1474                 pDev->core_freq_khz = pDev->core_freq_khz / 1000;
1475             }
1476
1477             /* Only GRSPW1 needs the Timer64 and Disconnect values
1478              * GRSPW2 and onwards doesn't have this register.
1479              */
1480             if ( pDev->core_ver <= 1 ){
1481                 /* Calculate Timer64 & Disconnect */
1482                 pDev->config.timer = grspw_calc_timer64(pDev->core_freq_khz);
1483                 pDev->config.disconnect = grspw_calc_disconnect(pDev->core_freq_khz);
1484
1485                 /* Set Timer64 & Disconnect Register */
1486                 SPW_WRITE(&pDev->regs->timer,
1487                 (SPW_READ(&pDev->regs->timer) & 0xFFC00000) |
1488                 ((pDev->config.disconnect & 0x3FF)<<12) |
1489                 (pDev->config.timer & 0xFFF));
1490
1491                 /* Check that the registers were written successfully */
1492                 tmp = SPW_READ(&pDev->regs->timer) & 0x003fffff;
1493                 if ( ((tmp & 0xFFF) != pDev->config.timer) ||
1494                      (((tmp >> 12) & 0x3FF) != pDev->config.disconnect) ) {
1495                     return RTEMS_IO_ERROR;
1496                 }
1497             }
1498             break;
1499
1500         case SPACEWIRE_IOCTRL_START:
1501             if ( pDev->running ){
1502                 return RTEMS_INVALID_NAME;
1503             }
1504
1505             /* Get timeout from userspace
1506              *  timeout:
1507              *   *  -1     = Default timeout
1508              *   *  less than -1 = forever
1509              *   *  0       = no wait, proceed if link is up
1510              *   *  positive     = specifies number of system clock ticks that
1511              *           startup will wait for link to enter ready mode.
1512              */
1513             timeout = (int)ioarg->buffer;
1514
1515             if ( (ret=grspw_hw_startup(pDev,timeout)) != RTEMS_SUCCESSFUL ) {
1516                 return ret;
1517             }
1518             pDev->running = 1;
1519             /* Register interrupt routine and unmask IRQ */
1520             drvmgr_interrupt_register(pDev->dev, 0, "grspw", grspw_interrupt, pDev);
1521
1522             break;
1523
1524         case SPACEWIRE_IOCTRL_STOP:
1525             if ( !pDev->running ){
1526                 return RTEMS_INVALID_NAME;
1527             }
1528             /* Disable interrupts */
1529             drvmgr_interrupt_unregister(dev, 0, grspw_interrupt, pDev);
1530
1531             pDev->running = 0;
1532
1533             /* Stop Receiver and transmitter */
1534             grspw_hw_stop(pDev,1,1);
1535             break;
1536
1537         /* Set time-code control register bits, and Enables/Disables
1538          * Time code interrupt, make sure to connect the callback
1539          * grspw_timecode_callback if using interrupts.
1540          */
1541         case SPACEWIRE_IOCTRL_SET_TCODE_CTRL:
1542             tmp = (unsigned int)ioarg->buffer;
1543             mask = tmp & (SPACEWIRE_TCODE_CTRL_IE_MSK|SPACEWIRE_TCODE_CTRL_TT_MSK|SPACEWIRE_TCODE_CTRL_TR_MSK);
1544             mask <<= 8;
1545             tmp &= mask;
1546             tmp = (SPW_CTRL_READ(pDev) & ~(mask | SPW_CTRL_IE)) | tmp;
1547             if (tmp & (SPW_CTRL_LI|SPW_CTRL_TQ))
1548                 tmp |= SPW_CTRL_IE;
1549             SPW_CTRL_WRITE(pDev, tmp);
1550             break;
1551
1552         /* Set time register and optionaly send a time code */
1553         case SPACEWIRE_IOCTRL_SET_TCODE:
1554             tmp = (unsigned int)ioarg->buffer;
1555             /* Set timecode register */
1556             if (tmp & SPACEWIRE_TCODE_SET) {
1557                 SPW_WRITE(&pDev->regs->time,
1558                     ((SPW_READ(&pDev->regs->time) & ~(0xff)) |
1559                     (tmp & SPACEWIRE_TCODE_TCODE)));
1560             }
1561             /* Send timecode directly (tick-in) ? */
1562             if (tmp & SPACEWIRE_TCODE_TX) {
1563                 SPW_CTRL_WRITE(pDev,
1564                 ((SPW_CTRL_READ(pDev) & ~(SPW_CTRL_TI)) | SPW_CTRL_TI));
1565             }
1566             break;
1567
1568         /* Read time code register and tick-out status bit */
1569         case SPACEWIRE_IOCTRL_GET_TCODE:
1570             tmp = (unsigned int)ioarg->buffer;
1571             if ( !tmp ){
1572                 return RTEMS_INVALID_NAME;
1573             }
1574
1575             /* Copy timecode register */
1576             if (SPW_READ(&pDev->regs->status) & SPW_STATUS_TO) {
1577                 *(unsigned int *)tmp = (1 << 8) | SPW_READ(&pDev->regs->time);
1578             } else {
1579                 *(unsigned int *)tmp = SPW_READ(&pDev->regs->time);
1580             }
1581             break;
1582
1583         case SPACEWIRE_IOCTRL_SET_READ_TIMEOUT:
1584             pDev->config.rtimeout = (unsigned int)ioarg->buffer;
1585             break;
1586
1587         default:
1588             return RTEMS_NOT_IMPLEMENTED;
1589     }
1590
1591     SPACEWIRE_DBGC(DBGSPW_IOCALLS, "SPW_IOCTRL Return\n");
1592     return RTEMS_SUCCESSFUL;
1593 }
1594
1595 /* ============================================================================== */
1596
1597 static int grspw_set_rxmaxlen(GRSPW_DEV *pDev) {
1598     unsigned int rxmax;
1599     SPW_WRITE(&pDev->regs->dma0rxmax,pDev->config.rxmaxlen); /*set rx maxlength*/
1600     rxmax = SPW_READ(&pDev->regs->dma0rxmax);
1601     if (rxmax != pDev->config.rxmaxlen) {
1602         return 0;
1603     }
1604     return 1;
1605 }
1606
1607 static int grspw_hw_init(GRSPW_DEV *pDev) {
1608     unsigned int ctrl;
1609
1610     ctrl = SPW_CTRL_READ(pDev);
1611
1612     pDev->rx = (SPACEWIRE_RXBD *) pDev->ptr_bd0;
1613     pDev->tx = (SPACEWIRE_TXBD *) (pDev->ptr_bd0 + SPACEWIRE_BDTABLE_SIZE);
1614
1615     /* Set up remote addresses */
1616     pDev->rx_remote = (unsigned int)pDev->ptr_bd0_remote;
1617     pDev->tx_remote = pDev->rx_remote + SPACEWIRE_BDTABLE_SIZE;
1618
1619     SPACEWIRE_DBG("hw_init [minor %i]\n", pDev->minor);
1620
1621     pDev->config.is_rmap = ctrl & SPW_CTRL_RA;
1622     pDev->config.is_rxunaligned = ctrl & SPW_CTRL_RX;
1623     pDev->config.is_rmapcrc = ctrl & SPW_CTRL_RC;
1624     return 0;
1625 }
1626
1627 static int grspw_hw_waitlink (GRSPW_DEV *pDev, int timeout)
1628 {
1629     int j;
1630
1631     /* No actual link interface on a DMA-only GRSPW2 connected to the
1632      * SPW router
1633      */
1634     if (pDev->core_ver == 3)
1635         return 0;
1636
1637     if ( timeout == -1 ){
1638         /* Wait default timeout */
1639         timeout = SPACEWIRE_INIT_TIMEOUT;
1640     }
1641
1642     j=0;
1643     while (SPW_LINKSTATE(SPW_STATUS_READ(pDev)) != 5) {
1644         if ( timeout < -1 ) {
1645             /* wait forever */
1646         }else if ( j >= timeout ){
1647             /* timeout reached, return fail */
1648             return 1;
1649         }
1650
1651         /* Sleep for 10 ticks */
1652         rtems_task_wake_after(10);
1653         j+=10;
1654     }
1655     return 0;
1656 }
1657
1658 static void grspw_hw_reset(GRSPW_DEV *pDev)
1659 {
1660     SPW_CTRL_WRITE(pDev, SPW_CTRL_RESET); /*reset core*/
1661     SPW_STATUS_WRITE(pDev, SPW_STATUS_TO | SPW_STATUS_CE | SPW_STATUS_ER | SPW_STATUS_DE | SPW_STATUS_PE |
1662              SPW_STATUS_WE | SPW_STATUS_IA | SPW_STATUS_EE); /*clear status*/
1663
1664     /* Add extra writes to make sure we wait the number of clocks required
1665      * after reset
1666      */
1667     SPW_STATUS_WRITE(pDev, SPW_STATUS_TO | SPW_STATUS_CE | SPW_STATUS_ER | SPW_STATUS_DE | SPW_STATUS_PE |
1668         SPW_STATUS_WE | SPW_STATUS_IA | SPW_STATUS_EE); /*clear status*/
1669     SPW_STATUS_WRITE(pDev, SPW_STATUS_TO | SPW_STATUS_CE | SPW_STATUS_ER | SPW_STATUS_DE | SPW_STATUS_PE |
1670         SPW_STATUS_WE | SPW_STATUS_IA | SPW_STATUS_EE); /*clear status*/
1671     SPW_STATUS_WRITE(pDev, SPW_STATUS_TO | SPW_STATUS_CE | SPW_STATUS_ER | SPW_STATUS_DE | SPW_STATUS_PE |
1672         SPW_STATUS_WE | SPW_STATUS_IA | SPW_STATUS_EE); /*clear status*/
1673     SPW_STATUS_WRITE(pDev, SPW_STATUS_TO | SPW_STATUS_CE | SPW_STATUS_ER | SPW_STATUS_DE | SPW_STATUS_PE |
1674         SPW_STATUS_WE | SPW_STATUS_IA | SPW_STATUS_EE); /*clear status*/
1675
1676     SPW_CTRL_WRITE(pDev, SPW_CTRL_LINKSTART); /*start link core*/
1677 }
1678
1679 static void grspw_hw_read_config(GRSPW_DEV *pDev)
1680 {
1681     unsigned int tmp;
1682
1683     tmp = SPW_READ(&pDev->regs->nodeaddr);
1684     pDev->config.nodeaddr = 0xFF & tmp;
1685     pDev->config.nodemask = 0xFF & (tmp>>8);
1686     pDev->config.destkey = 0xFF & SPW_READ(&pDev->regs->destkey);
1687     pDev->config.clkdiv = 0xFFFF & SPW_READ(&pDev->regs->clkdiv);
1688
1689     tmp = SPW_CTRL_READ(pDev);
1690     pDev->config.promiscuous = 1 & (tmp >> 5);
1691     pDev->config.rmapen = 1 & (tmp >> 16);
1692     pDev->config.rmapbufdis = 1 & (tmp >> 17);
1693     pDev->config.is_rmap = 1 & (tmp >> 31);
1694     pDev->config.is_rxunaligned = 1 & (tmp >> 30);
1695     pDev->config.is_rmapcrc = 1 & (tmp >> 29);
1696     pDev->config.linkdisabled = 1 & tmp;
1697     pDev->config.linkstart = 1 & (tmp >> 1);
1698
1699     if ( pDev->core_ver <= 1 ){
1700         tmp = SPW_READ(&pDev->regs->timer);
1701         pDev->config.timer = 0xFFF & tmp;
1702         pDev->config.disconnect = 0x3FF & (tmp >> 12);
1703     }else{
1704         pDev->config.timer = 0;
1705         pDev->config.disconnect = 0;
1706     }
1707
1708     return;
1709 }
1710
1711 /* timeout is given in ticks */
1712 static int grspw_hw_startup (GRSPW_DEV *pDev, int timeout)
1713 {
1714     int i;
1715     unsigned int dmactrl;
1716
1717     SPW_WRITE(&pDev->regs->status, (SPW_STATUS_TO|SPW_STATUS_CE|SPW_STATUS_ER|SPW_STATUS_DE|SPW_STATUS_PE|SPW_STATUS_WE|SPW_STATUS_IA|SPW_STATUS_EE)); /*clear status*/
1718
1719     if (grspw_hw_waitlink(pDev,timeout)) {
1720         SPACEWIRE_DBG2("Device open. Link is not up\n");
1721         return RTEMS_TIMEOUT;
1722     }
1723
1724     SPW_WRITE(&pDev->regs->dma0ctrl, SPW_DMACTRL_PS | SPW_DMACTRL_PR | SPW_DMACTRL_TA | SPW_DMACTRL_RA); /*clear status, set ctrl*/
1725
1726     if ((dmactrl = SPW_READ(&pDev->regs->dma0ctrl)) != 0) {
1727         SPACEWIRE_DBG2("DMACtrl is not cleared\n");
1728         return RTEMS_IO_ERROR;
1729     }
1730
1731     /* prepare transmit buffers */
1732     for (i = 0; i < pDev->txbufcnt; i++) {
1733         pDev->tx[i].ctrl = 0;
1734         pDev->tx[i].addr_header = ((unsigned int)&pDev->ptr_txhbuf0_remote[0]) + (i * pDev->txhbufsize);
1735         pDev->tx[i].addr_data = ((unsigned int)&pDev->ptr_txdbuf0_remote[0]) + (i * pDev->txdbufsize);
1736     }
1737     pDev->tx_cur = 0;
1738     pDev->tx_sent = 0;
1739     pDev->tx_all_in_use = 0;
1740
1741     /* prepare receive buffers */
1742     for (i = 0; i < pDev->rxbufcnt; i++) {
1743         if (i+1 == pDev->rxbufcnt) {
1744             pDev->rx[i].ctrl = SPW_RXBD_IE | SPW_RXBD_EN | SPW_RXBD_WR;
1745         } else {
1746             pDev->rx[i].ctrl = SPW_RXBD_IE | SPW_RXBD_EN;
1747         }
1748         pDev->rx[i].addr = ((unsigned int)&pDev->ptr_rxbuf0_remote[0]) + (i * pDev->rxbufsize);
1749     }
1750     pDev->rxcur = 0;
1751     pDev->rxbufcur = -1;
1752     grspw_set_rxmaxlen(pDev);
1753
1754     SPW_WRITE(&pDev->regs->dma0txdesc, pDev->tx_remote);
1755     SPW_WRITE(&pDev->regs->dma0rxdesc, pDev->rx_remote);
1756
1757     /* start RX */
1758     dmactrl = SPW_READ(&pDev->regs->dma0ctrl);
1759     SPW_WRITE(&pDev->regs->dma0ctrl, (dmactrl & SPW_PREPAREMASK_RX) | SPW_DMACTRL_RD | SPW_DMACTRL_RXEN | SPW_DMACTRL_NS | SPW_DMACTRL_TXIE | SPW_DMACTRL_RXIE);
1760
1761     SPACEWIRE_DBGC(DBGSPW_TX,"0x%x: setup complete\n", (unsigned int)pDev->regs);
1762     return RTEMS_SUCCESSFUL;
1763 }
1764
1765 /* Wait until the receiver is inactive */
1766 static void grspw_hw_wait_rx_inactive(GRSPW_DEV *pDev)
1767 {
1768     while( SPW_READ(&pDev->regs->dma0ctrl) & SPW_DMACTRL_RX ){
1769         /* switching may be needed:
1770          *  - low frequency GRSPW
1771          *  - mega packet incoming
1772          */
1773         rtems_task_wake_after(1);
1774     }
1775 }
1776
1777 /* Stop the rx or/and tx by disabling the receiver/transmitter */
1778 static int grspw_hw_stop (GRSPW_DEV *pDev, int rx, int tx)
1779 {
1780     unsigned int dmactrl;
1781
1782     /* stop rx and/or tx */
1783     dmactrl = SPW_READ(&pDev->regs->dma0ctrl);
1784     if ( rx ) {
1785         dmactrl &= ~(SPW_DMACTRL_RXEN|SPW_DMACTRL_RXIE|SPW_DMACTRL_RD);
1786     }
1787     if ( tx ) {
1788         dmactrl &= ~(SPW_DMACTRL_TXEN|SPW_DMACTRL_TXIE);
1789     }
1790     /*SPW_WRITE(&pDev->regs->dma0ctrl, (dmactrl & SPW_PREPAREMASK_RX) & ~(SPW_DMACTRL_RD | SPW_DMACTRL_RXEN) & ~(SPW_DMACTRL_TXEN));*/
1791
1792     /* don't clear status flags */
1793     dmactrl &= ~(SPW_DMACTRL_RA|SPW_DMACTRL_PR|SPW_DMACTRL_AI);
1794     SPW_WRITE(&pDev->regs->dma0ctrl, dmactrl);
1795     return RTEMS_SUCCESSFUL;
1796 }
1797
1798
1799
1800 int grspw_hw_send(GRSPW_DEV *pDev, unsigned int hlen, char *hdr, unsigned int dlen, char *data, unsigned int options)
1801 {
1802     unsigned int dmactrl, ctrl;
1803 #ifdef DEBUG_SPACEWIRE_ONOFF
1804     unsigned int k;
1805 #endif
1806     rtems_interrupt_level level;
1807     unsigned int cur = pDev->tx_cur, bdctrl;
1808     char *txh = pDev->ptr_txhbuf0 + (cur * pDev->txhbufsize);
1809     char *txd = pDev->ptr_txdbuf0 + (cur * pDev->txdbufsize);
1810     char *txh_remote = pDev->ptr_txhbuf0_remote + (cur * pDev->txhbufsize);
1811     char *txd_remote = pDev->ptr_txdbuf0_remote + (cur * pDev->txdbufsize);
1812
1813     ctrl = SPW_READ((volatile void *)&pDev->tx[cur].ctrl);
1814
1815     if (ctrl & SPW_TXBD_EN) {
1816         return 0;
1817     }
1818
1819     memcpy(&txd[0], data, dlen);
1820     memcpy(&txh[0], hdr, hlen);
1821
1822 #ifdef DEBUG_SPACEWIRE_ONOFF
1823     if (DEBUG_SPACEWIRE_FLAGS & DBGSPW_DUMP) {
1824         for (k = 0; k < hlen; k++){
1825             if (k % 16 == 0) {
1826                 printf ("\n");
1827             }
1828             printf ("%.2x(%c) ",txh[k] & 0xff,isprint(txh[k] & 0xff) ? txh[k] & 0xff : ' ');
1829         }
1830         printf ("\n");
1831     }
1832     if (DEBUG_SPACEWIRE_FLAGS & DBGSPW_DUMP) {
1833         for (k = 0; k < dlen; k++){
1834             if (k % 16 == 0) {
1835                 printf ("\n");
1836             }
1837             printf ("%.2x(%c) ",txd[k] & 0xff,isprint(txd[k] & 0xff) ? txd[k] & 0xff : ' ');
1838         }
1839         printf ("\n");
1840     }
1841 #endif
1842
1843     pDev->tx[cur].addr_header = (unsigned int)txh_remote;
1844     pDev->tx[cur].len = dlen;
1845     pDev->tx[cur].addr_data = (unsigned int)txd_remote;
1846
1847     bdctrl = SPW_TXBD_IE | SPW_TXBD_EN | hlen;
1848     if ( options & GRSPW_PKTSEND_OPTION_HDR_CRC )
1849         bdctrl |= SPW_TXBD_HC;
1850     if ( options & GRSPW_PKTSEND_OPTION_DATA_CRC )
1851         bdctrl |= SPW_TXBD_DC;
1852     bdctrl |= options & GRSPW_PKTSEND_OPTION_NOCRCLEN_MASK;
1853
1854     /* Update counters */
1855     rtems_interrupt_disable(level);
1856     if (pDev->tx_cur == (pDev->txbufcnt - 1) ) {
1857         bdctrl |= SPW_TXBD_WR;
1858     }
1859     pDev->tx[cur].ctrl = bdctrl;
1860
1861     dmactrl = SPW_READ(&pDev->regs->dma0ctrl);
1862     SPW_WRITE(&pDev->regs->dma0ctrl, (dmactrl & SPW_PREPAREMASK_TX) | SPW_DMACTRL_TXEN | SPW_DMACTRL_TXIE);
1863
1864     pDev->tx_cur = (pDev->tx_cur + 1) % pDev->txbufcnt;
1865     if (pDev->tx_cur == pDev->tx_sent) {
1866         pDev->tx_all_in_use = 1;
1867     }
1868     rtems_interrupt_enable(level);
1869
1870     /* In blocking mode wait until message is sent */
1871     if (pDev->config.tx_blocking) {
1872         while ( SPW_READ(&pDev->regs->dma0ctrl) & SPW_DMACTRL_TXEN) {
1873             /* if changed to blocking mode  */
1874             SPACEWIRE_DBGC(DBGSPW_TX, "Tx blocking\n");
1875             rtems_semaphore_obtain(pDev->txsp, RTEMS_WAIT, RTEMS_NO_TIMEOUT);
1876         }
1877     }
1878     SPACEWIRE_DBGC(DBGSPW_TX, "0x%x: transmitted <%i> bytes\n", (unsigned int) pDev->regs, dlen+hlen);
1879     return hlen + dlen;
1880 }
1881
1882 static int grspw_hw_receive(GRSPW_DEV *pDev, char *b, int c) {
1883     unsigned int len, rxlen, ctrl;
1884     unsigned int cur;
1885     unsigned int tmp;
1886     unsigned int dump_start_len;
1887     int i;
1888     char *rxb;
1889
1890     if ( pDev->config.promiscuous || pDev->config.keep_source ) {
1891         dump_start_len = 0; /* make sure address and prot can be read in promiscuous mode */
1892     } else if (pDev->config.rm_prot_id) {
1893         dump_start_len = 2; /* skip source address and protocol id */
1894     } else {
1895         dump_start_len = 1; /* default: skip only source address */
1896     }
1897
1898     rxlen = 0;
1899     cur = pDev->rxcur;
1900     rxb = pDev->ptr_rxbuf0 + (cur * pDev->rxbufsize);
1901
1902     SPACEWIRE_DBGC(DBGSPW_RX, "0x%x: waitin packet at pos %i\n", (unsigned int) pDev->regs, cur);
1903
1904     ctrl = SPW_READ((volatile void *)&pDev->rx[cur].ctrl);
1905     if (ctrl & SPW_RXBD_EN) {
1906         return rxlen;
1907     }
1908     SPACEWIRE_DBGC(DBGSPW_RX, "checking packet\n");
1909
1910     len = SPW_RXBD_LENGTH & ctrl;
1911     if (!((ctrl & SPW_RXBD_ERROR) || (pDev->config.check_rmap_err && (ctrl & SPW_RXBD_RMAPERROR)))) {
1912         if (pDev->rxbufcur == -1) {
1913             SPACEWIRE_DBGC(DBGSPW_RX, "incoming packet len %i\n", len);
1914             pDev->stat.packets_received++;
1915             pDev->rxbufcur = dump_start_len;
1916         }
1917         rxlen = tmp = len - pDev->rxbufcur;
1918         SPACEWIRE_DBGC(DBGSPW_RX, "C %i\n", c);
1919         SPACEWIRE_DBGC(DBGSPW_RX, "Dump %i\n", dump_start_len);
1920         SPACEWIRE_DBGC(DBGSPW_RX, "Bufcur %i\n", pDev->rxbufcur);
1921         SPACEWIRE_DBGC(DBGSPW_RX, "Rxlen %i\n", rxlen );
1922         if (rxlen > c) {
1923             rxlen = c;
1924         }
1925         if (GRLIB_DMA_IS_CACHE_COHERENT) {
1926 /*      if ( 1 ) {*/
1927             /*printf("RX_MEMCPY(0x%x, 0x%x, 0x%x)\n", (unsigned int)b, (unsigned int)(rxb+pDev->rxbufcur), (unsigned int)rxlen);*/
1928             memcpy(b, rxb+pDev->rxbufcur, rxlen);
1929         } else {
1930             int left = rxlen;
1931             /* Copy word wise if Aligned */
1932             if ( (((int)b & 3) == 0) && (((int)(rxb+pDev->rxbufcur) & 3) == 0) ){
1933                 while(left>=32){
1934                     *(int *)(b+0) = MEM_READ32(rxb+pDev->rxbufcur+0);
1935                     *(int *)(b+4) = MEM_READ32(rxb+pDev->rxbufcur+4);
1936                     *(int *)(b+8) = MEM_READ32(rxb+pDev->rxbufcur+8);
1937                     *(int *)(b+12) = MEM_READ32(rxb+pDev->rxbufcur+12);
1938                     *(int *)(b+16) = MEM_READ32(rxb+pDev->rxbufcur+16);
1939                     *(int *)(b+20) = MEM_READ32(rxb+pDev->rxbufcur+20);
1940                     *(int *)(b+24) = MEM_READ32(rxb+pDev->rxbufcur+24);
1941                     *(int *)(b+28) = MEM_READ32(rxb+pDev->rxbufcur+28);
1942                     rxb+=32;
1943                     b+=32;
1944                     left-=32;
1945                 }
1946                 while(left>=4){
1947                     *(int *)b = MEM_READ32(rxb+pDev->rxbufcur);
1948                     rxb+=4;
1949                     b+=4;
1950                     left-=4;
1951                 }
1952             }
1953             for(i = 0; i < left; i++) {
1954                 b[i] = MEM_READ8(rxb+pDev->rxbufcur+i);
1955             }
1956         }
1957
1958         pDev->rxbufcur += rxlen;
1959         if (c >= tmp) {
1960             SPACEWIRE_DBGC(DBGSPW_RX, "Next descriptor\n");
1961             grspw_rxnext(pDev);
1962         }
1963     } else {
1964         check_rx_errors(pDev, ctrl);
1965         grspw_rxnext(pDev);
1966     }
1967     return rxlen;
1968 }
1969
1970 static void grspw_rxnext(GRSPW_DEV *pDev)
1971 {
1972     unsigned int dmactrl;
1973     unsigned int cur = pDev->rxcur;
1974     unsigned int ctrl = 0;
1975     rtems_interrupt_level level;
1976
1977     rtems_interrupt_disable(level);
1978
1979     if (cur == (pDev->rxbufcnt - 1)) {
1980         pDev->rx[cur].ctrl = ctrl | SPW_RXBD_EN | SPW_RXBD_IE | SPW_RXBD_WR;
1981         cur = 0;
1982     } else {
1983         pDev->rx[cur].ctrl = ctrl | SPW_RXBD_EN | SPW_RXBD_IE;
1984         cur++;
1985     }
1986
1987     pDev->rxcur = cur;
1988     pDev->rxbufcur = -1;
1989
1990     /* start RX */
1991     dmactrl = SPW_READ(&pDev->regs->dma0ctrl);
1992     SPW_WRITE(&pDev->regs->dma0ctrl, (dmactrl & SPW_PREPAREMASK_RX) | SPW_DMACTRL_RD | SPW_DMACTRL_RXEN | SPW_DMACTRL_RXIE | SPW_DMACTRL_NS);
1993
1994     rtems_interrupt_enable(level);
1995 }
1996
1997 static void check_rx_errors(GRSPW_DEV *pDev, int ctrl)
1998 {
1999     if (ctrl & SPW_RXBD_EEP) {
2000         pDev->stat.rx_eep_err++;
2001     }
2002     if (ctrl & SPW_RXBD_EHC) {
2003         if (pDev->config.check_rmap_err) {
2004             pDev->stat.rx_rmap_header_crc_err++;
2005         }
2006     }
2007     if (ctrl & SPW_RXBD_EDC) {
2008         if (pDev->config.check_rmap_err) {
2009             pDev->stat.rx_rmap_data_crc_err++;
2010         }
2011     }
2012     if (ctrl & SPW_RXBD_ETR) {
2013         pDev->stat.rx_truncated++;
2014     }
2015 }
2016
2017
2018 static void grspw_print_dev(struct drvmgr_dev *dev, int options)
2019 {
2020     GRSPW_DEV *pDev = dev->priv;
2021
2022     /* Print */
2023     printf("--- GRSPW %s ---\n", pDev->devName);
2024     printf(" REGS:            0x%x\n", (unsigned int)pDev->regs);
2025     printf(" IRQ:             %d\n", pDev->irq);
2026     printf(" CORE VERSION:    %d\n", pDev->core_ver);
2027     printf(" CTRL:            0x%x\n", pDev->regs->ctrl);
2028     printf(" STATUS:          0x%x\n", pDev->regs->status);
2029     printf(" DMA0CTRL:        0x%x\n", pDev->regs->dma0ctrl);
2030     printf(" TXBD:            0x%x\n", (unsigned int)pDev->tx);
2031     printf(" RXBD:            0x%x\n", (unsigned int)pDev->rx);
2032 }
2033
2034 void grspw_print(int options)
2035 {
2036     struct amba_drv_info *drv = &grspw_drv_info;
2037     struct drvmgr_dev *dev;
2038
2039     dev = drv->general.dev;
2040     while(dev) {
2041         grspw_print_dev(dev, options);
2042         dev = dev->next_in_drv;
2043     }
2044 }