grlib/tmtc/grtc.c

0001 /* SPDX-License-Identifier: BSD-2-Clause */
0002
0003 /* GRTC Telecommand decoder driver
0004  *
0005  *  COPYRIGHT (c) 2007.
0006  *  Cobham Gaisler AB.
0007  *
0008  * Redistribution and use in source and binary forms, with or without
0009  * modification, are permitted provided that the following conditions
0010  * are met:
0011  * 1. Redistributions of source code must retain the above copyright
0012  *    notice, this list of conditions and the following disclaimer.
0013  * 2. Redistributions in binary form must reproduce the above copyright
0014  *    notice, this list of conditions and the following disclaimer in the
0015  *    documentation and/or other materials provided with the distribution.
0016  *
0017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
0018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
0019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
0020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
0021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
0022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
0023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
0024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
0025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
0026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
0027  * POSSIBILITY OF SUCH DAMAGE.
0028  */
0029
0030 #include <bsp.h>
0031 #include <rtems/libio.h>
0032 #include <stdlib.h>
0033 #include <stdio.h>
0034 #include <string.h>
0035 #include <assert.h>
0036 #include <ctype.h>
0037 #include <rtems/bspIo.h>
0038
0039 #include <drvmgr/drvmgr.h>
0040 #include <grlib/ambapp_bus.h>
0041 #include <grlib/ambapp.h>
0042 #include <grlib/grtc.h>
0043
0044 #include <grlib/grlib_impl.h>
0045
0046 /*
0047 #define DEBUG
0048 #define DEBUGFUNCS
0049 */
0050
0051 #include <grlib/debug_defs.h>
0052
0053 #ifdef DEBUG_ERROR
0054 #define DEBUG_ERR_LOG(device,error) grtc_log_error(device,error)
0055 #else
0056 #define DEBUG_ERR_LOG(device,error)
0057 #endif
0058
0059 /* GRTC register map */
0060 struct grtc_regs {
0061     volatile unsigned int   grst;   /* Global Reset Register (GRR 0x00) */
0062     volatile unsigned int   gctrl;  /* Global Control Register (GCR 0x04) */
0063     int unused0;
0064     volatile unsigned int   sir;    /* Spacecraft Identifier Register (SIR 0x0c) */
0065     volatile unsigned int   far;    /* Frame Acceptance Report Register (FAR 0x10) */
0066
0067     volatile unsigned int   clcw1;  /* CLCW Register 1 (CLCWR1 0x14) */
0068     volatile unsigned int   clcw2;  /* CLCW Register 2 (CLCWR2 0x18) */
0069     volatile unsigned int   phir;   /* Physical Interface Register (PHIR 0x1c) */
0070     volatile unsigned int   cor;    /* Control Register (COR 0x20) */
0071
0072     volatile unsigned int   str;    /* Status Register (STR 0x24) */
0073     volatile unsigned int   asr;    /* Address Space Register (ASR 0x28) */
0074     volatile unsigned int   rp; /* Receive Read Pointer Register (RRP 0x2c) */
0075     volatile unsigned int   wp; /* Receive Write Pointer Register (RWP 0x30) */
0076
0077     int unused1[(0x60-0x34)/4];
0078
0079     volatile unsigned int   pimsr;  /* Pending Interrupt Masked Status Register (PIMSR 0x60) */
0080     volatile unsigned int   pimr;   /* Pending Interrupt Masked Register (PIMR 0x64) */
0081     volatile unsigned int   pisr;   /* Pending Interrupt Status Register (PISR 0x68) */
0082     volatile unsigned int   pir;    /* Pending Interrupt Register (PIR 0x6c) */
0083     volatile unsigned int   imr;    /* Interrupt Mask Register (IMR 0x70) */
0084     volatile unsigned int   picr;   /* Pending Interrupt Clear Register (PICR 0x74) */
0085 };
0086
0087 /* Security Byte */
0088 #define GRTC_SEB        0x55000000
0089
0090 /* Global Reset Register (GRR 0x00) */
0091 #define GRTC_GRR_SRST       0x1
0092 #define GRTC_GRR_SRST_BIT   0
0093
0094 /* Global Control Register (GCR 0x04) */
0095 #define GRTC_GCR_PSR_BIT    10
0096 #define GRTC_GCR_NRZM_BIT   11
0097 #define GRTC_GCR_PSS_BIT    12
0098
0099 #define GRTC_GCR_PSR        (1<<GRTC_GCR_PSR_BIT)
0100 #define GRTC_GCR_NRZM       (1<<GRTC_GCR_NRZM_BIT)
0101 #define GRTC_GCR_PSS        (1<<GRTC_GCR_PSS_BIT)
0102
0103 /* Spacecraft Identifier Register (SIR 0x0c) */
0104
0105
0106 /* Frame Acceptance Report Register (FAR 0x10) */
0107 #define GRTC_FAR_SCI_BIT    10
0108 #define GRTC_FAR_CSEC_BIT   11
0109 #define GRTC_FAR_CAC_BIT    12
0110 #define GRTC_FAR_SSD_BIT    13
0111
0112 #define GRTC_FAR_SCI        (0x7<<GRTC_FAR_SCI_BIT)
0113 #define GRTC_FAR_CSEC       (0x7<<GRTC_FAR_CSEC_BIT)
0114 #define GRTC_FAR_CAC        (0x3f<<GRTC_FAR_CAC_BIT)
0115 #define GRTC_FAR_SSD        (1<<GRTC_FAR_SSD_BIT)
0116
0117 /* CLCW Register 1 (CLCWR1 0x14) */
0118 /* CLCW Register 2 (CLCWR2 0x18) */
0119 #define GRTC_CLCW_RVAL_BIT  0
0120 #define GRTC_CLCW_RTYPE_BIT 8
0121 #define GRTC_CLCW_FBCO_BIT  9
0122 #define GRTC_CLCW_RTMI_BIT  11
0123 #define GRTC_CLCW_WAIT_BIT  12
0124 #define GRTC_CLCW_LOUT_BIT  13
0125 #define GRTC_CLCW_NBLO_BIT  14
0126 #define GRTC_CLCW_NRFA_BIT  15
0127 #define GRTC_CLCW_VCI_BIT   18
0128 #define GRTC_CLCW_CIE_BIT   24
0129 #define GRTC_CLCW_STAF_BIT  26
0130 #define GRTC_CLCW_VNUM_BIT  29
0131 #define GRTC_CLCW_CWTY_BIT  31
0132
0133 #define GRTC_CLCW_RVAL      (0xff<<GRTC_CLCW_RVAL_BIT)
0134 #define GRTC_CLCW_RTYPE     (1<<GRTC_CLCW_RTYPE_BIT)
0135 #define GRTC_CLCW_FBCO      (0x3<<GRTC_CLCW_FBCO_BIT)
0136 #define GRTC_CLCW_RTMI      (0x3<<GRTC_CLCW_RTMI_BIT)
0137 #define GRTC_CLCW_WAIT      (1<<GRTC_CLCW_WAIT_BIT)
0138 #define GRTC_CLCW_LOUT      (1<<GRTC_CLCW_LOUT_BIT)
0139 #define GRTC_CLCW_NBLO      (1<<GRTC_CLCW_NBLO_BIT)
0140 #define GRTC_CLCW_NRFA      (1<<GRTC_CLCW_NRFA_BIT)
0141 #define GRTC_CLCW_VCI       (0x3f<<GRTC_CLCW_VCI_BIT)
0142 #define GRTC_CLCW_CIE       (0x3<<GRTC_CLCW_CIE_BIT)
0143 #define GRTC_CLCW_STAF      (0x3<<GRTC_CLCW_STAF_BIT)
0144 #define GRTC_CLCW_VNUM      (0x3<<GRTC_CLCW_VNUM_BIT)
0145 #define GRTC_CLCW_CWTY      (1<<GRTC_CLCW_CWTY_BIT)
0146
0147 /* Physical Interface Register (PIR 0x1c) */
0148 #define GRTC_PIR_BLO_BIT    0
0149 #define GRTC_PIR_RFA_BIT    8
0150
0151 #define GRTC_PIR_BLO        (0xff<<GRTC_PIR_BLO_BIT)
0152 #define GRTC_PIR_RFA        (0xff<<GRTC_PIR_RFA_BIT)
0153
0154 /* Control Register (COR 0x20) */
0155 #define GRTC_COR_RE_BIT     0
0156 #define GRTC_COR_CRST_BIT   9
0157
0158 #define GRTC_COR_RE     (1<<GRTC_COR_RE_BIT)
0159 #define GRTC_COR_CRST       (1<<GRTC_COR_CRST_BIT)
0160
0161 /* Status Register (STR 0x24) */
0162 #define GRTC_STR_CR_BIT     0
0163 #define GRTC_STR_OV_BIT     4
0164 #define GRTC_STR_RFF_BIT    7
0165 #define GRTC_STR_RBF_BIT    10
0166
0167 #define GRTC_STR_CR     (1<<GRTC_STR_CR_BIT)
0168 #define GRTC_STR_OV     (1<<GRTC_STR_OV_BIT)
0169 #define GRTC_STR_RFF        (1<<GRTC_STR_RFF_BIT)
0170 #define GRTC_STR_RBF        (1<<GRTC_STR_RBF_BIT)
0171
0172 /* Address Space Register (ASR 0x28) */
0173 #define GRTC_ASR_RXLEN_BIT  0
0174 #define GRTC_ASR_BUFST_BIT  10
0175
0176 #define GRTC_ASR_RXLEN      (0xff<<GRTC_ASR_RXLEN_BIT)
0177 #define GRTC_ASR_BUFST      (0x3fffff<<GRTC_ASR_BUFST_BIT)
0178
0179 /* Receive Read Pointer Register (RRP 0x2c) */
0180 #define GRTC_RRP_PTR_BIT    0
0181
0182 #define GRTC_RRP_PTR        (0xffffff<<GRTC_RRP_PTR_BIT)
0183
0184 /* Receive Write Pointer Register (RWP 0x30) */
0185 #define GRTC_RWP_PTR_BIT    0
0186
0187 #define GRTC_RWP_PTR        (0xffffff<<GRTC_RWP_PTR_BIT)
0188
0189 /* Pending Interrupt Masked Status Register (PIMSR 0x60) */
0190 /* Pending Interrupt Masked Register (PIMR 0x64) */
0191 /* Pending Interrupt Status Register (PISR 0x68) */
0192 /* Pending Interrupt Register (PIR 0x6c) */
0193 /* Interrupt Mask Register (IMR 0x70) */
0194 /* Pending Interrupt Clear Register (PICR 0x74) */
0195 #define GRTC_INT_RFA_BIT    0
0196 #define GRTC_INT_BLO_BIT    1
0197 #define GRTC_INT_FAR_BIT    2
0198 #define GRTC_INT_CR_BIT     3
0199 #define GRTC_INT_RBF_BIT    4
0200 #define GRTC_INT_OV_BIT     5
0201 #define GRTC_INT_CS_BIT     6
0202
0203 #define GRTC_INT_RFA        (1<<GRTC_INT_RFA_BIT)
0204 #define GRTC_INT_BLO        (1<<GRTC_INT_BLO_BIT)
0205 #define GRTC_INT_FAR        (1<<GRTC_INT_FAR_BIT)
0206 #define GRTC_INT_CR     (1<<GRTC_INT_CR_BIT)
0207 #define GRTC_INT_OV     (1<<GRTC_INT_OV_BIT)
0208 #define GRTC_INT_CS     (1<<GRTC_INT_CS_BIT)
0209
0210 #define GRTC_INT_ALL        (GRTC_INT_RFA|GRTC_INT_BLO|GRTC_INT_FAR|GRTC_INT_CR|GRTC_INT_OV|GRTC_INT_CS)
0211
0212 #define READ_REG(address)   (*(volatile unsigned int *)address)
0213
0214 /* Driver functions */
0215 static rtems_device_driver grtc_initialize(rtems_device_major_number  major, rtems_device_minor_number  minor, void *arg);
0216 static rtems_device_driver grtc_open(rtems_device_major_number major, rtems_device_minor_number minor, void *arg);
0217 static rtems_device_driver grtc_close(rtems_device_major_number major, rtems_device_minor_number minor, void *arg);
0218 static rtems_device_driver grtc_read(rtems_device_major_number major, rtems_device_minor_number minor, void *arg);
0219 static rtems_device_driver grtc_write(rtems_device_major_number major, rtems_device_minor_number minor, void *arg);
0220 static rtems_device_driver grtc_ioctl(rtems_device_major_number major, rtems_device_minor_number minor, void *arg);
0221
0222 #define GRTC_DRIVER_TABLE_ENTRY { grtc_initialize, grtc_open, grtc_close, grtc_read, grtc_write, grtc_ioctl }
0223
0224 static rtems_driver_address_table grtc_driver = GRTC_DRIVER_TABLE_ENTRY;
0225
0226 enum {
0227     FRM_STATE_NONE = 0,     /* not started */
0228     FRM_STATE_HDR = 1,      /* Reading Header (Frame length isn't known) */
0229     FRM_STATE_ALLOC = 2,        /* Allocate Frame to hold data */
0230     FRM_STATE_PAYLOAD = 3,      /* Reading Payload (Frame length is known) */
0231     FRM_STATE_FILLER = 4,       /* Check filler */
0232     FRM_STATE_DROP = 5      /* error, drop data until end marker */
0233 };
0234
0235 /* Frame pool, all frames in pool have the same buffer length (frame mode only) */
0236 struct grtc_frame_pool {
0237     unsigned int        frame_len;  /* Maximal length of frame (payload+hdr+crc..) */
0238     unsigned int        frame_cnt;  /* Current number of frames in pool (in frms) */
0239     struct grtc_frame   *frms;      /* Chain of frames in pool (this is the pool) */
0240 };
0241
0242 struct grtc_priv {
0243     struct drvmgr_dev       *dev;       /* Driver manager device */
0244     char            devName[52];    /* Device Name */
0245     struct grtc_regs    *regs;      /* TC Hardware Register MAP */
0246     int         irq;        /* IRQ number of TC core */
0247     SPIN_DECLARE(devlock);          /* spin-lock of registers */
0248
0249     int         major;      /* Driver major */
0250     int         minor;      /* Device Minor */
0251
0252     int         open;       /* Device has been opened by user */
0253     int         running;    /* TC receiver running */
0254     int         mode;       /* RAW or FRAME mode */
0255     int         overrun_condition;  /* Overrun condition */
0256     int         blocking;   /* Blocking/polling mode */
0257     rtems_interval      timeout;    /* Timeout in blocking mode */
0258     int         wait_for_nbytes;/* Number of bytes to wait for in blocking mode */
0259
0260     struct grtc_ioc_config  config;
0261
0262 /* RAW MODE ONLY */
0263     /* Buffer allocation (user provided or driver allocated using malloc) */
0264     void            *buf;
0265     void            *buf_remote;
0266     void            *_buf;
0267     int         buf_custom; /* 0=no custom buffer, 1=custom buffer (don't free it...) */
0268     unsigned int        len;
0269
0270 /* FRAME MODE ONLY */
0271     /* Frame management when user provides buffers. */
0272     int         pool_cnt;   /* Number of Pools */
0273     struct grtc_frame_pool  *pools;     /* Array of pools */
0274
0275     struct grtc_list    ready;      /* Ready queue (received frames) */
0276
0277     /* Frame read data (Frame mode only) */
0278     int         frame_state;
0279     int         filler;
0280     unsigned int        hdr[2];     /* 5 byte header */
0281     struct grtc_frame   *frm;       /* Frame currently beeing copied */
0282     int         frmlen;
0283
0284     struct grtc_ioc_stats   stats;      /* Statistics */
0285
0286     rtems_id sem_rx;
0287
0288 #ifdef DEBUG_ERROR
0289     /* Buffer read/write state */
0290     unsigned int rp;
0291     unsigned int    wp;
0292
0293     /* Debugging */
0294     int last_error[128];
0295     int last_error_cnt;
0296 #endif
0297 };
0298
0299 /* Prototypes */
0300 static void grtc_hw_reset(struct grtc_priv *priv);
0301 static void grtc_interrupt(void *arg);
0302
0303 /* Common Global Variables */
0304 static rtems_id grtc_dev_sem;
0305 static int grtc_driver_io_registered = 0;
0306 static rtems_device_major_number grtc_driver_io_major = 0;
0307
0308 /******************* Driver manager interface ***********************/
0309
0310 /* Driver prototypes */
0311 static int grtc_register_io(rtems_device_major_number *m);
0312 static int grtc_device_init(struct grtc_priv *pDev);
0313
0314 static int grtc_init2(struct drvmgr_dev *dev);
0315 static int grtc_init3(struct drvmgr_dev *dev);
0316
0317 static struct drvmgr_drv_ops grtc_ops =
0318 {
0319     {NULL, grtc_init2, grtc_init3, NULL},
0320     NULL,
0321     NULL,
0322 };
0323
0324 static struct amba_dev_id grtc_ids[] =
0325 {
0326     {VENDOR_GAISLER, GAISLER_GRTC},
0327     {0, 0}      /* Mark end of table */
0328 };
0329
0330 static struct amba_drv_info grtc_drv_info =
0331 {
0332     {
0333         DRVMGR_OBJ_DRV,         /* Driver */
0334         NULL,               /* Next driver */
0335         NULL,               /* Device list */
0336         DRIVER_AMBAPP_GAISLER_GRTC_ID,  /* Driver ID */
0337         "GRTC_DRV",         /* Driver Name */
0338         DRVMGR_BUS_TYPE_AMBAPP,     /* Bus Type */
0339         &grtc_ops,
0340         NULL,               /* Funcs */
0341         0,              /* No devices yet */
0342         sizeof(struct grtc_priv),
0343     },
0344     &grtc_ids[0]
0345 };
0346
0347 void grtc_register_drv (void)
0348 {
0349     DBG("Registering GRTC driver\n");
0350     drvmgr_drv_register(&grtc_drv_info.general);
0351 }
0352
0353 static int grtc_init2(struct drvmgr_dev *dev)
0354 {
0355     struct grtc_priv *priv;
0356
0357     DBG("GRTC[%d] on bus %s\n", dev->minor_drv, dev->parent->dev->name);
0358     priv = dev->priv;
0359     if ( !priv )
0360         return DRVMGR_NOMEM;
0361     priv->dev = dev;
0362
0363     /* This core will not find other cores, so we wait for init2() */
0364
0365     return DRVMGR_OK;
0366 }
0367
0368 static int grtc_init3(struct drvmgr_dev *dev)
0369 {
0370     struct grtc_priv *priv;
0371     char prefix[32];
0372     rtems_status_code status;
0373
0374     priv = dev->priv;
0375
0376     /* Do initialization */
0377
0378     if ( grtc_driver_io_registered == 0) {
0379         /* Register the I/O driver only once for all cores */
0380         if ( grtc_register_io(&grtc_driver_io_major) ) {
0381             /* Failed to register I/O driver */
0382             dev->priv = NULL;
0383             return DRVMGR_FAIL;
0384         }
0385
0386         grtc_driver_io_registered = 1;
0387     }
0388
0389     /* I/O system registered and initialized
0390      * Now we take care of device initialization.
0391      */
0392     if ( grtc_device_init(priv) ) {
0393         return DRVMGR_FAIL;
0394     }
0395
0396     /* Get Filesystem name prefix */
0397     prefix[0] = '\0';
0398     if ( drvmgr_get_dev_prefix(dev, prefix) ) {
0399         /* Failed to get prefix, make sure of a unique FS name
0400          * by using the driver minor.
0401          */
0402         sprintf(priv->devName, "/dev/grtc%d", dev->minor_drv);
0403     } else {
0404         /* Got special prefix, this means we have a bus prefix
0405          * And we should use our "bus minor"
0406          */
0407         sprintf(priv->devName, "/dev/%sgrtc%d", prefix, dev->minor_bus);
0408     }
0409
0410     SPIN_INIT(&priv->devlock, priv->devName);
0411
0412     /* Register Device */
0413     status = rtems_io_register_name(priv->devName, grtc_driver_io_major, dev->minor_drv);
0414     if (status != RTEMS_SUCCESSFUL) {
0415         return DRVMGR_FAIL;
0416     }
0417
0418     return DRVMGR_OK;
0419 }
0420
0421 /******************* Driver Implementation ***********************/
0422
0423 static int grtc_register_io(rtems_device_major_number *m)
0424 {
0425     rtems_status_code r;
0426
0427     if ((r = rtems_io_register_driver(0, &grtc_driver, m)) == RTEMS_SUCCESSFUL) {
0428         DBG("GRTC driver successfully registered, major: %d\n", *m);
0429     } else {
0430         switch(r) {
0431         case RTEMS_TOO_MANY:
0432             printk("GRTC rtems_io_register_driver failed: RTEMS_TOO_MANY\n");
0433             return -1;
0434         case RTEMS_INVALID_NUMBER:
0435             printk("GRTC rtems_io_register_driver failed: RTEMS_INVALID_NUMBER\n");
0436             return -1;
0437         case RTEMS_RESOURCE_IN_USE:
0438             printk("GRTC rtems_io_register_driver failed: RTEMS_RESOURCE_IN_USE\n");
0439             return -1;
0440         default:
0441             printk("GRTC rtems_io_register_driver failed\n");
0442             return -1;
0443         }
0444     }
0445     return 0;
0446 }
0447
0448 static int grtc_device_init(struct grtc_priv *pDev)
0449 {
0450     struct amba_dev_info *ambadev;
0451     struct ambapp_core *pnpinfo;
0452
0453     /* Get device information from AMBA PnP information */
0454     ambadev = (struct amba_dev_info *)pDev->dev->businfo;
0455     if ( ambadev == NULL ) {
0456         return -1;
0457     }
0458     pnpinfo = &ambadev->info;
0459     pDev->irq = pnpinfo->irq;
0460     pDev->regs = (struct grtc_regs *)pnpinfo->ahb_slv->start[0];
0461     pDev->minor = pDev->dev->minor_drv;
0462     pDev->open = 0;
0463     pDev->running = 0;
0464
0465     /* Create Binary RX Semaphore with count = 0 */
0466     if ( rtems_semaphore_create(rtems_build_name('G', 'R', 'C', '0' + pDev->minor),
0467         0,
0468         RTEMS_FIFO|RTEMS_SIMPLE_BINARY_SEMAPHORE|RTEMS_NO_INHERIT_PRIORITY|\
0469         RTEMS_LOCAL|RTEMS_NO_PRIORITY_CEILING,
0470         0,
0471         &pDev->sem_rx) != RTEMS_SUCCESSFUL ) {
0472         return -1;
0473     }
0474
0475     /* Reset Hardware before attaching IRQ handler */
0476     grtc_hw_reset(pDev);
0477
0478     return 0;
0479 }
0480
0481 static void grtc_hw_reset(struct grtc_priv *priv)
0482 {
0483     /* Reset Core */
0484     priv->regs->grst = GRTC_SEB | GRTC_GRR_SRST;
0485 }
0486
0487 static void grtc_hw_get_defaults(struct grtc_priv *pDev, struct grtc_ioc_config *config)
0488 {
0489     unsigned int gcr = READ_REG(&pDev->regs->gctrl);
0490
0491     config->psr_enable  = (gcr & GRTC_GCR_PSR)  ? 1:0;
0492     config->nrzm_enable = (gcr & GRTC_GCR_NRZM) ? 1:0;
0493     config->pss_enable  = (gcr & GRTC_GCR_PSS)  ? 1:0;
0494
0495     config->crc_calc    = 0;
0496 }
0497
0498 /* bufsize is given in bytes */
0499 static int __inline__ grtc_hw_data_avail_upper(unsigned int rrp, unsigned rwp, unsigned int bufsize)
0500 {
0501     if ( rrp == rwp )
0502         return 0;
0503
0504     if ( rwp > rrp ) {
0505         return rwp-rrp;
0506     }
0507
0508     return (bufsize-rrp);
0509 }
0510
0511 /* bufsize is given in bytes */
0512 static int __inline__ grtc_hw_data_avail_lower(unsigned int rrp, unsigned rwp, unsigned int bufsize)
0513 {
0514     if ( rrp == rwp )
0515         return 0;
0516
0517     if ( rwp > rrp ) {
0518         return 0;
0519     }
0520
0521     return rwp;
0522 }
0523
0524 /* bufsize is given in bytes */
0525 static int __inline__ grtc_hw_data_avail(unsigned int rrp, unsigned rwp, unsigned int bufsize)
0526 {
0527     if ( rrp == rwp )
0528         return 0;
0529
0530     if ( rwp > rrp ) {
0531         return rwp-rrp;
0532     }
0533
0534     return rwp+(bufsize-rrp);
0535 }
0536
0537 /* Reads as much as possible but not more than 'max' bytes from the TC receive buffer.
0538  * Number of bytes put into 'buf' is returned.
0539  */
0540 static int grtc_hw_read_try(struct grtc_priv *pDev, char *buf, int max)
0541 {
0542     struct grtc_regs *regs = pDev->regs;
0543     unsigned int rp, wp, asr, bufmax, rrp, rwp;
0544     unsigned int upper, lower;
0545     unsigned int count, cnt, left;
0546
0547     FUNCDBG();
0548
0549     if ( max < 1 )
0550         return 0;
0551
0552     rp = READ_REG(&regs->rp);
0553     asr = READ_REG(&regs->asr);
0554     bufmax = (asr & GRTC_ASR_RXLEN) >> GRTC_ASR_RXLEN_BIT;
0555     bufmax = (bufmax+1) << 10; /* Convert from 1kbyte blocks into bytes */
0556     wp = READ_REG(&regs->wp);
0557
0558     /* Relative rp and wp */
0559     rrp = rp - (asr & GRTC_ASR_BUFST);
0560     rwp = wp - (asr & GRTC_ASR_BUFST);
0561
0562     lower = grtc_hw_data_avail_lower(rrp,rwp,bufmax);
0563     upper = grtc_hw_data_avail_upper(rrp,rwp,bufmax);
0564
0565     DBG("grtc_hw_read_try: AVAIL: Lower: %d, Upper: %d\n",lower,upper);
0566     DBG("grtc_hw_read_try: rp: 0x%x, rrp: 0x%x, wp: 0x%x, rwp: 0x%x, bufmax: %d\n, start: 0x%x\n",
0567         rp,rrp,wp,rwp,bufmax,pDev->buffer);
0568
0569     if ( (upper+lower) == 0 )
0570         return 0;
0571
0572     /* Count bytes will be read */
0573     count = (upper+lower) > max ? max : (upper+lower);
0574     left = count;
0575
0576     /* Read from upper part of data buffer */
0577     if ( upper > 0 ){
0578         if ( left < upper ){
0579             cnt = left;
0580         }else{
0581             cnt = upper;    /* Read all upper data available */
0582         }
0583         DBG("grtc_hw_read_try: COPYING %d from upper\n",cnt);
0584         /* Convert from Remote address (RP) into CPU Local address */
0585         memcpy(buf, (void *)((rp - (unsigned int)pDev->buf_remote) + (unsigned int)pDev->buf), cnt);
0586         buf += cnt;
0587         left -= cnt;
0588     }
0589
0590     /* Read from lower part of data buffer */
0591     if ( left > 0 ){
0592         if ( left < lower ){
0593             cnt = left;
0594         }else{
0595             cnt = lower;    /* Read all lower data available */
0596         }
0597         DBG("grtc_hw_read_try: COPYING %d from lower\n",cnt);
0598         memcpy(buf, (void *)pDev->buf, cnt);
0599         buf += cnt;
0600         left -= cnt;
0601     }
0602
0603     /* Update hardware RP pointer to tell hardware about new space available */
0604     if ( (rp+count) >= ((asr&GRTC_ASR_BUFST)+bufmax) ){
0605         regs->rp = (rp+count-bufmax);
0606     } else {
0607         regs->rp = rp+count;
0608     }
0609
0610     return count;
0611 }
0612
0613 /* Reads as much as possible but not more than 'max' bytes from the TC receive buffer.
0614  * Number of bytes put into 'buf' is returned.
0615  */
0616 static int grtc_data_avail(struct grtc_priv *pDev)
0617 {
0618     unsigned int rp, wp, asr, bufmax, rrp, rwp;
0619     struct grtc_regs *regs = pDev->regs;
0620
0621     FUNCDBG();
0622
0623     rp = READ_REG(&regs->rp);
0624     asr = READ_REG(&regs->asr);
0625     bufmax = (asr & GRTC_ASR_RXLEN) >> GRTC_ASR_RXLEN_BIT;
0626     bufmax = (bufmax+1) << 10; /* Convert from 1kbyte blocks into bytes */
0627     wp = READ_REG(&regs->wp);
0628
0629     /* Relative rp and wp */
0630     rrp = rp - (asr & GRTC_ASR_BUFST);
0631     rwp = wp - (asr & GRTC_ASR_BUFST);
0632
0633     return grtc_hw_data_avail(rrp,rwp,bufmax);
0634 }
0635
0636 static void *grtc_memalign(unsigned int boundary, unsigned int length, void *realbuf)
0637 {
0638     *(int *)realbuf = (int)grlib_malloc(length+(~GRTC_ASR_BUFST)+1);
0639     DBG("GRTC: Alloced %d (0x%x) bytes, requested: %d\n",length+(~GRTC_ASR_BUFST)+1,length+(~GRTC_ASR_BUFST)+1,length);
0640     return (void *)(((*(unsigned int *)realbuf)+(~GRTC_ASR_BUFST)+1) & ~(boundary-1));
0641 }
0642
0643 static int grtc_start(struct grtc_priv *pDev)
0644 {
0645     struct grtc_regs *regs = pDev->regs;
0646     unsigned int tmp;
0647
0648     if ( !pDev->buf || (((unsigned int)pDev->buf & ~GRTC_ASR_BUFST) != 0) ||
0649          (pDev->len>(1024*0x100)) || (pDev->len<1024) || ((pDev->len & (1024-1)) != 0)
0650        ) {
0651         DBG("GRTC: start: buffer not properly allocated(0x%x,0x%x,0x%x,0x%x)\n",pDev->buf,pDev->len,((unsigned int)pDev->buf & ~GRTC_ASR_BUFST),(pDev->len & ~(1024-1)));
0652         return RTEMS_NO_MEMORY;
0653     }
0654
0655     memset(pDev->buf,0,pDev->len);
0656
0657     /* Software init */
0658     pDev->overrun_condition = 0;
0659 #ifdef DEBUG_ERROR
0660     pDev->last_error_cnt = 0;
0661     memset(&pDev->last_error[0],0,128*sizeof(int));
0662 #endif
0663     memset(&pDev->stats,0,sizeof(struct grtc_ioc_stats));
0664
0665     /* Reset the receiver */
0666     regs->cor = GRTC_SEB | GRTC_COR_CRST;
0667     if ( READ_REG(&regs->cor) & GRTC_COR_CRST ){
0668         /* Reset Failed */
0669         DBG("GRTC: start: Reseting receiver failed\n");
0670         return RTEMS_IO_ERROR;
0671     }
0672
0673     /* make sure the RX semaphore is in the correct state when starting.
0674      * In case of a previous overrun condition it could be in incorrect
0675      * state (where rtems_semaphore_flush was used).
0676      */
0677     rtems_semaphore_obtain(pDev->sem_rx, RTEMS_NO_WAIT, 0);
0678
0679     /* Set operating modes */
0680     tmp = 0;
0681     if ( pDev->config.psr_enable )
0682         tmp |= GRTC_GCR_PSR;
0683     if ( pDev->config.nrzm_enable )
0684         tmp |= GRTC_GCR_NRZM;
0685     if ( pDev->config.pss_enable )
0686         tmp |= GRTC_GCR_PSS;
0687     regs->gctrl = GRTC_SEB | tmp;
0688
0689     /* Clear any pending interrupt */
0690     tmp = READ_REG(&regs->pir);
0691     regs->picr = GRTC_INT_ALL;
0692
0693     /* Unmask only the Overrun interrupt */
0694     regs->imr = GRTC_INT_OV;
0695
0696     /* Set up DMA registers
0697      * 1. Let hardware know about our DMA area (size and location)
0698      * 2. Set DMA read/write posistions to zero.
0699      */
0700     regs->asr = (unsigned int)pDev->buf_remote | ((pDev->len>>10)-1);
0701     regs->rp = (unsigned int)pDev->buf_remote;
0702
0703     /* Mark running before enabling the receiver, we could receive
0704      * an interrupt directly after enabling the receiver and it would
0705      * then interpret the interrupt as spurious (see interrupt handler)
0706      */
0707     pDev->running = 1;
0708
0709     /* Enable receiver */
0710     regs->cor = GRTC_SEB | GRTC_COR_RE;
0711
0712     DBG("GRTC: STARTED\n");
0713
0714     return 0;
0715 }
0716
0717 static void grtc_stop(struct grtc_priv *pDev, int overrun)
0718 {
0719     struct grtc_regs *regs = pDev->regs;
0720     SPIN_IRQFLAGS(irqflags);
0721
0722     SPIN_LOCK_IRQ(&pDev->devlock, irqflags);
0723
0724     /* Disable the receiver */
0725     regs->cor = GRTC_SEB;
0726
0727     /* disable all interrupts and clear them */
0728     regs->imr = 0;
0729     READ_REG(&regs->pir);
0730     regs->picr = GRTC_INT_ALL;
0731
0732     DBG("GRTC: STOPPED\n");
0733
0734     if (overrun) {
0735         pDev->overrun_condition = 1;
0736     } else {
0737         pDev->running = 0;
0738     }
0739
0740     SPIN_UNLOCK_IRQ(&pDev->devlock, irqflags);
0741
0742     /* Flush semaphores in case a thread is stuck waiting for CLTUs (RX data) */
0743     rtems_semaphore_flush(pDev->sem_rx);
0744 }
0745
0746 /* Wait until 'count' bytes are available in receive buffer, or until
0747  * the timeout expires.
0748  */
0749 static int grtc_wait_data(struct grtc_priv *pDev, int count, rtems_interval timeout)
0750 {
0751     int avail;
0752     int ret;
0753     SPIN_IRQFLAGS(irqflags);
0754
0755     FUNCDBG();
0756
0757     if ( count < 1 )
0758         return 0;
0759
0760     SPIN_LOCK_IRQ(&pDev->devlock, irqflags);
0761
0762     /* Enable interrupts when receiving CLTUs, Also clear old pending CLTUs store
0763      * interrupts.
0764      */
0765     pDev->regs->picr = GRTC_INT_CS;
0766     pDev->regs->imr = READ_REG(&pDev->regs->imr) | GRTC_INT_CS;
0767
0768     avail = grtc_data_avail(pDev);
0769     if ( avail < count ) {
0770         /* Wait for interrupt. */
0771
0772         SPIN_UNLOCK_IRQ(&pDev->devlock, irqflags);
0773
0774         if ( timeout == 0 ){
0775             timeout = RTEMS_NO_TIMEOUT;
0776         }
0777         ret = rtems_semaphore_obtain(pDev->sem_rx,RTEMS_WAIT,timeout);
0778         /* RTEMS_SUCCESSFUL  = interrupt signaled data is available
0779          * RTEMS_TIMEOUT     = timeout expired, probably not enough data available
0780          * RTEMS_UNSATISFIED = driver has been closed or an error (overrun) occured
0781          *                     which should cancel this operation.
0782          * RTEMS_OBJECT_WAS_DELETED, RTEMS_INVALID_ID = driver error.
0783          */
0784         SPIN_LOCK_IRQ(&pDev->devlock, irqflags);
0785     }else{
0786         ret = RTEMS_SUCCESSFUL;
0787     }
0788
0789     /* Disable interrupts when receiving CLTUs */
0790     pDev->regs->imr = READ_REG(&pDev->regs->imr) & ~GRTC_INT_CS;
0791
0792     SPIN_UNLOCK_IRQ(&pDev->devlock, irqflags);
0793
0794     return ret;
0795 }
0796
0797 static rtems_device_driver grtc_open(
0798     rtems_device_major_number major,
0799     rtems_device_minor_number minor,
0800     void *arg)
0801 {
0802     struct grtc_priv *pDev;
0803     struct drvmgr_dev *dev;
0804
0805     FUNCDBG();
0806
0807     if ( drvmgr_get_dev(&grtc_drv_info.general, minor, &dev) ) {
0808         DBG("Wrong minor %d\n", minor);
0809         return RTEMS_INVALID_NUMBER;
0810     }
0811     pDev = (struct grtc_priv *)dev->priv;
0812
0813     /* Wait until we get semaphore */
0814     if ( rtems_semaphore_obtain(grtc_dev_sem, RTEMS_WAIT, RTEMS_NO_TIMEOUT) != RTEMS_SUCCESSFUL ){
0815         return RTEMS_INTERNAL_ERROR;
0816     }
0817
0818     /* Is device in use? */
0819     if ( pDev->open ){
0820         rtems_semaphore_release(grtc_dev_sem);
0821         return RTEMS_RESOURCE_IN_USE;
0822     }
0823
0824     /* Mark device taken */
0825     pDev->open = 1;
0826
0827     rtems_semaphore_release(grtc_dev_sem);
0828
0829     DBG("grtc_open: OPENED minor %d (pDev: 0x%x)\n",pDev->minor,(unsigned int)pDev);
0830
0831     /* Set defaults */
0832     pDev->buf = NULL;
0833     pDev->_buf = NULL;
0834     pDev->buf_custom = 0;
0835     pDev->buf_remote = 0;
0836     pDev->len = 0;
0837     pDev->timeout = 0; /* no timeout */
0838     pDev->blocking = 0; /* polling mode */
0839     pDev->mode = GRTC_MODE_RAW; /* Always default to Raw mode */
0840     pDev->ready.head = NULL;
0841     pDev->ready.tail = NULL;
0842     pDev->ready.cnt = 0;
0843
0844     pDev->running = 0;
0845     pDev->overrun_condition = 0;
0846
0847     memset(&pDev->config,0,sizeof(pDev->config));
0848
0849     /* The core has been reset when we execute here, so it is possible
0850      * to read out defualts from core.
0851      */
0852     grtc_hw_get_defaults(pDev,&pDev->config);
0853
0854     return RTEMS_SUCCESSFUL;
0855 }
0856
0857 static rtems_device_driver grtc_close(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
0858 {
0859     struct grtc_priv *pDev;
0860     struct drvmgr_dev *dev;
0861
0862     FUNCDBG();
0863
0864     if ( drvmgr_get_dev(&grtc_drv_info.general, minor, &dev) ) {
0865         return RTEMS_INVALID_NUMBER;
0866     }
0867     pDev = (struct grtc_priv *)dev->priv;
0868
0869     if ( pDev->running ){
0870         grtc_stop(pDev, 0);
0871     }
0872
0873     /* Reset core */
0874     grtc_hw_reset(pDev);
0875
0876     /* Mark not open */
0877     pDev->open = 0;
0878
0879     return RTEMS_SUCCESSFUL;
0880 }
0881
0882 static rtems_device_driver grtc_read(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
0883 {
0884     struct grtc_priv *pDev;
0885     struct drvmgr_dev *dev;
0886     int count;
0887     int left;
0888     int timedout;
0889     int err;
0890     rtems_interval timeout;
0891     rtems_libio_rw_args_t *rw_args;
0892
0893     FUNCDBG();
0894
0895     if ( drvmgr_get_dev(&grtc_drv_info.general, minor, &dev) ) {
0896         return RTEMS_INVALID_NUMBER;
0897     }
0898     pDev = (struct grtc_priv *)dev->priv;
0899
0900     if ( !pDev->running && !pDev->overrun_condition ) {
0901         return RTEMS_RESOURCE_IN_USE;
0902     }
0903
0904     if ( pDev->mode != GRTC_MODE_RAW ) {
0905         return RTEMS_NOT_DEFINED;
0906     }
0907
0908     rw_args = (rtems_libio_rw_args_t *) arg;
0909     left = rw_args->count;
0910     timedout = 0;
0911     timeout = pDev->timeout;
0912
0913 read_from_buffer:
0914     /* Read maximally rw_args->count bytes from receive buffer */
0915     count = grtc_hw_read_try(pDev,rw_args->buffer,left);
0916
0917     left -= count;
0918
0919     DBG("READ %d bytes from DMA, left: %d\n",count,left);
0920
0921     if ( !timedout && !pDev->overrun_condition && ((count < 1) || ((count < rw_args->count) && (pDev->blocking == GRTC_BLKMODE_COMPLETE))) ){
0922         /* didn't read anything (no data available) or we want to wait for all bytes requested.
0923          *
0924          * Wait for data to arrive only in blocking mode
0925          */
0926         if ( pDev->blocking ) {
0927             if ( (err=grtc_wait_data(pDev,left,timeout)) != RTEMS_SUCCESSFUL ){
0928                 /* Some kind of error, closed, overrun etc. */
0929                 if ( err == RTEMS_TIMEOUT ){
0930                     /* Got a timeout, we try to read as much as possible */
0931                     timedout = 1;
0932                     goto read_from_buffer;
0933                 }
0934                 return err;
0935             }
0936             goto read_from_buffer;
0937         }
0938         /* Non-blocking mode and no data read. */
0939         return RTEMS_TIMEOUT;
0940     }
0941
0942     /* Tell caller how much was read. */
0943
0944     DBG("READ returning %d bytes, left: %d\n",rw_args->count-left,left);
0945
0946     rw_args->bytes_moved = rw_args->count - left;
0947     if ( rw_args->bytes_moved == 0 ) {
0948         if ( pDev->overrun_condition ) {
0949             /* signal to the user that overrun has happend when
0950              * no more data can be read out.
0951              */
0952             return RTEMS_IO_ERROR;
0953         }
0954         return RTEMS_TIMEOUT;
0955     }
0956
0957     return RTEMS_SUCCESSFUL;
0958 }
0959
0960 static rtems_device_driver grtc_write(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
0961 {
0962     FUNCDBG();
0963     return RTEMS_NOT_IMPLEMENTED;
0964 }
0965
0966 static int grtc_pool_add_frms(struct grtc_frame *frms)
0967 {
0968     struct grtc_frame *frm, *next;
0969
0970     /* Add frames to pools */
0971     frm = frms;
0972     while(frm){
0973
0974         if ( !frm->pool ) {
0975             /* */
0976             DBG("GRTC: Frame not assigned to a pool\n");
0977             return -1;
0978         }
0979         next = frm->next; /* Remember next frame to process */
0980
0981         DBG("GRTC: adding frame 0x%x to pool %d (%d)\n",frm,frm->pool->frame_len,frm->pool->frame_cnt);
0982
0983         /* Insert Frame into pool */
0984         frm->next = frm->pool->frms;
0985         frm->pool->frms = frm;
0986         frm->pool->frame_cnt++;
0987
0988         frm = next;
0989     }
0990
0991     return 0;
0992 }
0993
0994 static struct grtc_frame *grtc_pool_get_frm(struct grtc_priv *pDev, int frame_len, int *error)
0995 {
0996     struct grtc_frame *frm;
0997     struct grtc_frame_pool *pool;
0998     int i;
0999
1000     /* Loop through all pools until a pool is found
1001      * with a matching (or larger) frame length
1002      */
1003     pool = pDev->pools;
1004     for (i=0; i<pDev->pool_cnt; i++,pool++) {
1005         if ( pool->frame_len >= frame_len ) {
1006             /* Found a good pool ==> get frame */
1007             frm = pool->frms;
1008             if ( !frm ) {
1009                 /* not enough frames available for this
1010                  * frame length, we try next
1011                  *
1012                  * If this is a severe error add your handling
1013                  * code here.
1014                  */
1015 #if 0
1016                 if ( error )
1017                     *error = 0;
1018                 return 0;
1019 #endif
1020                 continue;
1021             }
1022
1023             /* Got a frame, the frame is taken out of the
1024              * pool for usage.
1025              */
1026             pool->frms = frm->next;
1027             pool->frame_cnt--;
1028             return frm;
1029         }
1030     }
1031
1032     if ( error )
1033         *error = 1;
1034
1035     /* Didn't find any frames */
1036     return NULL;
1037 }
1038
1039 /* Return number of bytes processed, Stops at the first occurance
1040  * of the pattern given in 'pattern'
1041  */
1042 static int grtc_scan(unsigned short *src, int max, unsigned char pattern, int *found)
1043 {
1044     unsigned short tmp = 0;
1045     unsigned int left = max;
1046
1047     while ( (left>1) && (((tmp=*src) & 0x00ff) != pattern) ) {
1048         src++;
1049         left-=2;
1050     }
1051     if ( (tmp & 0xff) == pattern ) {
1052         *found = 1;
1053     } else {
1054         *found = 0;
1055     }
1056     return max-left;
1057 }
1058
1059 static int grtc_copy(unsigned short *src, unsigned char *buf, int cnt)
1060 {
1061     unsigned short tmp;
1062     int left = cnt;
1063
1064     while ( (left>0) && ((((tmp=*src) & 0x00ff) == 0x00) || ((tmp & 0x00ff) == 0x01)) ) {
1065         *buf++ = tmp>>8;
1066         src++;
1067         left--;
1068     }
1069
1070     return cnt-left;
1071 }
1072
1073
1074 static int grtc_hw_find_frm(struct grtc_priv *pDev)
1075 {
1076     struct grtc_regs *regs = pDev->regs;
1077     unsigned int rp, wp, asr, bufmax, rrp, rwp;
1078     unsigned int upper, lower;
1079     unsigned int count, cnt;
1080     int found;
1081
1082     FUNCDBG();
1083
1084     rp = READ_REG(&regs->rp);
1085     asr = READ_REG(&regs->asr);
1086     wp = READ_REG(&regs->wp);
1087
1088     /* Quick Check for most common case where Start of frame is at next
1089      * data byte.
1090      */
1091     if ( rp != wp ) {
1092         /* At least 1 byte in buffer */
1093         if ( ((*(unsigned short *)((rp - (unsigned int)pDev->buf_remote) + (unsigned int)pDev->buf)) & 0x00ff) == 0x01 ) {
1094             return 0;
1095         }
1096     }
1097
1098     bufmax = (asr & GRTC_ASR_RXLEN) >> GRTC_ASR_RXLEN_BIT;
1099     bufmax = (bufmax+1) << 10; /* Convert from 1kbyte blocks into bytes */
1100
1101     /* Relative rp and wp */
1102     rrp = rp - (asr & GRTC_ASR_BUFST);
1103     rwp = wp - (asr & GRTC_ASR_BUFST);
1104
1105     lower = grtc_hw_data_avail_lower(rrp,rwp,bufmax);
1106     upper = grtc_hw_data_avail_upper(rrp,rwp,bufmax);
1107
1108     DBG("grtc_hw_find_frm: AVAIL: Lower: %d, Upper: %d\n",lower,upper);
1109     DBG("grtc_hw_find_frm: rp: 0x%x, rrp: 0x%x, wp: 0x%x, rwp: 0x%x, bufmax: %d\n, start: 0x%x\n",
1110         rp,rrp,wp,rwp,bufmax,pDev->buf_remote);
1111
1112     if ( (upper+lower) == 0 )
1113         return 1;
1114
1115     /* Count bytes will be read */
1116     count = 0;
1117     found = 0;
1118
1119     /* Read from upper part of data buffer */
1120     if ( upper > 0 ){
1121         cnt = grtc_scan((unsigned short *)((rp - (unsigned int)pDev->buf_remote) + (unsigned int)pDev->buf), upper, 0x01, &found);
1122         count = cnt;
1123         if ( found ) {
1124             DBG("grtc_hw_find_frm: SCANNED upper %d bytes until found\n",cnt);
1125             goto out;
1126         }
1127
1128         DBG("grtc_hw_find_frm: SCANNED all upper %d bytes, not found\n",cnt);
1129     }
1130
1131     /* Read from lower part of data buffer */
1132     if ( lower > 0 ){
1133         cnt = grtc_scan((unsigned short *)pDev->buf, lower, 0x01, &found);
1134         count += cnt;
1135
1136         if ( found ) {
1137             DBG("grtc_hw_find_frm: SCANNED lower %d bytes until found\n",cnt);
1138             goto out;
1139         }
1140
1141         DBG("grtc_hw_find_frm: SCANNED all lower %d bytes, not found\n",cnt);
1142     }
1143
1144 out:
1145     /* Update hardware RP pointer to tell hardware about new space available */
1146     if ( count > 0 ) {
1147         if ( (rp+count) >= ((asr&GRTC_ASR_BUFST)+bufmax) ){
1148             regs->rp = (rp+count-bufmax);
1149         } else {
1150             regs->rp = rp+count;
1151         }
1152     }
1153     if ( found )
1154         return 0;
1155     return 1;
1156
1157 }
1158
1159 static int grtc_check_ending(unsigned short *src, int max, int end)
1160 {
1161     while ( max > 0 ) {
1162         /* Check Filler */
1163         if ( *src != 0x5500 ) {
1164             /* Filler is wrong */
1165             return -1;
1166         }
1167         src++;
1168         max-=2;
1169     }
1170
1171     /* Check ending (at least */
1172     if ( end ) {
1173         if ( (*src & 0x00ff) != 0x02 ) {
1174             return -1;
1175         }
1176     }
1177
1178     return 0;
1179 }
1180
1181 static int grtc_hw_check_ending(struct grtc_priv *pDev, int max)
1182 {
1183     struct grtc_regs *regs = pDev->regs;
1184     unsigned int rp, wp, asr, bufmax, rrp, rwp;
1185     unsigned int upper, lower;
1186     unsigned int count, cnt, left;
1187
1188     FUNCDBG();
1189
1190     if ( max < 1 )
1191         return 0;
1192     max = max*2;
1193     max += 2; /* Check ending also (2 byte extra) */
1194
1195     rp = READ_REG(&regs->rp);
1196     asr = READ_REG(&regs->asr);
1197     bufmax = (asr & GRTC_ASR_RXLEN) >> GRTC_ASR_RXLEN_BIT;
1198     bufmax = (bufmax+1) << 10; /* Convert from 1kbyte blocks into bytes */
1199     wp = READ_REG(&regs->wp);
1200
1201     /* Relative rp and wp */
1202     rrp = rp - (asr & GRTC_ASR_BUFST);
1203     rwp = wp - (asr & GRTC_ASR_BUFST);
1204
1205     lower = grtc_hw_data_avail_lower(rrp,rwp,bufmax);
1206     upper = grtc_hw_data_avail_upper(rrp,rwp,bufmax);
1207
1208     DBG("grtc_hw_check_ending: AVAIL: Lower: %d, Upper: %d\n",lower,upper);
1209     DBG("grtc_hw_check_ending: rp: 0x%x, rrp: 0x%x, wp: 0x%x, rwp: 0x%x, bufmax: %d\n, start: 0x%x\n",
1210         rp,rrp,wp,rwp,bufmax,pDev->buf_remote);
1211
1212     if ( (upper+lower) < max )
1213         return 0;
1214
1215     /* Count bytes will be read */
1216     count = max;
1217     left = count;
1218
1219     /* Read from upper part of data buffer */
1220     if ( upper > 0 ){
1221         if ( left <= upper ){
1222             cnt = left;
1223             if ( grtc_check_ending((unsigned short *)((rp-(unsigned int)pDev->buf_remote)+(unsigned int)pDev->buf), cnt-2, 1) ) {
1224                 return -1;
1225             }
1226         }else{
1227             cnt = upper;    /* Read all upper data available */
1228             if ( grtc_check_ending((unsigned short *)((rp-(unsigned int)pDev->buf_remote)+(unsigned int)pDev->buf), cnt, 0) ) {
1229                 return -1;
1230             }
1231         }
1232         left -= cnt;
1233     }
1234
1235     /* Read from lower part of data buffer */
1236     if ( left > 0 ){
1237         cnt = left;
1238         if ( grtc_check_ending((unsigned short *)pDev->buf, cnt-2, 1) ) {
1239             return -1;
1240         }
1241         left -= cnt;
1242     }
1243
1244     /* Update hardware RP pointer to tell hardware about new space available */
1245     if ( (rp+count) >= ((asr&GRTC_ASR_BUFST)+bufmax) ){
1246         regs->rp = (rp+count-bufmax);
1247     } else {
1248         regs->rp = rp+count;
1249     }
1250
1251     return 0;
1252 }
1253
1254 /* Copies Data from DMA area to buf, the control bytes are stripped. For
1255  * every data byte, in the DMA area, one control byte is stripped.
1256  */
1257 static int grtc_hw_copy(struct grtc_priv *pDev, unsigned char *buf, int max, int partial)
1258 {
1259     struct grtc_regs *regs = pDev->regs;
1260     unsigned int rp, wp, asr, bufmax, rrp, rwp;
1261     unsigned int upper, lower;
1262     unsigned int count, cnt, left;
1263     int ret, tot, tmp;
1264
1265     FUNCDBG();
1266
1267     if ( max < 1 )
1268         return 0;
1269
1270     rp = READ_REG(&regs->rp);
1271     asr = READ_REG(&regs->asr);
1272     bufmax = (asr & GRTC_ASR_RXLEN) >> GRTC_ASR_RXLEN_BIT;
1273     bufmax = (bufmax+1) << 10; /* Convert from 1kbyte blocks into bytes */
1274     wp = READ_REG(&regs->wp);
1275
1276     /* Relative rp and wp */
1277     rrp = rp - (asr & GRTC_ASR_BUFST);
1278     rwp = wp - (asr & GRTC_ASR_BUFST);
1279
1280     lower = grtc_hw_data_avail_lower(rrp,rwp,bufmax) >> 1;
1281     upper = grtc_hw_data_avail_upper(rrp,rwp,bufmax) >> 1;
1282
1283     DBG("grtc_hw_copy: AVAIL: Lower: %d, Upper: %d\n",lower,upper);
1284     DBG("grtc_hw_copy: rp: 0x%x, rrp: 0x%x, wp: 0x%x, rwp: 0x%x, bufmax: %d\n, start: 0x%x\n",
1285         rp,rrp,wp,rwp,bufmax,pDev->buf_remote);
1286
1287     if ( (upper+lower) == 0 || (!partial && ((upper+lower)<max) ) )
1288         return 0;
1289
1290     /* Count bytes will be read */
1291     count = (upper+lower) > max ? max : (upper+lower);
1292     left = count;
1293     tot = 0;
1294
1295     /* Read from upper part of data buffer */
1296     if ( upper > 0 ){
1297         if ( left < upper ){
1298             cnt = left;
1299         }else{
1300             cnt = upper;    /* Read all upper data available */
1301         }
1302         DBG("grtc_hw_copy: COPYING %d from upper\n",cnt);
1303         if ( (tot=grtc_copy((unsigned short *)((rp-(unsigned int)pDev->buf_remote)+(unsigned int)pDev->buf), buf, cnt)) != cnt ) {
1304             /* Failed to copy due to an receive error */
1305             DBG("grtc_hw_copy(upper): not all in DMA buffer (%d)\n",tot);
1306             count = tot;
1307             ret = -1;
1308             goto out;
1309         }
1310         buf += cnt;
1311         left -= cnt;
1312     }
1313
1314     /* Read from lower part of data buffer */
1315     if ( left > 0 ){
1316         if ( left < lower ){
1317             cnt = left;
1318         }else{
1319             cnt = lower;    /* Read all lower data available */
1320         }
1321         DBG("grtc_hw_copy: COPYING %d from lower\n",cnt);
1322         if ( (tmp=grtc_copy((unsigned short *)pDev->buf, buf, cnt)) != cnt ) {
1323             /* Failed to copy due to an receive error */
1324             DBG("grtc_hw_copy(lower): not all in DMA buffer (%d)\n",tot);
1325             count = tot+tmp;
1326             ret = -1;
1327             goto out;
1328         }
1329         buf += cnt;
1330         left -= cnt;
1331     }
1332     ret = count;
1333
1334 out:
1335     count = count*2;
1336     /* Update hardware RP pointer to tell hardware about new space available */
1337     if ( (rp+count) >= ((asr&GRTC_ASR_BUFST)+bufmax) ){
1338         regs->rp = (rp+count-bufmax);
1339     } else {
1340         regs->rp = rp+count;
1341     }
1342
1343     return ret;
1344 }
1345
1346 #ifdef DEBUG_ERROR
1347 void grtc_log_error(struct grtc_priv *pDev, int err)
1348 {
1349     /* Stop Receiver */
1350     *(volatile unsigned int *)&pDev->regs->cor = 0x55000000;
1351     *(volatile unsigned int *)&pDev->regs->cor = 0x55000000;
1352     pDev->last_error[pDev->last_error_cnt] = err;
1353     if ( ++pDev->last_error_cnt > 128 )
1354         pDev->last_error_cnt = 0;
1355 }
1356 #endif
1357
1358 /* Read one frame from DMA buffer
1359  *
1360  * Return Values
1361  *  Zero - nothing more to process
1362  *  1 - more to process, no free frames
1363  *  2 - more to process, frame received
1364  *  negative - more to process, frame dropped
1365  */
1366 static int process_dma(struct grtc_priv *pDev)
1367 {
1368     int ret, err;
1369     int left, total_len;
1370     unsigned char *dst;
1371     struct grtc_frame *frm;
1372
1373     switch( pDev->frame_state ) {
1374         case FRM_STATE_NONE:
1375         DBG2("FRAME_STATE_NONE\n");
1376
1377         /* Find Start of next frame by searching for 0x01 */
1378         ret = grtc_hw_find_frm(pDev);
1379         if ( ret != 0 ) {
1380             /* Frame start not found */
1381             return 0;
1382         }
1383
1384         /* Start of frame found, Try to copy header */
1385         pDev->frm = NULL;
1386         pDev->frame_state = FRM_STATE_HDR;
1387         /* Fall through */
1388
1389         case FRM_STATE_HDR:
1390         DBG2("FRAME_STATE_HDR\n");
1391
1392         /* Wait for all of header to be in place by setting partial to 0 */
1393         ret = grtc_hw_copy(pDev, (unsigned char *)pDev->hdr, 5, 0);
1394         if ( ret < 0 ) {
1395             /* Error copying header, restart scanning for new frame */
1396             DEBUG_ERR_LOG(pDev,1);
1397             pDev->stats.err++;
1398             pDev->stats.err_hdr++;
1399             DBG("FRAME_STATE_HDR: copying failed %d\n",ret);
1400             pDev->frame_state = FRM_STATE_NONE;
1401             return -1;
1402         } else if ( ret != 5 ) {
1403             DBG("FRAME_STATE_HDR: no header (%d)\n",ret);
1404             /* Not all bytes available, come back later */
1405             return 0;
1406         }
1407
1408         /* The complete header has been copied, parse it */
1409         pDev->frmlen = (((unsigned short *)pDev->hdr)[1] & 0x3ff)+1;
1410         if ( pDev->frmlen < 5 ) {
1411             /* Error: frame length is not correct */
1412             pDev->stats.err++;
1413             pDev->stats.err_hdr++;
1414             DBG("FRAME_STATE_HDR: frame length error: %d\n", pDev->frmlen);
1415             pDev->frame_state = FRM_STATE_NONE;
1416             return -1;
1417         }
1418         pDev->frame_state = FRM_STATE_ALLOC;
1419         /* Fall through */
1420
1421         case FRM_STATE_ALLOC:
1422         DBG2("FRAME_STATE_ALLOC\n");
1423         /* Header has been read, allocate a frame to put payload and header into */
1424
1425         /* Allocate Frame matching Frame length */
1426         err = 0;
1427         frm = grtc_pool_get_frm(pDev,pDev->frmlen,&err);
1428         if ( !frm ) {
1429             /* Couldn't find frame  */
1430             DEBUG_ERR_LOG(pDev,2);
1431             pDev->stats.dropped++;
1432             DBG2("No free frames\n");
1433             if ( err == 0 ){
1434                 /* Frame length exist in pool configuration, but no
1435                  * frames are available for that frame length.
1436                  */
1437                 DEBUG_ERR_LOG(pDev,3);
1438                 pDev->stats.dropped_no_buf++;
1439                 return 1;
1440             } else {
1441                 /* Frame length of incoming frame is larger than the
1442                  * frame length in any of the configured frame pools.
1443                  *
1444                  * This may be because of an corrupt header. We simply
1445                  * scan for the end of frame marker in the DMA buffer
1446                  * so we can drop the frame.
1447                  */
1448                 DEBUG_ERR_LOG(pDev,4);
1449                 pDev->stats.dropped_too_long++;
1450                 pDev->frame_state = FRM_STATE_NONE;
1451                 return -2;
1452             }
1453         }
1454         frm->len = 5; /* Only header currenlty in frame */
1455
1456         /* Copy Frame Header into frame structure */
1457         ((unsigned char*)&frm->hdr)[0] = ((unsigned char*)pDev->hdr)[0];
1458         ((unsigned char*)&frm->hdr)[1] = ((unsigned char*)pDev->hdr)[1];
1459         ((unsigned char*)&frm->hdr)[2] = ((unsigned char*)pDev->hdr)[2];
1460         ((unsigned char*)&frm->hdr)[3] = ((unsigned char*)pDev->hdr)[3];
1461         ((unsigned char*)&frm->hdr)[4] = ((unsigned char*)pDev->hdr)[4];
1462
1463         /* Calc Total and Filler byte count in frame */
1464         total_len = pDev->frmlen / 7;
1465         total_len = total_len * 7;
1466         if ( pDev->frmlen != total_len )
1467             total_len += 7;
1468
1469         pDev->filler = total_len - pDev->frmlen;
1470
1471         pDev->frame_state = FRM_STATE_PAYLOAD;
1472         pDev->frm = frm;
1473         /* Fall through */
1474
1475         case FRM_STATE_PAYLOAD:
1476         DBG2("FRAME_STATE_PAYLOAD\n");
1477         /* Parts of payload and the complete header has been read */
1478         frm = pDev->frm;
1479
1480         dst = (unsigned char *)&frm->data[frm->len-5];
1481         left = pDev->frmlen-frm->len;
1482
1483         ret = grtc_hw_copy(pDev,dst,left,1);
1484         if ( ret < 0 ) {
1485             DEBUG_ERR_LOG(pDev,5);
1486             /* Error copying header, restart scanning for new frame */
1487             pDev->frame_state = FRM_STATE_NONE;
1488             frm->next = NULL;
1489             grtc_pool_add_frms(frm);
1490             pDev->frm = NULL;
1491             pDev->stats.err++;
1492             pDev->stats.err_payload++;
1493             return -1;
1494         } else if ( ret != left ) {
1495             /* Not all bytes available, come back later */
1496             frm->len += ret;
1497             return 0;
1498         }
1499         frm->len += ret;
1500         pDev->frame_state = FRM_STATE_FILLER;
1501         /* Fall through */
1502
1503         case FRM_STATE_FILLER:
1504         DBG2("FRAME_STATE_FILLER\n");
1505         /* check filler data */
1506         frm = pDev->frm;
1507
1508         ret = grtc_hw_check_ending(pDev,pDev->filler);
1509         if ( ret != 0 ) {
1510             /* Error in frame, drop frame */
1511             DEBUG_ERR_LOG(pDev,6);
1512             pDev->frame_state = FRM_STATE_NONE;
1513             frm->next = NULL;
1514             grtc_pool_add_frms(frm);
1515             pDev->frm = NULL;
1516             pDev->stats.err++;
1517             pDev->stats.err_ending++;
1518             return -1;
1519         }
1520
1521         /* A complete frame received, put it into received frame queue */
1522         if ( pDev->ready.head ) {
1523             /* Queue not empty */
1524             pDev->ready.tail->next = frm;
1525         } else {
1526             /* Queue empty */
1527             pDev->ready.head = frm;
1528         }
1529         pDev->ready.tail = frm;
1530         frm->next = NULL;
1531         pDev->ready.cnt++;
1532         pDev->stats.frames_recv++;
1533
1534         pDev->frame_state = FRM_STATE_NONE;
1535         frm->next = NULL;
1536         return 2;
1537
1538 #if 0
1539         case FRM_STATE_DROP:
1540         DBG2("FRAME_STATE_DROP\n");
1541         break;
1542 #endif
1543
1544         default:
1545         printk("GRTC: internal error\n");
1546         pDev->frame_state = FRM_STATE_NONE;
1547         break;
1548     }
1549
1550     return 0;
1551 }
1552
1553 static rtems_device_driver grtc_ioctl(rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
1554 {
1555     struct grtc_priv *pDev;
1556     struct drvmgr_dev *dev;
1557     rtems_libio_ioctl_args_t *ioarg = (rtems_libio_ioctl_args_t *)arg;
1558     unsigned int *data;
1559     int status,frm_len,i,ret;
1560     struct grtc_ioc_buf_params *buf_arg;
1561     struct grtc_ioc_config *cfg;
1562     struct grtc_ioc_hw_status *hwregs;
1563     struct grtc_ioc_pools_setup *pocfg;
1564     struct grtc_ioc_assign_frm_pool *poassign;
1565     struct grtc_frame *frm, *frms;
1566     struct grtc_frame_pool *pool;
1567     struct grtc_list *frmlist;
1568     struct grtc_ioc_stats *stats;
1569     unsigned int mem;
1570     IRQ_LOCAL_DECLARE(oldLevel);
1571
1572     FUNCDBG();
1573
1574     if ( drvmgr_get_dev(&grtc_drv_info.general, minor, &dev) ) {
1575         return RTEMS_INVALID_NUMBER;
1576     }
1577     pDev = (struct grtc_priv *)dev->priv;
1578
1579     if (!ioarg)
1580         return RTEMS_INVALID_NAME;
1581
1582         data = ioarg->buffer;
1583
1584     ioarg->ioctl_return = 0;
1585     switch(ioarg->command) {
1586         case GRTC_IOC_START:
1587         if ( pDev->running ) {
1588             return RTEMS_RESOURCE_IN_USE; /* EBUSY */
1589         }
1590         if ( (status=grtc_start(pDev)) != RTEMS_SUCCESSFUL ){
1591             return status;
1592         }
1593         /* Register ISR and Unmask interrupt */
1594         drvmgr_interrupt_register(pDev->dev, 0, "grtc", grtc_interrupt, pDev);
1595
1596         /* Read and write are now open... */
1597         break;
1598
1599         case GRTC_IOC_STOP:
1600         if ( !pDev->running ) {
1601             return RTEMS_RESOURCE_IN_USE;
1602         }
1603         drvmgr_interrupt_unregister(pDev->dev, 0, grtc_interrupt, pDev);
1604         grtc_stop(pDev, 0);
1605         break;
1606
1607         case GRTC_IOC_ISSTARTED:
1608         if ( !pDev->running ) {
1609             return RTEMS_RESOURCE_IN_USE;
1610         } else if ( pDev->overrun_condition ) {
1611             return RTEMS_IO_ERROR;
1612         }
1613         break;
1614
1615         case GRTC_IOC_SET_BLOCKING_MODE:
1616         if ( (unsigned int)data > GRTC_BLKMODE_COMPLETE ) {
1617             return RTEMS_INVALID_NAME;
1618         }
1619         DBG("GRTC: Set blocking mode: %d\n",(unsigned int)data);
1620         pDev->blocking = (unsigned int)data;
1621         break;
1622
1623         case GRTC_IOC_SET_TIMEOUT:
1624         DBG("GRTC: Timeout: %d\n",(unsigned int)data);
1625         pDev->timeout = (rtems_interval)data;
1626         break;
1627
1628         case GRTC_IOC_SET_BUF_PARAM:
1629         if ( pDev->running ) {
1630             return RTEMS_RESOURCE_IN_USE; /* EBUSY */
1631         }
1632
1633         buf_arg = (struct grtc_ioc_buf_params *)data;
1634         if ( !buf_arg ) {
1635             return RTEMS_INVALID_NAME;
1636         }
1637
1638         DBG("GRTC: IOC_SET_BUF_PARAM: Len: 0x%x, Custom Buffer: 0x%x\n",buf_arg->length,buf_arg->custom_buffer);
1639
1640         /* Check alignment need, skip bit 0 since that bit only indicates remote address or not */
1641         if ( (unsigned int)buf_arg->custom_buffer & (~GRTC_BUF_MASK) & (~0x1) ) {
1642             return RTEMS_INVALID_NAME;
1643         }
1644
1645         if ( buf_arg->length > 0x100 ){
1646             DBG("GRTC: Too big buffer requested\n");
1647             return RTEMS_INVALID_NAME;
1648         }
1649
1650         /* If current buffer allocated by driver we must free it */
1651         if ( !pDev->buf_custom && pDev->buf ){
1652             free(pDev->_buf);
1653             pDev->_buf = NULL;
1654         }
1655         pDev->buf = NULL;
1656         pDev->len = buf_arg->length*1024;
1657
1658         if (pDev->len <= 0)
1659             break;
1660         mem = (unsigned int)buf_arg->custom_buffer;
1661         pDev->buf_custom = mem;
1662
1663         if (mem & 1) {
1664             /* Remote address given, the address is as the GRTC
1665              * core looks at it. Translate the base address into
1666              * an address that the CPU can understand.
1667              */
1668             pDev->buf_remote = (void *)(mem & ~0x1);
1669             drvmgr_translate_check(pDev->dev, DMAMEM_TO_CPU,
1670                         (void *)pDev->buf_remote,
1671                         (void **)&pDev->buf,
1672                         pDev->len);
1673         } else {
1674             if (mem == 0) {
1675                 pDev->buf = grtc_memalign((~GRTC_ASR_BUFST)+1,pDev->len,&pDev->_buf);
1676                 DBG("grtc_ioctl: SETBUF: new buf: 0x%x(0x%x), Len: %d\n",pDev->buf,pDev->_buf,pDev->len);
1677                 if (!pDev->buf){
1678                     pDev->len = 0;
1679                     pDev->buf_custom = 0;
1680                     pDev->_buf = NULL;
1681                     pDev->buf_remote = 0;
1682                     DBG("GRTC: Failed to allocate memory\n");
1683                     return RTEMS_NO_MEMORY;
1684                 }
1685             } else{
1686                 pDev->buf = buf_arg->custom_buffer;
1687             }
1688
1689             /* Translate into a remote address so that GRTC core
1690              * on a remote AMBA bus (for example over the PCI bus)
1691              * gets a valid address
1692              */
1693             drvmgr_translate_check(pDev->dev, CPUMEM_TO_DMA,
1694                         (void *)pDev->buf,
1695                         (void **)&pDev->buf_remote,
1696                         pDev->len);
1697         }
1698         break;
1699
1700         case GRTC_IOC_GET_BUF_PARAM:
1701         if ( pDev->running ) {
1702             return RTEMS_RESOURCE_IN_USE; /* EBUSY */
1703         }
1704
1705         buf_arg = (struct grtc_ioc_buf_params *)data;
1706         if ( !buf_arg ) {
1707             return RTEMS_INVALID_NAME;
1708         }
1709
1710         buf_arg->length = pDev->len >> 10; /* Length in 1kByte blocks */
1711         if ( pDev->buf_custom )
1712             buf_arg->custom_buffer =(void *)pDev->buf;
1713         else
1714             buf_arg->custom_buffer = 0; /* Don't reveal internal driver buffer */
1715         break;
1716
1717         case GRTC_IOC_SET_CONFIG:
1718         cfg = (struct grtc_ioc_config *)data;
1719         if ( !cfg ) {
1720             return RTEMS_INVALID_NAME;
1721         }
1722
1723         if ( pDev->running ) {
1724             return RTEMS_RESOURCE_IN_USE;
1725         }
1726
1727         pDev->config = *cfg;
1728         break;
1729
1730         case GRTC_IOC_GET_CONFIG:
1731         cfg = (struct grtc_ioc_config *)data;
1732         if ( !cfg ) {
1733             return RTEMS_INVALID_NAME;
1734         }
1735
1736         *cfg = pDev->config;
1737         break;
1738
1739         case GRTC_IOC_GET_HW_STATUS:
1740         hwregs = (struct grtc_ioc_hw_status *)data;
1741         if ( !hwregs ) {
1742             return RTEMS_INVALID_NAME;
1743         }
1744         /* We disable interrupt on the local CPU in order to get a
1745          * snapshot of the registers.
1746          */
1747         IRQ_LOCAL_DISABLE(oldLevel);
1748         hwregs->sir = READ_REG(&pDev->regs->sir);
1749         hwregs->far = READ_REG(&pDev->regs->far);
1750         hwregs->clcw1   = READ_REG(&pDev->regs->clcw1);
1751         hwregs->clcw2   = READ_REG(&pDev->regs->clcw2);
1752         hwregs->phir    = READ_REG(&pDev->regs->phir);
1753         hwregs->str = READ_REG(&pDev->regs->str);
1754         IRQ_LOCAL_ENABLE(oldLevel);
1755         break;
1756
1757         case GRTC_IOC_GET_STATS:
1758         stats = (struct grtc_ioc_stats *)data;
1759         if ( !stats ) {
1760             return RTEMS_INVALID_NAME;
1761         }
1762         memcpy(stats,&pDev->stats,sizeof(struct grtc_ioc_stats));
1763         break;
1764
1765         case GRTC_IOC_CLR_STATS:
1766         memset(&pDev->stats,0,sizeof(struct grtc_ioc_stats));
1767         break;
1768
1769         case GRTC_IOC_SET_MODE:
1770         if ( pDev->running ) {
1771             return RTEMS_RESOURCE_IN_USE;
1772         }
1773         if ( (int)data == GRTC_MODE_FRAME ) {
1774             pDev->mode = GRTC_MODE_FRAME;
1775         } else if ( (int)data == GRTC_MODE_RAW ) {
1776             pDev->mode = GRTC_MODE_RAW;
1777         } else {
1778             return RTEMS_INVALID_NAME;
1779         }
1780         break;
1781
1782         case GRTC_IOC_POOLS_SETUP:
1783         if ( pDev->running ) {
1784             return RTEMS_RESOURCE_IN_USE;
1785         }
1786         pocfg = (struct grtc_ioc_pools_setup *)data;
1787         if ( (pDev->mode != GRTC_MODE_FRAME) || !pocfg ) {
1788             return RTEMS_INVALID_NAME;
1789         }
1790
1791         /* Check that list is sorted */
1792         frm_len = 0;
1793         for(i=0;i<pocfg->pool_cnt;i++){
1794             if ( pocfg->pool_frame_len[i] <= frm_len ) {
1795                 return RTEMS_INVALID_NAME;
1796             }
1797             frm_len = pocfg->pool_frame_len[i];
1798         }
1799
1800         /* Ok, we trust user. The pool descriptions are allocated
1801          * but not frames, that the user must do self.
1802          */
1803         if ( pDev->pools ) {
1804             free(pDev->pools);
1805         }
1806         pDev->pools = grlib_malloc(pocfg->pool_cnt * sizeof(*pDev->pools));
1807         if ( !pDev->pools ) {
1808             pDev->pool_cnt = 0;
1809             return RTEMS_NO_MEMORY;
1810         }
1811         pDev->pool_cnt = pocfg->pool_cnt;
1812         for (i=0;i<pocfg->pool_cnt;i++) {
1813             pDev->pools[i].frame_len = pocfg->pool_frame_len[i];
1814             pDev->pools[i].frame_cnt = 0;
1815             pDev->pools[i].frms = NULL;
1816         }
1817         break;
1818
1819         case GRTC_IOC_ASSIGN_FRM_POOL:
1820         if ( pDev->running ) {
1821             return RTEMS_RESOURCE_IN_USE;
1822         }
1823
1824         if ( (pDev->mode != GRTC_MODE_FRAME) ) {
1825             return RTEMS_INVALID_NAME;
1826         }
1827
1828         poassign = (struct grtc_ioc_assign_frm_pool *)data;
1829         if ( !poassign ) {
1830             return RTEMS_INVALID_NAME;
1831         }
1832
1833         /* Find pool to assign the frames to */
1834         pool = NULL;
1835         for(i=0; i<pDev->pool_cnt; i++) {
1836             if ( pDev->pools[i].frame_len == poassign->frame_len ) {
1837                 pool = &pDev->pools[i];
1838                 break;
1839             }
1840         }
1841         if ( !pool ) {
1842             /* No Pool matching frame length */
1843             return RTEMS_INVALID_NAME;
1844         }
1845
1846         /* Assign frames to pool */
1847         frm = poassign->frames;
1848         while(frm){
1849             frm->pool = pool;   /* Assign Frame to pool */
1850             frm = frm->next;
1851         }
1852         break;
1853
1854         case GRTC_IOC_ADD_BUFF:
1855         frms = (struct grtc_frame *)data;
1856
1857         if ( (pDev->mode != GRTC_MODE_FRAME) ) {
1858             return RTEMS_NOT_DEFINED;
1859         }
1860         if ( !frms ) {
1861             return RTEMS_INVALID_NAME;
1862         }
1863
1864         /* Add frames to respicative pools */
1865         if ( grtc_pool_add_frms(frms) ) {
1866             return RTEMS_INVALID_NAME;
1867         }
1868         break;
1869
1870         /* Try to read as much data as possible from DMA area and
1871          * put it into free frames.
1872          *
1873          * If receiver is in stopped mode, let user only read previously
1874          * received frames.
1875          */
1876         case GRTC_IOC_RECV:
1877
1878         if ( (pDev->mode != GRTC_MODE_FRAME) ) {
1879             return RTEMS_NOT_DEFINED;
1880         }
1881
1882         while ( pDev->running && ((ret=process_dma(pDev) == 2) || (ret == -1)) ) {
1883             /* Frame received or dropped, process next frame */
1884         }
1885
1886         /* Take frames out from ready queue and put them to user */
1887         frmlist = (struct grtc_list *)data;
1888         if ( !frmlist ) {
1889             return RTEMS_INVALID_NAME;
1890         }
1891
1892         frmlist->head = pDev->ready.head;
1893         frmlist->tail = pDev->ready.tail;
1894         frmlist->cnt = pDev->ready.cnt;
1895
1896         /* Empty list */
1897         pDev->ready.head = NULL;
1898         pDev->ready.tail = NULL;
1899         pDev->ready.cnt = 0;
1900
1901         if ((frmlist->cnt == 0) && pDev->overrun_condition) {
1902             /* signal to the user that overrun has happend when
1903              * no more data can be read out.
1904              */
1905             return RTEMS_IO_ERROR;
1906         }
1907         break;
1908
1909         case GRTC_IOC_GET_CLCW_ADR:
1910         if ( !data ) {
1911             return RTEMS_INVALID_NAME;
1912         }
1913         *data = (unsigned int)&pDev->regs->clcw1;
1914         break;
1915
1916         default:
1917         return RTEMS_NOT_DEFINED;
1918     }
1919     return RTEMS_SUCCESSFUL;
1920 }
1921
1922 static void grtc_interrupt(void *arg)
1923 {
1924     struct grtc_priv *pDev = arg;
1925     struct grtc_regs *regs = pDev->regs;
1926     unsigned int status;
1927     SPIN_ISR_IRQFLAGS(irqflags);
1928
1929     /* Clear interrupt by reading it */
1930     status = READ_REG(&regs->pisr);
1931
1932     /* Spurious Interrupt? */
1933     if ( !pDev->running )
1934         return;
1935
1936     if ( status & GRTC_INT_OV ){
1937         /* Stop core (Disable receiver, interrupts), set overrun condition,
1938          * Flush semaphore if thread waiting for data in grtc_wait_data().
1939          */
1940         grtc_stop(pDev, 1);
1941
1942         /* No need to handle the reset of interrupts, we are still */
1943         goto out;
1944     }
1945
1946     if ( status & GRTC_INT_CS ){
1947         SPIN_LOCK(&pDev->devlock, irqflags);
1948
1949         if ( (pDev->blocking==GRTC_BLKMODE_COMPLETE) && pDev->timeout ){
1950             /* Signal to thread only if enough data is available */
1951             if ( pDev->wait_for_nbytes > grtc_data_avail(pDev) ){
1952                 /* Not enough data available */
1953                 goto procceed_processing_interrupts;
1954             }
1955
1956             /* Enough data is available which means that we should
1957              * wake up the thread sleeping.
1958              */
1959         }
1960
1961         /* Disable further CLTUs Stored interrupts, no point until
1962          * thread waiting for them says it want to wait for more.
1963          */
1964         regs->imr = READ_REG(&regs->imr) & ~GRTC_INT_CS;
1965         SPIN_UNLOCK(&pDev->devlock, irqflags);
1966
1967         /* Signal Semaphore to wake waiting thread in read() */
1968         rtems_semaphore_release(pDev->sem_rx);
1969     }
1970
1971 procceed_processing_interrupts:
1972
1973     if ( status & GRTC_INT_CR ){
1974
1975     }
1976
1977     if ( status & GRTC_INT_FAR ){
1978
1979     }
1980
1981     if ( status & GRTC_INT_BLO ){
1982
1983     }
1984
1985     if ( status & GRTC_INT_RFA ){
1986
1987     }
1988 out:
1989     if ( status )
1990         regs->picr = status;
1991 }
1992
1993 static rtems_device_driver grtc_initialize(
1994   rtems_device_major_number major,
1995   rtems_device_minor_number unused,
1996   void *arg
1997   )
1998 {
1999     /* Device Semaphore created with count = 1 */
2000     if ( rtems_semaphore_create(rtems_build_name('G', 'R', 'T', 'C'),
2001         1,
2002         RTEMS_FIFO|RTEMS_NO_INHERIT_PRIORITY|RTEMS_LOCAL|RTEMS_NO_PRIORITY_CEILING,
2003         0,
2004         &grtc_dev_sem) != RTEMS_SUCCESSFUL ) {
2005         return RTEMS_INTERNAL_ERROR;
2006     }
2007
2008     return RTEMS_SUCCESSFUL;
2009 }